From ac58998c692de330ffda1dc8b8efe8e7d51a239e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Nov 2023 22:32:15 +0100 Subject: [PATCH] fix tests for the new API --- admittance_controller/test/test_admittance_controller.hpp | 8 +++++++- .../test/test_trajectory_controller_utils.hpp | 8 +++++++- 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 db708db6c5..58203483df 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,8 @@ 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); + controller_->set_node_options(options); + auto result = controller_->init(controller_name, "", ""); 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 224265ad83..cbb54f021f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -67,6 +67,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 @@ -146,7 +148,10 @@ class TestableJointTrajectoryController double get_cmd_timeout() { return cmd_timeout_; } + void set_node_options(const rclcpp::NodeOptions & node_options) { node_options_ = node_options; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; + rclcpp::NodeOptions node_options_; }; class TrajectoryControllerTest : public ::testing::Test @@ -190,8 +195,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); - auto ret = traj_controller_->init(controller_name_, "", "", node_options); + auto ret = traj_controller_->init(controller_name_, "", ""); if (ret != controller_interface::return_type::OK) { FAIL();