From 4bf1065a32acbf84083baf082bd3bea3ae19a8ee Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Mon, 9 Sep 2024 10:34:56 +0200 Subject: [PATCH] select JointTrajectoryControllerState fields based on 'control_msgs' version --- joint_trajectory_controller/CMakeLists.txt | 3 + .../src/joint_trajectory_controller.cpp | 36 ++++++ .../test/test_trajectory_controller.cpp | 109 ++++++++++++------ .../test/test_trajectory_controller_utils.hpp | 8 ++ 4 files changed, 123 insertions(+), 33 deletions(-) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index f557a138..fab7c0d1 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -55,6 +55,9 @@ ament_target_dependencies(joint_trajectory_controller PUBLIC ${THIS_PACKAGE_INCL target_compile_definitions(joint_trajectory_controller PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") pluginlib_export_plugin_description_file(controller_interface joint_trajectory_plugin.xml) +# forward 'control_msgs' major version to handle different API +target_compile_definitions(joint_trajectory_controller PUBLIC control_msgs_VERSION_MAJOR=${control_msgs_VERSION_MAJOR}) + if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index d74698ae..3e0d59b7 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -681,6 +681,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( state_publisher_->lock(); state_publisher_->msg_.joint_names = params_.joints; +#if control_msgs_VERSION_MAJOR == 4 state_publisher_->msg_.desired.positions.resize(dof_); state_publisher_->msg_.desired.velocities.resize(dof_); state_publisher_->msg_.desired.accelerations.resize(dof_); @@ -696,6 +697,23 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( state_publisher_->msg_.actual.accelerations.resize(dof_); state_publisher_->msg_.error.accelerations.resize(dof_); } +#elif control_msgs_VERSION_MAJOR >= 5 + state_publisher_->msg_.reference.positions.resize(dof_); + state_publisher_->msg_.reference.velocities.resize(dof_); + state_publisher_->msg_.reference.accelerations.resize(dof_); + state_publisher_->msg_.feedback.positions.resize(dof_); + state_publisher_->msg_.error.positions.resize(dof_); + if (has_velocity_state_interface_) + { + state_publisher_->msg_.feedback.velocities.resize(dof_); + state_publisher_->msg_.error.velocities.resize(dof_); + } + if (has_acceleration_state_interface_) + { + state_publisher_->msg_.feedback.accelerations.resize(dof_); + state_publisher_->msg_.error.accelerations.resize(dof_); + } +#endif state_publisher_->unlock(); last_state_publish_time_ = get_node()->now(); @@ -908,6 +926,7 @@ void JointTrajectoryController::publish_state( { last_state_publish_time_ = get_node()->now(); state_publisher_->msg_.header.stamp = last_state_publish_time_; +#if control_msgs_VERSION_MAJOR == 4 state_publisher_->msg_.desired.positions = desired_state.positions; state_publisher_->msg_.desired.velocities = desired_state.velocities; state_publisher_->msg_.desired.accelerations = desired_state.accelerations; @@ -923,6 +942,23 @@ void JointTrajectoryController::publish_state( state_publisher_->msg_.actual.accelerations = current_state.accelerations; state_publisher_->msg_.error.accelerations = state_error.accelerations; } +#elif control_msgs_VERSION_MAJOR >= 5 + 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_) + { + state_publisher_->msg_.feedback.velocities = current_state.velocities; + state_publisher_->msg_.error.velocities = state_error.velocities; + } + if (has_acceleration_state_interface_) + { + state_publisher_->msg_.feedback.accelerations = current_state.accelerations; + state_publisher_->msg_.error.accelerations = state_error.accelerations; + } +#endif state_publisher_->unlockAndPublish(); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 42e573ca..88b34a05 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -470,32 +470,51 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) EXPECT_EQ(joint_names_[i], state->joint_names[i]); } +#if control_msgs_VERSION_MAJOR == 4 + // desired | reference + const std::vector &ref_pos = state->desired.positions; + const std::vector &ref_vel = state->desired.velocities; + const std::vector &ref_acc = state->desired.accelerations; + // actual | feedback + const std::vector &fb_pos = state->actual.positions; + const std::vector &fb_vel = state->actual.velocities; + const std::vector &fb_acc = state->actual.accelerations; +#elif control_msgs_VERSION_MAJOR >= 5 + // desired | reference + const std::vector &ref_pos = state->reference.positions; + const std::vector &ref_vel = state->reference.velocities; + const std::vector &ref_acc = state->reference.accelerations; + // actual | feedback + const std::vector &fb_pos = state->feedback.positions; + const std::vector &fb_vel = state->feedback.velocities; + const std::vector &fb_acc = state->feedback.accelerations; +#endif + // No trajectory by default, no desired state or error - EXPECT_TRUE(state->desired.positions.empty() || state->desired.positions == INITIAL_POS_JOINTS); - EXPECT_TRUE(state->desired.velocities.empty() || state->desired.velocities == INITIAL_VEL_JOINTS); - EXPECT_TRUE( - state->desired.accelerations.empty() || state->desired.accelerations == INITIAL_EFF_JOINTS); + EXPECT_TRUE(ref_pos.empty() || ref_pos == INITIAL_POS_JOINTS); + EXPECT_TRUE(ref_vel.empty() || ref_vel == INITIAL_VEL_JOINTS); + EXPECT_TRUE(ref_acc.empty() || ref_acc == INITIAL_EFF_JOINTS); - EXPECT_EQ(n_joints, state->actual.positions.size()); + EXPECT_EQ(n_joints, fb_pos.size()); if ( std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == state_interface_types_.end()) { - EXPECT_TRUE(state->actual.velocities.empty()); + EXPECT_TRUE(fb_vel.empty()); } else { - EXPECT_EQ(n_joints, state->actual.velocities.size()); + EXPECT_EQ(n_joints, fb_vel.size()); } if ( std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == state_interface_types_.end()) { - EXPECT_TRUE(state->actual.accelerations.empty()); + EXPECT_TRUE(fb_acc.empty()); } else { - EXPECT_EQ(n_joints, state->actual.accelerations.size()); + EXPECT_EQ(n_joints, fb_acc.size()); } std::vector zeros(3, 0); @@ -542,26 +561,38 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) const auto allowed_delta = 0.1; +#if control_msgs_VERSION_MAJOR == 4 + // desired | reference + const std::vector &ref_pos = state_msg->desired.positions; + // actual | feedback + const std::vector &fb_pos = state_msg->actual.positions; +#elif control_msgs_VERSION_MAJOR >= 5 + // desired | reference + const std::vector &ref_pos = state_msg->reference.positions; + // actual | feedback + const std::vector &fb_pos = state_msg->feedback.positions; +#endif + // no update of state_interface - EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + EXPECT_EQ(fb_pos, INITIAL_POS_JOINTS); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->desired.positions.size()); - EXPECT_EQ(n_joints, state_msg->actual.positions.size()); + EXPECT_EQ(n_joints, ref_pos.size()); + EXPECT_EQ(n_joints, fb_pos.size()); EXPECT_EQ(n_joints, state_msg->error.positions.size()); // are the correct desired positions used? - EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta); + EXPECT_NEAR(points[0][0], ref_pos[0], allowed_delta); + EXPECT_NEAR(points[0][1], ref_pos[1], allowed_delta); + EXPECT_NEAR(points[0][2], ref_pos[2], 3 * allowed_delta); // no normalization of position error EXPECT_NEAR( - state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS); + state_msg->error.positions[0], ref_pos[0] - INITIAL_POS_JOINTS[0], EPS); EXPECT_NEAR( - state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + state_msg->error.positions[1], ref_pos[1] - INITIAL_POS_JOINTS[1], EPS); EXPECT_NEAR( - state_msg->error.positions[2], state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2], EPS); + state_msg->error.positions[2], ref_pos[2] - INITIAL_POS_JOINTS[2], EPS); if (traj_controller_->has_position_command_interface()) { @@ -583,14 +614,14 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) { // we expect u = k_p * (s_d-s) EXPECT_NEAR( - k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * (ref_pos[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], k_p * allowed_delta); EXPECT_NEAR( - k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * (ref_pos[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], k_p * allowed_delta); // no normalization of position error EXPECT_NEAR( - k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], + k_p * (ref_pos[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], k_p * allowed_delta); } } @@ -642,27 +673,39 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) const auto allowed_delta = 0.1; +#if control_msgs_VERSION_MAJOR == 4 + // desired | reference + const std::vector &ref_pos = state_msg->desired.positions; + // actual | feedback + const std::vector &fb_pos = state_msg->actual.positions; +#elif control_msgs_VERSION_MAJOR >= 5 + // desired | reference + const std::vector &ref_pos = state_msg->reference.positions; + // actual | feedback + const std::vector &fb_pos = state_msg->feedback.positions; +#endif + // no update of state_interface - EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + EXPECT_EQ(fb_pos, INITIAL_POS_JOINTS); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->desired.positions.size()); - EXPECT_EQ(n_joints, state_msg->actual.positions.size()); + EXPECT_EQ(n_joints, ref_pos.size()); + EXPECT_EQ(n_joints, fb_pos.size()); EXPECT_EQ(n_joints, state_msg->error.positions.size()); // are the correct desired positions used? - EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta); + EXPECT_NEAR(points[0][0], ref_pos[0], allowed_delta); + EXPECT_NEAR(points[0][1], ref_pos[1], allowed_delta); + EXPECT_NEAR(points[0][2], ref_pos[2], 3 * allowed_delta); // is error.positions[2] normalized? EXPECT_NEAR( - state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS); + state_msg->error.positions[0], ref_pos[0] - INITIAL_POS_JOINTS[0], EPS); EXPECT_NEAR( - state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + state_msg->error.positions[1], ref_pos[1] - INITIAL_POS_JOINTS[1], EPS); EXPECT_NEAR( state_msg->error.positions[2], - state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); + ref_pos[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); if (traj_controller_->has_position_command_interface()) { @@ -684,14 +727,14 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) EXPECT_GE(0.0, joint_vel_[2]); // we expect u = k_p * (s_d-s) for positions[0] and positions[1] EXPECT_NEAR( - k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * (ref_pos[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], k_p * allowed_delta); EXPECT_NEAR( - k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * (ref_pos[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], k_p * allowed_delta); // is error of positions[2] normalized? EXPECT_NEAR( - k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], + k_p * (ref_pos[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], k_p * allowed_delta); } else diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 25612f56..f164a5a3 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -388,7 +388,11 @@ class TrajectoryControllerTest : public ::testing::Test // TODO(anyone): add checking for velocties and accelerations if (traj_controller_->has_position_command_interface()) { +#if control_msgs_VERSION_MAJOR == 4 EXPECT_NEAR(expected_actual.positions[i], state_msg->actual.positions[i], allowed_delta); +#elif control_msgs_VERSION_MAJOR >= 5 + EXPECT_NEAR(expected_actual.positions[i], state_msg->feedback.positions[i], allowed_delta); +#endif } } @@ -398,7 +402,11 @@ class TrajectoryControllerTest : public ::testing::Test // TODO(anyone): add checking for velocties and accelerations if (traj_controller_->has_position_command_interface()) { +#if control_msgs_VERSION_MAJOR == 4 EXPECT_NEAR(expected_desired.positions[i], state_msg->desired.positions[i], allowed_delta); +#elif control_msgs_VERSION_MAJOR >= 5 + EXPECT_NEAR(expected_desired.positions[i], state_msg->reference.positions[i], allowed_delta); +#endif } } }