From 6d4fb2d9c5dc44751a19b277054ffb570406eebc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 20 Nov 2023 22:33:01 +0100 Subject: [PATCH 1/7] fix tests for API break of passing controller manager update rate in init method (#854) --- .../test_ackermann_steering_controller.hpp | 2 +- .../test/test_admittance_controller.hpp | 2 +- .../test/test_bicycle_steering_controller.hpp | 2 +- .../test/test_diff_drive_controller.cpp | 34 +++++++++---------- .../test_joint_group_effort_controller.cpp | 2 +- .../test_force_torque_sensor_broadcaster.cpp | 2 +- .../test/test_forward_command_controller.cpp | 2 +- ...i_interface_forward_command_controller.cpp | 2 +- .../test/test_gripper_controllers.cpp | 2 +- .../test/test_imu_sensor_broadcaster.cpp | 2 +- .../test/test_joint_state_broadcaster.cpp | 2 +- .../test/test_trajectory_controller_utils.hpp | 2 +- .../test_joint_group_position_controller.cpp | 2 +- .../test/test_range_sensor_broadcaster.cpp | 2 +- .../test_steering_controllers_library.hpp | 2 +- .../test/test_tricycle_controller.cpp | 14 ++++---- .../test_tricycle_steering_controller.hpp | 2 +- .../test_joint_group_velocity_controller.cpp | 2 +- 18 files changed, 40 insertions(+), 40 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 8af2f6bc94..a2849d5742 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -147,7 +147,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_ackermann_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index db708db6c5..19908d7f9f 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -185,7 +185,7 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::return_type SetUpControllerCommon( const std::string & controller_name, const rclcpp::NodeOptions & options) { - auto result = controller_->init(controller_name, "", "", options); + auto result = controller_->init(controller_name, "", 0, "", options); controller_->export_reference_interfaces(); assign_interfaces(); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 34d6883a83..521506762b 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -144,7 +144,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_bicycle_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 505aa44d2b..eb970d34a3 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -195,7 +195,7 @@ class TestDiffDriveController : public ::testing::Test TEST_F(TestDiffDriveController, configure_fails_without_parameters) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -203,7 +203,7 @@ TEST_F(TestDiffDriveController, configure_fails_without_parameters) TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_side_defined) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -223,7 +223,7 @@ TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_sid TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -239,7 +239,7 @@ TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -259,7 +259,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -292,7 +292,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -327,7 +327,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -363,7 +363,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, test_namespace); + const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -398,7 +398,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, test_namespace); + const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -435,7 +435,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, test_namespace); + const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -469,7 +469,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -483,7 +483,7 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); // We implicitly test that by default position feedback is required @@ -499,7 +499,7 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -516,7 +516,7 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -533,7 +533,7 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -550,7 +550,7 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) TEST_F(TestDiffDriveController, cleanup) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -599,7 +599,7 @@ TEST_F(TestDiffDriveController, cleanup) TEST_F(TestDiffDriveController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 23555c578f..f9d72ab202 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -54,7 +54,7 @@ void JointGroupEffortControllerTest::TearDown() { controller_.reset(nullptr); } void JointGroupEffortControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_effort_controller", ""); + const auto result = controller_->init("test_joint_group_effort_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 82f677410e..bfe7561642 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -53,7 +53,7 @@ void ForceTorqueSensorBroadcasterTest::TearDown() { fts_broadcaster_.reset(nullp void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster() { - const auto result = fts_broadcaster_->init("test_force_torque_sensor_broadcaster", ""); + const auto result = fts_broadcaster_->init("test_force_torque_sensor_broadcaster", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 2ab507ef29..62d5512b4e 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -60,7 +60,7 @@ void ForwardCommandControllerTest::TearDown() { controller_.reset(nullptr); } void ForwardCommandControllerTest::SetUpController() { - const auto result = controller_->init("forward_command_controller", ""); + const auto result = controller_->init("forward_command_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 00ca4ae2d1..7826c85355 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -62,7 +62,7 @@ void MultiInterfaceForwardCommandControllerTest::TearDown() { controller_.reset( void MultiInterfaceForwardCommandControllerTest::SetUpController(bool set_params_and_activate) { - const auto result = controller_->init("multi_interface_forward_command_controller", ""); + const auto result = controller_->init("multi_interface_forward_command_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index 572e755743..0fc10c0c73 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -60,7 +60,7 @@ void GripperControllerTest::TearDown() template void GripperControllerTest::SetUpController() { - const auto result = controller_->init("gripper_controller", ""); + const auto result = controller_->init("gripper_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 0b782efc5f..0fb987ce77 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -53,7 +53,7 @@ void IMUSensorBroadcasterTest::TearDown() { imu_broadcaster_.reset(nullptr); } void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() { - const auto result = imu_broadcaster_->init("test_imu_sensor_broadcaster", ""); + const auto result = imu_broadcaster_->init("test_imu_sensor_broadcaster", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index a6c0708fd9..3a74f599b4 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -69,7 +69,7 @@ void JointStateBroadcasterTest::SetUpStateBroadcaster( void JointStateBroadcasterTest::init_broadcaster_and_set_parameters( const std::vector & joint_names, const std::vector & interfaces) { - const auto result = state_broadcaster_->init("joint_state_broadcaster", ""); + const auto result = state_broadcaster_->init("joint_state_broadcaster", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); state_broadcaster_->get_node()->set_parameter({"joints", joint_names}); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index fdf4305cec..3a7e7e5cf1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -196,7 +196,7 @@ class TrajectoryControllerTest : public ::testing::Test parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); - auto ret = traj_controller_->init(controller_name_, "", "", node_options); + auto ret = traj_controller_->init(controller_name_, "", 0, "", node_options); if (ret != controller_interface::return_type::OK) { FAIL(); diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 07f42e4eba..3b4f00be12 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -54,7 +54,7 @@ void JointGroupPositionControllerTest::TearDown() { controller_.reset(nullptr); void JointGroupPositionControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_position_controller", ""); + const auto result = controller_->init("test_joint_group_position_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index ab937146cb..a857141ea9 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -34,7 +34,7 @@ controller_interface::return_type RangeSensorBroadcasterTest::init_broadcaster( std::string broadcaster_name) { controller_interface::return_type result = controller_interface::return_type::ERROR; - result = range_broadcaster_->init(broadcaster_name, ""); + result = range_broadcaster_->init(broadcaster_name, "", 0); if (controller_interface::return_type::OK == result) { diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 226d4f4291..5caf347ac1 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -168,7 +168,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_steering_controllers_library") { - ASSERT_EQ(controller_->init(controller_name, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 054fff10ce..504ca381ce 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -173,14 +173,14 @@ class TestTricycleController : public ::testing::Test TEST_F(TestTricycleController, configure_fails_without_parameters) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F(TestTricycleController, configure_fails_if_only_traction_or_steering_side_defined) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -200,7 +200,7 @@ TEST_F(TestTricycleController, configure_fails_if_only_traction_or_steering_side TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -213,7 +213,7 @@ TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) TEST_F(TestTricycleController, activate_fails_without_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -227,7 +227,7 @@ TEST_F(TestTricycleController, activate_fails_without_resources_assigned) TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); // We implicitly test that by default position feedback is required @@ -243,7 +243,7 @@ TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) TEST_F(TestTricycleController, cleanup) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -292,7 +292,7 @@ TEST_F(TestTricycleController, cleanup) TEST_F(TestTricycleController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index e9d1023d98..c5f6985d4e 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -146,7 +146,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_tricycle_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 185c630bc9..4cbf1b7342 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -54,7 +54,7 @@ void JointGroupVelocityControllerTest::TearDown() { controller_.reset(nullptr); void JointGroupVelocityControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_velocity_controller", ""); + const auto result = controller_->init("test_joint_group_velocity_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; From ba7405443b1cdb78c0e0f3ec458738da37d690d0 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 21 Nov 2023 11:52:35 +0000 Subject: [PATCH 2/7] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 6 ++++++ admittance_controller/CHANGELOG.rst | 6 ++++++ bicycle_steering_controller/CHANGELOG.rst | 6 ++++++ diff_drive_controller/CHANGELOG.rst | 8 ++++++++ effort_controllers/CHANGELOG.rst | 6 ++++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ forward_command_controller/CHANGELOG.rst | 7 +++++++ gripper_controllers/CHANGELOG.rst | 6 ++++++ imu_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ joint_state_broadcaster/CHANGELOG.rst | 6 ++++++ joint_trajectory_controller/CHANGELOG.rst | 11 +++++++++++ position_controllers/CHANGELOG.rst | 6 ++++++ range_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 6 ++++++ tricycle_controller/CHANGELOG.rst | 6 ++++++ tricycle_steering_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 6 ++++++ 20 files changed, 119 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index d679a782d4..7b9a59976e 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- * Improve docs (`#785 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index ba5a131094..1a359ce855 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 7b14b2d617..f18f0188a7 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- * Improve docs (`#785 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index b3f4b127e3..6862ea574c 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* [diff_drive_controller] Fixed typos in diff_drive_controller_parameter.yaml. (`#822 `_) +* [diff_drive_controller] Remove non-stamped Twist option (`#812 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota, Tony Baltovski + 3.17.0 (2023-10-31) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index baaada7c22..15c3edef1b 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 9b6dea8ef9..8bc4a07db2 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 4bc9d41c74..77f249ccbd 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Resort overview page (`#846 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index b7c7242520..a59b2c24ce 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 8644b008a0..d1fdff1378 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index fd56b72c3b..456e81090b 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 9eece3af21..66d24ea513 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* [JTC] Fix dynamic reconfigure of tolerances (`#849 `_) +* [JTC] Remove unused home pose (`#845 `_) +* [JTC] Activate update of dynamic parameters (`#761 `_) +* [JTC] Fix tests when state offset is used (`#797 `_) +* [JTC] Remove deprecation warnings, set `allow_nonzero_velocity_at_trajectory_end` default false (`#834 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Sai Kishor Kothakota, Dr Denis + 3.17.0 (2023-10-31) ------------------- * Cleanup comments and unnecessary checks (`#803 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 5239c43407..b214480e86 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index b1084b4a24..1f04cf47e7 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 187db3d351..873dd5b55f 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.17.0 (2023-10-31) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index acca28227f..0d1e8ad2ef 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.17.0 (2023-10-31) ------------------- * [TestNodes] Optimize output about setup of JTC publisher (`#792 `_) diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index e723d39c6f..7263a948d5 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.17.0 (2023-10-31) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 8082dc9070..5279d003cf 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- * Steering controllers library: fix open loop mode (`#793 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index bda1c89d32..605a4f896d 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index a7e6854ea8..a41ee0623c 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- * Improve docs (`#785 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 358ee9f499..9d4102e69a 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- From 0d3fc520831d10d4c1d16784e7378c2a0bf7b10f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 21 Nov 2023 11:52:59 +0000 Subject: [PATCH 3/7] 4.0.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 42 files changed, 62 insertions(+), 62 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 7b9a59976e..7a94360e4b 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 656c88feae..421f098f96 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 3.17.0 + 4.0.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 1a359ce855..b94892ac83 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 62cf127d9f..a5f7e7e353 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.17.0 + 4.0.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index f18f0188a7..278921e18b 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 9ee4c53049..e3063672d6 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 3.17.0 + 4.0.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 6862ea574c..5f57b7e833 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * [diff_drive_controller] Fixed typos in diff_drive_controller_parameter.yaml. (`#822 `_) * [diff_drive_controller] Remove non-stamped Twist option (`#812 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 01dc69413a..56b9552f93 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.17.0 + 4.0.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 15c3edef1b..6c7d7d6fa3 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index d0627bea10..ef2610d34f 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.17.0 + 4.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 8bc4a07db2..17a6d4f280 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 81c865ddfb..4a1df9ff11 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.17.0 + 4.0.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 77f249ccbd..941b849edd 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Resort overview page (`#846 `_) * Adjust tests after passing URDF to controllers (`#817 `_) diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 246bc6777d..0fe44110b1 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.17.0 + 4.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index a59b2c24ce..f35dcd1a32 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 391bd1bb7d..08b0298e1f 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 3.17.0 + 4.0.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index d1fdff1378..4d2d2d01b3 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 2dcacc1f0f..1819b974e2 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.17.0 + 4.0.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 456e81090b..ce89b01912 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index e4fecb14d7..1364b1a164 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.17.0 + 4.0.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 66d24ea513..a4486e4e98 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * [JTC] Fix dynamic reconfigure of tolerances (`#849 `_) * [JTC] Remove unused home pose (`#845 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index f4efc2c060..fe10d9583d 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.17.0 + 4.0.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index b214480e86..26462d51a9 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/position_controllers/package.xml b/position_controllers/package.xml index b3cd9b6cfb..2112f7d8c5 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.17.0 + 4.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 1f04cf47e7..71ee9a7119 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index cf6545d65e..3ece4bda4b 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 3.17.0 + 4.0.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 873dd5b55f..2c2e5ec358 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ 3.17.0 (2023-10-31) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 49c87b152d..6a213308c5 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.17.0 + 4.0.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 0d1e8ad2ef..e8eaf9c382 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ 3.17.0 (2023-10-31) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index ef8c7b572d..5698930bff 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.17.0 + 4.0.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 102c39b607..a129b2d0a7 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="3.17.0", + version="4.0.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 7263a948d5..0c08780904 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ 3.17.0 (2023-10-31) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 08f885aef3..41cf5e85d7 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 3.17.0 + 4.0.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index c0f4a96c2a..5a5b979d41 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="3.17.0", + version="4.0.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 5279d003cf..1a76469d6c 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 0194cfd041..932fda9a35 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 3.17.0 + 4.0.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 605a4f896d..6bc3d26f53 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 9a6cf3ef2d..a663dcacde 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.17.0 + 4.0.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index a41ee0623c..0ade9bca61 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index e39971982c..7f9d14c2fe 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 3.17.0 + 4.0.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 9d4102e69a..bb7e231222 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index f336606b26..6ce13e6adf 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.17.0 + 4.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 2854a0bc5da016b6ddc55383e6f7bd7b8e1e57ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 26 Nov 2023 22:06:17 +0100 Subject: [PATCH 4/7] Use testing and remove wrong field of workflow_dispatch (#860) --- .github/workflows/humble-abi-compatibility.yml | 4 +--- .github/workflows/iron-abi-compatibility.yml | 4 +--- .github/workflows/rolling-abi-compatibility.yml | 4 +--- 3 files changed, 3 insertions(+), 9 deletions(-) diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 708ea5c1f4..5c288fabfb 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -1,8 +1,6 @@ name: Humble - ABI Compatibility Check on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble @@ -15,6 +13,6 @@ jobs: - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: humble - ROS_REPO: main + ROS_REPO: testing ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} NOT_TEST_BUILD: true diff --git a/.github/workflows/iron-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml index 20d93f5af1..ab6642625f 100644 --- a/.github/workflows/iron-abi-compatibility.yml +++ b/.github/workflows/iron-abi-compatibility.yml @@ -1,8 +1,6 @@ name: Iron - ABI Compatibility Check on: workflow_dispatch: - branches: - - iron pull_request: branches: - iron @@ -15,6 +13,6 @@ jobs: - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: iron - ROS_REPO: main + ROS_REPO: testing ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} NOT_TEST_BUILD: true diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index 3911434a23..73055ef3e5 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -1,8 +1,6 @@ name: Rolling - ABI Compatibility Check on: workflow_dispatch: - branches: - - master pull_request: branches: - master @@ -15,6 +13,6 @@ jobs: - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: rolling - ROS_REPO: main + ROS_REPO: testing ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} NOT_TEST_BUILD: true From b8532284853c990a83741b85983f18ed38ef5f89 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 27 Nov 2023 12:31:10 +0100 Subject: [PATCH 5/7] [JTC] Improve update methods for tests (#858) --- .../test/test_trajectory_actions.cpp | 16 +- .../test/test_trajectory_controller.cpp | 967 ++++++++---------- .../test/test_trajectory_controller_utils.hpp | 143 ++- 3 files changed, 565 insertions(+), 561 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 3b80abcb9b..ed13a24485 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -242,7 +242,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -298,7 +298,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -350,7 +350,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -409,7 +409,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -460,7 +460,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) @@ -509,7 +509,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) @@ -555,7 +555,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) @@ -603,7 +603,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) std::vector cancelled_position{joint_pos_[0], joint_pos_[1], joint_pos_[2]}; // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position, // i.e., active but trivial trajectory (one point only) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 8b29c2cb16..7c026d5e7a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -26,8 +26,6 @@ #include #include -#include "gmock/gmock.h" - #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" #include "controller_interface/controller_interface.hpp" @@ -103,77 +101,20 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names) const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - std::vector state_interface_names; - state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : state_interface_types_) - { - state_interface_names.push_back(joint + "/" + interface); - } - } - auto state_interfaces = traj_controller_->state_interface_configuration(); - EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); - ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); - - std::vector command_interface_names; - command_interface_names.reserve(joint_names_.size() * command_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : command_interface_types_) - { - command_interface_names.push_back(joint + "/" + interface); - } - } - auto command_interfaces = traj_controller_->command_interface_configuration(); - EXPECT_EQ( - command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(command_interfaces.names.size(), joint_names_.size() * command_interface_types_.size()); - ASSERT_THAT( - command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + compare_joints(joint_names_, joint_names_); } TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints) { rclcpp::executors::MultiThreadedExecutor executor; - // set command_joints parameter + // set command_joints parameter different than joint_names_ const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); SetUpTrajectoryController(executor, {command_joint_names_param}); const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - std::vector state_interface_names; - state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : state_interface_types_) - { - state_interface_names.push_back(joint + "/" + interface); - } - } - auto state_interfaces = traj_controller_->state_interface_configuration(); - EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); - ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); - - std::vector command_interface_names; - command_interface_names.reserve(command_joint_names_.size() * command_interface_types_.size()); - for (const auto & joint : command_joint_names_) - { - for (const auto & interface : command_interface_types_) - { - command_interface_names.push_back(joint + "/" + interface); - } - } - auto command_interfaces = traj_controller_->command_interface_configuration(); - EXPECT_EQ( - command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ( - command_interfaces.names.size(), command_joint_names_.size() * command_interface_types_.size()); - ASSERT_THAT( - command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + compare_joints(joint_names_, command_joint_names_); } TEST_P(TrajectoryControllerTestParameterized, activate) @@ -316,6 +257,11 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param executor.cancel(); } +/** + * @brief test if correct topic is received + * + * this test doesn't use class variables but subscribes to the state topic + */ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { rclcpp::executors::SingleThreadedExecutor executor; @@ -422,7 +368,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) SetUpAndActivateTrajectoryController(executor); - updateController(); + updateControllerAsync(); auto pids = traj_controller_->get_pids(); if (traj_controller_->use_closed_loop_pid_adapter()) @@ -433,7 +379,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) double kp = 1.0; SetPidParameters(kp); - updateController(); + updateControllerAsync(); pids = traj_controller_->get_pids(); EXPECT_EQ(pids.size(), 3); @@ -458,7 +404,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) SetUpAndActivateTrajectoryController(executor); - updateController(); + updateControllerAsync(); // test default parameters { @@ -486,7 +432,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) { traj_controller_->get_node()->set_parameter(param); } - updateController(); + updateControllerAsync(); { auto tols = traj_controller_->get_tolerances(); @@ -513,7 +459,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup) SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // after startup without start_with_holding being set, we expect no active trajectory ASSERT_FALSE(traj_controller_->has_active_traj()); @@ -531,7 +477,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // after startup with start_with_holding being set, we expect an active trajectory: ASSERT_TRUE(traj_controller_->has_active_traj()); // one point, being the position at startup @@ -552,7 +498,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun constexpr double k_p = 10.0; std::vector params = {}; SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -568,84 +513,206 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); - - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - const auto allowed_delta = 0.1; + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); // no update of state_interface - EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS); + EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); // are the correct reference positions used? - EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); + EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD); // no normalization of position error - EXPECT_NEAR( - state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); - EXPECT_NEAR( - state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); - EXPECT_NEAR( - state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); + EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR(state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); if (traj_controller_->has_position_command_interface()) { // check command interface - EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); - EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); - EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); - EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->output.positions[2], allowed_delta); + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) + { + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + } + } + + executor.cancel(); +} + +/** + * @brief check if position error of revolute joints are angle_wraparound if configured so + */ +TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + std::vector params = {}; + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + + // no update of state_interface + EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); + + // are the correct reference positions used? + EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD); + + // is error.positions[2] angle_wraparound? + EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR( + state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); + + if (traj_controller_->has_position_command_interface()) { // check command interface - EXPECT_LT(0.0, joint_vel_[0]); - EXPECT_LT(0.0, joint_vel_[1]); - EXPECT_LT(0.0, joint_vel_[2]); - EXPECT_LT(0.0, state_msg->output.velocities[0]); - EXPECT_LT(0.0, state_msg->output.velocities[1]); - EXPECT_LT(0.0, state_msg->output.velocities[2]); + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); + } + if (traj_controller_->has_velocity_command_interface()) + { // use_closed_loop_pid_adapter_ if (traj_controller_->use_closed_loop_pid_adapter()) { - // we expect u = k_p * (s_d-s) + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] EXPECT_NEAR( - k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], - k_p * allowed_delta); + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], - k_p * allowed_delta); - // no normalization of position error + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); + // is error of positions[2] angle_wraparound? + EXPECT_GT(0.0, joint_vel_[2]); EXPECT_NEAR( - k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], - k_p * allowed_delta); + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); } } if (traj_controller_->has_effort_command_interface()) { - // check command interface - EXPECT_LT(0.0, joint_eff_[0]); - EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, joint_eff_[2]); - EXPECT_LT(0.0, state_msg->output.effort[0]); - EXPECT_LT(0.0, state_msg->output.effort[1]); - EXPECT_LT(0.0, state_msg->output.effort[2]); + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * COMMON_THRESHOLD); + // is error of positions[2] angle_wraparound? + EXPECT_GT(0.0, joint_eff_[2]); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + } } executor.cancel(); @@ -685,13 +752,13 @@ TEST_P(TrajectoryControllerTestParameterized, decline_false_cmd_timeout) /** * @brief check if no timeout is triggered */ +// TODO(anyone) make test independent of clock source to use updateControllerAsync TEST_P(TrajectoryControllerTestParameterized, no_timeout) { rclcpp::executors::MultiThreadedExecutor executor; // zero is default value, just for demonstration rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 0.0); SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -707,28 +774,27 @@ TEST_P(TrajectoryControllerTestParameterized, no_timeout) publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); // is the trajectory still active? EXPECT_TRUE(traj_controller_->has_active_traj()); // should still hold the points from above EXPECT_TRUE(traj_controller_->has_nontrivial_traj()); - EXPECT_NEAR(state_msg->reference.positions[0], points.at(2).at(0), 1e-2); - EXPECT_NEAR(state_msg->reference.positions[1], points.at(2).at(1), 1e-2); - EXPECT_NEAR(state_msg->reference.positions[2], points.at(2).at(2), 1e-2); + EXPECT_NEAR(state_reference.positions[0], points.at(2).at(0), 1e-2); + EXPECT_NEAR(state_reference.positions[1], points.at(2).at(1), 1e-2); + EXPECT_NEAR(state_reference.positions[2], points.at(2).at(2), 1e-2); // value of velocities is different from above due to spline interpolation - EXPECT_GT(state_msg->reference.velocities[0], 0.0); - EXPECT_GT(state_msg->reference.velocities[1], 0.0); - EXPECT_GT(state_msg->reference.velocities[2], 0.0); + EXPECT_GT(state_reference.velocities[0], 0.0); + EXPECT_GT(state_reference.velocities[1], 0.0); + EXPECT_GT(state_reference.velocities[2], 0.0); executor.cancel(); } @@ -736,6 +802,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_timeout) /** * @brief check if timeout is triggered */ +// TODO(anyone) make test independent of clock source to use updateControllerAsync TEST_P(TrajectoryControllerTestParameterized, timeout) { rclcpp::executors::MultiThreadedExecutor executor; @@ -743,7 +810,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout); double kp = 1.0; // activate feedback control for testing velocity/effort PID SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false, kp); - subscribeToState(); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); @@ -768,11 +834,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) // update until timeout should have happened updateController(rclcpp::Duration(FIRST_POINT_TIME)); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); - // after timeout, set_hold_position adds a new trajectory // is a trajectory active? EXPECT_TRUE(traj_controller_->has_active_traj()); @@ -792,130 +853,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) executor.cancel(); } -/** - * @brief check if position error of revolute joints are angle_wraparound if configured so - */ -TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) -{ - rclcpp::executors::MultiThreadedExecutor executor; - constexpr double k_p = 10.0; - std::vector params = {}; - SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); - subscribeToState(); - - size_t n_joints = joint_names_.size(); - - // send msg - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; - // *INDENT-OFF* - std::vector> points{ - {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; - std::vector> points_velocities{ - {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; - // *INDENT-ON* - publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); - traj_controller_->wait_for_trajectory(executor); - - // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); - - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); - - const auto allowed_delta = 0.1; - - // no update of state_interface - EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS); - - // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); - - // are the correct reference positions used? - EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); - - // is error.positions[2] angle_wraparound? - EXPECT_NEAR( - state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); - EXPECT_NEAR( - state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); - EXPECT_NEAR( - state_msg->error.positions[2], - state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); - - if (traj_controller_->has_position_command_interface()) - { - // check command interface - EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); - EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); - EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); - } - - if (traj_controller_->has_velocity_command_interface()) - { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) - { - // we expect u = k_p * (s_d-s) for positions[0] and positions[1] - EXPECT_NEAR( - k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], - k_p * allowed_delta); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], - k_p * allowed_delta); - // is error of positions[2] angle_wraparound? - EXPECT_GT(0.0, joint_vel_[2]); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], - k_p * allowed_delta); - } - else - { - // interpolated points_velocities only - // check command interface - EXPECT_LT(0.0, joint_vel_[0]); - EXPECT_LT(0.0, joint_vel_[1]); - EXPECT_LT(0.0, joint_vel_[2]); - } - } - - if (traj_controller_->has_effort_command_interface()) - { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) - { - // we expect u = k_p * (s_d-s) for positions[0] and positions[1] - EXPECT_NEAR( - k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], - k_p * allowed_delta); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], - k_p * allowed_delta); - // is error of positions[2] angle_wraparound? - EXPECT_GT(0.0, joint_eff_[2]); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], - k_p * allowed_delta); - } - else - { - // interpolated points_velocities only - // check command interface - EXPECT_LT(0.0, joint_eff_[0]); - EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, joint_eff_[2]); - } - } - - executor.cancel(); -} - /** * @brief check if use_closed_loop_pid is active */ @@ -944,7 +881,6 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) { rclcpp::executors::MultiThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}, true); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -960,35 +896,34 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) publish(time_from_start, points_positions, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); if (traj_controller_->has_velocity_state_interface()) { - EXPECT_EQ(n_joints, state_msg->reference.velocities.size()); - EXPECT_EQ(n_joints, state_msg->feedback.velocities.size()); - EXPECT_EQ(n_joints, state_msg->error.velocities.size()); + EXPECT_EQ(n_joints, state_reference.velocities.size()); + EXPECT_EQ(n_joints, state_feedback.velocities.size()); + EXPECT_EQ(n_joints, state_error.velocities.size()); } if (traj_controller_->has_acceleration_state_interface()) { - EXPECT_EQ(n_joints, state_msg->reference.accelerations.size()); - EXPECT_EQ(n_joints, state_msg->feedback.accelerations.size()); - EXPECT_EQ(n_joints, state_msg->error.accelerations.size()); + EXPECT_EQ(n_joints, state_reference.accelerations.size()); + EXPECT_EQ(n_joints, state_feedback.accelerations.size()); + EXPECT_EQ(n_joints, state_error.accelerations.size()); } // no change in state interface should happen if (traj_controller_->has_velocity_state_interface()) { - EXPECT_EQ(state_msg->feedback.velocities, INITIAL_VEL_JOINTS); + EXPECT_EQ(state_feedback.velocities, INITIAL_VEL_JOINTS); } // is the velocity error correct? if ( @@ -998,9 +933,9 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) { // don't check against a value, because spline interpolation might overshoot depending on // interface combinations - EXPECT_GE(state_msg->error.velocities[0], points_velocities[0][0]); - EXPECT_GE(state_msg->error.velocities[1], points_velocities[0][1]); - EXPECT_GE(state_msg->error.velocities[2], points_velocities[0][2]); + EXPECT_GE(state_error.velocities[0], points_velocities[0][0]); + EXPECT_GE(state_error.velocities[1], points_velocities[0][1]); + EXPECT_GE(state_error.velocities[2], points_velocities[0][2]); } executor.cancel(); @@ -1016,6 +951,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) SetUpAndActivateTrajectoryController(executor); std::vector points_positions = {1.0, 2.0, 3.0}; std::vector jumble_map = {1, 2, 0}; + double dt = 0.25; { trajectory_msgs::msg::JointTrajectory traj_msg; const std::vector jumbled_joint_names{ @@ -1025,29 +961,27 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(dt); traj_msg.points[0].positions.resize(3); traj_msg.points[0].positions[0] = points_positions.at(jumble_map[0]); traj_msg.points[0].positions[1] = points_positions.at(jumble_map[1]); traj_msg.points[0].positions[2] = points_positions.at(jumble_map[2]); traj_msg.points[0].velocities.resize(3); - for (size_t i = 0; i < 3; i++) + traj_msg.points[0].accelerations.resize(3); + + for (size_t dof = 0; dof < 3; dof++) { - // factor 2 comes from the linear interpolation in the spline trajectory for constant - // acceleration - traj_msg.points[0].velocities[i] = - 2 * (traj_msg.points[0].positions[i] - joint_pos_[jumble_map[i]]) / 0.25; + traj_msg.points[0].velocities[dof] = + (traj_msg.points[0].positions[dof] - joint_pos_[jumble_map[dof]]) / dt; + traj_msg.points[0].accelerations[dof] = + (traj_msg.points[0].velocities[dof] - joint_vel_[jumble_map[dof]]) / dt; } trajectory_publisher_->publish(traj_msg); } traj_controller_->wait_for_trajectory(executor); - // update just before 0.25 seconds (shorter than the trajectory duration of 0.25 seconds, - // otherwise acceleration is zero from the spline trajectory) - // TODO(destogl): Make this time a bit shorter to increase stability on the CI? - // Currently COMMON_THRESHOLD is adjusted. - updateController(rclcpp::Duration::from_seconds(0.25 - 1e-3)); + updateControllerAsync(rclcpp::Duration::from_seconds(dt)); if (traj_controller_->has_position_command_interface()) { @@ -1068,19 +1002,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) if (traj_controller_->has_acceleration_command_interface()) { - if (traj_controller_->has_velocity_state_interface()) - { - EXPECT_GT(0.0, joint_acc_[0]); - EXPECT_GT(0.0, joint_acc_[1]); - EXPECT_GT(0.0, joint_acc_[2]); - } - else - { - // no velocity state interface: no acceleration from trajectory sampling - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - EXPECT_EQ(0.0, joint_acc_[2]); - } + EXPECT_GT(0.0, joint_acc_[0]); + EXPECT_GT(0.0, joint_acc_[1]); + EXPECT_GT(0.0, joint_acc_[2]); } if (traj_controller_->has_effort_command_interface()) @@ -1106,37 +1030,42 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) const double initial_joint1_cmd = joint_pos_[0]; const double initial_joint2_cmd = joint_pos_[1]; const double initial_joint3_cmd = joint_pos_[2]; + const double dt = 0.25; trajectory_msgs::msg::JointTrajectory traj_msg; { - std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; + std::vector jumble_map = {1, 0}; + std::vector partial_joint_names{ + joint_names_[jumble_map[0]], joint_names_[jumble_map[1]]}; traj_msg.joint_names = partial_joint_names; traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(dt); traj_msg.points[0].positions.resize(2); traj_msg.points[0].positions[0] = 2.0; traj_msg.points[0].positions[1] = 1.0; traj_msg.points[0].velocities.resize(2); - traj_msg.points[0].velocities[0] = - std::copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); - traj_msg.points[0].velocities[1] = - std::copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); + traj_msg.points[0].accelerations.resize(2); + for (size_t dof = 0; dof < 2; dof++) + { + traj_msg.points[0].velocities[dof] = + (traj_msg.points[0].positions[dof] - joint_pos_[jumble_map[dof]]) / dt; + traj_msg.points[0].accelerations[dof] = + (traj_msg.points[0].velocities[dof] - joint_vel_[jumble_map[dof]]) / dt; + } trajectory_publisher_->publish(traj_msg); } traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(0.25)); - - double threshold = 0.001; + updateControllerAsync(rclcpp::Duration::from_seconds(dt)); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); - EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], COMMON_THRESHOLD) << "Joint 3 command should be current position"; } @@ -1148,7 +1077,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); EXPECT_TRUE( is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); - EXPECT_NEAR(0.0, joint_vel_[2], threshold) + EXPECT_NEAR(0.0, joint_vel_[2], COMMON_THRESHOLD) << "Joint 3 velocity should be 0.0 since it's not in the goal"; } @@ -1156,24 +1085,15 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) { // estimate the sign of the acceleration // joint rotates forward - if (traj_controller_->has_velocity_state_interface()) - { - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_acc_[0])) - << "Joint1: " << traj_msg.points[0].positions[0] - initial_joint2_cmd << " vs. " - << joint_acc_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_acc_[1])) - << "Joint2: " << traj_msg.points[0].positions[1] - initial_joint1_cmd << " vs. " - << joint_acc_[1]; - } - else - { - // no velocity state interface: no acceleration from trajectory sampling - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - } - EXPECT_NEAR(0.0, joint_acc_[2], threshold) + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_acc_[0])) + << "Joint1: " << traj_msg.points[0].positions[0] - initial_joint2_cmd << " vs. " + << joint_acc_[0]; + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_acc_[1])) + << "Joint2: " << traj_msg.points[0].positions[1] - initial_joint1_cmd << " vs. " + << joint_acc_[1]; + EXPECT_NEAR(0.0, joint_acc_[2], COMMON_THRESHOLD) << "Joint 3 acc should be 0.0 since it's not in the goal"; } @@ -1185,7 +1105,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); EXPECT_TRUE( is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); - EXPECT_NEAR(0.0, joint_eff_[2], threshold) + EXPECT_NEAR(0.0, joint_eff_[2], COMMON_THRESHOLD) << "Joint 3 effort should be 0.0 since it's not in the goal"; } @@ -1227,47 +1147,45 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe traj_controller_->wait_for_trajectory(executor); // update for 0.5 seconds - updateController(rclcpp::Duration::from_seconds(0.25)); - - double threshold = 0.001; + updateControllerAsync(rclcpp::Duration::from_seconds(0.25)); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) + EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) + EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; } if (traj_controller_->has_velocity_command_interface()) { - EXPECT_NEAR(INITIAL_VEL_JOINTS[0], joint_vel_[0], threshold) + EXPECT_NEAR(INITIAL_VEL_JOINTS[0], joint_vel_[0], COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_VEL_JOINTS[1], joint_vel_[1], threshold) + EXPECT_NEAR(INITIAL_VEL_JOINTS[1], joint_vel_[1], COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_VEL_JOINTS[2], joint_vel_[2], threshold) + EXPECT_NEAR(INITIAL_VEL_JOINTS[2], joint_vel_[2], COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; } if (traj_controller_->has_acceleration_command_interface()) { - EXPECT_NEAR(INITIAL_ACC_JOINTS[0], joint_acc_[0], threshold) + EXPECT_NEAR(INITIAL_ACC_JOINTS[0], joint_acc_[0], COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_ACC_JOINTS[1], joint_acc_[1], threshold) + EXPECT_NEAR(INITIAL_ACC_JOINTS[1], joint_acc_[1], COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_ACC_JOINTS[2], joint_acc_[2], threshold) + EXPECT_NEAR(INITIAL_ACC_JOINTS[2], joint_acc_[2], COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; } if (traj_controller_->has_effort_command_interface()) { - EXPECT_NEAR(INITIAL_EFF_JOINTS[0], joint_eff_[0], threshold) + EXPECT_NEAR(INITIAL_EFF_JOINTS[0], joint_eff_[0], COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_EFF_JOINTS[1], joint_eff_[1], threshold) + EXPECT_NEAR(INITIAL_EFF_JOINTS[1], joint_eff_[1], COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_EFF_JOINTS[2], joint_eff_[2], threshold) + EXPECT_NEAR(INITIAL_EFF_JOINTS[2], joint_eff_[2], COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; } @@ -1408,8 +1326,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); - subscribeToState(); - std::vector> points_old{{{2., 3., 4.}}}; std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; std::vector> points_partial_new{{1.5}}; @@ -1424,8 +1340,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) expected_actual.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; // Check that we reached end of points_old trajectory - // Denis: delta was 0.1 with 0.2 works for me - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); points_partial_new_velocities[0][0] = @@ -1440,7 +1356,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) expected_desired.velocities[1] = 0.0; expected_desired.velocities[2] = 0.0; expected_actual = expected_desired; - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); } /** @@ -1450,7 +1367,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}); - subscribeToState(); // TODO(anyone): add expectations for velocities and accelerations std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; @@ -1464,7 +1380,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; // Check that we reached end of points_old[0] trajectory - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); // New trajectory will end before current time @@ -1473,14 +1390,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired = expected_actual; publish(time_from_start, points_new, new_traj_start); - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); } TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}); - subscribeToState(); std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; @@ -1493,19 +1410,20 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; // Check that we reached end of points_old[0]trajectory - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); // send points_new before the old trajectory is finished RCLCPP_INFO( traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); // New trajectory first point is in the past, second is in the future - rclcpp::Time new_traj_start = - rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); + rclcpp::Time new_traj_start = end_time - delay - std::chrono::milliseconds(100); publish(time_from_start, points_new, new_traj_start); // it should not have accepted the new goal but finish the old one expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired.positions = {points_old[1].begin(), points_old[1].end()}; - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); } TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) @@ -1513,7 +1431,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); - subscribeToState(); RCLCPP_WARN( traj_controller_->get_node()->get_logger(), @@ -1538,7 +1455,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; expected_desired = expected_actual; // Check that we reached end of points_old[0]trajectory and are starting points_old[1] - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); // Send partial trajectory starting after full trajecotry is complete RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); @@ -1551,28 +1469,21 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur expected_desired = expected_actual; waitAndCompareState( - expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); + expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1, end_time); } -// TODO(destogl) this test fails with errors -// second publish() gives an error, because end time is before current time -// as well as -// 2: The difference between joint_state_pos_[0] and joint_pos_[0] is 0.02999799000000003, -// which exceeds COMMON_THRESHOLD, where -// 2: joint_state_pos_[0] evaluates to 6.2999999999999998, -// 2: joint_pos_[0] evaluates to 6.2700020099999998, and -// 2: COMMON_THRESHOLD evaluates to 0.0011000000000000001. -// 2: [ FAILED ] PositionTrajectoryControllers/TrajectoryControllerTestParameterized. -// test_jump_when_state_tracking_error_updated/0, where GetParam() = -// ({ "position" }, { "position" }) (3372 ms) - -#if 0 TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) { rclcpp::executors::SingleThreadedExecutor executor; // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + + if (traj_controller_->has_position_command_interface() == false) + { + // only makes sense with position command interface + return; + } // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1585,95 +1496,95 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; + double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / + (time_from_start.sec + time_from_start.nanosec * 1e-9); std::vector> points{{first_goal}}; - publish(time_from_start, points, - rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); + publish( + time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(1.1)); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); - if (traj_controller_->has_position_command_interface()) - { - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points, - rclcpp::Time(1.0, 0.0, RCL_STEADY_TIME), {}, second_goal_velocities); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there should be a "jump" in opposite direction from command - // (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - updateController(rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], state_from_command_offset + COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there should be a "jump" in the goal direction from command - // (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - updateController(rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - } + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, second_goal_velocities); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in opposite direction from command + // (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first, consider advancement of the trajectory + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance + EXPECT_NEAR( + joint_state_pos_[0] + (second_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], + 0.1); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + + // Finally the second goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in the goal direction from command + // (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first, consider advancement of the trajectory + EXPECT_NEAR( + joint_state_pos_[0] + (first_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + + // Finally the first goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); executor.cancel(); } -#endif - -// TODO(destogl) this test fails -// 2: The difference between second_goal[0] and joint_pos_[0] is 0.032986635000000319, -// which exceeds COMMON_THRESHOLD, where -// 2: second_goal[0] evaluates to 6.5999999999999996, -// 2: joint_pos_[0] evaluates to 6.5670133649999993, and -// 2: COMMON_THRESHOLD evaluates to 0.0011000000000000001. -// 2: [ FAILED ] PositionTrajectoryControllers/TrajectoryControllerTestParameterized. -// test_no_jump_when_state_tracking_error_not_updated/1, where GetParam() = -// ({ "position" }, { "position", "velocity" }) (3374 ms) -#if 0 + TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) { rclcpp::executors::SingleThreadedExecutor executor; + // set open loop to true, this should change behavior from above rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + + if (traj_controller_->has_position_command_interface() == false) + { + // only makes sense with position command interface + return; + } // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1684,77 +1595,79 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; + double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / + (time_from_start.sec + time_from_start.nanosec * 1e-9); std::vector> points{{first_goal}}; publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(1.1)); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); - if (traj_controller_->has_position_command_interface()) - { - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there **should not** be a "jump" in opposite direction from - // command (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - updateController(rclcpp::Duration::from_seconds(0.01)); - // There should not be backward commands - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there **should not** be a "jump" in the goal direction from - // command (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - updateController(rclcpp::Duration::from_seconds(0.01)); - // There should not be a jump toward commands - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], second_goal[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - } + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in opposite direction from + // command (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + // There should not be backward commands + EXPECT_NEAR( + first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the second goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in the goal direction from + // command (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + // There should not be a jump toward commands + EXPECT_NEAR( + second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], second_goal[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the first goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); executor.cancel(); } -#endif // Testing that values are read from state interfaces when hardware is started for the first // time and hardware state has offset --> this is indicated by NaN values in command interfaces @@ -1902,7 +1815,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // it should have aborted and be holding now expectHoldingPoint(joint_state_pos_); @@ -1934,7 +1847,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration(4 * FIRST_POINT_TIME)); + auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); // it should have aborted and be holding now expectHoldingPoint(joint_state_pos_); @@ -1942,7 +1855,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // what happens if we wait longer but it harms the tolerance again? auto hold_position = joint_state_pos_; joint_state_pos_.at(0) = -3.3; - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME), end_time); // it should be still holding the old point expectHoldingPoint(hold_position); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 3a7e7e5cf1..3c8e252067 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -21,7 +21,7 @@ #include #include -#include "gtest/gtest.h" +#include "gmock/gmock.h" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" @@ -29,7 +29,7 @@ namespace { -const double COMMON_THRESHOLD = 0.0011; // destogl: increased for 0.0001 for stable CI builds? +const double COMMON_THRESHOLD = 0.001; const double INITIAL_POS_JOINT1 = 1.1; const double INITIAL_POS_JOINT2 = 2.1; const double INITIAL_POS_JOINT3 = 3.1; @@ -151,6 +151,10 @@ class TestableJointTrajectoryController double get_cmd_timeout() { return cmd_timeout_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_feedback() { return state_current_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -233,21 +237,24 @@ class TrajectoryControllerTest : public ::testing::Test const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) { - SetUpTrajectoryController(executor); - - // add this to simplify tests, can be overwritten by parameters - rclcpp::Parameter nonzero_vel_parameter("allow_nonzero_velocity_at_trajectory_end", true); - traj_controller_->get_node()->set_parameter(nonzero_vel_parameter); - - // set pid parameters before configure - SetPidParameters(k_p, ff, angle_wraparound); - - // set optional parameters - for (const auto & param : parameters) + auto has_nonzero_vel_param = + std::find_if( + parameters.begin(), parameters.end(), + [](const rclcpp::Parameter & param) { + return param.get_name() == "allow_nonzero_velocity_at_trajectory_end"; + }) != parameters.end(); + + std::vector parameters_local = parameters; + if (!has_nonzero_vel_param) { - traj_controller_->get_node()->set_parameter(param); + // add this to simplify tests, if not set already + parameters_local.emplace_back("allow_nonzero_velocity_at_trajectory_end", true); } + // read-only parameters have to be set before init -> won't be read otherwise + SetUpTrajectoryController(executor, parameters_local); + // set pid parameters before configure + SetPidParameters(k_p, ff, angle_wraparound); traj_controller_->get_node()->configure(); ActivateTrajectoryController( @@ -407,43 +414,85 @@ class TrajectoryControllerTest : public ::testing::Test trajectory_publisher_->publish(traj_msg); } - void updateController(rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2)) + /** + * @brief a wrapper for update() method of JTC, running synchronously with the clock + * @param wait_time - the time span for updating the controller + * @param update_rate - the rate at which the controller is updated + * + * @note use the faster updateControllerAsync() if no subscriptions etc. + * have to be used from the waitSet/executor + */ + void updateController( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), + const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01)) { auto clock = rclcpp::Clock(RCL_STEADY_TIME); const auto start_time = clock.now(); const auto end_time = start_time + wait_time; auto previous_time = start_time; - while (clock.now() < end_time) + while (clock.now() <= end_time) { auto now = clock.now(); traj_controller_->update(now, now - previous_time); previous_time = now; + std::this_thread::sleep_for(update_rate.to_chrono()); + } + } + + /** + * @brief a wrapper for update() method of JTC, running asynchronously from the clock + * @return the time at which the update finished + * @param wait_time - the time span for updating the controller + * @param start_time - the time at which the update should start + * @param update_rate - the rate at which the controller is updated + * + * @note this is faster than updateController() and can be used if no subscriptions etc. + * have to be used from the waitSet/executor + */ + rclcpp::Time updateControllerAsync( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), + rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME), + const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01)) + { + if (start_time == rclcpp::Time(0, 0, RCL_STEADY_TIME)) + { + start_time = rclcpp::Clock(RCL_STEADY_TIME).now(); } + const auto end_time = start_time + wait_time; + auto time_counter = start_time; + while (time_counter <= end_time) + { + traj_controller_->update(time_counter, update_rate); + time_counter += update_rate; + } + return end_time; } - void waitAndCompareState( + rclcpp::Time waitAndCompareState( trajectory_msgs::msg::JointTrajectoryPoint expected_actual, trajectory_msgs::msg::JointTrajectoryPoint expected_desired, rclcpp::Executor & executor, - rclcpp::Duration controller_wait_time, double allowed_delta) + rclcpp::Duration controller_wait_time, double allowed_delta, + rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME)) { { std::lock_guard guard(state_mutex_); state_msg_.reset(); } traj_controller_->wait_for_trajectory(executor); - updateController(controller_wait_time); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + auto end_time = updateControllerAsync(controller_wait_time, start_time); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + for (size_t i = 0; i < expected_actual.positions.size(); ++i) { SCOPED_TRACE("Joint " + std::to_string(i)); // TODO(anyone): add checking for velocities and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(expected_actual.positions[i], state_msg->feedback.positions[i], allowed_delta); + EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], allowed_delta); } } @@ -453,10 +502,11 @@ class TrajectoryControllerTest : public ::testing::Test // TODO(anyone): add checking for velocities and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR( - expected_desired.positions[i], state_msg->reference.positions[i], allowed_delta); + EXPECT_NEAR(expected_desired.positions[i], state_reference.positions[i], allowed_delta); } } + + return end_time; } std::shared_ptr getState() const @@ -573,6 +623,47 @@ class TrajectoryControllerTest : public ::testing::Test } } + /** + * @brief compares the joint names and interface types of the controller with the given ones + */ + void compare_joints( + std::vector state_joint_names, std::vector command_joint_names) + { + std::vector state_interface_names; + state_interface_names.reserve(state_joint_names.size() * state_interface_types_.size()); + for (const auto & joint : state_joint_names) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(joint + "/" + interface); + } + } + auto state_interfaces = traj_controller_->state_interface_configuration(); + EXPECT_EQ( + state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + state_interfaces.names.size(), state_joint_names.size() * state_interface_types_.size()); + ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); + + std::vector command_interface_names; + command_interface_names.reserve(command_joint_names.size() * command_interface_types_.size()); + for (const auto & joint : command_joint_names) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(joint + "/" + interface); + } + } + auto command_interfaces = traj_controller_->command_interface_configuration(); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + command_interfaces.names.size(), + command_joint_names.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + } + std::string controller_name_; std::vector joint_names_; From fe0c91d4f36939866276d63c1d8f552544f6818f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 27 Nov 2023 12:32:27 +0100 Subject: [PATCH 6/7] joint_state_broadcaster: Add proper subscription to TestCustomInterfaceMappingUpdate (#859) --- .../test/test_joint_state_broadcaster.cpp | 21 +++++++++---------- .../test/test_joint_state_broadcaster.hpp | 3 +++ 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 3a74f599b4..b44152a453 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -531,19 +531,12 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate) state_broadcaster_->get_node()->set_parameter( {std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_}); - // configure ok - ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ( - state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + sensor_msgs::msg::JointState joint_state_msg; + activate_and_get_joint_state_message("joint_states", joint_state_msg); const size_t NUM_JOINTS = JOINT_NAMES.size(); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); ASSERT_EQ(joint_state_msg.position[0], custom_joint_value_); @@ -585,7 +578,8 @@ TEST_F(JointStateBroadcasterTest, UpdateTest) controller_interface::return_type::OK); } -void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) +void JointStateBroadcasterTest::activate_and_get_joint_state_message( + const std::string & topic, sensor_msgs::msg::JointState & joint_state_msg) { auto node_state = state_broadcaster_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -616,9 +610,14 @@ void JointStateBroadcasterTest::test_published_joint_state_message(const std::st "controller/broadcaster update loop"; // take message from subscription - sensor_msgs::msg::JointState joint_state_msg; rclcpp::MessageInfo msg_info; ASSERT_TRUE(subscription->take(joint_state_msg, msg_info)); +} + +void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) +{ + sensor_msgs::msg::JointState joint_state_msg; + activate_and_get_joint_state_message(topic, joint_state_msg); const size_t NUM_JOINTS = joint_names_.size(); ASSERT_THAT(joint_state_msg.name, SizeIs(NUM_JOINTS)); diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 63cc7a5483..fcaa40e8d5 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -71,6 +71,9 @@ class JointStateBroadcasterTest : public ::testing::Test void test_published_dynamic_joint_state_message(const std::string & topic); + void activate_and_get_joint_state_message( + const std::string & topic, sensor_msgs::msg::JointState & msg); + protected: // dummy joint state values used for tests const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; From b6841ead459f6efb76153e8474a3f5443d48d2b2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 27 Nov 2023 12:54:41 +0100 Subject: [PATCH 7/7] Increase test coverage of interface configuration getters (#856) --- .../test_force_torque_sensor_broadcaster.cpp | 25 +++++++- .../test_force_torque_sensor_broadcaster.hpp | 2 +- .../test/test_forward_command_controller.cpp | 42 +++++++++++++ ...i_interface_forward_command_controller.cpp | 23 +++++++ .../test/test_gripper_controllers.cpp | 12 ++++ .../test/test_imu_sensor_broadcaster.cpp | 23 +++++++ .../test/test_joint_state_broadcaster.cpp | 62 ++++++++++++++++++- .../test/test_joint_state_broadcaster.hpp | 4 +- .../test/test_range_sensor_broadcaster.cpp | 27 +++++++- .../test/test_tricycle_controller.cpp | 15 +++++ 10 files changed, 228 insertions(+), 7 deletions(-) diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index bfe7561642..ce6a04ec1f 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -32,6 +32,8 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedStateInterface; +using testing::IsEmpty; +using testing::SizeIs; namespace { @@ -157,6 +159,12 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = fts_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); } TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) @@ -175,7 +183,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); } -TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Activate_Success) +TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) { SetUpFTSBroadcaster(); @@ -186,6 +194,21 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Activate_Success) // configure and activate success ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = fts_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); + + // deactivate passed + ASSERT_EQ(fts_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + cmd_if_conf = fts_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + state_if_conf = fts_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); // did not change } TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index 5477b3fa6f..fe5b0ab3ba 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -36,7 +36,7 @@ class FriendForceTorqueSensorBroadcaster FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorNameParameterIsEmpty); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, InterfaceNameParameterIsEmpty); - FRIEND_TEST(ForceTorqueSensorBroadcasterTest, ActivateSuccess); + FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, UpdateTest); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorStatePublishTest); }; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 62d5512b4e..236cb14edd 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -34,6 +34,8 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; +using testing::IsEmpty; +using testing::SizeIs; namespace { @@ -128,6 +130,13 @@ TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess) ASSERT_EQ( controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); } TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) @@ -173,9 +182,23 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) ASSERT_EQ( controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ( controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); } TEST_F(ForwardCommandControllerTest, CommandSuccessTest) @@ -323,9 +346,22 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + auto command_msg = std::make_shared(); command_msg->data = {10.0, 20.0, 30.0}; @@ -344,6 +380,12 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); // did not change + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( controller_->rt_command_ptr_.readFromNonRT() && diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 7826c85355..2d3b61e211 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -36,6 +36,8 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; +using testing::IsEmpty; +using testing::SizeIs; namespace { @@ -148,6 +150,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ConfigureParamsSuccess) ASSERT_EQ( controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(3lu)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); } TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) @@ -282,6 +291,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes { SetUpController(true); + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(3lu)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // send command auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0, 30.0}; @@ -300,6 +316,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes auto node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(3lu)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( controller_->rt_command_ptr_.readFromNonRT() && diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index 0fc10c0c73..da9e15840e 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -31,6 +31,8 @@ using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; using GripperCommandAction = control_msgs::action::GripperCommand; using GoalHandle = rclcpp_action::ServerGoalHandle; +using testing::SizeIs; +using testing::UnorderedElementsAre; template void GripperControllerTest::SetUpTestCase() @@ -108,6 +110,16 @@ TYPED_TEST(GripperControllerTest, ConfigureParamsSuccess) ASSERT_EQ( this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = this->controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu)); + ASSERT_THAT(cmd_if_conf.names, UnorderedElementsAre(std::string("joint_1/") + TypeParam::value)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = this->controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); + ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint_1/position", "joint_1/velocity")); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TYPED_TEST(GripperControllerTest, ActivateWithWrongJointsNamesFails) diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 0fb987ce77..83edc044d3 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -32,6 +32,8 @@ #include "sensor_msgs/msg/imu.hpp" using hardware_interface::LoanedStateInterface; +using testing::IsEmpty; +using testing::SizeIs; namespace { @@ -113,6 +115,12 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) // configure passed ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = imu_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); } TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) @@ -126,6 +134,21 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) // configure and activate success ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = imu_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); + + // deactivate passed + ASSERT_EQ(imu_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + cmd_if_conf = imu_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + state_if_conf = imu_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); // did not change } TEST_F(IMUSensorBroadcasterTest, SensorName_Update_Success) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index b44152a453..3e45740bbc 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -163,7 +163,7 @@ TEST_F(JointStateBroadcasterTest, ConfigureErrorTest) ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); } -TEST_F(JointStateBroadcasterTest, ActivateTest) +TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) { // publishers not initialized yet ASSERT_FALSE(state_broadcaster_->joint_state_publisher_); @@ -177,6 +177,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTest) const size_t NUM_JOINTS = joint_names_.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -218,6 +224,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) const size_t NUM_JOINTS = joint_names_.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -259,6 +271,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) const size_t NUM_JOINTS = joint_names_.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -287,7 +305,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) ElementsAreArray(interface_names_)); } -TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) +TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) { const std::vector JOINT_NAMES = {joint_names_[0], joint_names_[1]}; const std::vector IF_NAMES = {interface_names_[0]}; @@ -300,6 +318,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -329,6 +353,16 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray(IF_NAMES)); ASSERT_THAT( dynamic_joint_state_msg.interface_values[1].interface_names, ElementsAreArray(IF_NAMES)); + + // deactivate + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT( + state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); // does not change } TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) @@ -344,6 +378,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -412,6 +452,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -455,6 +501,12 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; ASSERT_THAT(joint_state_msg.name, SizeIs(0)); @@ -492,6 +544,12 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index fcaa40e8d5..fa9d29c936 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -33,10 +33,10 @@ using hardware_interface::HW_IF_VELOCITY; class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBroadcaster { FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest); - FRIEND_TEST(JointStateBroadcasterTest, ActivateTest); + FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter); - FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface); + FRIEND_TEST(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing); diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index a857141ea9..7b8e6d0e02 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -22,6 +22,9 @@ #include "hardware_interface/loaned_state_interface.hpp" +using testing::IsEmpty; +using testing::SizeIs; + void RangeSensorBroadcasterTest::SetUp() { // initialize controller @@ -130,9 +133,15 @@ TEST_F(RangeSensorBroadcasterTest, Configure_RangeBroadcaster_Success) ASSERT_EQ( range_broadcaster_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = range_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = range_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); } -TEST_F(RangeSensorBroadcasterTest, Activate_RangeBroadcaster_Success) +TEST_F(RangeSensorBroadcasterTest, ActivateDeactivate_RangeBroadcaster_Success) { init_broadcaster("test_range_sensor_broadcaster"); @@ -141,6 +150,22 @@ TEST_F(RangeSensorBroadcasterTest, Activate_RangeBroadcaster_Success) ASSERT_EQ( range_broadcaster_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = range_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = range_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); + + ASSERT_EQ( + range_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + cmd_if_conf = range_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + state_if_conf = range_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); // did not change } TEST_F(RangeSensorBroadcasterTest, Update_RangeBroadcaster_Success) diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 504ca381ce..d8beedae42 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -39,6 +39,7 @@ using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; using lifecycle_msgs::msg::State; using testing::SizeIs; +using testing::UnorderedElementsAre; class TestableTricycleController : public tricycle_controller::TricycleController { @@ -209,6 +210,20 @@ TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); + ASSERT_THAT( + cmd_if_conf.names, + UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); + ASSERT_THAT( + state_if_conf.names, + UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(TestTricycleController, activate_fails_without_resources_assigned)