From 70dce93a26aa5afb7a93e7e22fea060fc9b6fc9c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 29 Sep 2025 12:19:34 +0000 Subject: [PATCH 1/2] Fix usage of deprecated upstream methods --- .../test_ackermann_steering_controller.hpp | 46 ++--- .../test/test_admittance_controller.hpp | 26 +-- .../test/test_bicycle_steering_controller.hpp | 30 +-- .../test/test_chained_filter.cpp | 6 +- .../test/test_chained_filter.hpp | 3 +- .../test/test_multiple_chained_filter.cpp | 28 +-- .../test/test_multiple_chained_filter.hpp | 6 +- .../test/test_diff_drive_controller.cpp | 191 +++++++++--------- .../test_joint_group_effort_controller.cpp | 54 ++--- .../test_joint_group_effort_controller.hpp | 9 +- .../test_force_torque_sensor_broadcaster.cpp | 12 +- .../test_force_torque_sensor_broadcaster.hpp | 24 ++- .../test/test_forward_command_controller.cpp | 78 +++---- .../test/test_forward_command_controller.hpp | 9 +- ...i_interface_forward_command_controller.cpp | 54 ++--- ...i_interface_forward_command_controller.hpp | 9 +- .../test/test_gpio_command_controller.cpp | 114 ++++++----- .../test/test_gps_sensor_broadcaster.cpp | 51 +++-- .../test/test_imu_sensor_broadcaster.cpp | 20 +- .../test/test_imu_sensor_broadcaster.hpp | 50 +++-- .../test/test_joint_state_broadcaster.cpp | 62 ++++-- .../test/test_joint_state_broadcaster.hpp | 56 ++--- .../test/test_trajectory_controller_utils.hpp | 76 +++---- .../test/test_mecanum_drive_controller.hpp | 22 +- ...t_motion_primitives_forward_controller.hpp | 22 +- .../test/test_omni_wheel_drive_controller.cpp | 62 +++--- .../test/test_omni_wheel_drive_controller.hpp | 42 ++-- .../test/test_parallel_gripper_controller.cpp | 12 +- .../test/test_parallel_gripper_controller.hpp | 15 +- pid_controller/test/test_pid_controller.hpp | 23 ++- .../test/test_pose_broadcaster.cpp | 26 +-- .../test/test_pose_broadcaster.hpp | 32 ++- .../test_joint_group_position_controller.cpp | 42 ++-- .../test_joint_group_position_controller.hpp | 9 +- .../test/test_range_sensor_broadcaster.cpp | 2 +- .../test/test_range_sensor_broadcaster.hpp | 3 +- .../test_steering_controllers_library.hpp | 70 ++++--- .../test/test_tricycle_controller.cpp | 52 ++--- .../test_tricycle_steering_controller.hpp | 38 ++-- .../test_joint_group_velocity_controller.cpp | 54 ++--- .../test_joint_group_velocity_controller.hpp | 9 +- 41 files changed, 837 insertions(+), 712 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 63a8c22618..e7f3a6b345 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -146,63 +146,63 @@ class AckermannSteeringControllerFixture : public ::testing::Test traction_interface_name_ = "velocity"; } - std::vector command_ifs; + std::vector loaned_command_ifs; command_itfs_.reserve(joint_command_values_.size()); - command_ifs.reserve(joint_command_values_.size()); + loaned_command_ifs.reserve(joint_command_values_.size()); command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( traction_joints_names_[0], traction_interface_name_, &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); - command_ifs.emplace_back(command_itfs_.back()); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( traction_joints_names_[1], steering_interface_name_, &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); - command_ifs.emplace_back(command_itfs_.back()); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( steering_joints_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); - command_ifs.emplace_back(command_itfs_.back()); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( steering_joints_names_[1], steering_interface_name_, &joint_command_values_[CMD_STEER_LEFT_WHEEL])); - command_ifs.emplace_back(command_itfs_.back()); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); - std::vector state_ifs; + std::vector loaned_state_ifs; state_itfs_.reserve(joint_state_values_.size()); - state_ifs.reserve(joint_state_values_.size()); + loaned_state_ifs.reserve(joint_state_values_.size()); state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( traction_joints_names_[0], traction_interface_name_, &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); - state_ifs.emplace_back(state_itfs_.back()); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( traction_joints_names_[1], traction_interface_name_, &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); - state_ifs.emplace_back(state_itfs_.back()); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( steering_joints_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); - state_ifs.emplace_back(state_itfs_.back()); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( steering_joints_names_[1], steering_interface_name_, &joint_state_values_[STATE_STEER_LEFT_WHEEL])); - state_ifs.emplace_back(state_itfs_.back()); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs)); } void subscribe_and_get_messages(ControllerStateMsg & msg) @@ -305,8 +305,8 @@ class AckermannSteeringControllerFixture : public ::testing::Test std::string traction_interface_name_ = ""; std::string preceding_prefix_ = "pid_controller"; - 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/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index dd854bce7d..2288f675fd 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -171,32 +171,32 @@ class AdmittanceControllerTest : public ::testing::Test void assign_interfaces() { - std::vector command_ifs; + std::vector loaned_command_ifs; command_itfs_.reserve(joint_command_values_.size()); - command_ifs.reserve(joint_command_values_.size()); + loaned_command_ifs.reserve(joint_command_values_.size()); for (auto i = 0u; i < joint_command_values_.size(); ++i) { command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( joint_names_[i], command_interface_types_[0], &joint_command_values_[i])); - command_ifs.emplace_back(command_itfs_.back()); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); } auto sc_fts = semantic_components::ForceTorqueSensor(ft_sensor_name_); fts_state_names_ = sc_fts.get_state_interface_names(); - std::vector state_ifs; + std::vector loaned_state_ifs; const size_t num_state_ifs = joint_state_values_.size() + fts_state_names_.size(); state_itfs_.reserve(num_state_ifs); - state_ifs.reserve(num_state_ifs); + loaned_state_ifs.reserve(num_state_ifs); for (auto i = 0u; i < joint_state_values_.size(); ++i) { state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( joint_names_[i], state_interface_types_[0], &joint_state_values_[i])); - state_ifs.emplace_back(state_itfs_.back()); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); } std::vector fts_itf_names = {"force.x", "force.y", "force.z", @@ -205,12 +205,12 @@ class AdmittanceControllerTest : public ::testing::Test for (auto i = 0u; i < fts_state_names_.size(); ++i) { state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i])); - state_ifs.emplace_back(state_itfs_.back()); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); } - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs)); } void broadcast_tfs() @@ -386,8 +386,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/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 9b329de17b..d594bef3e9 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -144,39 +144,39 @@ class BicycleSteeringControllerFixture : public ::testing::Test traction_interface_name_ = "velocity"; } - std::vector command_ifs; + std::vector loaned_command_ifs; command_itfs_.reserve(joint_command_values_.size()); - command_ifs.reserve(joint_command_values_.size()); + loaned_command_ifs.reserve(joint_command_values_.size()); command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( traction_joints_names_[0], traction_interface_name_, &joint_command_values_[CMD_TRACTION_WHEEL])); - command_ifs.emplace_back(command_itfs_.back()); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( steering_joints_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); - command_ifs.emplace_back(command_itfs_.back()); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); - std::vector state_ifs; + std::vector loaned_state_ifs; state_itfs_.reserve(joint_state_values_.size()); - state_ifs.reserve(joint_state_values_.size()); + loaned_state_ifs.reserve(joint_state_values_.size()); state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( traction_joints_names_[0], traction_interface_name_, &joint_state_values_[STATE_TRACTION_WHEEL])); - state_ifs.emplace_back(state_itfs_.back()); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( steering_joints_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); - state_ifs.emplace_back(state_itfs_.back()); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs)); } void subscribe_and_get_messages(ControllerStateMsg & msg) @@ -275,8 +275,8 @@ class BicycleSteeringControllerFixture : public ::testing::Test std::string traction_interface_name_ = ""; std::string preceding_prefix_ = "pid_controller"; - 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/chained_filter_controller/test/test_chained_filter.cpp b/chained_filter_controller/test/test_chained_filter.cpp index d1238c00f7..32ff4ffd38 100644 --- a/chained_filter_controller/test/test_chained_filter.cpp +++ b/chained_filter_controller/test/test_chained_filter.cpp @@ -123,18 +123,18 @@ TEST_F(ChainedFilterTest, UpdateFilter) controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), controller_interface::return_type::OK); // input state interface should not change - EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]); + EXPECT_EQ(joint_1_pos_->get_optional().value(), joint_states_[0]); // output should be the same auto state_if_exported_conf = controller_->export_state_interfaces(); ASSERT_THAT(state_if_exported_conf, SizeIs(1u)); EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), joint_states_[0]); - ASSERT_TRUE(joint_1_pos_.set_value(2.0)); + ASSERT_TRUE(joint_1_pos_->set_value(2.0)); ASSERT_EQ( controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), controller_interface::return_type::OK); // input and output should have changed - EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]); + EXPECT_EQ(joint_1_pos_->get_optional().value(), joint_states_[0]); EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), 1.55); ASSERT_EQ( controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), diff --git a/chained_filter_controller/test/test_chained_filter.hpp b/chained_filter_controller/test/test_chained_filter.hpp index 8ce63433ac..06918ae8f1 100644 --- a/chained_filter_controller/test/test_chained_filter.hpp +++ b/chained_filter_controller/test/test_chained_filter.hpp @@ -49,7 +49,8 @@ class ChainedFilterTest : public ::testing::Test const std::vector joint_names_ = {"wheel_left"}; std::vector joint_states_ = {1.1}; - StateInterface joint_1_pos_{joint_names_[0], HW_IF_POSITION, &joint_states_[0]}; + StateInterface::SharedPtr joint_1_pos_ = + std::make_shared(joint_names_[0], HW_IF_POSITION, &joint_states_[0]); rclcpp::executors::SingleThreadedExecutor executor; }; diff --git a/chained_filter_controller/test/test_multiple_chained_filter.cpp b/chained_filter_controller/test/test_multiple_chained_filter.cpp index 0ec774a8c4..c29d52feed 100644 --- a/chained_filter_controller/test/test_multiple_chained_filter.cpp +++ b/chained_filter_controller/test/test_multiple_chained_filter.cpp @@ -52,8 +52,8 @@ void MultipleChainedFilterTest::SetUpController( ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; - state_ifs.emplace_back(joint_1_pos_); - state_ifs.emplace_back(joint_2_pos_); + state_ifs.emplace_back(joint_1_pos_, nullptr); + state_ifs.emplace_back(joint_2_pos_, nullptr); controller_->assign_interfaces({}, std::move(state_ifs)); executor.add_node(controller_->get_node()->get_node_base_interface()); } @@ -118,23 +118,23 @@ TEST_F(MultipleChainedFilterTest, UpdateFilter_multiple_interfaces) controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), controller_interface::return_type::OK); // input state interface should not change - EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]); - EXPECT_EQ(joint_2_pos_.get_optional().value(), joint_states_[1]); + EXPECT_EQ(joint_1_pos_->get_optional().value(), joint_states_[0]); + EXPECT_EQ(joint_2_pos_->get_optional().value(), joint_states_[1]); // output should be the same auto state_if_exported_conf = controller_->export_state_interfaces(); ASSERT_THAT(state_if_exported_conf, SizeIs(2u)); EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), joint_states_[0]); EXPECT_EQ(state_if_exported_conf[1]->get_optional().value(), joint_states_[1]); - ASSERT_TRUE(joint_1_pos_.set_value(2.0)); - ASSERT_TRUE(joint_2_pos_.set_value(3.0)); + ASSERT_TRUE(joint_1_pos_->set_value(2.0)); + ASSERT_TRUE(joint_2_pos_->set_value(3.0)); ASSERT_EQ( controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), controller_interface::return_type::OK); // input and output should have changed - EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]); + EXPECT_EQ(joint_1_pos_->get_optional().value(), joint_states_[0]); EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), 1.55); - EXPECT_EQ(joint_2_pos_.get_optional().value(), joint_states_[1]); + EXPECT_EQ(joint_2_pos_->get_optional().value(), joint_states_[1]); EXPECT_EQ(state_if_exported_conf[1]->get_optional().value(), 2.6); ASSERT_EQ( @@ -155,23 +155,23 @@ TEST_F(MultipleChainedFilterTest, UpdateFilter_multiple_interfaces_config_per_in controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), controller_interface::return_type::OK); // input state interface should not change - EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]); - EXPECT_EQ(joint_2_pos_.get_optional().value(), joint_states_[1]); + EXPECT_EQ(joint_1_pos_->get_optional().value(), joint_states_[0]); + EXPECT_EQ(joint_2_pos_->get_optional().value(), joint_states_[1]); // output should be the same auto state_if_exported_conf = controller_->export_state_interfaces(); ASSERT_THAT(state_if_exported_conf, SizeIs(2u)); EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), joint_states_[0]); EXPECT_EQ(state_if_exported_conf[1]->get_optional().value(), joint_states_[1]); - ASSERT_TRUE(joint_1_pos_.set_value(2.0)); - ASSERT_TRUE(joint_2_pos_.set_value(2.8)); + ASSERT_TRUE(joint_1_pos_->set_value(2.0)); + ASSERT_TRUE(joint_2_pos_->set_value(2.8)); ASSERT_EQ( controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), controller_interface::return_type::OK); // input and output should have changed - EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]); + EXPECT_EQ(joint_1_pos_->get_optional().value(), joint_states_[0]); EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), 1.55); - EXPECT_EQ(joint_2_pos_.get_optional().value(), joint_states_[1]); + EXPECT_EQ(joint_2_pos_->get_optional().value(), joint_states_[1]); // second update call, mean of (2.2, 2.8) EXPECT_EQ(state_if_exported_conf[1]->get_optional().value(), 2.5); diff --git a/chained_filter_controller/test/test_multiple_chained_filter.hpp b/chained_filter_controller/test/test_multiple_chained_filter.hpp index d2be05a7d9..801d5ded6b 100644 --- a/chained_filter_controller/test/test_multiple_chained_filter.hpp +++ b/chained_filter_controller/test/test_multiple_chained_filter.hpp @@ -49,8 +49,10 @@ class MultipleChainedFilterTest : public ::testing::Test const std::vector joint_names_ = {"wheel_left", "wheel_right"}; std::vector joint_states_ = {1.1, 2.2}; - StateInterface joint_1_pos_{joint_names_[0], HW_IF_POSITION, &joint_states_[0]}; - StateInterface joint_2_pos_{joint_names_[1], HW_IF_POSITION, &joint_states_[1]}; + StateInterface::SharedPtr joint_1_pos_ = + std::make_shared(joint_names_[0], HW_IF_POSITION, &joint_states_[0]); + StateInterface::SharedPtr joint_2_pos_ = + std::make_shared(joint_names_[1], HW_IF_POSITION, &joint_states_[1]); rclcpp::executors::SingleThreadedExecutor executor; }; diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 8b442875c7..a180b3cef7 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -149,12 +149,12 @@ class TestDiffDriveController : public ::testing::Test void assignResourcesPosFeedback() { std::vector state_ifs; - state_ifs.emplace_back(left_wheel_pos_state_); - state_ifs.emplace_back(right_wheel_pos_state_); + state_ifs.emplace_back(left_wheel_pos_state_, nullptr); + state_ifs.emplace_back(right_wheel_pos_state_, nullptr); std::vector command_ifs; - command_ifs.emplace_back(left_wheel_vel_cmd_); - command_ifs.emplace_back(right_wheel_vel_cmd_); + command_ifs.emplace_back(left_wheel_vel_cmd_, nullptr); + command_ifs.emplace_back(right_wheel_vel_cmd_, nullptr); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } @@ -162,12 +162,12 @@ class TestDiffDriveController : public ::testing::Test void assignResourcesVelFeedback() { std::vector state_ifs; - state_ifs.emplace_back(left_wheel_vel_state_); - state_ifs.emplace_back(right_wheel_vel_state_); + state_ifs.emplace_back(left_wheel_vel_state_, nullptr); + state_ifs.emplace_back(right_wheel_vel_state_, nullptr); std::vector command_ifs; - command_ifs.emplace_back(left_wheel_vel_cmd_); - command_ifs.emplace_back(right_wheel_vel_cmd_); + command_ifs.emplace_back(left_wheel_vel_cmd_, nullptr); + command_ifs.emplace_back(right_wheel_vel_cmd_, nullptr); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } @@ -175,8 +175,8 @@ class TestDiffDriveController : public ::testing::Test void assignResourcesNoFeedback() { std::vector command_ifs; - command_ifs.emplace_back(left_wheel_vel_cmd_); - command_ifs.emplace_back(right_wheel_vel_cmd_); + command_ifs.emplace_back(left_wheel_vel_cmd_, nullptr); + command_ifs.emplace_back(right_wheel_vel_cmd_, nullptr); controller_->assign_interfaces(std::move(command_ifs), {}); } @@ -210,18 +210,24 @@ 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]}; - 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]}; + hardware_interface::StateInterface::SharedPtr left_wheel_pos_state_ = + std::make_shared( + left_wheel_names[0], HW_IF_POSITION, &position_values_[0]); + hardware_interface::StateInterface::SharedPtr right_wheel_pos_state_ = + std::make_shared( + right_wheel_names[0], HW_IF_POSITION, &position_values_[1]); + hardware_interface::StateInterface::SharedPtr left_wheel_vel_state_ = + std::make_shared( + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]); + hardware_interface::StateInterface::SharedPtr right_wheel_vel_state_ = + std::make_shared( + right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]); + hardware_interface::CommandInterface::SharedPtr left_wheel_vel_cmd_ = + std::make_shared( + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]); + hardware_interface::CommandInterface::SharedPtr 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; @@ -546,8 +552,8 @@ TEST_F(TestDiffDriveController, test_speed_limiter) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR(0.0, left_wheel_vel_cmd_.get_optional().value(), 1e-3); - EXPECT_NEAR(0.0, right_wheel_vel_cmd_.get_optional().value(), 1e-3); + EXPECT_NEAR(0.0, left_wheel_vel_cmd_->get_optional().value(), 1e-3); + EXPECT_NEAR(0.0, right_wheel_vel_cmd_->get_optional().value(), 1e-3); } const double dt = 0.001; @@ -567,18 +573,18 @@ TEST_F(TestDiffDriveController, test_speed_limiter) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); - EXPECT_GT(linear / wheel_radius, left_wheel_vel_cmd_.get_optional().value()) + EXPECT_GT(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value()) << "at t: " << i * dt << "s, but this angular velocity should only be achieved at t: " << time_acc; - EXPECT_GT(linear / wheel_radius, right_wheel_vel_cmd_.get_optional().value()) + EXPECT_GT(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value()) << "at t: " << i * dt << "s, but this angular velocity should only be achieved at t: " << time_acc; } ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); - EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_optional().value(), 1e-3); - EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3); // wait for the speed limiter to fill the queue for (int i = 0; i < 3; ++i) @@ -586,8 +592,8 @@ TEST_F(TestDiffDriveController, test_speed_limiter) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(linear / wheel_radius, left_wheel_vel_cmd_.get_optional().value()); - EXPECT_EQ(linear / wheel_radius, right_wheel_vel_cmd_.get_optional().value()); + EXPECT_EQ(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value()); + EXPECT_EQ(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value()); } } @@ -604,18 +610,18 @@ TEST_F(TestDiffDriveController, test_speed_limiter) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); - EXPECT_LT(linear / wheel_radius, left_wheel_vel_cmd_.get_optional().value()) + EXPECT_LT(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value()) << "at t: " << i * dt << "s, but this angular velocity should only be achieved at t: " << time_acc; - EXPECT_LT(linear / wheel_radius, right_wheel_vel_cmd_.get_optional().value()) + EXPECT_LT(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value()) << "at t: " << i * dt << "s, but this angular velocity should only be achieved at t: " << time_acc; } ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); - EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_optional().value(), 1e-3); - EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3); // wait for the speed limiter to fill the queue for (int i = 0; i < 3; ++i) @@ -623,8 +629,8 @@ TEST_F(TestDiffDriveController, test_speed_limiter) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_optional().value(), 1e-3); - EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3); } } @@ -641,18 +647,18 @@ TEST_F(TestDiffDriveController, test_speed_limiter) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); - EXPECT_LT(linear / wheel_radius, left_wheel_vel_cmd_.get_optional().value()) + EXPECT_LT(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value()) << "at t: " << i * dt << "s, but this angular velocity should only be achieved at t: " << time_acc; - EXPECT_LT(linear / wheel_radius, right_wheel_vel_cmd_.get_optional().value()) + EXPECT_LT(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value()) << "at t: " << i * dt << "s, but this angular velocity should only be achieved at t: " << time_acc; } ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); - EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_optional().value(), 1e-3); - EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3); // wait for the speed limiter to fill the queue for (int i = 0; i < 3; ++i) @@ -660,8 +666,8 @@ TEST_F(TestDiffDriveController, test_speed_limiter) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_optional().value(), 1e-3); - EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3); } } @@ -678,18 +684,18 @@ TEST_F(TestDiffDriveController, test_speed_limiter) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); - EXPECT_GT(linear / wheel_radius, left_wheel_vel_cmd_.get_optional().value()) + EXPECT_GT(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value()) << "at t: " << i * dt << "s, but this angular velocity should only be achieved at t: " << time_acc; - EXPECT_GT(linear / wheel_radius, right_wheel_vel_cmd_.get_optional().value()) + EXPECT_GT(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value()) << "at t: " << i * dt << "s, but this angular velocity should only be achieved at t: " << time_acc; } ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); - EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_optional().value(), 1e-3); - EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3); // wait for the speed limiter to fill the queue for (int i = 0; i < 3; ++i) @@ -697,8 +703,8 @@ TEST_F(TestDiffDriveController, test_speed_limiter) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_optional().value(), 1e-3); - EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3); } } } @@ -787,24 +793,24 @@ TEST_F(TestDiffDriveController, cleanup) controller_interface::return_type::OK); // should be moving - EXPECT_LT(0.0, left_wheel_vel_cmd_.get_optional().value()); - EXPECT_LT(0.0, right_wheel_vel_cmd_.get_optional().value()); + EXPECT_LT(0.0, left_wheel_vel_cmd_->get_optional().value()); + EXPECT_LT(0.0, right_wheel_vel_cmd_->get_optional().value()); state = controller_->get_node()->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); // should be stopped - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_optional().value()) + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_optional().value()) << "Wheels should be halted on deactivate()"; - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()) + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_optional().value()) << "Wheels should be halted on deactivate()"; state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_optional().value()); - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()); + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_optional().value()); + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_optional().value()); executor.cancel(); } @@ -824,8 +830,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_optional().value()); - EXPECT_EQ(0.02, right_wheel_vel_cmd_.get_optional().value()); + EXPECT_EQ(0.01, left_wheel_vel_cmd_->get_optional().value()); + EXPECT_EQ(0.02, right_wheel_vel_cmd_->get_optional().value()); state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -840,8 +846,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_optional().value()); - EXPECT_EQ(1.0, right_wheel_vel_cmd_.get_optional().value()); + EXPECT_EQ(1.0, left_wheel_vel_cmd_->get_optional().value()); + EXPECT_EQ(1.0, right_wheel_vel_cmd_->get_optional().value()); // deactivated // wait so controller process the second point when deactivated @@ -852,15 +858,16 @@ 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_optional().value()) << "Wheels are halted on deactivate()"; - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()) + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_optional().value()) + << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_optional().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_optional().value()); - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()); + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_optional().value()); + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_optional().value()); state = controller_->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); @@ -906,8 +913,8 @@ TEST_F(TestDiffDriveController, chainable_controller_unchained_mode) } // But NaNs should not propagate to command interfaces // (these are set to 0.1 and 0.2 in InitController) - ASSERT_FALSE(std::isnan(left_wheel_vel_cmd_.get_optional().value())); - ASSERT_FALSE(std::isnan(right_wheel_vel_cmd_.get_optional().value())); + ASSERT_FALSE(std::isnan(left_wheel_vel_cmd_->get_optional().value())); + ASSERT_FALSE(std::isnan(right_wheel_vel_cmd_->get_optional().value())); // Check that a late command message causes the command interfaces to be set to 0.0 const double linear = 1.0; @@ -920,9 +927,9 @@ TEST_F(TestDiffDriveController, chainable_controller_unchained_mode) ASSERT_EQ( controller_->update(pub_node->get_clock()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_optional().value()) + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_optional().value()) << "Wheels should halt if command message is older than cmd_vel_timeout"; - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()) + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_optional().value()) << "Wheels should halt if command message is older than cmd_vel_timeout"; // Now check that a timely published command message sets the command interfaces to the correct @@ -934,8 +941,8 @@ TEST_F(TestDiffDriveController, chainable_controller_unchained_mode) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(linear, left_wheel_vel_cmd_.get_optional().value()); - EXPECT_EQ(linear, right_wheel_vel_cmd_.get_optional().value()); + EXPECT_EQ(linear, left_wheel_vel_cmd_->get_optional().value()); + EXPECT_EQ(linear, right_wheel_vel_cmd_->get_optional().value()); // Now check that the command interfaces are set to 0.0 on deactivation // (despite calls to update()) @@ -946,17 +953,17 @@ TEST_F(TestDiffDriveController, chainable_controller_unchained_mode) 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_optional().value()) + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_optional().value()) << "Wheels should be halted on deactivate()"; - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()) + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_optional().value()) << "Wheels should be 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_optional().value()) + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_optional().value()) << "Wheels should be halted on cleanup()"; - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()) + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_optional().value()) << "Wheels should be halted on cleanup()"; state = controller_->configure(); @@ -1000,8 +1007,8 @@ TEST_F(TestDiffDriveController, chainable_controller_chained_mode) } // But NaNs should not propagate to command interfaces // (these are set to 0.1 and 0.2 in InitController) - ASSERT_FALSE(std::isnan(left_wheel_vel_cmd_.get_optional().value())); - ASSERT_FALSE(std::isnan(right_wheel_vel_cmd_.get_optional().value())); + ASSERT_FALSE(std::isnan(left_wheel_vel_cmd_->get_optional().value())); + ASSERT_FALSE(std::isnan(right_wheel_vel_cmd_->get_optional().value())); // Imitate preceding controllers by setting reference_interfaces_ // (Note: reference_interfaces_ is protected, but this is @@ -1013,8 +1020,8 @@ TEST_F(TestDiffDriveController, chainable_controller_chained_mode) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(linear, left_wheel_vel_cmd_.get_optional().value()); - EXPECT_EQ(linear, right_wheel_vel_cmd_.get_optional().value()); + EXPECT_EQ(linear, left_wheel_vel_cmd_->get_optional().value()); + EXPECT_EQ(linear, right_wheel_vel_cmd_->get_optional().value()); // Now check that the command interfaces are set to 0.0 on deactivation // (despite calls to update()) @@ -1025,17 +1032,17 @@ TEST_F(TestDiffDriveController, chainable_controller_chained_mode) 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_optional().value()) + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_optional().value()) << "Wheels should be halted on deactivate()"; - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()) + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_optional().value()) << "Wheels should be 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_optional().value()) + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_optional().value()) << "Wheels should be halted on cleanup()"; - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()) + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_optional().value()) << "Wheels should be halted on cleanup()"; state = controller_->configure(); @@ -1106,8 +1113,8 @@ TEST_F(TestDiffDriveController, deactivate_then_activate) } // But NaNs should not propagate to command interfaces // (these are set to 0.1 and 0.2 in InitController) - ASSERT_FALSE(std::isnan(left_wheel_vel_cmd_.get_optional().value())); - ASSERT_FALSE(std::isnan(right_wheel_vel_cmd_.get_optional().value())); + ASSERT_FALSE(std::isnan(left_wheel_vel_cmd_->get_optional().value())); + ASSERT_FALSE(std::isnan(right_wheel_vel_cmd_->get_optional().value())); // published command message sets the command interfaces to the correct values const double linear = 1.0; @@ -1118,8 +1125,8 @@ TEST_F(TestDiffDriveController, deactivate_then_activate) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(linear, left_wheel_vel_cmd_.get_optional().value()); - EXPECT_EQ(linear, right_wheel_vel_cmd_.get_optional().value()); + EXPECT_EQ(linear, left_wheel_vel_cmd_->get_optional().value()); + EXPECT_EQ(linear, right_wheel_vel_cmd_->get_optional().value()); // Now check that the command interfaces are set to 0.0 on deactivation // (despite calls to update()) @@ -1130,9 +1137,9 @@ TEST_F(TestDiffDriveController, deactivate_then_activate) 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_optional().value()) + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_optional().value()) << "Wheels should be halted on deactivate()"; - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()) + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_optional().value()) << "Wheels should be halted on deactivate()"; // Activate again @@ -1148,9 +1155,9 @@ TEST_F(TestDiffDriveController, deactivate_then_activate) EXPECT_TRUE(std::isnan(interface)) << "Reference interfaces should initially be NaN on activation"; } - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_optional().value()) + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_optional().value()) << "Wheels should still have the same command as when they were last set (on deactivation)"; - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()) + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_optional().value()) << "Wheels should still have the same command as when they were last set (on deactivation)"; // A new command should work as expected @@ -1160,8 +1167,8 @@ TEST_F(TestDiffDriveController, deactivate_then_activate) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(linear, left_wheel_vel_cmd_.get_optional().value()); - EXPECT_EQ(linear, right_wheel_vel_cmd_.get_optional().value()); + EXPECT_EQ(linear, left_wheel_vel_cmd_->get_optional().value()); + EXPECT_EQ(linear, right_wheel_vel_cmd_->get_optional().value()); // Deactivate again and cleanup std::this_thread::sleep_for(std::chrono::milliseconds(300)); @@ -1204,8 +1211,8 @@ TEST_F(TestDiffDriveController, command_with_zero_timestamp_is_accepted_with_war ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(linear, left_wheel_vel_cmd_.get_optional().value()); - EXPECT_EQ(linear, right_wheel_vel_cmd_.get_optional().value()); + EXPECT_EQ(linear, left_wheel_vel_cmd_->get_optional().value()); + EXPECT_EQ(linear, right_wheel_vel_cmd_->get_optional().value()); // Deactivate and cleanup std::this_thread::sleep_for(std::chrono::milliseconds(300)); diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 8423b2736d..0a74913ebb 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -47,9 +47,9 @@ void JointGroupEffortControllerTest::SetUpController( ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; - command_ifs.emplace_back(joint_1_cmd_); - command_ifs.emplace_back(joint_2_cmd_); - command_ifs.emplace_back(joint_3_cmd_); + command_ifs.emplace_back(joint_1_cmd_, nullptr); + command_ifs.emplace_back(joint_2_cmd_, nullptr); + command_ifs.emplace_back(joint_3_cmd_, nullptr); controller_->assign_interfaces(std::move(command_ifs), {}); executor.add_node(controller_->get_node()->get_node_base_interface()); } @@ -76,9 +76,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); // send command forward_command_controller::CmdType command; @@ -91,9 +91,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_optional().value(), 30.0); } TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) @@ -114,9 +114,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); } TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) @@ -132,9 +132,9 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); } TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) @@ -142,9 +142,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) SetUpController({rclcpp::Parameter("joints", joint_names_)}); // default values - ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -175,9 +175,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_optional().value(), 30.0); } TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) @@ -188,15 +188,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_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_optional().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_optional().value(), 0.0); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 0.0); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 0.0); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 0.0); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 0.0); + ASSERT_EQ(joint_3_cmd_->get_optional().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 cf05bfab26..74562623ed 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -55,9 +55,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]}; + CommandInterface::SharedPtr joint_1_cmd_ = + std::make_shared(joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]); + CommandInterface::SharedPtr joint_2_cmd_ = + std::make_shared(joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]); + CommandInterface::SharedPtr joint_3_cmd_ = + std::make_shared(joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]); rclcpp::executors::SingleThreadedExecutor executor; }; 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 017928888f..3c36ece783 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 @@ -59,12 +59,12 @@ void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster(std::string node_name ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; - state_ifs.emplace_back(fts_force_x_); - state_ifs.emplace_back(fts_force_y_); - state_ifs.emplace_back(fts_force_z_); - state_ifs.emplace_back(fts_torque_x_); - state_ifs.emplace_back(fts_torque_y_); - state_ifs.emplace_back(fts_torque_z_); + state_ifs.emplace_back(fts_force_x_, nullptr); + state_ifs.emplace_back(fts_force_y_, nullptr); + state_ifs.emplace_back(fts_force_z_, nullptr); + state_ifs.emplace_back(fts_torque_x_, nullptr); + state_ifs.emplace_back(fts_torque_y_, nullptr); + state_ifs.emplace_back(fts_torque_z_, nullptr); fts_broadcaster_->assign_interfaces({}, std::move(state_ifs)); } 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 5bd6b62452..293d1ddacc 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 @@ -62,12 +62,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]}; + hardware_interface::StateInterface::SharedPtr fts_force_x_ = + std::make_shared( + sensor_name_, "force.x", &sensor_values_[0]); + hardware_interface::StateInterface::SharedPtr fts_force_y_ = + std::make_shared( + sensor_name_, "force.y", &sensor_values_[1]); + hardware_interface::StateInterface::SharedPtr fts_force_z_ = + std::make_shared( + sensor_name_, "force.z", &sensor_values_[2]); + hardware_interface::StateInterface::SharedPtr fts_torque_x_ = + std::make_shared( + sensor_name_, "torque.x", &sensor_values_[3]); + hardware_interface::StateInterface::SharedPtr fts_torque_y_ = + std::make_shared( + sensor_name_, "torque.y", &sensor_values_[4]); + hardware_interface::StateInterface::SharedPtr fts_torque_z_ = + std::make_shared( + sensor_name_, "torque.z", &sensor_values_[5]); std::unique_ptr fts_broadcaster_; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 57e7ed7021..75bb97d18b 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -59,9 +59,9 @@ void ForwardCommandControllerTest::SetUpController( ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; - command_ifs.emplace_back(joint_1_pos_cmd_); - command_ifs.emplace_back(joint_2_pos_cmd_); - command_ifs.emplace_back(joint_3_pos_cmd_); + command_ifs.emplace_back(joint_1_pos_cmd_, nullptr); + command_ifs.emplace_back(joint_2_pos_cmd_, nullptr); + command_ifs.emplace_back(joint_3_pos_cmd_, nullptr); controller_->assign_interfaces(std::move(command_ifs), {}); executor.add_node(controller_->get_node()->get_node_base_interface()); } @@ -133,9 +133,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_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); // send command forward_command_controller::CmdType command_msg; @@ -148,9 +148,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 30.0); } TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) @@ -174,9 +174,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_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); } TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) @@ -195,9 +195,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_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); } TEST_F(ForwardCommandControllerTest, CommandCallbackTest) @@ -206,9 +206,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) {rclcpp::Parameter("joints", joint_names_), rclcpp::Parameter("interface_name", "position")}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -239,9 +239,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 30.0); } TEST_F(ForwardCommandControllerTest, DropInfiniteCommandCallbackTest) @@ -250,9 +250,9 @@ TEST_F(ForwardCommandControllerTest, DropInfiniteCommandCallbackTest) {rclcpp::Parameter("joints", joint_names_), rclcpp::Parameter("interface_name", "position")}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -283,9 +283,9 @@ TEST_F(ForwardCommandControllerTest, DropInfiniteCommandCallbackTest) controller_interface::return_type::OK); // check message containing infinite command value was rejected - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); } TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) @@ -294,9 +294,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) {rclcpp::Parameter("joints", joint_names_), rclcpp::Parameter("interface_name", "position")}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -331,9 +331,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 10); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 20); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 30); node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -379,9 +379,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // values should not change - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 10); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 20); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 30); // set commands again controller_->rt_command_.set(command); @@ -392,7 +392,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 5.5); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 6.6); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 7.7); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 5.5); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 6.6); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().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 dc5de90744..2f9abfd8e6 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]}; + CommandInterface::SharedPtr joint_1_pos_cmd_ = + std::make_shared(joint_names_[0], HW_IF_POSITION, &joint_commands_[0]); + CommandInterface::SharedPtr joint_2_pos_cmd_ = + std::make_shared(joint_names_[1], HW_IF_POSITION, &joint_commands_[1]); + CommandInterface::SharedPtr joint_3_pos_cmd_ = + std::make_shared(joint_names_[2], HW_IF_POSITION, &joint_commands_[2]); rclcpp::executors::SingleThreadedExecutor executor; }; 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 c01eb7a980..e4f6a5b735 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 @@ -70,9 +70,9 @@ void MultiInterfaceForwardCommandControllerTest::SetUpController( ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; - command_ifs.emplace_back(joint_1_pos_cmd_); - command_ifs.emplace_back(joint_1_vel_cmd_); - command_ifs.emplace_back(joint_1_eff_cmd_); + command_ifs.emplace_back(joint_1_pos_cmd_, nullptr); + command_ifs.emplace_back(joint_1_vel_cmd_, nullptr); + command_ifs.emplace_back(joint_1_eff_cmd_, nullptr); controller_->assign_interfaces(std::move(command_ifs), {}); executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -110,9 +110,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateSuccess) SetUpController(true); // check joint commands are the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_->get_optional().value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) @@ -130,9 +130,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_optional().value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_optional().value(), 30.0); } TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest) @@ -145,9 +145,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_optional().value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_->get_optional().value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) @@ -165,9 +165,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_optional().value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_->get_optional().value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) @@ -197,9 +197,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_optional().value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_optional().value(), 30.0); } TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) @@ -225,9 +225,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_optional().value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_optional().value(), 30.0); auto node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -273,9 +273,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // values should not change - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_optional().value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_optional().value(), 30.0); // set commands again controller_->rt_command_.set(command); @@ -286,7 +286,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 5.5); - ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 6.6); - ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 7.7); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 5.5); + ASSERT_EQ(joint_1_vel_cmd_->get_optional().value(), 6.6); + ASSERT_EQ(joint_1_eff_cmd_->get_optional().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 b51e0d52fb..6e9da2c50e 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 @@ -75,9 +75,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_}; + CommandInterface::SharedPtr joint_1_pos_cmd_ = + std::make_shared(joint_name_, HW_IF_POSITION, &pos_cmd_); + CommandInterface::SharedPtr joint_1_vel_cmd_ = + std::make_shared(joint_name_, HW_IF_VELOCITY, &vel_cmd_); + CommandInterface::SharedPtr joint_1_eff_cmd_ = + std::make_shared(joint_name_, HW_IF_EFFORT, &eff_cmd_); rclcpp::executors::SingleThreadedExecutor executor; }; diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 9bf26e91d9..24190edc01 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -105,14 +105,14 @@ class GpioCommandControllerTestSuite : public ::testing::Test void setup_command_and_state_interfaces() { std::vector command_interfaces; - command_interfaces.emplace_back(gpio_1_1_dig_cmd); - command_interfaces.emplace_back(gpio_1_2_dig_cmd); - command_interfaces.emplace_back(gpio_2_ana_cmd); + command_interfaces.emplace_back(gpio_1_1_dig_cmd, nullptr); + command_interfaces.emplace_back(gpio_1_2_dig_cmd, nullptr); + command_interfaces.emplace_back(gpio_2_ana_cmd, nullptr); std::vector state_interfaces; - state_interfaces.emplace_back(gpio_1_1_dig_state); - state_interfaces.emplace_back(gpio_1_2_dig_state); - state_interfaces.emplace_back(gpio_2_ana_state); + state_interfaces.emplace_back(gpio_1_1_dig_state, nullptr); + state_interfaces.emplace_back(gpio_1_2_dig_state, nullptr); + state_interfaces.emplace_back(gpio_2_ana_state, nullptr); controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); } @@ -133,12 +133,12 @@ class GpioCommandControllerTestSuite : public ::testing::Test void assert_default_command_and_state_values() { - ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), gpio_commands.at(0)); - ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), gpio_commands.at(1)); - ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), gpio_commands.at(2)); - ASSERT_EQ(gpio_1_1_dig_state.get_optional().value(), gpio_states.at(0)); - ASSERT_EQ(gpio_1_2_dig_state.get_optional().value(), gpio_states.at(1)); - ASSERT_EQ(gpio_2_ana_state.get_optional().value(), gpio_states.at(2)); + ASSERT_EQ(gpio_1_1_dig_cmd->get_optional().value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd->get_optional().value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd->get_optional().value(), gpio_commands.at(2)); + ASSERT_EQ(gpio_1_1_dig_state->get_optional().value(), gpio_states.at(0)); + ASSERT_EQ(gpio_1_2_dig_state->get_optional().value(), gpio_states.at(1)); + ASSERT_EQ(gpio_2_ana_state->get_optional().value(), gpio_states.at(2)); } void update_controller_loop() @@ -203,13 +203,19 @@ class GpioCommandControllerTestSuite : public ::testing::Test std::vector gpio_commands{1.0, 0.0, 3.1}; std::vector gpio_states{1.0, 0.0, 3.1}; - CommandInterface gpio_1_1_dig_cmd{gpio_names.at(0), "dig.1", &gpio_commands.at(0)}; - CommandInterface gpio_1_2_dig_cmd{gpio_names.at(0), "dig.2", &gpio_commands.at(1)}; - CommandInterface gpio_2_ana_cmd{gpio_names.at(1), "ana.1", &gpio_commands.at(2)}; - - StateInterface gpio_1_1_dig_state{gpio_names.at(0), "dig.1", &gpio_states.at(0)}; - StateInterface gpio_1_2_dig_state{gpio_names.at(0), "dig.2", &gpio_states.at(1)}; - StateInterface gpio_2_ana_state{gpio_names.at(1), "ana.1", &gpio_states.at(2)}; + CommandInterface::SharedPtr gpio_1_1_dig_cmd = + std::make_shared(gpio_names.at(0), "dig.1", &gpio_commands.at(0)); + CommandInterface::SharedPtr gpio_1_2_dig_cmd = + std::make_shared(gpio_names.at(0), "dig.2", &gpio_commands.at(1)); + CommandInterface::SharedPtr gpio_2_ana_cmd = + std::make_shared(gpio_names.at(1), "ana.1", &gpio_commands.at(2)); + + StateInterface::SharedPtr gpio_1_1_dig_state = + std::make_shared(gpio_names.at(0), "dig.1", &gpio_states.at(0)); + StateInterface::SharedPtr gpio_1_2_dig_state = + std::make_shared(gpio_names.at(0), "dig.2", &gpio_states.at(1)); + StateInterface::SharedPtr gpio_2_ana_state = + std::make_shared(gpio_names.at(1), "ana.1", &gpio_states.at(2)); std::unique_ptr node; }; @@ -341,13 +347,13 @@ TEST_F( ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); std::vector command_interfaces; - command_interfaces.emplace_back(gpio_1_1_dig_cmd); - command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_1_1_dig_cmd, nullptr); + command_interfaces.emplace_back(gpio_1_2_dig_cmd, nullptr); std::vector state_interfaces; - state_interfaces.emplace_back(gpio_1_1_dig_state); - state_interfaces.emplace_back(gpio_1_2_dig_state); - state_interfaces.emplace_back(gpio_2_ana_state); + state_interfaces.emplace_back(gpio_1_1_dig_state, nullptr); + state_interfaces.emplace_back(gpio_1_2_dig_state, nullptr); + state_interfaces.emplace_back(gpio_2_ana_state, nullptr); controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -369,13 +375,13 @@ TEST_F( ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); std::vector command_interfaces; - command_interfaces.emplace_back(gpio_1_1_dig_cmd); - command_interfaces.emplace_back(gpio_1_2_dig_cmd); - command_interfaces.emplace_back(gpio_2_ana_cmd); + command_interfaces.emplace_back(gpio_1_1_dig_cmd, nullptr); + command_interfaces.emplace_back(gpio_1_2_dig_cmd, nullptr); + command_interfaces.emplace_back(gpio_2_ana_cmd, nullptr); std::vector state_interfaces; - state_interfaces.emplace_back(gpio_1_1_dig_state); - state_interfaces.emplace_back(gpio_1_2_dig_state); + state_interfaces.emplace_back(gpio_1_1_dig_state, nullptr); + state_interfaces.emplace_back(gpio_1_2_dig_state, nullptr); controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); @@ -398,13 +404,13 @@ TEST_F( ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); std::vector command_interfaces; - command_interfaces.emplace_back(gpio_1_1_dig_cmd); - command_interfaces.emplace_back(gpio_1_2_dig_cmd); - command_interfaces.emplace_back(gpio_2_ana_cmd); + command_interfaces.emplace_back(gpio_1_1_dig_cmd, nullptr); + command_interfaces.emplace_back(gpio_1_2_dig_cmd, nullptr); + command_interfaces.emplace_back(gpio_2_ana_cmd, nullptr); std::vector state_interfaces; - state_interfaces.emplace_back(gpio_1_1_dig_state); - state_interfaces.emplace_back(gpio_2_ana_state); + state_interfaces.emplace_back(gpio_1_1_dig_state, nullptr); + state_interfaces.emplace_back(gpio_2_ana_state, nullptr); controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); @@ -492,9 +498,9 @@ TEST_F( controller_->rt_command_.set(command); update_controller_loop(); - ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0); - ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0); - ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), 30.0); + ASSERT_EQ(gpio_1_1_dig_cmd->get_optional().value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd->get_optional().value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd->get_optional().value(), 30.0); } TEST_F( @@ -516,9 +522,9 @@ TEST_F( controller_->rt_command_.set(command); update_controller_loop(); - ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0); - ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0); - ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), 30.0); + ASSERT_EQ(gpio_1_1_dig_cmd->get_optional().value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd->get_optional().value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd->get_optional().value(), 30.0); } TEST_F( @@ -539,9 +545,9 @@ TEST_F( controller_->rt_command_.set(command); update_controller_loop(); - ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0); - ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0); - ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), gpio_commands[2]); + ASSERT_EQ(gpio_1_1_dig_cmd->get_optional().value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd->get_optional().value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd->get_optional().value(), gpio_commands[2]); } TEST_F( @@ -563,9 +569,9 @@ TEST_F( controller_->rt_command_.set(command); update_controller_loop(); - ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), gpio_commands.at(0)); - ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), gpio_commands.at(1)); - ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), gpio_commands.at(2)); + ASSERT_EQ(gpio_1_1_dig_cmd->get_optional().value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd->get_optional().value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd->get_optional().value(), gpio_commands.at(2)); } TEST_F( @@ -590,9 +596,9 @@ TEST_F( wait_one_miliseconds_for_timeout(); update_controller_loop(); - ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0); - ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0); - ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), 30.0); + ASSERT_EQ(gpio_1_1_dig_cmd->get_optional().value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd->get_optional().value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd->get_optional().value(), 30.0); } TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurrentValues) @@ -637,11 +643,11 @@ TEST_F( {"command_interfaces.gpio1.interfaces", std::vector{"dig.1"}}}); std::vector command_interfaces; - command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_1_dig_cmd, nullptr); std::vector state_interfaces; - state_interfaces.emplace_back(gpio_1_1_dig_state); - state_interfaces.emplace_back(gpio_2_ana_state); + state_interfaces.emplace_back(gpio_1_1_dig_state, nullptr); + state_interfaces.emplace_back(gpio_2_ana_state, nullptr); const auto result_of_initialization = controller_->init( "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); @@ -676,8 +682,8 @@ TEST_F( {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); std::vector state_interfaces; - state_interfaces.emplace_back(gpio_1_1_dig_state); - state_interfaces.emplace_back(gpio_2_ana_state); + state_interfaces.emplace_back(gpio_1_1_dig_state, nullptr); + state_interfaces.emplace_back(gpio_2_ana_state, nullptr); const auto result_of_initialization = controller_->init("test_gpio_command_controller", "", 0, "", node_options); diff --git a/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp b/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp index e4ac047052..ccc0c6dcc4 100644 --- a/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp +++ b/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp @@ -75,16 +75,16 @@ class GPSSensorBroadcasterTest : public ::testing::Test void setup_gps_broadcaster() { std::vector state_ifs; - state_ifs.emplace_back(gps_status_); - state_ifs.emplace_back(gps_service_); - state_ifs.emplace_back(gps_latitude_); - state_ifs.emplace_back(gps_longitude_); - state_ifs.emplace_back(gps_altitude_); + state_ifs.emplace_back(gps_status_, nullptr); + state_ifs.emplace_back(gps_service_, nullptr); + state_ifs.emplace_back(gps_latitude_, nullptr); + state_ifs.emplace_back(gps_longitude_, nullptr); + state_ifs.emplace_back(gps_altitude_, nullptr); if constexpr (sensor_option == semantic_components::GPSSensorOption::WithCovariance) { - state_ifs.emplace_back(latitude_covariance_); - state_ifs.emplace_back(longitude_covariance_); - state_ifs.emplace_back(altitude_covariance_); + state_ifs.emplace_back(latitude_covariance_, nullptr); + state_ifs.emplace_back(longitude_covariance_, nullptr); + state_ifs.emplace_back(altitude_covariance_, nullptr); } gps_broadcaster_->assign_interfaces({}, std::move(state_ifs)); @@ -110,17 +110,30 @@ class GPSSensorBroadcasterTest : public ::testing::Test const std::string sensor_name_ = sensor_name_param_.get_value(); const rclcpp::Parameter frame_id_ = rclcpp::Parameter("frame_id", "gps_sensor_frame"); std::array sensor_values_ = {{1.0, 1.0, 1.1, 2.2, 3.3, 0.5, 0.7, 0.9}}; - hardware_interface::StateInterface gps_status_{sensor_name_, "status", &sensor_values_[0]}; - hardware_interface::StateInterface gps_service_{sensor_name_, "service", &sensor_values_[1]}; - hardware_interface::StateInterface gps_latitude_{sensor_name_, "latitude", &sensor_values_[2]}; - hardware_interface::StateInterface gps_longitude_{sensor_name_, "longitude", &sensor_values_[3]}; - hardware_interface::StateInterface gps_altitude_{sensor_name_, "altitude", &sensor_values_[4]}; - hardware_interface::StateInterface latitude_covariance_{ - sensor_name_, "latitude_covariance", &sensor_values_[5]}; - hardware_interface::StateInterface longitude_covariance_{ - sensor_name_, "longitude_covariance", &sensor_values_[6]}; - hardware_interface::StateInterface altitude_covariance_{ - sensor_name_, "altitude_covariance", &sensor_values_[7]}; + hardware_interface::StateInterface::SharedPtr gps_status_ = + std::make_shared( + sensor_name_, "status", &sensor_values_[0]); + hardware_interface::StateInterface::SharedPtr gps_service_ = + std::make_shared( + sensor_name_, "service", &sensor_values_[1]); + hardware_interface::StateInterface::SharedPtr gps_latitude_ = + std::make_shared( + sensor_name_, "latitude", &sensor_values_[2]); + hardware_interface::StateInterface::SharedPtr gps_longitude_ = + std::make_shared( + sensor_name_, "longitude", &sensor_values_[3]); + hardware_interface::StateInterface::SharedPtr gps_altitude_ = + std::make_shared( + sensor_name_, "altitude", &sensor_values_[4]); + hardware_interface::StateInterface::SharedPtr latitude_covariance_ = + std::make_shared( + sensor_name_, "latitude_covariance", &sensor_values_[5]); + hardware_interface::StateInterface::SharedPtr longitude_covariance_ = + std::make_shared( + sensor_name_, "longitude_covariance", &sensor_values_[6]); + hardware_interface::StateInterface::SharedPtr altitude_covariance_ = + std::make_shared( + sensor_name_, "altitude_covariance", &sensor_values_[7]); std::unique_ptr gps_broadcaster_; }; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index a6ea1978ad..2e18d0de73 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -66,16 +66,16 @@ void IMUSensorBroadcasterTest::SetUpIMUBroadcaster( ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; - state_ifs.emplace_back(imu_orientation_x_); - state_ifs.emplace_back(imu_orientation_y_); - state_ifs.emplace_back(imu_orientation_z_); - state_ifs.emplace_back(imu_orientation_w_); - state_ifs.emplace_back(imu_angular_velocity_x_); - state_ifs.emplace_back(imu_angular_velocity_y_); - state_ifs.emplace_back(imu_angular_velocity_z_); - state_ifs.emplace_back(imu_linear_acceleration_x_); - state_ifs.emplace_back(imu_linear_acceleration_y_); - state_ifs.emplace_back(imu_linear_acceleration_z_); + state_ifs.emplace_back(imu_orientation_x_, nullptr); + state_ifs.emplace_back(imu_orientation_y_, nullptr); + state_ifs.emplace_back(imu_orientation_z_, nullptr); + state_ifs.emplace_back(imu_orientation_w_, nullptr); + state_ifs.emplace_back(imu_angular_velocity_x_, nullptr); + state_ifs.emplace_back(imu_angular_velocity_y_, nullptr); + state_ifs.emplace_back(imu_angular_velocity_z_, nullptr); + state_ifs.emplace_back(imu_linear_acceleration_x_, nullptr); + state_ifs.emplace_back(imu_linear_acceleration_y_, nullptr); + state_ifs.emplace_back(imu_linear_acceleration_z_, nullptr); imu_broadcaster_->assign_interfaces({}, std::move(state_ifs)); } diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 1ecb605f6b..6c5c2e44ca 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -58,26 +58,36 @@ class IMUSensorBroadcasterTest : public ::testing::Test const std::string frame_id_ = "imu_sensor_frame"; std::array sensor_values_ = { {0.1826, 0.3651, 0.5477, 0.7303, 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]}; + hardware_interface::StateInterface::SharedPtr imu_orientation_x_ = + std::make_shared( + sensor_name_, "orientation.x", &sensor_values_[0]); + hardware_interface::StateInterface::SharedPtr imu_orientation_y_ = + std::make_shared( + sensor_name_, "orientation.y", &sensor_values_[1]); + hardware_interface::StateInterface::SharedPtr imu_orientation_z_ = + std::make_shared( + sensor_name_, "orientation.z", &sensor_values_[2]); + hardware_interface::StateInterface::SharedPtr imu_orientation_w_ = + std::make_shared( + sensor_name_, "orientation.w", &sensor_values_[3]); + hardware_interface::StateInterface::SharedPtr imu_angular_velocity_x_ = + std::make_shared( + sensor_name_, "angular_velocity.x", &sensor_values_[4]); + hardware_interface::StateInterface::SharedPtr imu_angular_velocity_y_ = + std::make_shared( + sensor_name_, "angular_velocity.y", &sensor_values_[5]); + hardware_interface::StateInterface::SharedPtr imu_angular_velocity_z_ = + std::make_shared( + sensor_name_, "angular_velocity.z", &sensor_values_[6]); + hardware_interface::StateInterface::SharedPtr imu_linear_acceleration_x_ = + std::make_shared( + sensor_name_, "linear_acceleration.x", &sensor_values_[7]); + hardware_interface::StateInterface::SharedPtr imu_linear_acceleration_y_ = + std::make_shared( + sensor_name_, "linear_acceleration.y", &sensor_values_[8]); + hardware_interface::StateInterface::SharedPtr 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.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index fc3d430a02..5dd7af92dc 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -980,50 +980,70 @@ TEST_F(JointStateBroadcasterTest, UpdatePerformanceTest) // standard test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "position", &custom_joint_value_}); + std::make_shared( + joint_name, "position", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "velocity", &custom_joint_value_}); + std::make_shared( + joint_name, "velocity", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "effort", &custom_joint_value_}); + std::make_shared( + joint_name, "effort", &custom_joint_value_)); // non standard test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "mode", &custom_joint_value_}); + std::make_shared( + joint_name, "mode", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "absolute_position", &custom_joint_value_}); + std::make_shared( + joint_name, "absolute_position", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "acceleration", &custom_joint_value_}); + std::make_shared( + joint_name, "acceleration", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "current", &custom_joint_value_}); + std::make_shared( + joint_name, "current", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "torque", &custom_joint_value_}); + std::make_shared( + joint_name, "torque", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "force", &custom_joint_value_}); + std::make_shared( + joint_name, "force", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "temperature_board", &custom_joint_value_}); + std::make_shared( + joint_name, "temperature_board", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "temperature_motor", &custom_joint_value_}); + std::make_shared( + joint_name, "temperature_motor", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "position.kd", &custom_joint_value_}); + std::make_shared( + joint_name, "position.kd", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "position.ki", &custom_joint_value_}); + std::make_shared( + joint_name, "position.ki", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "position.kp", &custom_joint_value_}); + std::make_shared( + joint_name, "position.kp", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "velocity.kd", &custom_joint_value_}); + std::make_shared( + joint_name, "velocity.kd", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "velocity.ki", &custom_joint_value_}); + std::make_shared( + joint_name, "velocity.ki", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "velocity.kp", &custom_joint_value_}); + std::make_shared( + joint_name, "velocity.kp", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "current.kd", &custom_joint_value_}); + std::make_shared( + joint_name, "current.kd", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "current.ki", &custom_joint_value_}); + std::make_shared( + joint_name, "current.ki", &custom_joint_value_)); test_interfaces_.emplace_back( - hardware_interface::StateInterface{joint_name, "current.kp", &custom_joint_value_}); + std::make_shared( + joint_name, "current.kp", &custom_joint_value_)); } RCLCPP_INFO( @@ -1033,7 +1053,7 @@ TEST_F(JointStateBroadcasterTest, UpdatePerformanceTest) std::vector state_interfaces; for (const auto & tif : test_interfaces_) { - state_interfaces.emplace_back(tif); + state_interfaces.emplace_back(tif, nullptr); } state_broadcaster_->assign_interfaces({}, std::move(state_interfaces)); diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 158c921ddc..ae3d833384 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -87,29 +87,39 @@ 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::vector test_interfaces_; + hardware_interface::StateInterface::SharedPtr joint_1_pos_state_ = + std::make_shared( + joint_names_[0], interface_names_[0], &joint_values_[0]); + hardware_interface::StateInterface::SharedPtr joint_2_pos_state_ = + std::make_shared( + joint_names_[1], interface_names_[0], &joint_values_[1]); + hardware_interface::StateInterface::SharedPtr joint_3_pos_state_ = + std::make_shared( + joint_names_[2], interface_names_[0], &joint_values_[2]); + hardware_interface::StateInterface::SharedPtr joint_1_vel_state_ = + std::make_shared( + joint_names_[0], interface_names_[1], &joint_values_[0]); + hardware_interface::StateInterface::SharedPtr joint_2_vel_state_ = + std::make_shared( + joint_names_[1], interface_names_[1], &joint_values_[1]); + hardware_interface::StateInterface::SharedPtr joint_3_vel_state_ = + std::make_shared( + joint_names_[2], interface_names_[1], &joint_values_[2]); + hardware_interface::StateInterface::SharedPtr joint_1_eff_state_ = + std::make_shared( + joint_names_[0], interface_names_[2], &joint_values_[0]); + hardware_interface::StateInterface::SharedPtr joint_2_eff_state_ = + std::make_shared( + joint_names_[1], interface_names_[2], &joint_values_[1]); + hardware_interface::StateInterface::SharedPtr joint_3_eff_state_ = + std::make_shared( + joint_names_[2], interface_names_[2], &joint_values_[2]); + + hardware_interface::StateInterface::SharedPtr joint_X_custom_state = + std::make_shared( + joint_names_[0], custom_interface_name_, &custom_joint_value_); + + std::vector test_interfaces_; std::unique_ptr state_broadcaster_; std::string frame_id_ = "base_link"; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 7ecd9818db..3756c59d22 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -349,8 +349,8 @@ class TrajectoryControllerTest : public ::testing::Test const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) { - std::vector cmd_interfaces; - std::vector state_interfaces; + std::vector loaned_command_ifs; + std::vector loaned_state_ifs; pos_cmd_interfaces_.reserve(joint_names_.size()); vel_cmd_interfaces_.reserve(joint_names_.size()); acc_cmd_interfaces_.reserve(joint_names_.size()); @@ -361,65 +361,65 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < joint_names_.size(); ++i) { pos_cmd_interfaces_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( joint_names_[i], hardware_interface::HW_IF_POSITION, &joint_pos_[i])); vel_cmd_interfaces_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( joint_names_[i], hardware_interface::HW_IF_VELOCITY, &joint_vel_[i])); acc_cmd_interfaces_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( joint_names_[i], hardware_interface::HW_IF_ACCELERATION, &joint_acc_[i])); eff_cmd_interfaces_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( joint_names_[i], hardware_interface::HW_IF_EFFORT, &joint_eff_[i])); pos_state_interfaces_.emplace_back( - hardware_interface::StateInterface( + 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( + 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( + std::make_shared( joint_names_[i], hardware_interface::HW_IF_ACCELERATION, separate_cmd_and_state_values ? &joint_state_acc_[i] : &joint_acc_[i])); // Add to export lists and set initial values (explicitly discarding return value) - cmd_interfaces.emplace_back(pos_cmd_interfaces_.back()); - (void)cmd_interfaces.back().set_value(initial_pos_joints[i]); - cmd_interfaces.emplace_back(vel_cmd_interfaces_.back()); - (void)cmd_interfaces.back().set_value(initial_vel_joints[i]); - cmd_interfaces.emplace_back(acc_cmd_interfaces_.back()); - (void)cmd_interfaces.back().set_value(initial_acc_joints[i]); - cmd_interfaces.emplace_back(eff_cmd_interfaces_.back()); - (void)cmd_interfaces.back().set_value(initial_eff_joints[i]); + loaned_command_ifs.emplace_back(pos_cmd_interfaces_.back(), nullptr); + (void)loaned_command_ifs.back().set_value(initial_pos_joints[i]); + loaned_command_ifs.emplace_back(vel_cmd_interfaces_.back(), nullptr); + (void)loaned_command_ifs.back().set_value(initial_vel_joints[i]); + loaned_command_ifs.emplace_back(acc_cmd_interfaces_.back(), nullptr); + (void)loaned_command_ifs.back().set_value(initial_acc_joints[i]); + loaned_command_ifs.emplace_back(eff_cmd_interfaces_.back(), nullptr); + (void)loaned_command_ifs.back().set_value(initial_eff_joints[i]); if (separate_cmd_and_state_values) { joint_state_pos_[i] = INITIAL_POS_JOINTS[i]; joint_state_vel_[i] = INITIAL_VEL_JOINTS[i]; joint_state_acc_[i] = INITIAL_ACC_JOINTS[i]; } - state_interfaces.emplace_back(pos_state_interfaces_.back()); - state_interfaces.emplace_back(vel_state_interfaces_.back()); - state_interfaces.emplace_back(acc_state_interfaces_.back()); + loaned_state_ifs.emplace_back(pos_state_interfaces_.back(), nullptr); + loaned_state_ifs.emplace_back(vel_state_interfaces_.back(), nullptr); + loaned_state_ifs.emplace_back(acc_state_interfaces_.back(), nullptr); } speed_scaling_factor_ = 1.0; target_speed_scaling_factor_ = 1.0; gpio_state_interfaces.emplace_back( - hardware_interface::StateInterface( + std::make_shared( "speed_scaling", "speed_scaling_factor", separate_cmd_and_state_values ? &speed_scaling_factor_ : &target_speed_scaling_factor_)); - state_interfaces.emplace_back(gpio_state_interfaces.back()); + loaned_state_ifs.emplace_back(gpio_state_interfaces.back(), nullptr); gpio_command_interfaces_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( "speed_scaling", "target_speed_fraction_cmd", &target_speed_scaling_factor_)); - cmd_interfaces.emplace_back(gpio_command_interfaces_.back()); + loaned_command_ifs.emplace_back(gpio_command_interfaces_.back(), nullptr); - traj_controller_->assign_interfaces(std::move(cmd_interfaces), std::move(state_interfaces)); + traj_controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs)); return traj_controller_->get_node()->activate(); } @@ -698,9 +698,9 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < 3; i++) { EXPECT_TRUE(is_same_sign_or_zero( - position.at(i) - pos_state_interfaces_[i].get_optional().value(), joint_vel_[i])) + position.at(i) - pos_state_interfaces_[i]->get_optional().value(), joint_vel_[i])) << "test position point " << position.at(i) << ", position state is " - << pos_state_interfaces_[i].get_optional().value() << ", velocity command is " + << pos_state_interfaces_[i]->get_optional().value() << ", velocity command is " << joint_vel_[i]; } } @@ -709,10 +709,10 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < 3; i++) { EXPECT_TRUE(is_same_sign_or_zero( - position.at(i) - pos_state_interfaces_[i].get_optional().value() + effort.at(i), + position.at(i) - pos_state_interfaces_[i]->get_optional().value() + effort.at(i), joint_eff_[i])) << "test position point " << position.at(i) << ", position state is " - << pos_state_interfaces_[i].get_optional().value() << ", effort command is " + << pos_state_interfaces_[i]->get_optional().value() << ", effort command is " << joint_eff_[i]; } } @@ -819,15 +819,15 @@ class TrajectoryControllerTest : public ::testing::Test std::vector joint_state_acc_; double speed_scaling_factor_; double target_speed_scaling_factor_; - std::vector pos_cmd_interfaces_; - std::vector vel_cmd_interfaces_; - std::vector acc_cmd_interfaces_; - std::vector eff_cmd_interfaces_; - std::vector gpio_command_interfaces_; - std::vector pos_state_interfaces_; - std::vector vel_state_interfaces_; - std::vector acc_state_interfaces_; - std::vector gpio_state_interfaces; + std::vector pos_cmd_interfaces_; + std::vector vel_cmd_interfaces_; + std::vector acc_cmd_interfaces_; + std::vector eff_cmd_interfaces_; + std::vector gpio_command_interfaces_; + std::vector pos_state_interfaces_; + std::vector vel_state_interfaces_; + std::vector acc_state_interfaces_; + std::vector gpio_state_interfaces; }; // From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 7d42c96c74..ea2f279450 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -181,31 +181,31 @@ class MecanumDriveControllerFixture : public ::testing::Test controller_->init(controller_name, urdf, 0, ns, node_options), controller_interface::return_type::OK); - std::vector command_ifs; + std::vector loaned_command_ifs; command_itfs_.reserve(joint_command_values_.size()); - command_ifs.reserve(joint_command_values_.size()); + loaned_command_ifs.reserve(joint_command_values_.size()); for (size_t i = 0; i < joint_command_values_.size(); ++i) { command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( command_joint_names_[i], interface_name_, &joint_command_values_[i])); - command_ifs.emplace_back(command_itfs_.back()); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); } - std::vector state_ifs; + std::vector loaned_state_ifs; state_itfs_.reserve(joint_state_values_.size()); - state_ifs.reserve(joint_state_values_.size()); + loaned_state_ifs.reserve(joint_state_values_.size()); for (size_t i = 0; i < joint_state_values_.size(); ++i) { state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( command_joint_names_[i], interface_name_, &joint_state_values_[i])); - state_ifs.emplace_back(state_itfs_.back()); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); } - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs)); } void subscribe_to_controller_status_execute_update_and_get_messages(ControllerStateMsg & msg) @@ -313,8 +313,8 @@ class MecanumDriveControllerFixture : public ::testing::Test static constexpr double TEST_ANGULAR_VELOCITY_Z = 0.0; double command_lin_x = 111; - std::vector state_itfs_; - std::vector command_itfs_; + std::vector state_itfs_; + std::vector command_itfs_; double ref_timeout_ = 0.1; diff --git a/motion_primitives_controllers/test/test_motion_primitives_forward_controller.hpp b/motion_primitives_controllers/test/test_motion_primitives_forward_controller.hpp index eb66f636a6..3030e74d42 100644 --- a/motion_primitives_controllers/test/test_motion_primitives_forward_controller.hpp +++ b/motion_primitives_controllers/test/test_motion_primitives_forward_controller.hpp @@ -113,34 +113,34 @@ class MotionPrimitivesForwardControllerFixture : public ::testing::Test protected: void SetUpController() { - std::vector command_ifs; - std::vector state_ifs; + std::vector loaned_command_ifs; + std::vector loaned_state_ifs; command_itfs_.clear(); command_itfs_.reserve(command_values_.size()); - command_ifs.reserve(command_values_.size()); + loaned_command_ifs.reserve(command_values_.size()); for (size_t i = 0; i < command_values_.size(); ++i) { command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( interface_namespace_, command_interface_names_[i], &command_values_[i])); - command_ifs.emplace_back(command_itfs_.back()); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); } state_itfs_.clear(); state_itfs_.reserve(state_values_.size()); - state_ifs.reserve(state_values_.size()); + loaned_state_ifs.reserve(state_values_.size()); for (size_t i = 0; i < state_values_.size(); ++i) { state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( interface_namespace_, state_interface_names_[i], &state_values_[i])); - state_ifs.emplace_back(state_itfs_.back()); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); } - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs)); } void send_single_motion_sequence_goal( @@ -205,8 +205,8 @@ class MotionPrimitivesForwardControllerFixture : public ::testing::Test 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101}}; - std::vector state_itfs_; - std::vector command_itfs_; + std::vector state_itfs_; + std::vector command_itfs_; std::unique_ptr controller_; rclcpp_action::Client::SharedPtr action_client_; diff --git a/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.cpp b/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.cpp index d0ecb366a2..d94d201378 100644 --- a/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.cpp +++ b/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.cpp @@ -332,7 +332,7 @@ TEST_F(OmniWheelDriveControllerTest, chainable_controller_unchained_mode) // But NaNs should not propagate to command interfaces for (size_t i = 0; i < command_itfs_.size(); i++) { - ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + ASSERT_FALSE(std::isnan(command_itfs_[i]->get_optional().value())); } // Check that a late command message causes the command interfaces to be set to 0.0 @@ -348,7 +348,7 @@ TEST_F(OmniWheelDriveControllerTest, chainable_controller_unchained_mode) controller_interface::return_type::OK); for (size_t i = 0; i < command_itfs_.size(); i++) { - EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + EXPECT_EQ(command_itfs_[i]->get_optional().value(), 0.0); } // Now check that a timely published command message sets the command interfaces to the correct @@ -363,7 +363,7 @@ TEST_F(OmniWheelDriveControllerTest, chainable_controller_unchained_mode) std::vector expected_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; for (size_t i = 0; i < command_itfs_.size(); i++) { - EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); + EXPECT_DOUBLE_EQ(command_itfs_[i]->get_optional().value(), expected_wheels_vel_cmds[i]); } // Now check that the command interfaces are set to 0.0 on deactivation @@ -372,7 +372,7 @@ TEST_F(OmniWheelDriveControllerTest, chainable_controller_unchained_mode) ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); for (size_t i = 0; i < command_itfs_.size(); i++) { - EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + EXPECT_EQ(command_itfs_[i]->get_optional().value(), 0.0); } // cleanup @@ -380,7 +380,7 @@ TEST_F(OmniWheelDriveControllerTest, chainable_controller_unchained_mode) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); for (size_t i = 0; i < command_itfs_.size(); i++) { - EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + EXPECT_EQ(command_itfs_[i]->get_optional().value(), 0.0); } state = controller_->get_node()->configure(); @@ -420,7 +420,7 @@ TEST_F(OmniWheelDriveControllerTest, chainable_controller_chained_mode) // But NaNs should not propagate to command interfaces for (size_t i = 0; i < command_itfs_.size(); i++) { - ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + ASSERT_FALSE(std::isnan(command_itfs_[i]->get_optional().value())); } // Imitate preceding controllers by setting reference_interfaces_ @@ -434,7 +434,7 @@ TEST_F(OmniWheelDriveControllerTest, chainable_controller_chained_mode) std::vector expected_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; for (size_t i = 0; i < command_itfs_.size(); i++) { - EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); + EXPECT_DOUBLE_EQ(command_itfs_[i]->get_optional().value(), expected_wheels_vel_cmds[i]); } // Now check that the command interfaces are set to 0.0 on deactivation @@ -443,7 +443,7 @@ TEST_F(OmniWheelDriveControllerTest, chainable_controller_chained_mode) ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); for (size_t i = 0; i < command_itfs_.size(); i++) { - EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + EXPECT_EQ(command_itfs_[i]->get_optional().value(), 0.0); } // cleanup @@ -451,7 +451,7 @@ TEST_F(OmniWheelDriveControllerTest, chainable_controller_chained_mode) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); for (size_t i = 0; i < command_itfs_.size(); i++) { - EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + EXPECT_EQ(command_itfs_[i]->get_optional().value(), 0.0); } state = controller_->get_node()->configure(); @@ -488,7 +488,7 @@ TEST_F(OmniWheelDriveControllerTest, deactivate_then_activate) // But NaNs should not propagate to command interfaces for (size_t i = 0; i < command_itfs_.size(); i++) { - ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + ASSERT_FALSE(std::isnan(command_itfs_[i]->get_optional().value())); } // Now check that a timely published command message sets the command interfaces to the correct @@ -503,7 +503,7 @@ TEST_F(OmniWheelDriveControllerTest, deactivate_then_activate) std::vector expected_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; for (size_t i = 0; i < command_itfs_.size(); i++) { - EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); + EXPECT_DOUBLE_EQ(command_itfs_[i]->get_optional().value(), expected_wheels_vel_cmds[i]); } // Now check that the command interfaces are set to 0.0 on deactivation @@ -516,7 +516,7 @@ TEST_F(OmniWheelDriveControllerTest, deactivate_then_activate) controller_interface::return_type::OK); for (size_t i = 0; i < command_itfs_.size(); i++) { - EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + EXPECT_EQ(command_itfs_[i]->get_optional().value(), 0.0); } // Activate again @@ -532,7 +532,7 @@ TEST_F(OmniWheelDriveControllerTest, deactivate_then_activate) } for (size_t i = 0; i < command_itfs_.size(); i++) { - EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + EXPECT_EQ(command_itfs_[i]->get_optional().value(), 0.0); } // A new command should work as expected @@ -544,7 +544,7 @@ TEST_F(OmniWheelDriveControllerTest, deactivate_then_activate) controller_interface::return_type::OK); for (size_t i = 0; i < command_itfs_.size(); i++) { - EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); + EXPECT_DOUBLE_EQ(command_itfs_[i]->get_optional().value(), expected_wheels_vel_cmds[i]); } // Deactivate again and cleanup @@ -585,7 +585,7 @@ TEST_F(OmniWheelDriveControllerTest, command_with_zero_timestamp_is_accepted_wit std::vector expected_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; for (size_t i = 0; i < command_itfs_.size(); i++) { - EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); + EXPECT_DOUBLE_EQ(command_itfs_[i]->get_optional().value(), expected_wheels_vel_cmds[i]); } // Deactivate and cleanup @@ -625,7 +625,7 @@ TEST_F(OmniWheelDriveControllerTest, 3_wheel_test) // But NaNs should not propagate to command interfaces for (size_t i = 0; i < 3; i++) { - ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + ASSERT_FALSE(std::isnan(command_itfs_[i]->get_optional().value())); } // Check that a published command msg sets the command interfaces to the correct values @@ -639,7 +639,7 @@ TEST_F(OmniWheelDriveControllerTest, 3_wheel_test) std::vector expected_wheels_vel_cmds = {-15.0, 8.66025, -8.66025}; for (size_t i = 0; i < 3; i++) { - EXPECT_NEAR(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i], 0.0001); + EXPECT_NEAR(command_itfs_[i]->get_optional().value(), expected_wheels_vel_cmds[i], 0.0001); } // Now check that the command interfaces are set to 0.0 on deactivation @@ -648,7 +648,7 @@ TEST_F(OmniWheelDriveControllerTest, 3_wheel_test) ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); for (size_t i = 0; i < 3; i++) { - EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + EXPECT_EQ(command_itfs_[i]->get_optional().value(), 0.0); } // cleanup @@ -656,7 +656,7 @@ TEST_F(OmniWheelDriveControllerTest, 3_wheel_test) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); for (size_t i = 0; i < 3; i++) { - EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + EXPECT_EQ(command_itfs_[i]->get_optional().value(), 0.0); } state = controller_->get_node()->configure(); @@ -693,7 +693,7 @@ TEST_F(OmniWheelDriveControllerTest, 3_wheel_rot_test) // But NaNs should not propagate to command interfaces for (size_t i = 0; i < 3; i++) { - ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + ASSERT_FALSE(std::isnan(command_itfs_[i]->get_optional().value())); } // Check that a published command msg sets the command interfaces to the correct values @@ -707,7 +707,7 @@ TEST_F(OmniWheelDriveControllerTest, 3_wheel_rot_test) std::vector expected_wheels_vel_cmds = {-1.33975, 5.0, -18.6603}; for (size_t i = 0; i < 3; i++) { - EXPECT_NEAR(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i], 0.0001); + EXPECT_NEAR(command_itfs_[i]->get_optional().value(), expected_wheels_vel_cmds[i], 0.0001); } // Now check that the command interfaces are set to 0.0 on deactivation @@ -716,7 +716,7 @@ TEST_F(OmniWheelDriveControllerTest, 3_wheel_rot_test) ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); for (size_t i = 0; i < 3; i++) { - EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + EXPECT_EQ(command_itfs_[i]->get_optional().value(), 0.0); } // cleanup @@ -724,7 +724,7 @@ TEST_F(OmniWheelDriveControllerTest, 3_wheel_rot_test) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); for (size_t i = 0; i < 3; i++) { - EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + EXPECT_EQ(command_itfs_[i]->get_optional().value(), 0.0); } state = controller_->get_node()->configure(); @@ -761,7 +761,7 @@ TEST_F(OmniWheelDriveControllerTest, 4_wheel_rot_test) // But NaNs should not propagate to command interfaces for (size_t i = 0; i < 4; i++) { - ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + ASSERT_FALSE(std::isnan(command_itfs_[i]->get_optional().value())); } // Check that a published command msg sets the command interfaces to the correct values @@ -775,7 +775,7 @@ TEST_F(OmniWheelDriveControllerTest, 4_wheel_rot_test) std::vector expected_wheels_vel_cmds = {-5.0, 9.14214, -5.0, -19.1421}; for (size_t i = 0; i < 4; i++) { - EXPECT_NEAR(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i], 0.0001); + EXPECT_NEAR(command_itfs_[i]->get_optional().value(), expected_wheels_vel_cmds[i], 0.0001); } // Now check that the command interfaces are set to 0.0 on deactivation @@ -784,7 +784,7 @@ TEST_F(OmniWheelDriveControllerTest, 4_wheel_rot_test) ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); for (size_t i = 0; i < 4; i++) { - EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + EXPECT_EQ(command_itfs_[i]->get_optional().value(), 0.0); } // cleanup @@ -792,7 +792,7 @@ TEST_F(OmniWheelDriveControllerTest, 4_wheel_rot_test) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); for (size_t i = 0; i < 4; i++) { - EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + EXPECT_EQ(command_itfs_[i]->get_optional().value(), 0.0); } state = controller_->get_node()->configure(); @@ -829,7 +829,7 @@ TEST_F(OmniWheelDriveControllerTest, 5_wheel_test) // But NaNs should not propagate to command interfaces for (size_t i = 0; i < 5; i++) { - ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + ASSERT_FALSE(std::isnan(command_itfs_[i]->get_optional().value())); } // Check that a published command msg sets the command interfaces to the correct values @@ -843,7 +843,7 @@ TEST_F(OmniWheelDriveControllerTest, 5_wheel_test) std::vector expected_wheels_vel_cmds = {-15.0, 1.42040, 8.96802, -2.78768, -17.6007}; for (size_t i = 0; i < 5; i++) { - EXPECT_NEAR(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i], 0.0001); + EXPECT_NEAR(command_itfs_[i]->get_optional().value(), expected_wheels_vel_cmds[i], 0.0001); } // Now check that the command interfaces are set to 0.0 on deactivation @@ -852,7 +852,7 @@ TEST_F(OmniWheelDriveControllerTest, 5_wheel_test) ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); for (size_t i = 0; i < 5; i++) { - EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + EXPECT_EQ(command_itfs_[i]->get_optional().value(), 0.0); } // cleanup @@ -860,7 +860,7 @@ TEST_F(OmniWheelDriveControllerTest, 5_wheel_test) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); for (size_t i = 0; i < 5; i++) { - EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + EXPECT_EQ(command_itfs_[i]->get_optional().value(), 0.0); } state = controller_->get_node()->configure(); diff --git a/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.hpp b/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.hpp index 6e0bc45a36..896d76b911 100644 --- a/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.hpp +++ b/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.hpp @@ -157,54 +157,56 @@ class OmniWheelDriveControllerFixture : public ::testing::Test void assignResourcesPosFeedback(const std::vector wheel_names = wheel_names_) { - std::vector state_ifs; - state_ifs.reserve(wheels_pos_states_.size()); + std::vector loaned_state_ifs; + loaned_state_ifs.reserve(wheels_pos_states_.size()); state_itfs_.reserve(wheels_pos_states_.size()); for (size_t i = 0; i < wheels_pos_states_.size(); ++i) { state_itfs_.emplace_back( - hardware_interface::StateInterface(wheel_names[i], HW_IF_POSITION, &wheels_pos_states_[i])); - state_ifs.emplace_back(state_itfs_.back()); + std::make_shared( + wheel_names[i], HW_IF_POSITION, &wheels_pos_states_[i])); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); } - std::vector command_ifs; - command_ifs.reserve(wheels_vel_cmds_.size()); + std::vector loaned_command_ifs; + loaned_command_ifs.reserve(wheels_vel_cmds_.size()); command_itfs_.reserve(wheels_vel_cmds_.size()); for (size_t i = 0; i < wheels_vel_cmds_.size(); ++i) { command_itfs_.emplace_back( - hardware_interface::CommandInterface(wheel_names[i], HW_IF_VELOCITY, &wheels_vel_cmds_[i])); - command_ifs.emplace_back(command_itfs_.back()); + std::make_shared( + wheel_names[i], HW_IF_VELOCITY, &wheels_vel_cmds_[i])); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); } - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs)); } void assignResourcesVelFeedback() { - std::vector state_ifs; - state_ifs.reserve(wheels_vel_states_.size()); + std::vector loaned_state_ifs; + loaned_state_ifs.reserve(wheels_vel_states_.size()); state_itfs_.reserve(wheels_vel_states_.size()); for (size_t i = 0; i < wheels_vel_states_.size(); ++i) { state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( wheel_names_[i], HW_IF_VELOCITY, &wheels_vel_states_[i])); - state_ifs.emplace_back(state_itfs_.back()); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); } - std::vector command_ifs; - command_ifs.reserve(wheels_vel_cmds_.size()); + std::vector loaned_command_ifs; + loaned_command_ifs.reserve(wheels_vel_cmds_.size()); command_itfs_.reserve(wheels_vel_cmds_.size()); for (size_t i = 0; i < wheels_vel_cmds_.size(); ++i) { command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( wheel_names_[i], HW_IF_VELOCITY, &wheels_vel_cmds_[i])); - command_ifs.emplace_back(command_itfs_.back()); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); } - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs)); } controller_interface::return_type InitController( @@ -232,8 +234,8 @@ class OmniWheelDriveControllerFixture : public ::testing::Test std::vector wheels_vel_states_ = {1, 1, 1, 1}; std::vector wheels_vel_cmds_ = {0.1, 0.2, 0.3, 0.4}; - std::vector state_itfs_; - std::vector command_itfs_; + std::vector state_itfs_; + std::vector command_itfs_; std::vector reference_interface_names = {"linear/x", "linear/y", "angular/z"}; diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp index 1831e8ab92..1af3576462 100644 --- a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp @@ -55,10 +55,10 @@ void GripperControllerTest::SetUpController( ASSERT_EQ(result, expected_result); std::vector command_ifs; - command_ifs.emplace_back(this->joint_1_cmd_); + command_ifs.emplace_back(this->joint_1_cmd_, nullptr); std::vector state_ifs; - state_ifs.emplace_back(this->joint_1_pos_state_); - state_ifs.emplace_back(this->joint_1_vel_state_); + state_ifs.emplace_back(this->joint_1_pos_state_, nullptr); + state_ifs.emplace_back(this->joint_1_vel_state_, nullptr); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } @@ -138,10 +138,10 @@ TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess) // re-assign interfaces std::vector command_ifs; - command_ifs.emplace_back(this->joint_1_cmd_); + command_ifs.emplace_back(this->joint_1_cmd_, nullptr); std::vector state_ifs; - state_ifs.emplace_back(this->joint_1_pos_state_); - state_ifs.emplace_back(this->joint_1_vel_state_); + state_ifs.emplace_back(this->joint_1_pos_state_, nullptr); + state_ifs.emplace_back(this->joint_1_vel_state_, nullptr); this->controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); ASSERT_EQ( diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp index d206097782..14e56f1378 100644 --- a/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp @@ -54,12 +54,15 @@ class GripperControllerTest : public ::testing::Test std::vector joint_states_ = {1.1, 2.1}; std::vector joint_commands_ = {3.1}; - hardware_interface::StateInterface joint_1_pos_state_{ - joint_name_, hardware_interface::HW_IF_POSITION, &joint_states_[0]}; - hardware_interface::StateInterface joint_1_vel_state_{ - joint_name_, hardware_interface::HW_IF_VELOCITY, &joint_states_[1]}; - hardware_interface::CommandInterface joint_1_cmd_{ - joint_name_, hardware_interface::HW_IF_POSITION, &joint_commands_[0]}; + hardware_interface::StateInterface::SharedPtr joint_1_pos_state_ = + std::make_shared( + joint_name_, hardware_interface::HW_IF_POSITION, &joint_states_[0]); + hardware_interface::StateInterface::SharedPtr joint_1_vel_state_ = + std::make_shared( + joint_name_, hardware_interface::HW_IF_VELOCITY, &joint_states_[1]); + hardware_interface::CommandInterface::SharedPtr joint_1_cmd_ = + std::make_shared( + joint_name_, hardware_interface::HW_IF_POSITION, &joint_commands_[0]); }; } // anonymous namespace diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 2a3f60464d..3efb170af5 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -155,20 +155,20 @@ class PidControllerFixture : public ::testing::Test controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), controller_interface::return_type::OK); - std::vector command_ifs; + std::vector loaned_command_ifs; command_itfs_.reserve(dof_names_.size()); - command_ifs.reserve(dof_names_.size()); + loaned_command_ifs.reserve(dof_names_.size()); for (size_t i = 0; i < dof_names_.size(); ++i) { command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( dof_names_[i], command_interface_, &dof_command_values_[i])); - command_ifs.emplace_back(command_itfs_.back()); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); } - std::vector state_ifs; - state_ifs.reserve(dof_names_.size() * state_interfaces_.size()); + std::vector loaned_state_ifs; + loaned_state_ifs.reserve(dof_names_.size() * state_interfaces_.size()); state_itfs_.reserve(dof_names_.size() * state_interfaces_.size()); size_t index = 0; for (const auto & interface : state_interfaces_) @@ -176,13 +176,14 @@ class PidControllerFixture : public ::testing::Test for (const auto & dof_name : dof_names_) { state_itfs_.emplace_back( - hardware_interface::StateInterface(dof_name, interface, &dof_state_values_[index])); - state_ifs.emplace_back(state_itfs_.back()); + std::make_shared( + dof_name, interface, &dof_state_values_[index])); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); ++index; } } - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs)); } void subscribe_and_get_messages(ControllerStateMsg & msg) @@ -267,8 +268,8 @@ class PidControllerFixture : public ::testing::Test std::vector dof_command_values_; std::vector reference_and_state_dof_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/pose_broadcaster/test/test_pose_broadcaster.cpp b/pose_broadcaster/test/test_pose_broadcaster.cpp index 89668f3ea3..6006cae8b4 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.cpp +++ b/pose_broadcaster/test/test_pose_broadcaster.cpp @@ -32,13 +32,13 @@ void PoseBroadcasterTest::SetUpPoseBroadcaster() controller_interface::return_type::OK); std::vector state_interfaces; - state_interfaces.emplace_back(pose_position_x_); - state_interfaces.emplace_back(pose_position_y_); - state_interfaces.emplace_back(pose_position_z_); - state_interfaces.emplace_back(pose_orientation_x_); - state_interfaces.emplace_back(pose_orientation_y_); - state_interfaces.emplace_back(pose_orientation_z_); - state_interfaces.emplace_back(pose_orientation_w_); + state_interfaces.emplace_back(pose_position_x_, nullptr); + state_interfaces.emplace_back(pose_position_y_, nullptr); + state_interfaces.emplace_back(pose_position_z_, nullptr); + state_interfaces.emplace_back(pose_orientation_x_, nullptr); + state_interfaces.emplace_back(pose_orientation_y_, nullptr); + state_interfaces.emplace_back(pose_orientation_z_, nullptr); + state_interfaces.emplace_back(pose_orientation_w_, nullptr); pose_broadcaster_->assign_interfaces({}, std::move(state_interfaces)); } @@ -208,7 +208,7 @@ TEST_F(PoseBroadcasterTest, invalid_pose_no_tf_published) pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), controller_interface::CallbackReturn::SUCCESS); - ASSERT_TRUE(pose_position_x_.set_value(std::numeric_limits::quiet_NaN())); + ASSERT_TRUE(pose_position_x_->set_value(std::numeric_limits::quiet_NaN())); // Subscribe to pose topic geometry_msgs::msg::PoseStamped pose_msg; @@ -231,11 +231,11 @@ TEST_F(PoseBroadcasterTest, invalid_pose_no_tf_published) ASSERT_EQ(tf_msg.transforms.size(), 0lu); // Set valid position but invalid quaternion - ASSERT_TRUE(pose_position_x_.set_value(0.0)); - ASSERT_TRUE(pose_orientation_x_.set_value(0.0)); - ASSERT_TRUE(pose_orientation_y_.set_value(0.0)); - ASSERT_TRUE(pose_orientation_z_.set_value(0.0)); - ASSERT_TRUE(pose_orientation_w_.set_value(0.0)); + ASSERT_TRUE(pose_position_x_->set_value(0.0)); + ASSERT_TRUE(pose_orientation_x_->set_value(0.0)); + ASSERT_TRUE(pose_orientation_y_->set_value(0.0)); + ASSERT_TRUE(pose_orientation_z_->set_value(0.0)); + ASSERT_TRUE(pose_orientation_w_->set_value(0.0)); EXPECT_THROW(subscribe_and_get_message("/tf", tf_msg), std::runtime_error); // Verify that no tf message was sent diff --git a/pose_broadcaster/test/test_pose_broadcaster.hpp b/pose_broadcaster/test/test_pose_broadcaster.hpp index e3b84c0307..65b655a1ce 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.hpp +++ b/pose_broadcaster/test/test_pose_broadcaster.hpp @@ -42,17 +42,27 @@ class PoseBroadcasterTest : public ::testing::Test std::array pose_values_ = { {1.1, 2.2, 3.3, 0.39190382, 0.20056212, 0.53197575, 0.72331744}}; - hardware_interface::StateInterface pose_position_x_{pose_name_, "position.x", &pose_values_[0]}; - hardware_interface::StateInterface pose_position_y_{pose_name_, "position.x", &pose_values_[1]}; - hardware_interface::StateInterface pose_position_z_{pose_name_, "position.x", &pose_values_[2]}; - hardware_interface::StateInterface pose_orientation_x_{ - pose_name_, "orientation.x", &pose_values_[3]}; - hardware_interface::StateInterface pose_orientation_y_{ - pose_name_, "orientation.y", &pose_values_[4]}; - hardware_interface::StateInterface pose_orientation_z_{ - pose_name_, "orientation.z", &pose_values_[5]}; - hardware_interface::StateInterface pose_orientation_w_{ - pose_name_, "orientation.w", &pose_values_[6]}; + hardware_interface::StateInterface::SharedPtr pose_position_x_ = + std::make_shared( + pose_name_, "position.x", &pose_values_[0]); + hardware_interface::StateInterface::SharedPtr pose_position_y_ = + std::make_shared( + pose_name_, "position.y", &pose_values_[1]); + hardware_interface::StateInterface::SharedPtr pose_position_z_ = + std::make_shared( + pose_name_, "position.z", &pose_values_[2]); + hardware_interface::StateInterface::SharedPtr pose_orientation_x_ = + std::make_shared( + pose_name_, "orientation.x", &pose_values_[3]); + hardware_interface::StateInterface::SharedPtr pose_orientation_y_ = + std::make_shared( + pose_name_, "orientation.y", &pose_values_[4]); + hardware_interface::StateInterface::SharedPtr pose_orientation_z_ = + std::make_shared( + pose_name_, "orientation.z", &pose_values_[5]); + hardware_interface::StateInterface::SharedPtr pose_orientation_w_ = + std::make_shared( + pose_name_, "orientation.w", &pose_values_[6]); std::unique_ptr pose_broadcaster_; diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 905c5036df..bf0d315bac 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -47,9 +47,9 @@ void JointGroupPositionControllerTest::SetUpController( ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; - command_ifs.emplace_back(joint_1_pos_cmd_); - command_ifs.emplace_back(joint_2_pos_cmd_); - command_ifs.emplace_back(joint_3_pos_cmd_); + command_ifs.emplace_back(joint_1_pos_cmd_, nullptr); + command_ifs.emplace_back(joint_2_pos_cmd_, nullptr); + command_ifs.emplace_back(joint_3_pos_cmd_, nullptr); controller_->assign_interfaces(std::move(command_ifs), {}); executor.add_node(controller_->get_node()->get_node_base_interface()); } @@ -74,9 +74,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_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); // send command forward_command_controller::CmdType command; @@ -89,9 +89,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 30.0); } TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) @@ -110,9 +110,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_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); } TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) @@ -126,9 +126,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_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); } TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) @@ -136,9 +136,9 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) SetUpController({rclcpp::Parameter("joints", joint_names_)}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -169,7 +169,7 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_optional().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 f39a413d1c..404f60b0ed 100644 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ b/position_controllers/test/test_joint_group_position_controller.hpp @@ -56,9 +56,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]}; + CommandInterface::SharedPtr joint_1_pos_cmd_ = + std::make_shared(joint_names_[0], HW_IF_POSITION, &joint_commands_[0]); + CommandInterface::SharedPtr joint_2_pos_cmd_ = + std::make_shared(joint_names_[1], HW_IF_POSITION, &joint_commands_[1]); + CommandInterface::SharedPtr joint_3_pos_cmd_ = + std::make_shared(joint_names_[2], HW_IF_POSITION, &joint_commands_[2]); rclcpp::executors::SingleThreadedExecutor executor; }; diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index 052c0384d3..e2fa00685f 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -45,7 +45,7 @@ controller_interface::return_type RangeSensorBroadcasterTest::init_broadcaster( if (controller_interface::return_type::OK == result) { std::vector state_interfaces; - state_interfaces.emplace_back(range_); + state_interfaces.emplace_back(range_, nullptr); range_broadcaster_->assign_interfaces({}, std::move(state_interfaces)); } diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp index b7ffa7fe4a..6fa655c972 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp @@ -47,7 +47,8 @@ class RangeSensorBroadcasterTest : public ::testing::Test const double variance_ = 1.0; double sensor_range_ = 3.1; - hardware_interface::StateInterface range_{sensor_name_, "range", &sensor_range_}; + hardware_interface::StateInterface::SharedPtr range_ = + std::make_shared(sensor_name_, "range", &sensor_range_); std::unique_ptr range_broadcaster_; diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 5aac580b33..98577b2c42 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -166,63 +166,71 @@ class SteeringControllersLibraryFixture : public ::testing::Test traction_interface_name_ = "velocity"; } - std::vector command_ifs; + std::vector loaned_command_ifs; command_itfs_.reserve(joint_command_values_.size()); - command_ifs.reserve(joint_command_values_.size()); + loaned_command_ifs.reserve(joint_command_values_.size()); command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( traction_joints_names_[0], traction_interface_name_, - &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); - command_ifs.emplace_back(command_itfs_.back()); + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL]), + nullptr); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( traction_joints_names_[1], traction_interface_name_, - &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); - command_ifs.emplace_back(command_itfs_.back()); + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL]), + nullptr); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( steering_joints_names_[0], steering_interface_name_, - &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); - command_ifs.emplace_back(command_itfs_.back()); + &joint_command_values_[CMD_STEER_RIGHT_WHEEL]), + nullptr); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( steering_joints_names_[1], steering_interface_name_, - &joint_command_values_[CMD_STEER_LEFT_WHEEL])); - command_ifs.emplace_back(command_itfs_.back()); + &joint_command_values_[CMD_STEER_LEFT_WHEEL]), + nullptr); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); - std::vector state_ifs; + std::vector loaned_state_ifs; state_itfs_.reserve(joint_state_values_.size()); - state_ifs.reserve(joint_state_values_.size()); + loaned_state_ifs.reserve(joint_state_values_.size()); state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( traction_joints_names_[0], traction_interface_name_, - &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); - state_ifs.emplace_back(state_itfs_.back()); + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL]), + nullptr); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( traction_joints_names_[1], traction_interface_name_, - &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); - state_ifs.emplace_back(state_itfs_.back()); + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL]), + nullptr); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( steering_joints_names_[0], steering_interface_name_, - &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); - state_ifs.emplace_back(state_itfs_.back()); + &joint_state_values_[STATE_STEER_RIGHT_WHEEL]), + nullptr); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( steering_joints_names_[1], steering_interface_name_, - &joint_state_values_[STATE_STEER_LEFT_WHEEL])); - state_ifs.emplace_back(state_itfs_.back()); + &joint_state_values_[STATE_STEER_LEFT_WHEEL]), + nullptr); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs)); } void subscribe_and_get_messages(ControllerStateMsg & msg) @@ -322,8 +330,8 @@ class SteeringControllersLibraryFixture : public ::testing::Test std::string traction_interface_name_ = ""; std::string preceding_prefix_ = "pid_controller"; - 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/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index a933ef1fa8..583f086412 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -139,12 +139,12 @@ class TestTricycleController : public ::testing::Test void assignResources() { std::vector state_ifs; - state_ifs.emplace_back(steering_joint_pos_state_); - state_ifs.emplace_back(traction_joint_vel_state_); + state_ifs.emplace_back(steering_joint_pos_state_, nullptr); + state_ifs.emplace_back(traction_joint_vel_state_, nullptr); std::vector command_ifs; - command_ifs.emplace_back(steering_joint_pos_cmd_); - command_ifs.emplace_back(traction_joint_vel_cmd_); + command_ifs.emplace_back(steering_joint_pos_cmd_, nullptr); + command_ifs.emplace_back(traction_joint_vel_cmd_, nullptr); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } @@ -177,17 +177,21 @@ 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_}; + hardware_interface::StateInterface::SharedPtr 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_}; + hardware_interface::StateInterface::SharedPtr 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_}; + hardware_interface::CommandInterface::SharedPtr 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_}; + hardware_interface::CommandInterface::SharedPtr traction_joint_vel_cmd_ = + std::make_shared( + traction_joint_name, HW_IF_VELOCITY, &velocity_); rclcpp::Node::SharedPtr pub_node; rclcpp::Publisher::SharedPtr velocity_publisher; @@ -285,15 +289,15 @@ TEST_F(TestTricycleController, cleanup) ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); // should be stopped - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_optional().value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_->get_optional().value()); state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_optional().value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_->get_optional().value()); executor.cancel(); } @@ -313,8 +317,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_optional().value()); - EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_optional().value()); + EXPECT_EQ(position_, steering_joint_pos_cmd_->get_optional().value()); + EXPECT_EQ(velocity_, traction_joint_vel_cmd_->get_optional().value()); state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -329,8 +333,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_optional().value()); - EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_optional().value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_optional().value()); + EXPECT_EQ(1.0, traction_joint_vel_cmd_->get_optional().value()); // deactivated // wait so controller process the second point when deactivated @@ -341,16 +345,16 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) state = controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()) + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_optional().value()) << "Wheels are halted on deactivate()"; - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()) + EXPECT_EQ(0.0, traction_joint_vel_cmd_->get_optional().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_optional().value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_optional().value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_->get_optional().value()); state = controller_->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 757ac7fe01..8517773e96 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -145,51 +145,51 @@ class TricycleSteeringControllerFixture : public ::testing::Test traction_interface_name_ = "velocity"; } - std::vector command_ifs; + std::vector loaned_command_ifs; command_itfs_.reserve(joint_command_values_.size()); - command_ifs.reserve(joint_command_values_.size()); + loaned_command_ifs.reserve(joint_command_values_.size()); command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( traction_joints_names_[0], traction_interface_name_, &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); - command_ifs.emplace_back(command_itfs_.back()); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( traction_joints_names_[1], steering_interface_name_, &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); - command_ifs.emplace_back(command_itfs_.back()); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); command_itfs_.emplace_back( - hardware_interface::CommandInterface( + std::make_shared( steering_joints_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); - command_ifs.emplace_back(command_itfs_.back()); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); - std::vector state_ifs; + std::vector loaned_state_ifs; state_itfs_.reserve(joint_state_values_.size()); - state_ifs.reserve(joint_state_values_.size()); + loaned_state_ifs.reserve(joint_state_values_.size()); state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( traction_joints_names_[0], traction_interface_name_, &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); - state_ifs.emplace_back(state_itfs_.back()); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( traction_joints_names_[1], traction_interface_name_, &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); - state_ifs.emplace_back(state_itfs_.back()); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); state_itfs_.emplace_back( - hardware_interface::StateInterface( + std::make_shared( steering_joints_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); - state_ifs.emplace_back(state_itfs_.back()); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs)); } void subscribe_and_get_messages(ControllerStateMsg & msg) @@ -290,8 +290,8 @@ class TricycleSteeringControllerFixture : public ::testing::Test std::string traction_interface_name_ = ""; std::string preceding_prefix_ = "pid_controller"; - 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/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index b1c7839b7d..de138571af 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -47,9 +47,9 @@ void JointGroupVelocityControllerTest::SetUpController( ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; - command_ifs.emplace_back(joint_1_cmd_); - command_ifs.emplace_back(joint_2_cmd_); - command_ifs.emplace_back(joint_3_cmd_); + command_ifs.emplace_back(joint_1_cmd_, nullptr); + command_ifs.emplace_back(joint_2_cmd_, nullptr); + command_ifs.emplace_back(joint_3_cmd_, nullptr); controller_->assign_interfaces(std::move(command_ifs), {}); executor.add_node(controller_->get_node()->get_node_base_interface()); } @@ -75,9 +75,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); // send command forward_command_controller::CmdType command; @@ -90,9 +90,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_optional().value(), 30.0); } TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) @@ -112,9 +112,9 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); } TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) @@ -129,9 +129,9 @@ TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); } TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) @@ -139,9 +139,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) SetUpController({rclcpp::Parameter("joints", joint_names_)}); // default values - ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -172,9 +172,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_optional().value(), 30.0); } TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) @@ -185,15 +185,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_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_optional().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_optional().value(), 0.0); - ASSERT_EQ(joint_2_cmd_.get_optional().value(), 0.0); - ASSERT_EQ(joint_3_cmd_.get_optional().value(), 0.0); + ASSERT_EQ(joint_1_cmd_->get_optional().value(), 0.0); + ASSERT_EQ(joint_2_cmd_->get_optional().value(), 0.0); + ASSERT_EQ(joint_3_cmd_->get_optional().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 f428e1828f..fff024bd2a 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.hpp @@ -56,9 +56,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]}; + CommandInterface::SharedPtr joint_1_cmd_ = + std::make_shared(joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]); + CommandInterface::SharedPtr joint_2_cmd_ = + std::make_shared(joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]); + CommandInterface::SharedPtr joint_3_cmd_ = + std::make_shared(joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]); rclcpp::executors::SingleThreadedExecutor executor; }; From 4901d5a242348b69d8b8b147369d32015ab3938a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 29 Sep 2025 13:28:10 +0000 Subject: [PATCH 2/2] Fix vector interfaces generation --- .../test_steering_controllers_library.hpp | 24 +++++++------------ 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 98577b2c42..e1b22ae0fa 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -173,29 +173,25 @@ class SteeringControllersLibraryFixture : public ::testing::Test command_itfs_.emplace_back( std::make_shared( traction_joints_names_[0], traction_interface_name_, - &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL]), - nullptr); + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); command_itfs_.emplace_back( std::make_shared( traction_joints_names_[1], traction_interface_name_, - &joint_command_values_[CMD_TRACTION_LEFT_WHEEL]), - nullptr); + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); command_itfs_.emplace_back( std::make_shared( steering_joints_names_[0], steering_interface_name_, - &joint_command_values_[CMD_STEER_RIGHT_WHEEL]), - nullptr); + &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); command_itfs_.emplace_back( std::make_shared( steering_joints_names_[1], steering_interface_name_, - &joint_command_values_[CMD_STEER_LEFT_WHEEL]), - nullptr); + &joint_command_values_[CMD_STEER_LEFT_WHEEL])); loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); std::vector loaned_state_ifs; @@ -205,29 +201,25 @@ class SteeringControllersLibraryFixture : public ::testing::Test state_itfs_.emplace_back( std::make_shared( traction_joints_names_[0], traction_interface_name_, - &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL]), - nullptr); + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); state_itfs_.emplace_back( std::make_shared( traction_joints_names_[1], traction_interface_name_, - &joint_state_values_[STATE_TRACTION_LEFT_WHEEL]), - nullptr); + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); state_itfs_.emplace_back( std::make_shared( steering_joints_names_[0], steering_interface_name_, - &joint_state_values_[STATE_STEER_RIGHT_WHEEL]), - nullptr); + &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); state_itfs_.emplace_back( std::make_shared( steering_joints_names_[1], steering_interface_name_, - &joint_state_values_[STATE_STEER_LEFT_WHEEL]), - nullptr); + &joint_state_values_[STATE_STEER_LEFT_WHEEL])); loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs));