From 822dcbeced95df903daeae5e3ab023935db16b06 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 21 Nov 2022 15:00:41 +0100 Subject: [PATCH 1/4] adapt to changes made to loandedInterface --- .../test/test_diff_drive_controller.cpp | 20 ++++--- .../test_force_torque_sensor_broadcaster.hpp | 24 ++++++--- .../test/test_imu_sensor_broadcaster.hpp | 50 +++++++++++------- .../test/test_joint_state_broadcaster.hpp | 52 +++++++++++-------- .../test/test_trajectory_controller_utils.hpp | 12 ++--- .../test/test_tricycle_controller.cpp | 10 ++-- 6 files changed, 103 insertions(+), 65 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 236f34e9a2..d046418c90 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -163,14 +163,18 @@ class TestDiffDriveController : public ::testing::Test std::vector position_values_ = {0.1, 0.2}; std::vector velocity_values_ = {0.01, 0.02}; - hardware_interface::StateInterface left_wheel_pos_state_{ - left_wheel_names[0], HW_IF_POSITION, &position_values_[0]}; - hardware_interface::StateInterface right_wheel_pos_state_{ - right_wheel_names[0], HW_IF_POSITION, &position_values_[1]}; - hardware_interface::StateInterface left_wheel_vel_state_{ - left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; - hardware_interface::StateInterface right_wheel_vel_state_{ - right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; + std::shared_ptr left_wheel_pos_state_ = + std::make_shared( + left_wheel_names[0], HW_IF_POSITION, &position_values_[0]); + std::shared_ptr right_wheel_pos_state_ = + std::make_shared( + right_wheel_names[0], HW_IF_POSITION, &position_values_[1]); + std::shared_ptr left_wheel_vel_state_ = + std::make_shared( + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]); + std::shared_ptr right_wheel_vel_state_ = + std::make_shared( + right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]); hardware_interface::CommandInterface left_wheel_vel_cmd_{ left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; hardware_interface::CommandInterface right_wheel_vel_cmd_{ 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..46ede8f606 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 @@ -57,12 +57,24 @@ class ForceTorqueSensorBroadcasterTest : public ::testing::Test const std::string frame_id_ = "fts_sensor_frame"; std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; - hardware_interface::StateInterface fts_force_x_{sensor_name_, "force.x", &sensor_values_[0]}; - hardware_interface::StateInterface fts_force_y_{sensor_name_, "force.y", &sensor_values_[1]}; - hardware_interface::StateInterface fts_force_z_{sensor_name_, "force.z", &sensor_values_[2]}; - hardware_interface::StateInterface fts_torque_x_{sensor_name_, "torque.x", &sensor_values_[3]}; - hardware_interface::StateInterface fts_torque_y_{sensor_name_, "torque.y", &sensor_values_[4]}; - hardware_interface::StateInterface fts_torque_z_{sensor_name_, "torque.z", &sensor_values_[5]}; + std::shared_ptr fts_force_x_ = + std::make_shared( + sensor_name_, "force.x", &sensor_values_[0]); + std::shared_ptr fts_force_y_ = + std::make_shared( + sensor_name_, "force.y", &sensor_values_[1]); + std::shared_ptr fts_force_z_ = + std::make_shared( + sensor_name_, "force.z", &sensor_values_[2]); + std::shared_ptr fts_torque_x_ = + std::make_shared( + sensor_name_, "torque.x", &sensor_values_[3]); + std::shared_ptr fts_torque_y_ = + std::make_shared( + sensor_name_, "torque.y", &sensor_values_[4]); + std::shared_ptr fts_torque_z_ = + std::make_shared( + sensor_name_, "torque.z", &sensor_values_[5]); std::unique_ptr fts_broadcaster_; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 01423724b8..8a3feb98ca 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -55,26 +55,36 @@ class IMUSensorBroadcasterTest : public ::testing::Test const std::string sensor_name_ = "imu_sensor"; const std::string frame_id_ = "imu_sensor_frame"; std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; - hardware_interface::StateInterface imu_orientation_x_{ - sensor_name_, "orientation.x", &sensor_values_[0]}; - hardware_interface::StateInterface imu_orientation_y_{ - sensor_name_, "orientation.y", &sensor_values_[1]}; - hardware_interface::StateInterface imu_orientation_z_{ - sensor_name_, "orientation.z", &sensor_values_[2]}; - hardware_interface::StateInterface imu_orientation_w_{ - sensor_name_, "orientation.w", &sensor_values_[3]}; - hardware_interface::StateInterface imu_angular_velocity_x_{ - sensor_name_, "angular_velocity.x", &sensor_values_[4]}; - hardware_interface::StateInterface imu_angular_velocity_y_{ - sensor_name_, "angular_velocity.y", &sensor_values_[5]}; - hardware_interface::StateInterface imu_angular_velocity_z_{ - sensor_name_, "angular_velocity.z", &sensor_values_[6]}; - hardware_interface::StateInterface imu_linear_acceleration_x_{ - sensor_name_, "linear_acceleration.x", &sensor_values_[7]}; - hardware_interface::StateInterface imu_linear_acceleration_y_{ - sensor_name_, "linear_acceleration.y", &sensor_values_[8]}; - hardware_interface::StateInterface imu_linear_acceleration_z_{ - sensor_name_, "linear_acceleration.z", &sensor_values_[9]}; + std::shared_ptr imu_orientation_x_ = + std::make_shared( + sensor_name_, "orientation.x", &sensor_values_[0]); + std::shared_ptr imu_orientation_y_ = + std::make_shared( + sensor_name_, "orientation.y", &sensor_values_[1]); + std::shared_ptr imu_orientation_z_ = + std::make_shared( + sensor_name_, "orientation.z", &sensor_values_[2]); + std::shared_ptr imu_orientation_w_ = + std::make_shared( + sensor_name_, "orientation.w", &sensor_values_[3]); + std::shared_ptr imu_angular_velocity_x_ = + std::make_shared( + sensor_name_, "angular_velocity.x", &sensor_values_[4]); + std::shared_ptr imu_angular_velocity_y_ = + std::make_shared( + sensor_name_, "angular_velocity.y", &sensor_values_[5]); + std::shared_ptr imu_angular_velocity_z_ = + std::make_shared( + sensor_name_, "angular_velocity.z", &sensor_values_[6]); + std::shared_ptr imu_linear_acceleration_x_ = + std::make_shared( + sensor_name_, "linear_acceleration.x", &sensor_values_[7]); + std::shared_ptr imu_linear_acceleration_y_ = + std::make_shared( + sensor_name_, "linear_acceleration.y", &sensor_values_[8]); + std::shared_ptr imu_linear_acceleration_z_ = + std::make_shared( + sensor_name_, "linear_acceleration.z", &sensor_values_[9]); std::unique_ptr imu_broadcaster_; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 63cc7a5483..e5372233de 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -79,27 +79,37 @@ class JointStateBroadcasterTest : public ::testing::Test std::vector joint_values_ = {1.1, 2.1, 3.1}; double custom_joint_value_ = 3.5; - hardware_interface::StateInterface joint_1_pos_state_{ - joint_names_[0], interface_names_[0], &joint_values_[0]}; - hardware_interface::StateInterface joint_2_pos_state_{ - joint_names_[1], interface_names_[0], &joint_values_[1]}; - hardware_interface::StateInterface joint_3_pos_state_{ - joint_names_[2], interface_names_[0], &joint_values_[2]}; - hardware_interface::StateInterface joint_1_vel_state_{ - joint_names_[0], interface_names_[1], &joint_values_[0]}; - hardware_interface::StateInterface joint_2_vel_state_{ - joint_names_[1], interface_names_[1], &joint_values_[1]}; - hardware_interface::StateInterface joint_3_vel_state_{ - joint_names_[2], interface_names_[1], &joint_values_[2]}; - hardware_interface::StateInterface joint_1_eff_state_{ - joint_names_[0], interface_names_[2], &joint_values_[0]}; - hardware_interface::StateInterface joint_2_eff_state_{ - joint_names_[1], interface_names_[2], &joint_values_[1]}; - hardware_interface::StateInterface joint_3_eff_state_{ - joint_names_[2], interface_names_[2], &joint_values_[2]}; - - hardware_interface::StateInterface joint_X_custom_state{ - joint_names_[0], custom_interface_name_, &custom_joint_value_}; + std::shared_ptr joint_1_pos_state_ = + std::make_shared( + joint_names_[0], interface_names_[0], &joint_values_[0]); + std::shared_ptr joint_2_pos_state_ = + std::make_shared( + joint_names_[1], interface_names_[0], &joint_values_[1]); + std::shared_ptr joint_3_pos_state_ = + std::make_shared( + joint_names_[2], interface_names_[0], &joint_values_[2]); + std::shared_ptr joint_1_vel_state_ = + std::make_shared( + joint_names_[0], interface_names_[1], &joint_values_[0]); + std::shared_ptr joint_2_vel_state_ = + std::make_shared( + joint_names_[1], interface_names_[1], &joint_values_[1]); + std::shared_ptr joint_3_vel_state_ = + std::make_shared( + joint_names_[2], interface_names_[1], &joint_values_[2]); + std::shared_ptr joint_1_eff_state_ = + std::make_shared( + joint_names_[0], interface_names_[2], &joint_values_[0]); + std::shared_ptr joint_2_eff_state_ = + std::make_shared( + joint_names_[1], interface_names_[2], &joint_values_[1]); + std::shared_ptr joint_3_eff_state_ = + std::make_shared( + joint_names_[2], interface_names_[2], &joint_values_[2]); + + std::shared_ptr joint_X_custom_state = + std::make_shared( + joint_names_[0], custom_interface_name_, &custom_joint_value_); std::unique_ptr state_broadcaster_; }; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index c85eb59f58..7b22e0638b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -237,13 +237,13 @@ class TrajectoryControllerTest : public ::testing::Test eff_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( joint_names_[i], hardware_interface::HW_IF_EFFORT, &joint_eff_[i])); - pos_state_interfaces_.emplace_back(hardware_interface::StateInterface( + pos_state_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_POSITION, separate_cmd_and_state_values ? &joint_state_pos_[i] : &joint_pos_[i])); - vel_state_interfaces_.emplace_back(hardware_interface::StateInterface( + vel_state_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_VELOCITY, separate_cmd_and_state_values ? &joint_state_vel_[i] : &joint_vel_[i])); - acc_state_interfaces_.emplace_back(hardware_interface::StateInterface( + acc_state_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_ACCELERATION, separate_cmd_and_state_values ? &joint_state_acc_[i] : &joint_acc_[i])); @@ -437,9 +437,9 @@ class TrajectoryControllerTest : public ::testing::Test std::vector vel_cmd_interfaces_; std::vector acc_cmd_interfaces_; std::vector eff_cmd_interfaces_; - std::vector pos_state_interfaces_; - std::vector vel_state_interfaces_; - std::vector acc_state_interfaces_; + std::vector> pos_state_interfaces_; + std::vector> vel_state_interfaces_; + std::vector> acc_state_interfaces_; }; // From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index a1f09ddaf8..f75e868dc5 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -153,11 +153,13 @@ class TestTricycleController : public ::testing::Test double position_ = 0.1; double velocity_ = 0.2; - hardware_interface::StateInterface steering_joint_pos_state_{ - steering_joint_name, HW_IF_POSITION, &position_}; + std::shared_ptr steering_joint_pos_state_ = + std::make_shared( + steering_joint_name, HW_IF_POSITION, &position_); - hardware_interface::StateInterface traction_joint_vel_state_{ - traction_joint_name, HW_IF_VELOCITY, &velocity_}; + std::shared_ptr traction_joint_vel_state_ = + std::make_shared( + traction_joint_name, HW_IF_VELOCITY, &velocity_); hardware_interface::CommandInterface steering_joint_pos_cmd_{ steering_joint_name, HW_IF_POSITION, &position_}; From 1cc40be1bbf362034d1de205a38fbde057f4f8ca Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 5 Dec 2022 10:56:41 +0100 Subject: [PATCH 2/4] adjust tests to reflect changes in handles --- .../test/test_diff_drive_controller.cpp | 30 +++++----- .../test_joint_group_effort_controller.cpp | 48 +++++++-------- .../test_joint_group_effort_controller.hpp | 9 ++- .../test/test_forward_command_controller.cpp | 60 +++++++++---------- .../test/test_forward_command_controller.hpp | 9 ++- ...i_interface_forward_command_controller.cpp | 48 +++++++-------- ...i_interface_forward_command_controller.hpp | 9 ++- .../test/test_trajectory_controller_utils.hpp | 16 ++--- .../test_joint_group_position_controller.cpp | 36 +++++------ .../test_joint_group_position_controller.hpp | 9 ++- .../test/test_tricycle_controller.cpp | 30 +++++----- .../test_joint_group_velocity_controller.cpp | 48 +++++++-------- .../test_joint_group_velocity_controller.hpp | 9 ++- 13 files changed, 190 insertions(+), 171 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index d046418c90..2c710b05a8 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -175,10 +175,12 @@ class TestDiffDriveController : public ::testing::Test std::shared_ptr right_wheel_vel_state_ = std::make_shared( right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]); - hardware_interface::CommandInterface left_wheel_vel_cmd_{ - left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; - hardware_interface::CommandInterface right_wheel_vel_cmd_{ - right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; + std::shared_ptr left_wheel_vel_cmd_ = + std::make_shared( + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]); + std::shared_ptr right_wheel_vel_cmd_ = + std::make_shared( + right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]); rclcpp::Node::SharedPtr pub_node; rclcpp::Publisher::SharedPtr velocity_publisher; @@ -372,8 +374,8 @@ TEST_F(TestDiffDriveController, cleanup) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_value()); + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_value()); executor.cancel(); } @@ -397,8 +399,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) assignResourcesPosFeedback(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(0.01, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(0.02, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.01, left_wheel_vel_cmd_->get_value()); + EXPECT_EQ(0.02, right_wheel_vel_cmd_->get_value()); state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -413,8 +415,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(1.0, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(1.0, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(1.0, left_wheel_vel_cmd_->get_value()); + EXPECT_EQ(1.0, right_wheel_vel_cmd_->get_value()); // deactivated // wait so controller process the second point when deactivated @@ -425,14 +427,14 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_value()) << "Wheels are halted on deactivate()"; // cleanup state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_value()); + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_value()); state = controller_->get_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 256a4ce465..d4af71f3b0 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -119,9 +119,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -134,9 +134,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 30.0); } TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) @@ -156,9 +156,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); } TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) @@ -173,9 +173,9 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); } TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) @@ -184,9 +184,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -214,9 +214,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 30.0); } TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) @@ -228,15 +228,15 @@ TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); // stop the controller ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 0.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 0.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 0.0); } diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp index 6ae9db4670..160ebb1309 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -54,9 +54,12 @@ class JointGroupEffortControllerTest : public ::testing::Test const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]}; - CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]}; - CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]}; + std::shared_ptr joint_1_cmd_ = + std::make_shared(joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]); + std::shared_ptr joint_2_cmd_ = + std::make_shared(joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]); + std::shared_ptr joint_3_cmd_ = + std::make_shared(joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]); }; #endif // TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 697e42d671..6ec16f4717 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -195,9 +195,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -210,9 +210,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30.0); } TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) @@ -238,9 +238,9 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); } TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) @@ -260,9 +260,9 @@ TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); } TEST_F(ForwardCommandControllerTest, CommandCallbackTest) @@ -273,9 +273,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"interface_name", "position"}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -303,9 +303,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30.0); } TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) @@ -316,9 +316,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_->get_node()->set_parameter({"interface_name", "position"}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -337,9 +337,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30); node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -381,9 +381,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // values should not change - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30); // set commands again controller_->rt_command_ptr_.writeFromNonRT(command_msg); @@ -394,7 +394,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 6.6); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 7.7); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 5.5); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 6.6); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 7.7); } diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 9c6bd2a352..61e454ecd8 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -66,9 +66,12 @@ class ForwardCommandControllerTest : public ::testing::Test const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; - CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; - CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + std::shared_ptr joint_1_pos_cmd_ = + std::make_shared(joint_names_[0], HW_IF_POSITION, &joint_commands_[0]); + std::shared_ptr joint_2_pos_cmd_ = + std::make_shared(joint_names_[1], HW_IF_POSITION, &joint_commands_[1]); + std::shared_ptr joint_3_pos_cmd_ = + std::make_shared(joint_names_[2], HW_IF_POSITION, &joint_commands_[2]); }; #endif // TEST_FORWARD_COMMAND_CONTROLLER_HPP_ 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 0cada04859..ae7efd6141 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 @@ -189,9 +189,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateSuccess) SetUpController(true); // check joint commands are the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) @@ -209,9 +209,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 30.0); } TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest) @@ -224,9 +224,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) @@ -244,9 +244,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) @@ -273,9 +273,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 30.0); } TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) @@ -293,9 +293,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 30.0); auto node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -336,9 +336,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // values should not change - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 30.0); // set commands again controller_->rt_command_ptr_.writeFromNonRT(command_msg); @@ -349,7 +349,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 6.6); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 7.7); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 5.5); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 6.6); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 7.7); } diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp index 62a4d4e981..e427776bab 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp @@ -74,9 +74,12 @@ class MultiInterfaceForwardCommandControllerTest : public ::testing::Test double vel_cmd_ = 2.1; double eff_cmd_ = 3.1; - CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &pos_cmd_}; - CommandInterface joint_1_vel_cmd_{joint_name_, HW_IF_VELOCITY, &vel_cmd_}; - CommandInterface joint_1_eff_cmd_{joint_name_, HW_IF_EFFORT, &eff_cmd_}; + std::shared_ptr joint_1_pos_cmd_ = + std::make_shared(joint_name_, HW_IF_POSITION, &pos_cmd_); + std::shared_ptr joint_1_vel_cmd_ = + std::make_shared(joint_name_, HW_IF_VELOCITY, &vel_cmd_); + std::shared_ptr joint_1_eff_cmd_ = + std::make_shared(joint_name_, HW_IF_EFFORT, &eff_cmd_); }; #endif // TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 7b22e0638b..df1cc20fac 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -228,13 +228,13 @@ class TrajectoryControllerTest : public ::testing::Test acc_state_interfaces_.reserve(joint_names_.size()); for (size_t i = 0; i < joint_names_.size(); ++i) { - pos_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + pos_cmd_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_POSITION, &joint_pos_[i])); - vel_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + vel_cmd_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_VELOCITY, &joint_vel_[i])); - acc_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + acc_cmd_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_ACCELERATION, &joint_acc_[i])); - eff_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + eff_cmd_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_EFFORT, &joint_eff_[i])); pos_state_interfaces_.emplace_back(std::make_shared( @@ -433,10 +433,10 @@ class TrajectoryControllerTest : public ::testing::Test std::vector joint_state_pos_; std::vector joint_state_vel_; std::vector joint_state_acc_; - std::vector pos_cmd_interfaces_; - std::vector vel_cmd_interfaces_; - std::vector acc_cmd_interfaces_; - std::vector eff_cmd_interfaces_; + std::vector> pos_cmd_interfaces_; + std::vector> vel_cmd_interfaces_; + std::vector> acc_cmd_interfaces_; + std::vector> eff_cmd_interfaces_; std::vector> pos_state_interfaces_; std::vector> vel_state_interfaces_; std::vector> acc_state_interfaces_; diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index a712cc81c2..163f2b710b 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -119,9 +119,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -134,9 +134,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30.0); } TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) @@ -156,9 +156,9 @@ TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); } TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) @@ -173,9 +173,9 @@ TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); } TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) @@ -184,9 +184,9 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -214,7 +214,7 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30.0); } diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp index 93149d8e19..152c73bf23 100644 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ b/position_controllers/test/test_joint_group_position_controller.hpp @@ -55,9 +55,12 @@ class JointGroupPositionControllerTest : public ::testing::Test const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; - CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; - CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + std::shared_ptr joint_1_pos_cmd_ = + std::make_shared(joint_names_[0], HW_IF_POSITION, &joint_commands_[0]); + std::shared_ptr joint_2_pos_cmd_ = + std::make_shared(joint_names_[1], HW_IF_POSITION, &joint_commands_[1]); + std::shared_ptr joint_3_pos_cmd_ = + std::make_shared(joint_names_[2], HW_IF_POSITION, &joint_commands_[2]); }; #endif // TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index f75e868dc5..53fb1c41d6 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -161,11 +161,13 @@ class TestTricycleController : public ::testing::Test std::make_shared( traction_joint_name, HW_IF_VELOCITY, &velocity_); - hardware_interface::CommandInterface steering_joint_pos_cmd_{ - steering_joint_name, HW_IF_POSITION, &position_}; + std::shared_ptr steering_joint_pos_cmd_ = + std::make_shared( + steering_joint_name, HW_IF_POSITION, &position_); - hardware_interface::CommandInterface traction_joint_vel_cmd_{ - traction_joint_name, HW_IF_VELOCITY, &velocity_}; + std::shared_ptr traction_joint_vel_cmd_ = + std::make_shared( + traction_joint_name, HW_IF_VELOCITY, &velocity_); rclcpp::Node::SharedPtr pub_node; rclcpp::Publisher::SharedPtr velocity_publisher; @@ -284,8 +286,8 @@ TEST_F(TestTricycleController, cleanup) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_->get_value()); executor.cancel(); } @@ -309,8 +311,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) assignResources(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(position_, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(position_, steering_joint_pos_cmd_->get_value()); + EXPECT_EQ(velocity_, traction_joint_vel_cmd_->get_value()); state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -325,8 +327,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_value()); + EXPECT_EQ(1.0, traction_joint_vel_cmd_->get_value()); // deactivated // wait so controller process the second point when deactivated @@ -337,14 +339,14 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()"; - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, traction_joint_vel_cmd_->get_value()) << "Wheels are halted on deactivate()"; // cleanup state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_->get_value()); state = controller_->get_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 43c1545461..8d350ec201 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -119,9 +119,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -134,9 +134,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 30.0); } TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) @@ -156,9 +156,9 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); } TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) @@ -173,9 +173,9 @@ TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); } TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) @@ -184,9 +184,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -214,9 +214,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 30.0); } TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) @@ -228,15 +228,15 @@ TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); // stop the controller ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 0.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 0.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 0.0); } diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp index e94f7f082d..32854085be 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.hpp @@ -55,9 +55,12 @@ class JointGroupVelocityControllerTest : public ::testing::Test const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]}; - CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]}; - CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]}; + std::shared_ptr joint_1_cmd_ = + std::make_shared(joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]); + std::shared_ptr joint_2_cmd_ = + std::make_shared(joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]); + std::shared_ptr joint_3_cmd_ = + std::make_shared(joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]); }; #endif // TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ From af6fdb545907797e05642389a833ced82a917427 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 11 Jan 2023 08:58:49 +0100 Subject: [PATCH 3/4] joint_state_broadcaster if no local topic us "/" as prefix --- joint_state_broadcaster/src/joint_state_broadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 74a19c9ed0..c449e9ae56 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -146,7 +146,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( try { - const std::string topic_name_prefix = params_.use_local_topics ? "~/" : ""; + const std::string topic_name_prefix = params_.use_local_topics ? "~/" : "/"; joint_state_publisher_ = get_node()->create_publisher( topic_name_prefix + "joint_states", rclcpp::SystemDefaultsQoS()); From 8d2e023e4704149528c56bbdb80c821eb9c03ceb Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 11 Jan 2023 09:55:48 +0100 Subject: [PATCH 4/4] adapt admittance_controller tests --- .../test/test_admittance_controller.hpp | 10 +++++----- gripper_controllers/test/test_gripper_controllers.hpp | 9 ++++++--- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 7dca2e2010..5a17f05102 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -201,7 +201,7 @@ class AdmittanceControllerTest : public ::testing::Test for (auto i = 0u; i < joint_command_values_.size(); ++i) { - command_itfs_.emplace_back(hardware_interface::CommandInterface( + command_itfs_.emplace_back(std::make_shared( joint_names_[i], command_interface_types_[0], &joint_command_values_[i])); command_ifs.emplace_back(command_itfs_.back()); } @@ -216,7 +216,7 @@ class AdmittanceControllerTest : public ::testing::Test for (auto i = 0u; i < joint_state_values_.size(); ++i) { - state_itfs_.emplace_back(hardware_interface::StateInterface( + state_itfs_.emplace_back(std::make_shared( joint_names_[i], state_interface_types_[0], &joint_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); } @@ -226,7 +226,7 @@ class AdmittanceControllerTest : public ::testing::Test for (auto i = 0u; i < fts_state_names_.size(); ++i) { - state_itfs_.emplace_back(hardware_interface::StateInterface( + state_itfs_.emplace_back(std::make_shared( ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); } @@ -397,8 +397,8 @@ class AdmittanceControllerTest : public ::testing::Test std::array fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; std::vector fts_state_names_; - std::vector state_itfs_; - std::vector command_itfs_; + std::vector> state_itfs_; + std::vector> command_itfs_; // Test related parameters std::unique_ptr controller_; diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp index 3e9750a603..b002aaf9c4 100644 --- a/gripper_controllers/test/test_gripper_controllers.hpp +++ b/gripper_controllers/test/test_gripper_controllers.hpp @@ -57,9 +57,12 @@ class GripperControllerTest : public ::testing::Test std::vector joint_states_ = {1.1, 2.1}; std::vector joint_commands_ = {3.1}; - StateInterface joint_1_pos_state_{joint_name_, HW_IF_POSITION, &joint_states_[0]}; - StateInterface joint_1_vel_state_{joint_name_, HW_IF_VELOCITY, &joint_states_[1]}; - CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &joint_commands_[0]}; + std::shared_ptr joint_1_pos_state_ = + std::make_shared(joint_name_, HW_IF_POSITION, &joint_states_[0]); + std::shared_ptr joint_1_vel_state_ = + std::make_shared(joint_name_, HW_IF_VELOCITY, &joint_states_[1]); + std::shared_ptr joint_1_pos_cmd_ = + std::make_shared(joint_name_, HW_IF_POSITION, &joint_commands_[0]); }; #endif // TEST_GRIPPER_CONTROLLERS_HPP_