diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 7dca2e2010..5a17f05102 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -201,7 +201,7 @@ class AdmittanceControllerTest : public ::testing::Test for (auto i = 0u; i < joint_command_values_.size(); ++i) { - command_itfs_.emplace_back(hardware_interface::CommandInterface( + command_itfs_.emplace_back(std::make_shared( joint_names_[i], command_interface_types_[0], &joint_command_values_[i])); command_ifs.emplace_back(command_itfs_.back()); } @@ -216,7 +216,7 @@ class AdmittanceControllerTest : public ::testing::Test for (auto i = 0u; i < joint_state_values_.size(); ++i) { - state_itfs_.emplace_back(hardware_interface::StateInterface( + state_itfs_.emplace_back(std::make_shared( joint_names_[i], state_interface_types_[0], &joint_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); } @@ -226,7 +226,7 @@ class AdmittanceControllerTest : public ::testing::Test for (auto i = 0u; i < fts_state_names_.size(); ++i) { - state_itfs_.emplace_back(hardware_interface::StateInterface( + state_itfs_.emplace_back(std::make_shared( ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); } @@ -397,8 +397,8 @@ class AdmittanceControllerTest : public ::testing::Test std::array fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; std::vector fts_state_names_; - std::vector state_itfs_; - std::vector command_itfs_; + std::vector> state_itfs_; + std::vector> command_itfs_; // Test related parameters std::unique_ptr controller_; diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 236f34e9a2..2c710b05a8 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -163,18 +163,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]}; + std::shared_ptr left_wheel_pos_state_ = + std::make_shared( + left_wheel_names[0], HW_IF_POSITION, &position_values_[0]); + std::shared_ptr right_wheel_pos_state_ = + std::make_shared( + right_wheel_names[0], HW_IF_POSITION, &position_values_[1]); + std::shared_ptr left_wheel_vel_state_ = + std::make_shared( + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]); + std::shared_ptr right_wheel_vel_state_ = + std::make_shared( + right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]); + std::shared_ptr left_wheel_vel_cmd_ = + std::make_shared( + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]); + std::shared_ptr right_wheel_vel_cmd_ = + std::make_shared( + right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]); rclcpp::Node::SharedPtr pub_node; rclcpp::Publisher::SharedPtr velocity_publisher; @@ -368,8 +374,8 @@ TEST_F(TestDiffDriveController, cleanup) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_value()); + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_value()); executor.cancel(); } @@ -393,8 +399,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) assignResourcesPosFeedback(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(0.01, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(0.02, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.01, left_wheel_vel_cmd_->get_value()); + EXPECT_EQ(0.02, right_wheel_vel_cmd_->get_value()); state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -409,8 +415,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(1.0, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(1.0, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(1.0, left_wheel_vel_cmd_->get_value()); + EXPECT_EQ(1.0, right_wheel_vel_cmd_->get_value()); // deactivated // wait so controller process the second point when deactivated @@ -421,14 +427,14 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_value()) << "Wheels are halted on deactivate()"; // cleanup state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.0, left_wheel_vel_cmd_->get_value()); + EXPECT_EQ(0.0, right_wheel_vel_cmd_->get_value()); state = controller_->get_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 256a4ce465..d4af71f3b0 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -119,9 +119,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -134,9 +134,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 30.0); } TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) @@ -156,9 +156,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); } TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) @@ -173,9 +173,9 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); } TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) @@ -184,9 +184,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -214,9 +214,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 30.0); } TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) @@ -228,15 +228,15 @@ TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); // stop the controller ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 0.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 0.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 0.0); } diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp index 6ae9db4670..160ebb1309 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -54,9 +54,12 @@ class JointGroupEffortControllerTest : public ::testing::Test const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]}; - CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]}; - CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]}; + std::shared_ptr joint_1_cmd_ = + std::make_shared(joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]); + std::shared_ptr joint_2_cmd_ = + std::make_shared(joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]); + std::shared_ptr joint_3_cmd_ = + std::make_shared(joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]); }; #endif // TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index 5477b3fa6f..46ede8f606 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -57,12 +57,24 @@ class ForceTorqueSensorBroadcasterTest : public ::testing::Test const std::string frame_id_ = "fts_sensor_frame"; std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; - hardware_interface::StateInterface fts_force_x_{sensor_name_, "force.x", &sensor_values_[0]}; - hardware_interface::StateInterface fts_force_y_{sensor_name_, "force.y", &sensor_values_[1]}; - hardware_interface::StateInterface fts_force_z_{sensor_name_, "force.z", &sensor_values_[2]}; - hardware_interface::StateInterface fts_torque_x_{sensor_name_, "torque.x", &sensor_values_[3]}; - hardware_interface::StateInterface fts_torque_y_{sensor_name_, "torque.y", &sensor_values_[4]}; - hardware_interface::StateInterface fts_torque_z_{sensor_name_, "torque.z", &sensor_values_[5]}; + std::shared_ptr fts_force_x_ = + std::make_shared( + sensor_name_, "force.x", &sensor_values_[0]); + std::shared_ptr fts_force_y_ = + std::make_shared( + sensor_name_, "force.y", &sensor_values_[1]); + std::shared_ptr fts_force_z_ = + std::make_shared( + sensor_name_, "force.z", &sensor_values_[2]); + std::shared_ptr fts_torque_x_ = + std::make_shared( + sensor_name_, "torque.x", &sensor_values_[3]); + std::shared_ptr fts_torque_y_ = + std::make_shared( + sensor_name_, "torque.y", &sensor_values_[4]); + std::shared_ptr fts_torque_z_ = + std::make_shared( + sensor_name_, "torque.z", &sensor_values_[5]); std::unique_ptr fts_broadcaster_; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 697e42d671..6ec16f4717 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -195,9 +195,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -210,9 +210,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30.0); } TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) @@ -238,9 +238,9 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); } TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) @@ -260,9 +260,9 @@ TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); } TEST_F(ForwardCommandControllerTest, CommandCallbackTest) @@ -273,9 +273,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"interface_name", "position"}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -303,9 +303,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30.0); } TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) @@ -316,9 +316,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_->get_node()->set_parameter({"interface_name", "position"}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -337,9 +337,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30); node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -381,9 +381,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // values should not change - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30); // set commands again controller_->rt_command_ptr_.writeFromNonRT(command_msg); @@ -394,7 +394,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 6.6); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 7.7); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 5.5); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 6.6); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 7.7); } diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 9c6bd2a352..61e454ecd8 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -66,9 +66,12 @@ class ForwardCommandControllerTest : public ::testing::Test const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; - CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; - CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + std::shared_ptr joint_1_pos_cmd_ = + std::make_shared(joint_names_[0], HW_IF_POSITION, &joint_commands_[0]); + std::shared_ptr joint_2_pos_cmd_ = + std::make_shared(joint_names_[1], HW_IF_POSITION, &joint_commands_[1]); + std::shared_ptr joint_3_pos_cmd_ = + std::make_shared(joint_names_[2], HW_IF_POSITION, &joint_commands_[2]); }; #endif // TEST_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 0cada04859..ae7efd6141 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -189,9 +189,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateSuccess) SetUpController(true); // check joint commands are the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) @@ -209,9 +209,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 30.0); } TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest) @@ -224,9 +224,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) @@ -244,9 +244,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) @@ -273,9 +273,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 30.0); } TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) @@ -293,9 +293,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 30.0); auto node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -336,9 +336,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // values should not change - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 30.0); // set commands again controller_->rt_command_ptr_.writeFromNonRT(command_msg); @@ -349,7 +349,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 6.6); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 7.7); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 5.5); + ASSERT_EQ(joint_1_vel_cmd_->get_value(), 6.6); + ASSERT_EQ(joint_1_eff_cmd_->get_value(), 7.7); } diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp index 62a4d4e981..e427776bab 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp @@ -74,9 +74,12 @@ class MultiInterfaceForwardCommandControllerTest : public ::testing::Test double vel_cmd_ = 2.1; double eff_cmd_ = 3.1; - CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &pos_cmd_}; - CommandInterface joint_1_vel_cmd_{joint_name_, HW_IF_VELOCITY, &vel_cmd_}; - CommandInterface joint_1_eff_cmd_{joint_name_, HW_IF_EFFORT, &eff_cmd_}; + std::shared_ptr joint_1_pos_cmd_ = + std::make_shared(joint_name_, HW_IF_POSITION, &pos_cmd_); + std::shared_ptr joint_1_vel_cmd_ = + std::make_shared(joint_name_, HW_IF_VELOCITY, &vel_cmd_); + std::shared_ptr joint_1_eff_cmd_ = + std::make_shared(joint_name_, HW_IF_EFFORT, &eff_cmd_); }; #endif // TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp index 3e9750a603..b002aaf9c4 100644 --- a/gripper_controllers/test/test_gripper_controllers.hpp +++ b/gripper_controllers/test/test_gripper_controllers.hpp @@ -57,9 +57,12 @@ class GripperControllerTest : public ::testing::Test std::vector joint_states_ = {1.1, 2.1}; std::vector joint_commands_ = {3.1}; - StateInterface joint_1_pos_state_{joint_name_, HW_IF_POSITION, &joint_states_[0]}; - StateInterface joint_1_vel_state_{joint_name_, HW_IF_VELOCITY, &joint_states_[1]}; - CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &joint_commands_[0]}; + std::shared_ptr joint_1_pos_state_ = + std::make_shared(joint_name_, HW_IF_POSITION, &joint_states_[0]); + std::shared_ptr joint_1_vel_state_ = + std::make_shared(joint_name_, HW_IF_VELOCITY, &joint_states_[1]); + std::shared_ptr joint_1_pos_cmd_ = + std::make_shared(joint_name_, HW_IF_POSITION, &joint_commands_[0]); }; #endif // TEST_GRIPPER_CONTROLLERS_HPP_ diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 01423724b8..8a3feb98ca 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -55,26 +55,36 @@ class IMUSensorBroadcasterTest : public ::testing::Test const std::string sensor_name_ = "imu_sensor"; const std::string frame_id_ = "imu_sensor_frame"; std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; - hardware_interface::StateInterface imu_orientation_x_{ - sensor_name_, "orientation.x", &sensor_values_[0]}; - hardware_interface::StateInterface imu_orientation_y_{ - sensor_name_, "orientation.y", &sensor_values_[1]}; - hardware_interface::StateInterface imu_orientation_z_{ - sensor_name_, "orientation.z", &sensor_values_[2]}; - hardware_interface::StateInterface imu_orientation_w_{ - sensor_name_, "orientation.w", &sensor_values_[3]}; - hardware_interface::StateInterface imu_angular_velocity_x_{ - sensor_name_, "angular_velocity.x", &sensor_values_[4]}; - hardware_interface::StateInterface imu_angular_velocity_y_{ - sensor_name_, "angular_velocity.y", &sensor_values_[5]}; - hardware_interface::StateInterface imu_angular_velocity_z_{ - sensor_name_, "angular_velocity.z", &sensor_values_[6]}; - hardware_interface::StateInterface imu_linear_acceleration_x_{ - sensor_name_, "linear_acceleration.x", &sensor_values_[7]}; - hardware_interface::StateInterface imu_linear_acceleration_y_{ - sensor_name_, "linear_acceleration.y", &sensor_values_[8]}; - hardware_interface::StateInterface imu_linear_acceleration_z_{ - sensor_name_, "linear_acceleration.z", &sensor_values_[9]}; + std::shared_ptr imu_orientation_x_ = + std::make_shared( + sensor_name_, "orientation.x", &sensor_values_[0]); + std::shared_ptr imu_orientation_y_ = + std::make_shared( + sensor_name_, "orientation.y", &sensor_values_[1]); + std::shared_ptr imu_orientation_z_ = + std::make_shared( + sensor_name_, "orientation.z", &sensor_values_[2]); + std::shared_ptr imu_orientation_w_ = + std::make_shared( + sensor_name_, "orientation.w", &sensor_values_[3]); + std::shared_ptr imu_angular_velocity_x_ = + std::make_shared( + sensor_name_, "angular_velocity.x", &sensor_values_[4]); + std::shared_ptr imu_angular_velocity_y_ = + std::make_shared( + sensor_name_, "angular_velocity.y", &sensor_values_[5]); + std::shared_ptr imu_angular_velocity_z_ = + std::make_shared( + sensor_name_, "angular_velocity.z", &sensor_values_[6]); + std::shared_ptr imu_linear_acceleration_x_ = + std::make_shared( + sensor_name_, "linear_acceleration.x", &sensor_values_[7]); + std::shared_ptr imu_linear_acceleration_y_ = + std::make_shared( + sensor_name_, "linear_acceleration.y", &sensor_values_[8]); + std::shared_ptr imu_linear_acceleration_z_ = + std::make_shared( + sensor_name_, "linear_acceleration.z", &sensor_values_[9]); std::unique_ptr imu_broadcaster_; diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 74a19c9ed0..c449e9ae56 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -146,7 +146,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( try { - const std::string topic_name_prefix = params_.use_local_topics ? "~/" : ""; + const std::string topic_name_prefix = params_.use_local_topics ? "~/" : "/"; joint_state_publisher_ = get_node()->create_publisher( topic_name_prefix + "joint_states", rclcpp::SystemDefaultsQoS()); diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 63cc7a5483..e5372233de 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -79,27 +79,37 @@ class JointStateBroadcasterTest : public ::testing::Test std::vector joint_values_ = {1.1, 2.1, 3.1}; double custom_joint_value_ = 3.5; - hardware_interface::StateInterface joint_1_pos_state_{ - joint_names_[0], interface_names_[0], &joint_values_[0]}; - hardware_interface::StateInterface joint_2_pos_state_{ - joint_names_[1], interface_names_[0], &joint_values_[1]}; - hardware_interface::StateInterface joint_3_pos_state_{ - joint_names_[2], interface_names_[0], &joint_values_[2]}; - hardware_interface::StateInterface joint_1_vel_state_{ - joint_names_[0], interface_names_[1], &joint_values_[0]}; - hardware_interface::StateInterface joint_2_vel_state_{ - joint_names_[1], interface_names_[1], &joint_values_[1]}; - hardware_interface::StateInterface joint_3_vel_state_{ - joint_names_[2], interface_names_[1], &joint_values_[2]}; - hardware_interface::StateInterface joint_1_eff_state_{ - joint_names_[0], interface_names_[2], &joint_values_[0]}; - hardware_interface::StateInterface joint_2_eff_state_{ - joint_names_[1], interface_names_[2], &joint_values_[1]}; - hardware_interface::StateInterface joint_3_eff_state_{ - joint_names_[2], interface_names_[2], &joint_values_[2]}; - - hardware_interface::StateInterface joint_X_custom_state{ - joint_names_[0], custom_interface_name_, &custom_joint_value_}; + std::shared_ptr joint_1_pos_state_ = + std::make_shared( + joint_names_[0], interface_names_[0], &joint_values_[0]); + std::shared_ptr joint_2_pos_state_ = + std::make_shared( + joint_names_[1], interface_names_[0], &joint_values_[1]); + std::shared_ptr joint_3_pos_state_ = + std::make_shared( + joint_names_[2], interface_names_[0], &joint_values_[2]); + std::shared_ptr joint_1_vel_state_ = + std::make_shared( + joint_names_[0], interface_names_[1], &joint_values_[0]); + std::shared_ptr joint_2_vel_state_ = + std::make_shared( + joint_names_[1], interface_names_[1], &joint_values_[1]); + std::shared_ptr joint_3_vel_state_ = + std::make_shared( + joint_names_[2], interface_names_[1], &joint_values_[2]); + std::shared_ptr joint_1_eff_state_ = + std::make_shared( + joint_names_[0], interface_names_[2], &joint_values_[0]); + std::shared_ptr joint_2_eff_state_ = + std::make_shared( + joint_names_[1], interface_names_[2], &joint_values_[1]); + std::shared_ptr joint_3_eff_state_ = + std::make_shared( + joint_names_[2], interface_names_[2], &joint_values_[2]); + + std::shared_ptr joint_X_custom_state = + std::make_shared( + joint_names_[0], custom_interface_name_, &custom_joint_value_); std::unique_ptr state_broadcaster_; }; diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index dfbdf7436e..77b0e5ccf7 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -26,7 +26,7 @@ #include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "control_msgs/srv/query_trajectory_state.hpp" #include "control_toolbox/pid.hpp" -#include "controller_interface/controller_interface.hpp" +#include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/tolerances.hpp" @@ -64,7 +64,7 @@ namespace joint_trajectory_controller { class Trajectory; -class JointTrajectoryController : public controller_interface::ControllerInterface +class JointTrajectoryController : public controller_interface::ChainableControllerInterface { public: JOINT_TRAJECTORY_CONTROLLER_PUBLIC @@ -84,10 +84,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; @@ -116,6 +112,20 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const rclcpp_lifecycle::State & previous_state) override; protected: + // internal reference values + std::vector> position_reference_; + std::vector> velocity_reference_; + std::vector> acceleration_reference_; + std::vector> effort_reference_; + std::vector joint_names_; + std::vector last_references_ = {}; + bool first_time_ = true; + rclcpp::Time last_time_; + + std::vector on_export_reference_interfaces() override; + + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; // To reduce number of variables and to make the code shorter the interfaces are ordered in types // as the following constants const std::vector allowed_interface_types_ = { @@ -125,6 +135,26 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa hardware_interface::HW_IF_EFFORT, }; + bool reference_changed(); + + template + void copy_reference_interfaces_values( + std::vector & msg_container, std::vector> & reference_input); + + /** + * If controller is in chained mode we create new JointTrajectory msgs + * from the reference interface input. + * If controller is NOT in chained mode we get the JointTrajectory msg + * from topic. + * + * For detailed explanation on this have a look at the update_reference_from_subscribers + * method which normally would handle such task + */ + void update_joint_trajectory_point_from_input(const rclcpp::Time & time); + + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + // Preallocate variables used in the realtime update() function trajectory_msgs::msg::JointTrajectoryPoint state_current_; trajectory_msgs::msg::JointTrajectoryPoint command_current_; diff --git a/joint_trajectory_controller/joint_trajectory_plugin.xml b/joint_trajectory_controller/joint_trajectory_plugin.xml index 27930380ea..a9f0647a60 100644 --- a/joint_trajectory_controller/joint_trajectory_plugin.xml +++ b/joint_trajectory_controller/joint_trajectory_plugin.xml @@ -1,5 +1,5 @@ - + The joint trajectory controller executes joint-space trajectories on a set of joints diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 07080bb1fc..0d7661cd59 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -43,10 +43,11 @@ #include "rclcpp_lifecycle/state.hpp" #include "std_msgs/msg/header.hpp" +#include "rclcpp/rclcpp.hpp" namespace joint_trajectory_controller { JointTrajectoryController::JointTrajectoryController() -: controller_interface::ControllerInterface(), dof_(0) +: controller_interface::ChainableControllerInterface(), dof_(0) { } @@ -108,264 +109,6 @@ JointTrajectoryController::state_interface_configuration() const return conf; } -controller_interface::return_type JointTrajectoryController::update( - const rclcpp::Time & time, const rclcpp::Duration & period) -{ - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - return controller_interface::return_type::OK; - } - - auto compute_error_for_joint = [&]( - JointTrajectoryPoint & error, int index, - const JointTrajectoryPoint & current, - const JointTrajectoryPoint & desired) - { - // error defined as the difference between current and desired - if (normalize_joint_error_[index]) - { - // if desired, the shortest_angular_distance is calculated, i.e., the error is - // normalized between -piget_trajectory_msg(); - auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); - if (current_external_msg != *new_external_msg) - { - fill_partial_goal(*new_external_msg); - sort_to_local_joint_order(*new_external_msg); - // TODO(denis): Add here integration of position and velocity - traj_external_point_ptr_->update(*new_external_msg); - // set the active trajectory pointer to the new goal - traj_point_active_ptr_ = &traj_external_point_ptr_; - } - - // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not - // changed, but its value only? - auto assign_interface_from_point = - [&](auto & joint_interface, const std::vector & trajectory_point_interface) - { - for (size_t index = 0; index < dof_; ++index) - { - joint_interface[index].get().set_value(trajectory_point_interface[index]); - } - }; - - // current state update - state_current_.time_from_start.set__sec(0); - read_state_from_hardware(state_current_); - - // currently carrying out a trajectory - if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) - { - bool first_sample = false; - // if sampling the first time, set the point before you sample - if (!(*traj_point_active_ptr_)->is_sampled_already()) - { - first_sample = true; - if (params_.open_loop_control) - { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, last_commanded_state_); - } - else - { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, state_current_); - } - } - - // find segment for current timestamp - TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = - (*traj_point_active_ptr_) - ->sample(time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); - - if (valid_point) - { - bool tolerance_violated_while_moving = false; - bool outside_goal_tolerance = false; - bool within_goal_time = true; - double time_difference = 0.0; - const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); - - // Check state/goal tolerance - for (size_t index = 0; index < dof_; ++index) - { - compute_error_for_joint(state_error_, index, state_current_, state_desired_); - - // Always check the state tolerance on the first sample in case the first sample - // is the last point - if ( - (before_last_point || first_sample) && - !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.state_tolerance[index], false)) - { - tolerance_violated_while_moving = true; - } - // past the final point, check that we end up inside goal tolerance - if ( - !before_last_point && - !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], false)) - { - outside_goal_tolerance = true; - - if (default_tolerances_.goal_time_tolerance != 0.0) - { - // if we exceed goal_time_tolerance set it to aborted - const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); - const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; - - time_difference = get_node()->now().seconds() - traj_end.seconds(); - - if (time_difference > default_tolerances_.goal_time_tolerance) - { - within_goal_time = false; - } - } - } - } - - // set values for next hardware write() if tolerance is met - if (!tolerance_violated_while_moving && within_goal_time) - { - if (use_closed_loop_pid_adapter_) - { - // Update PIDs - for (auto i = 0ul; i < dof_; ++i) - { - tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + - pids_[i]->computeCommand( - state_error_.positions[i], state_error_.velocities[i], - (uint64_t)period.nanoseconds()); - } - } - - // set values for next hardware write() - if (has_position_command_interface_) - { - assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); - } - if (has_velocity_command_interface_) - { - if (use_closed_loop_pid_adapter_) - { - assign_interface_from_point(joint_command_interface_[1], tmp_command_); - } - else - { - assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); - } - } - if (has_acceleration_command_interface_) - { - assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); - } - if (has_effort_command_interface_) - { - if (use_closed_loop_pid_adapter_) - { - assign_interface_from_point(joint_command_interface_[3], tmp_command_); - } - else - { - assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); - } - } - - // store the previous command. Used in open-loop control mode - last_commanded_state_ = state_desired_; - } - - const auto active_goal = *rt_active_goal_.readFromRT(); - if (active_goal) - { - // send feedback - auto feedback = std::make_shared(); - feedback->header.stamp = time; - feedback->joint_names = params_.joints; - - feedback->actual = state_current_; - feedback->desired = state_desired_; - feedback->error = state_error_; - active_goal->setFeedback(feedback); - - // check abort - if (tolerance_violated_while_moving) - { - set_hold_position(); - auto result = std::make_shared(); - - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); - result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); - active_goal->setAborted(result); - // TODO(matthew-reynolds): Need a lock-free write here - // See https://github.com/ros-controls/ros2_controllers/issues/168 - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - // remove the active trajectory pointer so that we stop commanding the hardware - traj_point_active_ptr_ = nullptr; - - // check goal tolerance - } - else if (!before_last_point) - { - if (!outside_goal_tolerance) - { - auto res = std::make_shared(); - res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); - active_goal->setSucceeded(res); - // TODO(matthew-reynolds): Need a lock-free write here - // See https://github.com/ros-controls/ros2_controllers/issues/168 - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - // remove the active trajectory pointer so that we stop commanding the hardware - traj_point_active_ptr_ = nullptr; - - RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); - } - else if (!within_goal_time) - { - set_hold_position(); - auto result = std::make_shared(); - result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); - active_goal->setAborted(result); - // TODO(matthew-reynolds): Need a lock-free write here - // See https://github.com/ros-controls/ros2_controllers/issues/168 - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - RCLCPP_WARN( - get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", - time_difference); - } - // else, run another cycle while waiting for outside_goal_tolerance - // to be satisfied or violated within the goal_time_tolerance - } - } - else if (tolerance_violated_while_moving) - { - set_hold_position(); - RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); - } - } - } - - publish_state(time, state_desired_, state_current_, state_error_); - return controller_interface::return_type::OK; -} - void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) { auto assign_point_from_interface = @@ -1021,16 +764,449 @@ controller_interface::CallbackReturn JointTrajectoryController::on_shutdown( return CallbackReturn::SUCCESS; } -void JointTrajectoryController::publish_state( - const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, - const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) +std::vector +JointTrajectoryController::on_export_reference_interfaces() { - if (state_publisher_->trylock()) - { - state_publisher_->msg_.header.stamp = time; - state_publisher_->msg_.reference.positions = desired_state.positions; - state_publisher_->msg_.reference.velocities = desired_state.velocities; - state_publisher_->msg_.reference.accelerations = desired_state.accelerations; + // TODO(Manuel) : only for fast poc export every available command_interface + // as chainable + std::vector chainable_command_interfaces; + const auto command_interfaces = params_.command_interfaces; + const auto joints = params_.joints; + const auto num_chainable_interfaces = joints.size() * command_interfaces.size(); + + // allocate dynamic memory + chainable_command_interfaces.reserve(num_chainable_interfaces); + reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); + + position_reference_ = {}; + velocity_reference_ = {}; + acceleration_reference_ = {}; + effort_reference_ = {}; + joint_names_ = {}; + + // assign reference interfaces + auto index = 0ul; + for (const auto & interface : command_interfaces) + { + for (const auto & joint : joints) + { + joint_names_.push_back(joint); + if (hardware_interface::HW_IF_POSITION == interface) + position_reference_.emplace_back(reference_interfaces_[index]); + else if (hardware_interface::HW_IF_VELOCITY == interface) + { + velocity_reference_.emplace_back(reference_interfaces_[index]); + } + else if (hardware_interface::HW_IF_ACCELERATION == interface) + { + acceleration_reference_.emplace_back(reference_interfaces_[index]); + } + else if (hardware_interface::HW_IF_EFFORT == interface) + { + effort_reference_.emplace_back(reference_interfaces_[index]); + } + const auto full_name = joint + "/" + interface; + chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface( + std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index)); + + ++index; + } + } + + return chainable_command_interfaces; +} + +controller_interface::return_type JointTrajectoryController::update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // Updating the reference interface from the subscriber doesn't make much sense for jtc in my opinion. + // This is due to the fact that a JointTrajectory msg can have multiple JointTrajectoryPoint points + // This way we would need to have multi dimensional reference interface for each interface type: + // ( position, velocity ,...). + // E.g. msg1{[JointTrajectoryPoint 1, JointTrajectoryPoint 2, ... ,JointTrajectoryPoint n]}. + // - JointTrajectoryPoint 1{position 1, velocity 1} + // .... + // - JointTrajectoryPoint n{position n, velocity n} + // => reference interface for position would need to get passed position 1 - position n. + // It is easier to check in the update_and_write_commands() if we are currently in chained mode or not. + // if we are in chained mode then create a JointTrajectory msg and do calculation like in external mode. + + //****************************************************************************************************** + // Code example to make the problem clear: * + // note not working since this example would only pass contents of JointTrajectoryPoint n. * + //****************************************************************************************************** + + // auto external_joint_trajectory = traj_msg_external_point_ptr_.readFromRT(); + // if (!external_joint_trajectory->get()) + // { + // // Is this correct? + // return controller_interface::return_type::ERROR; + // } + // for (const auto point : external_joint_trajectory->get()->points) + // { + // for (size_t i = 0; i < point.positions.size(); ++i) + // { + // { + // position_reference_[i].get() = point.positions[i]; + // } + // for (size_t i = 0; i < point.velocities.size(); ++i) + // { + // velocity_reference_[i].get() = point.velocities[i]; + // } + // for (size_t i = 0; i < point.accelerations.size(); ++i) + // { + // acceleration_reference_[i].get() = point.velocities[i]; + // } + // for (size_t i = 0; i < point.effort.size(); ++i) + // { + // effort_reference_[i].get() = point.velocities[i]; + // } + // } + // } + + return controller_interface::return_type::OK; +} + +template +void JointTrajectoryController::copy_reference_interfaces_values( + std::vector & msg_container, std::vector> & reference_input) +{ + msg_container.resize(reference_input.size()); + std::transform( + reference_input.begin(), reference_input.end(), msg_container.begin(), + [](const std::reference_wrapper & ref) { return ref.get(); }); +} + +// TODO(Manuel): just for testing should not be merged, not a good solution +bool JointTrajectoryController::reference_changed() +{ + if (last_references_.size() != reference_interfaces_.size()) + { + last_references_ = reference_interfaces_; + return true; + } + for (int i = 0; i < last_references_.size(); ++i) + { + if (last_references_[i] != reference_interfaces_[i]) + { + last_references_ = reference_interfaces_; + return true; + } + } + return false; +} + +void JointTrajectoryController::update_joint_trajectory_point_from_input(const rclcpp::Time & time) +{ + std::shared_ptr current_external_msg = + traj_external_point_ptr_->get_trajectory_msg(); + std::shared_ptr new_trajectory_msg; + if (is_in_chained_mode()) + { + if (reference_changed()) + { + // if we are in chained mode we create a trajectory msg from the reference + // interface input. For background on why look at the comment in the + // update_reference_from_subscribers(...) function + auto trajectory_point = trajectory_msgs::msg::JointTrajectoryPoint(); + auto duration = builtin_interfaces::msg::Duration(); + duration.sec = 2; + trajectory_point.time_from_start = duration; + + copy_reference_interfaces_values(trajectory_point.positions, position_reference_); + copy_reference_interfaces_values(trajectory_point.velocities, velocity_reference_); + copy_reference_interfaces_values(trajectory_point.accelerations, acceleration_reference_); + copy_reference_interfaces_values(trajectory_point.effort, effort_reference_); + + std::shared_ptr trajectory = + std::make_shared(); + + trajectory->joint_names = joint_names_; + trajectory->points.push_back(trajectory_point); + new_trajectory_msg = trajectory; + + fill_partial_goal(new_trajectory_msg); + sort_to_local_joint_order(new_trajectory_msg); + traj_external_point_ptr_->update(new_trajectory_msg); + // set the active trajectory pointer to the new goal + traj_point_active_ptr_ = &traj_external_point_ptr_; + } + } + else + { + // if not in chained mode + // get new JointTrajectory msg from external subscription + new_trajectory_msg = *(traj_msg_external_point_ptr_.readFromRT()); + // Check if a new external message has been received from nonRT threads + if (current_external_msg != new_trajectory_msg) + { + fill_partial_goal(new_trajectory_msg); + sort_to_local_joint_order(new_trajectory_msg); + // TODO(denis): Add here integration of position and velocity + traj_external_point_ptr_->update(new_trajectory_msg); + // set the active trajectory pointer to the new goal + traj_point_active_ptr_ = &traj_external_point_ptr_; + } + } +} + +controller_interface::return_type JointTrajectoryController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + return controller_interface::return_type::OK; + } + + auto compute_error_for_joint = [&]( + JointTrajectoryPoint & error, int index, + const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) + { + // error defined as the difference between current and desired + if (normalize_joint_error_[index]) + { + // if desired, the shortest_angular_distance is calculated, i.e., the error is + // normalized between -pi & trajectory_point_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + joint_interface[index].get().set_value(trajectory_point_interface[index]); + } + }; + + // current state update + state_current_.time_from_start.set__sec(0); + read_state_from_hardware(state_current_); + + // currently carrying out a trajectory + if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) + { + bool first_sample = false; + // if sampling the first time, set the point before you sample + if (!(*traj_point_active_ptr_)->is_sampled_already()) + { + first_sample = true; + if (params_.open_loop_control) + { + (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, last_commanded_state_); + } + else + { + (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, state_current_); + } + } + + // find segment for current timestamp + TrajectoryPointConstIter start_segment_itr, end_segment_itr; + const bool valid_point = + (*traj_point_active_ptr_) + ->sample(time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + + if (valid_point) + { + bool tolerance_violated_while_moving = false; + bool outside_goal_tolerance = false; + bool within_goal_time = true; + double time_difference = 0.0; + const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); + + // Check state/goal tolerance + for (size_t index = 0; index < dof_; ++index) + { + compute_error_for_joint(state_error_, index, state_current_, state_desired_); + + // Always check the state tolerance on the first sample in case the first sample + // is the last point + if ( + (before_last_point || first_sample) && + !check_state_tolerance_per_joint( + state_error_, index, default_tolerances_.state_tolerance[index], false)) + { + tolerance_violated_while_moving = true; + } + // past the final point, check that we end up inside goal tolerance + if ( + !before_last_point && + !check_state_tolerance_per_joint( + state_error_, index, default_tolerances_.goal_state_tolerance[index], false)) + { + outside_goal_tolerance = true; + + if (default_tolerances_.goal_time_tolerance != 0.0) + { + // if we exceed goal_time_tolerance set it to aborted + const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); + const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; + + time_difference = get_node()->now().seconds() - traj_end.seconds(); + + if (time_difference > default_tolerances_.goal_time_tolerance) + { + within_goal_time = false; + } + } + } + } + + // set values for next hardware write() if tolerance is met + if (!tolerance_violated_while_moving && within_goal_time) + { + if (use_closed_loop_pid_adapter_) + { + // Update PIDs + for (auto i = 0ul; i < dof_; ++i) + { + tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + + pids_[i]->computeCommand( + state_error_.positions[i], state_error_.velocities[i], + (uint64_t)period.nanoseconds()); + } + } + + // set values for next hardware write() + if (has_position_command_interface_) + { + assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); + } + if (has_velocity_command_interface_) + { + if (use_closed_loop_pid_adapter_) + { + assign_interface_from_point(joint_command_interface_[1], tmp_command_); + } + else + { + assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); + } + } + if (has_acceleration_command_interface_) + { + assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); + } + if (has_effort_command_interface_) + { + if (use_closed_loop_pid_adapter_) + { + assign_interface_from_point(joint_command_interface_[3], tmp_command_); + } + else + { + assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); + } + } + + // store the previous command. Used in open-loop control mode + last_commanded_state_ = state_desired_; + } + + const auto active_goal = *rt_active_goal_.readFromRT(); + if (active_goal) + { + // send feedback + auto feedback = std::make_shared(); + feedback->header.stamp = time; + feedback->joint_names = params_.joints; + + feedback->actual = state_current_; + feedback->desired = state_desired_; + feedback->error = state_error_; + active_goal->setFeedback(feedback); + + // check abort + if (tolerance_violated_while_moving) + { + set_hold_position(); + auto result = std::make_shared(); + + RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); + result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); + active_goal->setAborted(result); + // TODO(matthew-reynolds): Need a lock-free write here + // See https://github.com/ros-controls/ros2_controllers/issues/168 + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + // remove the active trajectory pointer so that we stop commanding the hardware + traj_point_active_ptr_ = nullptr; + + // check goal tolerance + } + else if (!before_last_point) + { + if (!outside_goal_tolerance) + { + auto res = std::make_shared(); + res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); + active_goal->setSucceeded(res); + // TODO(matthew-reynolds): Need a lock-free write here + // See https://github.com/ros-controls/ros2_controllers/issues/168 + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + // remove the active trajectory pointer so that we stop commanding the hardware + traj_point_active_ptr_ = nullptr; + + RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); + } + else if (!within_goal_time) + { + set_hold_position(); + auto result = std::make_shared(); + result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); + active_goal->setAborted(result); + // TODO(matthew-reynolds): Need a lock-free write here + // See https://github.com/ros-controls/ros2_controllers/issues/168 + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + RCLCPP_WARN( + get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", + time_difference); + } + // else, run another cycle while waiting for outside_goal_tolerance + // to be satisfied or violated within the goal_time_tolerance + } + } + else if (tolerance_violated_while_moving) + { + set_hold_position(); + RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); + } + } + } + + publish_state(time, state_desired_, state_current_, state_error_); + return controller_interface::return_type::OK; +} + +void JointTrajectoryController::publish_state( + const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, + const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) +{ + if (state_publisher_->trylock()) + { + state_publisher_->msg_.header.stamp = time; + state_publisher_->msg_.reference.positions = desired_state.positions; + state_publisher_->msg_.reference.velocities = desired_state.velocities; + state_publisher_->msg_.reference.accelerations = desired_state.accelerations; state_publisher_->msg_.feedback.positions = current_state.positions; state_publisher_->msg_.error.positions = state_error.positions; if (has_velocity_state_interface_) @@ -1206,8 +1382,8 @@ void JointTrajectoryController::sort_to_local_joint_order( // rearrange all points in the trajectory message based on mapping std::vector mapping_vector = mapping(trajectory_msg->joint_names, params_.joints); auto remap = [this]( - const std::vector & to_remap, - const std::vector & mapping) -> std::vector + const std::vector & to_remap, const std::vector & mapping, + const std::string & type) -> std::vector { if (to_remap.empty()) { @@ -1216,7 +1392,9 @@ void JointTrajectoryController::sort_to_local_joint_order( if (to_remap.size() != mapping.size()) { RCLCPP_WARN( - get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); + get_node()->get_logger(), + "Invalid input size (%zu) for sorting with mapping size (%zu) for %s", to_remap.size(), + mapping.size(), type.c_str()); return to_remap; } std::vector output; @@ -1232,16 +1410,16 @@ void JointTrajectoryController::sort_to_local_joint_order( for (size_t index = 0; index < trajectory_msg->points.size(); ++index) { trajectory_msg->points[index].positions = - remap(trajectory_msg->points[index].positions, mapping_vector); + remap(trajectory_msg->points[index].positions, mapping_vector, "positions"); trajectory_msg->points[index].velocities = - remap(trajectory_msg->points[index].velocities, mapping_vector); + remap(trajectory_msg->points[index].velocities, mapping_vector, "velocities"); trajectory_msg->points[index].accelerations = - remap(trajectory_msg->points[index].accelerations, mapping_vector); + remap(trajectory_msg->points[index].accelerations, mapping_vector, "accelerations"); trajectory_msg->points[index].effort = - remap(trajectory_msg->points[index].effort, mapping_vector); + remap(trajectory_msg->points[index].effort, mapping_vector, "effort"); } } @@ -1444,4 +1622,5 @@ void JointTrajectoryController::resize_joint_trajectory_point_command( #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - joint_trajectory_controller::JointTrajectoryController, controller_interface::ControllerInterface) + joint_trajectory_controller::JointTrajectoryController, + controller_interface::ChainableControllerInterface) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index c85eb59f58..df1cc20fac 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -228,22 +228,22 @@ class TrajectoryControllerTest : public ::testing::Test acc_state_interfaces_.reserve(joint_names_.size()); for (size_t i = 0; i < joint_names_.size(); ++i) { - pos_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + pos_cmd_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_POSITION, &joint_pos_[i])); - vel_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + vel_cmd_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_VELOCITY, &joint_vel_[i])); - acc_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + acc_cmd_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_ACCELERATION, &joint_acc_[i])); - eff_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + eff_cmd_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_EFFORT, &joint_eff_[i])); - pos_state_interfaces_.emplace_back(hardware_interface::StateInterface( + pos_state_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_POSITION, separate_cmd_and_state_values ? &joint_state_pos_[i] : &joint_pos_[i])); - vel_state_interfaces_.emplace_back(hardware_interface::StateInterface( + vel_state_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_VELOCITY, separate_cmd_and_state_values ? &joint_state_vel_[i] : &joint_vel_[i])); - acc_state_interfaces_.emplace_back(hardware_interface::StateInterface( + acc_state_interfaces_.emplace_back(std::make_shared( joint_names_[i], hardware_interface::HW_IF_ACCELERATION, separate_cmd_and_state_values ? &joint_state_acc_[i] : &joint_acc_[i])); @@ -433,13 +433,13 @@ class TrajectoryControllerTest : public ::testing::Test std::vector joint_state_pos_; std::vector joint_state_vel_; std::vector joint_state_acc_; - std::vector pos_cmd_interfaces_; - std::vector vel_cmd_interfaces_; - std::vector acc_cmd_interfaces_; - std::vector eff_cmd_interfaces_; - std::vector pos_state_interfaces_; - std::vector vel_state_interfaces_; - std::vector acc_state_interfaces_; + std::vector> pos_cmd_interfaces_; + std::vector> vel_cmd_interfaces_; + std::vector> acc_cmd_interfaces_; + std::vector> eff_cmd_interfaces_; + std::vector> pos_state_interfaces_; + std::vector> vel_state_interfaces_; + std::vector> acc_state_interfaces_; }; // From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index a712cc81c2..163f2b710b 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -119,9 +119,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -134,9 +134,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30.0); } TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) @@ -156,9 +156,9 @@ TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); } TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) @@ -173,9 +173,9 @@ TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); } TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) @@ -184,9 +184,9 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -214,7 +214,7 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_->get_value(), 30.0); } diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp index 93149d8e19..152c73bf23 100644 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ b/position_controllers/test/test_joint_group_position_controller.hpp @@ -55,9 +55,12 @@ class JointGroupPositionControllerTest : public ::testing::Test const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; - CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; - CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + std::shared_ptr joint_1_pos_cmd_ = + std::make_shared(joint_names_[0], HW_IF_POSITION, &joint_commands_[0]); + std::shared_ptr joint_2_pos_cmd_ = + std::make_shared(joint_names_[1], HW_IF_POSITION, &joint_commands_[1]); + std::shared_ptr joint_3_pos_cmd_ = + std::make_shared(joint_names_[2], HW_IF_POSITION, &joint_commands_[2]); }; #endif // TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/move_lin.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/move_lin.py new file mode 100644 index 0000000000..3b9e4ae206 --- /dev/null +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/move_lin.py @@ -0,0 +1,214 @@ +# Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Authors: Manuel Muth +# + + +import rclpy +from rclpy.node import Node + +import yaml +from std_msgs.msg import Header +from builtin_interfaces.msg import Duration +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from sensor_msgs.msg import JointState + + +class MoveLin(Node): + def __init__(self): + super().__init__("move_lin") + # Declare all parameters + self.declare_parameter("controller_name", "position_trajectory_controller") + self.declare_parameter("wait_sec_between_publish", 5) + self.declare_parameter("joints", [""]) + self.declare_parameter("goal_names", ["traj1", "traj2"]) + self.declare_parameter("check_starting_point", False) + self.declare_parameter("duplicate", False) + + # Read parameters + wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value + goal_names = self.get_parameter("goal_names").value + controller_name = self.get_parameter("controller_name").value + self.joints = self.get_parameter("joints").value + self.check_starting_point = self.get_parameter("check_starting_point").value + self.duplicate = self.get_parameter("duplicate").value + self.starting_point = {} + self.trajectories = [] + + # starting point stuff + if self.check_starting_point: + # declare nested params + for name in self.joints: + param_name_tmp = "starting_point_limits" + "." + name + self.declare_parameter(param_name_tmp, [-2 * 3.14159, 2 * 3.14159]) + self.starting_point[name] = self.get_parameter(param_name_tmp).value + + for name in self.joints: + if len(self.starting_point[name]) != 2: + raise Exception('"starting_point" parameter is not set correctly!') + self.joint_state_sub = self.create_subscription( + JointState, "joint_states", self.joint_state_callback, 10 + ) + # initialize starting point status + self.starting_point_ok = not self.check_starting_point + + self.joint_state_msg_received = False + + # Read all positions from parameters + self.goals = "" + for name in goal_names: + self.declare_parameter(name) + goal = self.get_parameter(name).value + if goal is None: + raise Exception(f'Values for goal "{name}" not set!') + + print(f"goal path:{goal}") + self.trajectories.append(self.get_traj(goal)) + self.trajectories.append(self.get_traj_reverted(goal)) + # float_goal = [float(value) for value in goal] + # self.goals.append(float_goal) + + publish_topic = "/" + controller_name + "/" + "joint_trajectory" + self.get_logger().info( + f'Publishing {len(goal_names)} goals on topic "{publish_topic}"\ + every {wait_sec_between_publish} s' + ) + + self.get_logger().info(f"Publishing on topic:{publish_topic}") + self.publisher_ = self.create_publisher(JointTrajectory, publish_topic, 1) + self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback) + self.i = 0 + + def timer_callback(self): + + if self.starting_point_ok: + + self.get_logger().info( + f"Sending trajectory for joints{self.trajectories[self.i].joint_names}." + ) + + self.publisher_.publish(self.trajectories[self.i]) + + self.i += 1 + self.i %= len(self.trajectories) + + elif self.check_starting_point and not self.joint_state_msg_received: + self.get_logger().warn( + 'Start configuration could not be checked! Check "joint_state" topic!' + ) + else: + self.get_logger().warn( + "Start configuration is not within configured limits!" + ) + + def get_traj(self, traj_path): + with open(traj_path) as file: + file_node = yaml.load(file, Loader=yaml.FullLoader) + + traj_msg = JointTrajectory() + traj_node = file_node["trajectory"] + + traj_msg.header = Header() + traj_msg.header.frame_id = traj_node["header"]["frame_id"] + traj_msg.header.stamp.sec = traj_node["header"]["stamp"]["secs"] + traj_msg.header.stamp.nanosec = traj_node["header"]["stamp"]["nanosec"] + + for joint_name in self.joints: + traj_msg.joint_names.append(joint_name) + + for i in range(len(traj_node["points"])): + point_node = traj_node["points"][i] + + point = JointTrajectoryPoint() + if self.duplicate: + point.positions = point_node["positions"] + point_node["positions"] + point.velocities = point_node["velocities"] + point_node["velocities"] + point.accelerations = ( + point_node["accelerations"] + point_node["accelerations"] + ) + point.effort = point_node["effort"] + point_node["effort"] + else: + point.positions = point_node["positions"] + point.velocities = point_node["velocities"] + point.accelerations = point_node["accelerations"] + point.effort = point_node["effort"] + + point.time_from_start = Duration() + point.time_from_start.sec = point_node["time_from_start"]["secs"] + point.time_from_start.nanosec = point_node["time_from_start"]["nanosec"] + + traj_msg.points.append(point) + + return traj_msg + + def get_traj_reverted(self, traj_path): + with open(traj_path) as file: + file_node = yaml.load(file, Loader=yaml.FullLoader) + + traj_msg = JointTrajectory() + traj_node = file_node["trajectory"] + + traj_msg.header = Header() + traj_msg.header.frame_id = traj_node["header"]["frame_id"] + traj_msg.header.stamp.sec = traj_node["header"]["stamp"]["secs"] + traj_msg.header.stamp.nanosec = traj_node["header"]["stamp"]["nanosec"] + + for joint_name in self.joints: + traj_msg.joint_names.append(joint_name) + + for i in range(len(traj_node["points"])): + point_node = traj_node["points"][i] + point_node_revert = traj_node["points"][len(traj_node["points"]) - 1 - i] + + point = JointTrajectoryPoint() + if self.duplicate: + point.positions = ( + point_node_revert["positions"] + point_node_revert["positions"] + ) + point.velocities = ( + point_node_revert["velocities"] + point_node_revert["velocities"] + ) + point.accelerations = ( + point_node_revert["accelerations"] + + point_node_revert["accelerations"] + ) + point.effort = point_node_revert["effort"] + point_node_revert["effort"] + else: + point.positions = point_node_revert["positions"] + point.velocities = point_node_revert["velocities"] + point.accelerations = point_node_revert["accelerations"] + point.effort = point_node_revert["effort"] + + point.time_from_start = Duration() + point.time_from_start.sec = point_node["time_from_start"]["secs"] + point.time_from_start.nanosec = point_node["time_from_start"]["nanosec"] + + traj_msg.points.append(point) + + return traj_msg + + +def main(args=None): + rclpy.init(args=args) + + publisher_forward_position = MoveLin() + + rclpy.spin(publisher_forward_position) + publisher_forward_position.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index b8be85a628..8385cf78c6 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -52,6 +52,8 @@ ros2_controllers_test_nodes.publisher_forward_position_controller:main", "publisher_joint_trajectory_controller = \ ros2_controllers_test_nodes.publisher_joint_trajectory_controller:main", + "move_lin = \ + ros2_controllers_test_nodes.move_lin:main", ], }, ) diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index a1f09ddaf8..53fb1c41d6 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -153,17 +153,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_}; + std::shared_ptr steering_joint_pos_state_ = + std::make_shared( + steering_joint_name, HW_IF_POSITION, &position_); - hardware_interface::StateInterface traction_joint_vel_state_{ - traction_joint_name, HW_IF_VELOCITY, &velocity_}; + std::shared_ptr traction_joint_vel_state_ = + std::make_shared( + traction_joint_name, HW_IF_VELOCITY, &velocity_); - hardware_interface::CommandInterface steering_joint_pos_cmd_{ - steering_joint_name, HW_IF_POSITION, &position_}; + std::shared_ptr steering_joint_pos_cmd_ = + std::make_shared( + steering_joint_name, HW_IF_POSITION, &position_); - hardware_interface::CommandInterface traction_joint_vel_cmd_{ - traction_joint_name, HW_IF_VELOCITY, &velocity_}; + std::shared_ptr traction_joint_vel_cmd_ = + std::make_shared( + traction_joint_name, HW_IF_VELOCITY, &velocity_); rclcpp::Node::SharedPtr pub_node; rclcpp::Publisher::SharedPtr velocity_publisher; @@ -282,8 +286,8 @@ TEST_F(TestTricycleController, cleanup) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_->get_value()); executor.cancel(); } @@ -307,8 +311,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) assignResources(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(position_, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(position_, steering_joint_pos_cmd_->get_value()); + EXPECT_EQ(velocity_, traction_joint_vel_cmd_->get_value()); state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -323,8 +327,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_value()); + EXPECT_EQ(1.0, traction_joint_vel_cmd_->get_value()); // deactivated // wait so controller process the second point when deactivated @@ -335,14 +339,14 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()"; - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, traction_joint_vel_cmd_->get_value()) << "Wheels are halted on deactivate()"; // cleanup state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_->get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_->get_value()); state = controller_->get_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 43c1545461..8d350ec201 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -119,9 +119,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -134,9 +134,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 30.0); } TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) @@ -156,9 +156,9 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); } TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) @@ -173,9 +173,9 @@ TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); } TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) @@ -184,9 +184,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -214,9 +214,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 30.0); } TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) @@ -228,15 +228,15 @@ TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_->get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_->get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_->get_value(), 3.1); // stop the controller ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_1_cmd_->get_value(), 0.0); + ASSERT_EQ(joint_2_cmd_->get_value(), 0.0); + ASSERT_EQ(joint_3_cmd_->get_value(), 0.0); } diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp index e94f7f082d..32854085be 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.hpp @@ -55,9 +55,12 @@ class JointGroupVelocityControllerTest : public ::testing::Test const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]}; - CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]}; - CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]}; + std::shared_ptr joint_1_cmd_ = + std::make_shared(joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]); + std::shared_ptr joint_2_cmd_ = + std::make_shared(joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]); + std::shared_ptr joint_3_cmd_ = + std::make_shared(joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]); }; #endif // TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_