From 099655ce9a69c7b7ee4c509dbb8dafee6b16b8cd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Nov 2023 22:32:15 +0100 Subject: [PATCH 1/3] fix tests for the new API --- .../test/test_admittance_controller.hpp | 7 ++++++- .../test/test_trajectory_controller_utils.hpp | 9 ++++++++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 19908d7f9f..a934a8fe32 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -122,11 +122,16 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon return success; } + rclcpp::NodeOptions get_node_options() const override { return node_options_; } + + void set_node_options(const rclcpp::NodeOptions & node_options) { node_options_ = node_options; } + private: rclcpp::WaitSet input_wrench_command_subscriber_wait_set_; rclcpp::WaitSet input_pose_command_subscriber_wait_set_; const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; + rclcpp::NodeOptions node_options_; }; class AdmittanceControllerTest : public ::testing::Test @@ -185,7 +190,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, "", 0, "", options); + auto result = controller_->init(controller_name, "", 0, ""); controller_->export_reference_interfaces(); assign_interfaces(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index e636fad4b7..20ce0cfb77 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -66,6 +66,8 @@ class TestableJointTrajectoryController return ret; } + rclcpp::NodeOptions get_node_options() const override { return node_options_; } + /** * @brief wait_for_trajectory block until a new JointTrajectory is received. * Requires that the executor is not spinned elsewhere between the @@ -150,11 +152,15 @@ class TestableJointTrajectoryController double get_cmd_timeout() { return cmd_timeout_; } + void set_node_options(const rclcpp::NodeOptions & node_options) { node_options_ = node_options; } + 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_; + rclcpp::NodeOptions node_options_; }; class TrajectoryControllerTest : public ::testing::Test @@ -209,8 +215,9 @@ class TrajectoryControllerTest : public ::testing::Test parameter_overrides.push_back(rclcpp::Parameter("state_interfaces", state_interface_types_)); parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); + traj_controller_->set_node_options(node_options); - return traj_controller_->init(controller_name_, "", 0, "", node_options); + return traj_controller_->init(controller_name_, "", 0, ""); } void SetPidParameters( From e72ba347b241d687296c92f44e9efec7fecc91e8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 20 Nov 2023 20:37:43 +0100 Subject: [PATCH 2/3] Update the documention on writing new controllers --- doc/writing_new_controller.rst | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 501c231def..1a9ffce714 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -42,7 +42,9 @@ The following is a step-by-step guide to create source files, basic tests, and c 5. Add a constructor without parameters and the following public methods overriding the ``ControllerInterface`` definition: ``on_init``, ``command_interface_configuration``, ``state_interface_configuration``, ``on_configure``, ``on_activate``, ``on_deactivate``, ``update``. For exact definitions check the ``controller_interface/controller_interface.hpp`` header or one of the controllers from `ros2_controllers `_. - 6. (optional) Often, controllers accept lists of joint names and interface names as parameters. + 6. (Optional) The NodeOptions of the LifecycleNode can be personalized by overriding the default method ``get_node_options``. + + 7. (Optional) Often, controllers accept lists of joint names and interface names as parameters. If so, you can add two protected string vectors to store those values. 4. **Adding definitions into source file (.cpp)** From 355005eedb0e5609c555d2e5d9a57e40aba239d2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 31 Jan 2024 23:23:24 +0100 Subject: [PATCH 3/3] fix the init methods to properly instantiate the test controllers --- .../test/test_ackermann_steering_controller.hpp | 4 +++- admittance_controller/test/test_admittance_controller.hpp | 7 +------ .../test/test_bicycle_steering_controller.hpp | 4 +++- diff_drive_controller/test/test_diff_drive_controller.cpp | 3 ++- .../test/test_joint_group_effort_controller.cpp | 3 ++- .../test/test_force_torque_sensor_broadcaster.cpp | 4 +++- .../test/test_forward_command_controller.cpp | 3 ++- .../test_multi_interface_forward_command_controller.cpp | 4 +++- gripper_controllers/test/test_gripper_controllers.cpp | 3 ++- .../test/test_imu_sensor_broadcaster.cpp | 3 ++- .../test/test_joint_state_broadcaster.cpp | 3 ++- .../test/test_trajectory_controller_utils.hpp | 5 +++-- pid_controller/test/test_pid_controller.hpp | 4 +++- .../test/test_joint_group_position_controller.cpp | 3 ++- .../test/test_range_sensor_broadcaster.cpp | 3 ++- .../test/test_steering_controllers_library.hpp | 4 +++- tricycle_controller/test/test_tricycle_controller.cpp | 3 ++- .../test/test_tricycle_steering_controller.hpp | 4 +++- .../test/test_joint_group_velocity_controller.cpp | 3 ++- 19 files changed, 45 insertions(+), 25 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 7c279d6323..a047186d14 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -147,7 +147,9 @@ class AckermannSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_ackermann_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + 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 a934a8fe32..19908d7f9f 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -122,16 +122,11 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon return success; } - rclcpp::NodeOptions get_node_options() const override { return node_options_; } - - void set_node_options(const rclcpp::NodeOptions & node_options) { node_options_ = node_options; } - private: rclcpp::WaitSet input_wrench_command_subscriber_wait_set_; rclcpp::WaitSet input_pose_command_subscriber_wait_set_; const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; - rclcpp::NodeOptions node_options_; }; class AdmittanceControllerTest : public ::testing::Test @@ -190,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, "", 0, ""); + 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 6e84342bea..5e21ff228c 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -144,7 +144,9 @@ class BicycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_bicycle_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + 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 43dae41a9b..9ab3022a9f 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -218,7 +218,8 @@ class TestDiffDriveController : public ::testing::Test TEST_F(TestDiffDriveController, init_fails_without_parameters) { - const auto ret = controller_->init(controller_name, urdf_, 0); + const auto ret = + controller_->init(controller_name, urdf_, 0, "", controller_->define_custom_node_options()); ASSERT_EQ(ret, controller_interface::return_type::ERROR); } diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index f9d72ab202..200a1beda8 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -54,7 +54,8 @@ void JointGroupEffortControllerTest::TearDown() { controller_.reset(nullptr); } void JointGroupEffortControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_effort_controller", "", 0); + const auto result = controller_->init( + "test_joint_group_effort_controller", "", 0, "", controller_->define_custom_node_options()); 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 8b994307fa..2412361352 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 @@ -55,7 +55,9 @@ void ForceTorqueSensorBroadcasterTest::TearDown() { fts_broadcaster_.reset(nullp void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster() { - const auto result = fts_broadcaster_->init("test_force_torque_sensor_broadcaster", "", 0); + const auto result = fts_broadcaster_->init( + "test_force_torque_sensor_broadcaster", "", 0, "", + fts_broadcaster_->define_custom_node_options()); 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 937144b48c..c31c8a964c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -62,7 +62,8 @@ void ForwardCommandControllerTest::TearDown() { controller_.reset(nullptr); } void ForwardCommandControllerTest::SetUpController() { - const auto result = controller_->init("forward_command_controller", "", 0); + const auto result = controller_->init( + "forward_command_controller", "", 0, "", controller_->define_custom_node_options()); 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 f8f073c103..7879d5c1d7 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 @@ -64,7 +64,9 @@ void MultiInterfaceForwardCommandControllerTest::TearDown() { controller_.reset( void MultiInterfaceForwardCommandControllerTest::SetUpController(bool set_params_and_activate) { - const auto result = controller_->init("multi_interface_forward_command_controller", "", 0); + const auto result = controller_->init( + "multi_interface_forward_command_controller", "", 0, "", + controller_->define_custom_node_options()); 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 da9e15840e..9f7e024917 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -62,7 +62,8 @@ void GripperControllerTest::TearDown() template void GripperControllerTest::SetUpController() { - const auto result = controller_->init("gripper_controller", "", 0); + const auto result = + controller_->init("gripper_controller", "", 0, "", controller_->define_custom_node_options()); 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 62179a99ff..25a39a8b4d 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -55,7 +55,8 @@ void IMUSensorBroadcasterTest::TearDown() { imu_broadcaster_.reset(nullptr); } void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() { - const auto result = imu_broadcaster_->init("test_imu_sensor_broadcaster", "", 0); + const auto result = imu_broadcaster_->init( + "test_imu_sensor_broadcaster", "", 0, "", imu_broadcaster_->define_custom_node_options()); 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 c10f51aaa2..2faa55f467 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -69,7 +69,8 @@ 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", "", 0); + const auto result = state_broadcaster_->init( + "joint_state_broadcaster", "", 0, "", state_broadcaster_->define_custom_node_options()); 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 ad28b34b5c..6978d0e452 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -66,7 +66,7 @@ class TestableJointTrajectoryController return ret; } - rclcpp::NodeOptions get_node_options() const override { return node_options_; } + rclcpp::NodeOptions define_custom_node_options() const override { return node_options_; } /** * @brief wait_for_trajectory block until a new JointTrajectory is received. @@ -240,7 +240,8 @@ class TrajectoryControllerTest : public ::testing::Test node_options.parameter_overrides(parameter_overrides); traj_controller_->set_node_options(node_options); - return traj_controller_->init(controller_name_, "", 0, ""); + return traj_controller_->init( + controller_name_, "", 0, "", traj_controller_->define_custom_node_options()); } void SetPidParameters( diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index ab32f5cb48..1c356263e7 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -146,7 +146,9 @@ class PidControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_pid_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); std::vector command_ifs; command_itfs_.reserve(dof_names_.size()); diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 3b4f00be12..60bff556db 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -54,7 +54,8 @@ void JointGroupPositionControllerTest::TearDown() { controller_.reset(nullptr); void JointGroupPositionControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_position_controller", "", 0); + const auto result = controller_->init( + "test_joint_group_position_controller", "", 0, "", controller_->define_custom_node_options()); 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 64f68a7e7a..010f18c1a6 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -37,7 +37,8 @@ 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, "", 0); + result = range_broadcaster_->init( + broadcaster_name, "", 0, "", range_broadcaster_->define_custom_node_options()); 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 83e6054bd4..1284b72096 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -168,7 +168,9 @@ class SteeringControllersLibraryFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_steering_controllers_library") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + 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 018727e260..5280aaf244 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -197,7 +197,8 @@ class TestTricycleController : public ::testing::Test TEST_F(TestTricycleController, init_fails_without_parameters) { - const auto ret = controller_->init(controller_name, urdf_, 0); + const auto ret = + controller_->init(controller_name, urdf_, 0, "", controller_->define_custom_node_options()); ASSERT_EQ(ret, controller_interface::return_type::ERROR); } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index e97e2a45bd..6a516691b8 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -146,7 +146,9 @@ class TricycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_tricycle_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + 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 4cbf1b7342..a99ffaeebf 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -54,7 +54,8 @@ void JointGroupVelocityControllerTest::TearDown() { controller_.reset(nullptr); void JointGroupVelocityControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_velocity_controller", "", 0); + const auto result = controller_->init( + "test_joint_group_velocity_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs;