diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index fc223d2eb0..2370e9ca20 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -230,7 +230,7 @@ ROS2 interface of the controller ~/joint_trajectory (input topic) [trajectory_msgs::msg::JointTrajectory] Topic for commanding the controller. -~/state (output topic) [control_msgs::msg::JointTrajectoryControllerState] +~/controller_state (output topic) [control_msgs::msg::JointTrajectoryControllerState] Topic publishing internal states with the update-rate of the controller manager. ~/follow_joint_trajectory (action server) [control_msgs::action::FollowJointTrajectory] 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 2a6454f454..dfbdf7436e 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 @@ -127,6 +127,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Preallocate variables used in the realtime update() function trajectory_msgs::msg::JointTrajectoryPoint state_current_; + trajectory_msgs::msg::JointTrajectoryPoint command_current_; trajectory_msgs::msg::JointTrajectoryPoint state_desired_; trajectory_msgs::msg::JointTrajectoryPoint state_error_; @@ -256,6 +257,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void read_state_from_hardware(JointTrajectoryPoint & state); bool read_state_from_command_interfaces(JointTrajectoryPoint & state); + bool read_commands_from_command_interfaces(JointTrajectoryPoint & commands); void query_state_service( const std::shared_ptr request, @@ -267,6 +269,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void resize_joint_trajectory_point( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + void resize_joint_trajectory_point_command( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); }; } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5c719f7471..b6a60526fa 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -467,6 +467,81 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto return has_values; } +bool JointTrajectoryController::read_commands_from_command_interfaces( + JointTrajectoryPoint & commands) +{ + bool has_values = true; + + auto assign_point_from_interface = + [&](std::vector & trajectory_point_interface, const auto & joint_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); + } + }; + + auto interface_has_values = [](const auto & joint_interface) + { + return std::find_if( + joint_interface.begin(), joint_interface.end(), + [](const auto & interface) + { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); + }; + + // Assign values from the command interfaces as command. + if (has_position_command_interface_) + { + if (interface_has_values(joint_command_interface_[0])) + { + assign_point_from_interface(commands.positions, joint_command_interface_[0]); + } + else + { + commands.positions.clear(); + has_values = false; + } + } + if (has_velocity_command_interface_) + { + if (interface_has_values(joint_command_interface_[1])) + { + assign_point_from_interface(commands.velocities, joint_command_interface_[1]); + } + else + { + commands.velocities.clear(); + has_values = false; + } + } + if (has_acceleration_command_interface_) + { + if (interface_has_values(joint_command_interface_[2])) + { + assign_point_from_interface(commands.accelerations, joint_command_interface_[2]); + } + else + { + commands.accelerations.clear(); + has_values = false; + } + } + if (has_effort_command_interface_) + { + if (interface_has_values(joint_command_interface_[3])) + { + assign_point_from_interface(commands.effort, joint_command_interface_[3]); + } + else + { + commands.effort.clear(); + has_values = false; + } + } + + return has_values; +} + void JointTrajectoryController::query_state_service( const std::shared_ptr request, std::shared_ptr response) @@ -706,27 +781,44 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1)); - publisher_ = - get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(publisher_); state_publisher_->lock(); state_publisher_->msg_.joint_names = params_.joints; - state_publisher_->msg_.desired.positions.resize(dof_); - state_publisher_->msg_.desired.velocities.resize(dof_); - state_publisher_->msg_.desired.accelerations.resize(dof_); - state_publisher_->msg_.actual.positions.resize(dof_); + 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_.actual.velocities.resize(dof_); + state_publisher_->msg_.feedback.velocities.resize(dof_); state_publisher_->msg_.error.velocities.resize(dof_); } if (has_acceleration_state_interface_) { - state_publisher_->msg_.actual.accelerations.resize(dof_); + state_publisher_->msg_.feedback.accelerations.resize(dof_); state_publisher_->msg_.error.accelerations.resize(dof_); } + if (has_position_command_interface_) + { + state_publisher_->msg_.output.positions.resize(dof_); + } + if (has_velocity_command_interface_) + { + state_publisher_->msg_.output.velocities.resize(dof_); + } + if (has_acceleration_command_interface_) + { + state_publisher_->msg_.output.accelerations.resize(dof_); + } + if (has_effort_command_interface_) + { + state_publisher_->msg_.output.effort.resize(dof_); + } + state_publisher_->unlock(); // action server configuration @@ -749,6 +841,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::bind(&JointTrajectoryController::goal_accepted_callback, this, _1)); resize_joint_trajectory_point(state_current_, dof_); + resize_joint_trajectory_point_command(command_current_, dof_); resize_joint_trajectory_point(state_desired_, dof_); resize_joint_trajectory_point(state_error_, dof_); resize_joint_trajectory_point(last_commanded_state_, dof_); @@ -929,21 +1022,25 @@ void JointTrajectoryController::publish_state( if (state_publisher_->trylock()) { state_publisher_->msg_.header.stamp = time; - state_publisher_->msg_.desired.positions = desired_state.positions; - state_publisher_->msg_.desired.velocities = desired_state.velocities; - state_publisher_->msg_.desired.accelerations = desired_state.accelerations; - state_publisher_->msg_.actual.positions = current_state.positions; + 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_.actual.velocities = current_state.velocities; + state_publisher_->msg_.feedback.velocities = current_state.velocities; state_publisher_->msg_.error.velocities = state_error.velocities; } if (has_acceleration_state_interface_) { - state_publisher_->msg_.actual.accelerations = current_state.accelerations; + state_publisher_->msg_.feedback.accelerations = current_state.accelerations; state_publisher_->msg_.error.accelerations = state_error.accelerations; } + if (read_commands_from_command_interfaces(command_current_)) + { + state_publisher_->msg_.output = command_current_; + } state_publisher_->unlockAndPublish(); } @@ -1315,6 +1412,27 @@ void JointTrajectoryController::resize_joint_trajectory_point( } } +void JointTrajectoryController::resize_joint_trajectory_point_command( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) +{ + if (has_position_command_interface_) + { + point.positions.resize(size, 0.0); + } + if (has_velocity_command_interface_) + { + point.velocities.resize(size, 0.0); + } + if (has_acceleration_command_interface_) + { + point.accelerations.resize(size, 0.0); + } + if (has_effort_command_interface_) + { + point.effort.resize(size, 0.0); + } +} + } // namespace joint_trajectory_controller #include "pluginlib/class_list_macros.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 1c0dbb5482..e8073086dc 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -29,7 +29,6 @@ #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" -#include "control_msgs/msg/detail/joint_trajectory_controller_state__struct.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/resource_manager.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" @@ -49,10 +48,11 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/state.hpp" #include "std_msgs/msg/header.hpp" -#include "test_trajectory_controller_utils.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "test_trajectory_controller_utils.hpp" + using lifecycle_msgs::msg::State; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; @@ -469,38 +469,83 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) EXPECT_EQ(joint_names_[i], state->joint_names[i]); } - // 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); + // No trajectory by default, no reference state or error + EXPECT_TRUE( + state->reference.positions.empty() || state->reference.positions == INITIAL_POS_JOINTS); EXPECT_TRUE( - state->desired.accelerations.empty() || state->desired.accelerations == INITIAL_EFF_JOINTS); + state->reference.velocities.empty() || state->reference.velocities == INITIAL_VEL_JOINTS); + EXPECT_TRUE( + state->reference.accelerations.empty() || state->reference.accelerations == INITIAL_EFF_JOINTS); - EXPECT_EQ(n_joints, state->actual.positions.size()); + std::vector zeros(3, 0); + EXPECT_EQ(state->error.positions, zeros); + EXPECT_TRUE(state->error.velocities.empty() || state->error.velocities == zeros); + EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); + + // expect feedback including all state_interfaces + EXPECT_EQ(n_joints, state->feedback.positions.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(state->feedback.velocities.empty()); } else { - EXPECT_EQ(n_joints, state->actual.velocities.size()); + EXPECT_EQ(n_joints, state->feedback.velocities.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(state->feedback.accelerations.empty()); } else { - EXPECT_EQ(n_joints, state->actual.accelerations.size()); + EXPECT_EQ(n_joints, state->feedback.accelerations.size()); } - std::vector zeros(3, 0); - EXPECT_EQ(state->error.positions, zeros); - EXPECT_TRUE(state->error.velocities.empty() || state->error.velocities == zeros); - EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); + // expect output including all command_interfaces + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "position") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.positions.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.positions.size()); + } + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.velocities.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.velocities.size()); + } + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "acceleration") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.accelerations.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.accelerations.size()); + } + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.effort.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.effort.size()); + } } // Floating-point value comparison threshold @@ -540,25 +585,25 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) const auto allowed_delta = 0.1; // no update of state_interface - EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + EXPECT_EQ(state_msg->feedback.positions, 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, state_msg->reference.positions.size()); + EXPECT_EQ(n_joints, state_msg->feedback.positions.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); + // are the correct reference positions used? + EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->reference.positions[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], state_msg->reference.positions[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], state_msg->reference.positions[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], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); if (traj_controller_->has_position_command_interface()) { @@ -566,28 +611,34 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); + EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->output.positions[2], allowed_delta); } if (traj_controller_->has_velocity_command_interface()) { // check command interface - EXPECT_LE(0.0, joint_vel_[0]); - EXPECT_LE(0.0, joint_vel_[1]); - EXPECT_LE(0.0, joint_vel_[2]); + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); + EXPECT_LT(0.0, state_msg->output.velocities[0]); + EXPECT_LT(0.0, state_msg->output.velocities[1]); + EXPECT_LT(0.0, state_msg->output.velocities[2]); // use_closed_loop_pid_adapter_ if (traj_controller_->use_closed_loop_pid_adapter()) { // 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 * (state_msg->reference.positions[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 * (state_msg->reference.positions[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 * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], k_p * allowed_delta); } } @@ -595,9 +646,12 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) if (traj_controller_->has_effort_command_interface()) { // check command interface - EXPECT_LE(0.0, joint_eff_[0]); - EXPECT_LE(0.0, joint_eff_[1]); - EXPECT_LE(0.0, joint_eff_[2]); + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + EXPECT_LT(0.0, state_msg->output.effort[0]); + EXPECT_LT(0.0, state_msg->output.effort[1]); + EXPECT_LT(0.0, state_msg->output.effort[2]); } executor.cancel(); @@ -638,26 +692,26 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) const auto allowed_delta = 0.1; // no update of state_interface - EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + EXPECT_EQ(state_msg->feedback.positions, 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, state_msg->reference.positions.size()); + EXPECT_EQ(n_joints, state_msg->feedback.positions.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); + // are the correct reference positions used? + EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->reference.positions[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], state_msg->reference.positions[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], state_msg->reference.positions[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); + state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); if (traj_controller_->has_position_command_interface()) { @@ -670,38 +724,38 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) if (traj_controller_->has_velocity_command_interface()) { // check command interface - EXPECT_LE(0.0, joint_vel_[0]); - EXPECT_LE(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); // use_closed_loop_pid_adapter_ if (traj_controller_->use_closed_loop_pid_adapter()) { - EXPECT_GE(0.0, joint_vel_[2]); + EXPECT_GT(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 * (state_msg->reference.positions[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 * (state_msg->reference.positions[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 * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], k_p * allowed_delta); } else { // interpolated points_velocities only - EXPECT_LE(0.0, joint_vel_[2]); + EXPECT_LT(0.0, joint_vel_[2]); } } if (traj_controller_->has_effort_command_interface()) { // check command interface - EXPECT_LE(0.0, joint_eff_[0]); - EXPECT_LE(0.0, joint_eff_[1]); - EXPECT_LE(0.0, joint_eff_[2]); + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); } executor.cancel(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 25612f56e9..c85eb59f58 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -281,7 +281,7 @@ class TrajectoryControllerTest : public ::testing::Test // I do not understand why spin_some provides only one message qos.keep_last(1); state_subscriber_ = traj_lifecycle_node->create_subscription( - controller_name_ + "/state", qos, + controller_name_ + "/controller_state", qos, [&](std::shared_ptr msg) { std::lock_guard guard(state_mutex_); @@ -385,20 +385,21 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < expected_actual.positions.size(); ++i) { SCOPED_TRACE("Joint " + std::to_string(i)); - // TODO(anyone): add checking for velocties and accelerations + // TODO(anyone): add checking for velocities and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(expected_actual.positions[i], state_msg->actual.positions[i], allowed_delta); + EXPECT_NEAR(expected_actual.positions[i], state_msg->feedback.positions[i], allowed_delta); } } for (size_t i = 0; i < expected_desired.positions.size(); ++i) { SCOPED_TRACE("Joint " + std::to_string(i)); - // TODO(anyone): add checking for velocties and accelerations + // TODO(anyone): add checking for velocities and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(expected_desired.positions[i], state_msg->desired.positions[i], allowed_delta); + EXPECT_NEAR( + expected_desired.positions[i], state_msg->reference.positions[i], allowed_delta); } } }