diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index f28ab76202..d6ce91f681 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -32,9 +32,7 @@ #include "diff_drive_controller/visibility_control.h" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "hardware_interface/joint_command_handle.hpp" #include "hardware_interface/operation_mode_handle.hpp" -#include "hardware_interface/robot_hardware.hpp" #include "nav_msgs/msg/odometry.hpp" #include "odometry.hpp" #include "rclcpp/rclcpp.hpp" @@ -44,6 +42,12 @@ #include "sensor_msgs/msg/joint_state.hpp" #include "tf2_msgs/msg/tf_message.hpp" +namespace hardware_interface +{ +class JointHandle; +class RobotHardware; +} // namespace hardware_interface + namespace diff_drive_controller { class DiffDriveController : public controller_interface::ControllerInterface @@ -96,8 +100,8 @@ class DiffDriveController : public controller_interface::ControllerInterface protected: struct WheelHandle { - const hardware_interface::JointStateHandle * state = nullptr; - hardware_interface::JointCommandHandle * command = nullptr; + std::shared_ptr state = nullptr; + std::shared_ptr command = nullptr; }; CallbackReturn configure_side( diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index ca3132abe0..f908a777fb 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -23,6 +23,8 @@ #include #include "diff_drive_controller/diff_drive_controller.hpp" +#include "hardware_interface/joint_handle.hpp" +#include "hardware_interface/robot_hardware.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "tf2/LinearMath/Quaternion.h" @@ -147,8 +149,8 @@ controller_interface::return_type DiffDriveController::update() double left_position_mean = 0.0; double right_position_mean = 0.0; for (size_t index = 0; index < wheels.wheels_per_side; ++index) { - const double left_position = registered_left_wheel_handles_[index].state->get_position(); - const double right_position = registered_right_wheel_handles_[index].state->get_position(); + const double left_position = registered_left_wheel_handles_[index].state->get_value(); + const double right_position = registered_right_wheel_handles_[index].state->get_value(); if (std::isnan(left_position) || std::isnan(right_position)) { RCLCPP_ERROR( @@ -243,8 +245,8 @@ controller_interface::return_type DiffDriveController::update() // Set wheels velocities: for (size_t index = 0; index < wheels.wheels_per_side; ++index) { - registered_left_wheel_handles_[index].command->set_cmd(velocity_left); - registered_right_wheel_handles_[index].command->set_cmd(velocity_right); + registered_left_wheel_handles_[index].command->set_value(velocity_left); + registered_right_wheel_handles_[index].command->set_value(velocity_right); } set_op_mode(hardware_interface::OperationMode::ACTIVE); @@ -532,7 +534,7 @@ void DiffDriveController::halt() { const auto halt_wheels = [](auto & wheel_handles) { for (const auto & wheel_handle : wheel_handles) { - wheel_handle.command->set_cmd(0.0); + wheel_handle.command->set_value(0.0); } }; @@ -562,17 +564,27 @@ CallbackReturn DiffDriveController::configure_side( const auto wheel_name = wheel_names[index].c_str(); auto & wheel_handle = registered_handles[index]; - auto result = robot_hardware.get_joint_state_handle(wheel_name, &wheel_handle.state); + auto joint_state_handle = std::make_shared( + wheel_name, + "position"); + auto joint_command_handle = std::make_shared( + wheel_name, + "velocity_command"); + + auto result = robot_hardware.get_joint_handle(*joint_state_handle); if (result != hardware_interface::return_type::OK) { RCLCPP_WARN(logger, "unable to obtain joint state handle for %s", wheel_name); return CallbackReturn::FAILURE; } - auto ret = robot_hardware.get_joint_command_handle(wheel_name, &wheel_handle.command); - if (ret != hardware_interface::return_type::OK) { + result = robot_hardware.get_joint_handle(*joint_command_handle); + if (result != hardware_interface::return_type::OK) { RCLCPP_WARN(logger, "unable to obtain joint command handle for %s", wheel_name); return CallbackReturn::FAILURE; } + + wheel_handle.state = joint_state_handle; + wheel_handle.command = joint_command_handle; } return CallbackReturn::SUCCESS; diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index d104ee84e4..db5968ffb2 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -71,12 +71,27 @@ class TestDiffDriveController : public ::testing::Test { test_robot = std::make_shared(); test_robot->init(); - left_wheel_names = - {{test_robot->joint_name1, test_robot->joint_name2, test_robot->joint_name3}}; - right_wheel_names = - {{test_robot->joint_name1, test_robot->joint_name2, test_robot->joint_name3}}; + left_wheel_names = test_robot->joint_names; + right_wheel_names = test_robot->joint_names; op_mode = {{test_robot->write_op_handle_name1}}; + // create joint handles + auto get_handle = [&](const std::string & joint_name, const std::string & interface_name) + { + auto joint_handle = std::make_shared( + joint_name, + interface_name); + test_robot->get_joint_handle(*joint_handle); + return joint_handle; + }; + + joint1_pos_handle_ = get_handle("joint1", "position"); + joint2_pos_handle_ = get_handle("joint2", "position"); + joint3_pos_handle_ = get_handle("joint3", "position"); + joint1_vel_cmd_handle_ = get_handle("joint1", "velocity_command"); + joint2_vel_cmd_handle_ = get_handle("joint2", "velocity_command"); + joint3_vel_cmd_handle_ = get_handle("joint3", "velocity_command"); + pub_node = std::make_shared("velocity_publisher"); velocity_publisher = pub_node->create_publisher( controller_name + "/cmd_vel", @@ -145,10 +160,18 @@ class TestDiffDriveController : public ::testing::Test std::string controller_name = "test_diff_drive_controller"; std::shared_ptr test_robot; + std::vector left_wheel_names; std::vector right_wheel_names; std::vector op_mode; + std::shared_ptr joint1_pos_handle_; + std::shared_ptr joint2_pos_handle_; + std::shared_ptr joint3_pos_handle_; + std::shared_ptr joint1_vel_cmd_handle_; + std::shared_ptr joint2_vel_cmd_handle_; + std::shared_ptr joint3_vel_cmd_handle_; + rclcpp::Node::SharedPtr pub_node; rclcpp::Publisher::SharedPtr velocity_publisher; }; @@ -180,9 +203,9 @@ TEST_F(TestDiffDriveController, correct_initialization) auto inactive_state = diff_drive_controller->get_lifecycle_node()->configure(); EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, inactive_state.id()); - EXPECT_EQ(1.1, initialized_robot->pos1); - EXPECT_EQ(2.2, initialized_robot->pos2); - EXPECT_EQ(3.3, initialized_robot->pos3); + EXPECT_EQ(1.1, joint1_pos_handle_->get_value()); + EXPECT_EQ(2.1, joint2_pos_handle_->get_value()); + EXPECT_EQ(3.1, joint3_pos_handle_->get_value()); } TEST_F(TestDiffDriveController, configuration) @@ -250,8 +273,8 @@ TEST_F(TestDiffDriveController, cleanup) test_robot->write(); // shouild be stopped - EXPECT_EQ(0, test_robot->cmd1); - EXPECT_EQ(0, test_robot->cmd2); + EXPECT_EQ(0.0, joint1_vel_cmd_handle_->get_value()); + EXPECT_EQ(0.0, joint2_vel_cmd_handle_->get_value()); executor.cancel(); } @@ -286,8 +309,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) auto state = diff_drive_lifecycle_node->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(1.1, test_robot->cmd1); - EXPECT_EQ(2.2, test_robot->cmd2); + EXPECT_EQ(1.2, joint1_vel_cmd_handle_->get_value()); + EXPECT_EQ(2.2, joint2_vel_cmd_handle_->get_value()); state = diff_drive_lifecycle_node->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -301,8 +324,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) diff_drive_controller->update(); test_robot->write(); - EXPECT_EQ(1.0, test_robot->cmd1); - EXPECT_EQ(1.0, test_robot->cmd2); + EXPECT_EQ(1.0, joint1_vel_cmd_handle_->get_value()); + EXPECT_EQ(1.0, joint2_vel_cmd_handle_->get_value()); // deactivated // wait so controller process the second point when deactivated @@ -312,15 +335,15 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) diff_drive_controller->update(); test_robot->write(); - EXPECT_EQ(0., test_robot->cmd1) << "Wheels are halted on deactivate()"; - EXPECT_EQ(0., test_robot->cmd2) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, joint1_vel_cmd_handle_->get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, joint2_vel_cmd_handle_->get_value()) << "Wheels are halted on deactivate()"; // cleanup state = diff_drive_lifecycle_node->cleanup(); test_robot->write(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(0, test_robot->cmd1); - EXPECT_EQ(0, test_robot->cmd2); + EXPECT_EQ(0.0, joint1_vel_cmd_handle_->get_value()); + EXPECT_EQ(0.0, joint2_vel_cmd_handle_->get_value()); state = diff_drive_lifecycle_node->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); diff --git a/joint_state_controller/include/joint_state_controller/joint_state_controller.hpp b/joint_state_controller/include/joint_state_controller/joint_state_controller.hpp index 6aeb668625..e8ee8ffc31 100644 --- a/joint_state_controller/include/joint_state_controller/joint_state_controller.hpp +++ b/joint_state_controller/include/joint_state_controller/joint_state_controller.hpp @@ -16,6 +16,7 @@ #define JOINT_STATE_CONTROLLER__JOINT_STATE_CONTROLLER_HPP_ #include +#include #include #include "control_msgs/msg/dynamic_joint_state.hpp" @@ -27,8 +28,9 @@ namespace hardware_interface { -class JointStateHandle; +class JointHandle; } // namespace hardware_interface + namespace rclcpp_lifecycle { class State; @@ -60,7 +62,12 @@ class JointStateController : public controller_interface::ControllerInterface on_deactivate(const rclcpp_lifecycle::State & previous_state) override; protected: - std::vector registered_joint_handles_; + bool init_joint_data(); + void init_joint_state_msg(); + void init_dynamic_joint_state_msg(); + +protected: + std::vector joint_names_; std::shared_ptr> joint_state_publisher_; @@ -69,6 +76,8 @@ class JointStateController : public controller_interface::ControllerInterface std::shared_ptr> dynamic_joint_state_publisher_; control_msgs::msg::DynamicJointState dynamic_joint_state_msg_; + + std::vector>> joint_handles_; }; } // namespace joint_state_controller diff --git a/joint_state_controller/src/joint_state_controller.cpp b/joint_state_controller/src/joint_state_controller.cpp index 3217cdeb6b..2a185cf3b6 100644 --- a/joint_state_controller/src/joint_state_controller.cpp +++ b/joint_state_controller/src/joint_state_controller.cpp @@ -18,9 +18,9 @@ #include #include #include +#include - -#include "hardware_interface/joint_state_handle.hpp" +#include "hardware_interface/joint_handle.hpp" #include "hardware_interface/robot_hardware.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/clock.hpp" @@ -38,6 +38,9 @@ class State; namespace joint_state_controller { +static const auto kPositionIndex = 0ul; +static const auto kVelocityIndex = 1ul; +static const auto kEffortIndex = 2ul; JointStateController::JointStateController() : controller_interface::ControllerInterface() @@ -46,38 +49,12 @@ JointStateController::JointStateController() rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn JointStateController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - if (auto sptr = robot_hardware_.lock()) { - registered_joint_handles_ = sptr->get_registered_joint_state_handles(); - } else { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } - - if (registered_joint_handles_.empty()) { + if (!init_joint_data()) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - const size_t num_joints = registered_joint_handles_.size(); - - // default initialization for joint state message - joint_state_msg_.position.resize(num_joints); - joint_state_msg_.velocity.resize(num_joints); - joint_state_msg_.effort.resize(num_joints); - - // default initialization for dynamic joint state message - control_msgs::msg::InterfaceValue default_if_value; - default_if_value.interface_names = {"position", "velocity", "effort"}; - default_if_value.values.resize( - default_if_value.interface_names.size(), std::numeric_limits::quiet_NaN()); - - // set known joint names - joint_state_msg_.name.reserve(num_joints); - dynamic_joint_state_msg_.joint_names.reserve(num_joints); - for (const auto joint_handle : registered_joint_handles_) { - joint_state_msg_.name.push_back(joint_handle->get_name()); - - dynamic_joint_state_msg_.joint_names.push_back(joint_handle->get_name()); - dynamic_joint_state_msg_.interface_values.push_back(default_if_value); - } + init_joint_state_msg(); + init_dynamic_joint_state_msg(); joint_state_publisher_ = lifecycle_node_->create_publisher( "joint_states", rclcpp::SystemDefaultsQoS()); @@ -107,6 +84,90 @@ JointStateController::on_deactivate(const rclcpp_lifecycle::State & /*previous_s return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } +bool JointStateController::init_joint_data() +{ + if (auto rh_ptr = robot_hardware_.lock()) { + joint_names_ = rh_ptr->get_registered_joint_names(); + + if (joint_names_.empty()) { + return false; + } + + // reset any previous joint data + joint_handles_.clear(); + + for (const auto & joint_name : joint_names_) { + // always check for these interfaces, even if they are not supported by the joint + std::vector interface_names = {"position", "velocity", "effort"}; + const std::vector joint_interface_names = + rh_ptr->get_registered_joint_interface_names(joint_name); + + // add the rest of the interfaces + // ideally this could be done with std::sort + std::unique, but we rather have + // position, velocity and effort in known indexes + std::copy_if( + joint_interface_names.begin(), joint_interface_names.end(), + std::back_inserter(interface_names), [&](const auto & value) { + return std::find( + interface_names.begin(), + interface_names.end(), value) == interface_names.end(); + }); + + std::vector> joint_interface_handles; + for (const auto & interface_name : interface_names) { + /// @todo is there a better way of skipping command interfaces? + if (interface_name.find("command") != std::string::npos) { + continue; + } + + auto joint_handle = std::make_shared( + joint_name, + interface_name); + if (rh_ptr->get_joint_handle(*joint_handle) == hardware_interface::return_type::OK) { + joint_interface_handles.emplace_back(joint_handle); + } else { + // leave a nullptr for not supported interfaces + joint_interface_handles.emplace_back(nullptr); + } + } + joint_handles_.emplace_back(joint_interface_handles); + } + return true; + } else { + return false; + } +} + +void JointStateController::init_joint_state_msg() +{ + const size_t num_joints = joint_handles_.size(); + + /// @note joint_state_msg publishes position, velocity and effort for all joints, + /// regardless of the joints actually supporting these interfaces + + // default initialization for joint state message + joint_state_msg_.name = joint_names_; + joint_state_msg_.position.resize(num_joints, std::numeric_limits::quiet_NaN()); + joint_state_msg_.velocity.resize(num_joints, std::numeric_limits::quiet_NaN()); + joint_state_msg_.effort.resize(num_joints, std::numeric_limits::quiet_NaN()); +} + +void JointStateController::init_dynamic_joint_state_msg() +{ + dynamic_joint_state_msg_.joint_names = joint_names_; + for (const auto & joint_handle : joint_handles_) { + control_msgs::msg::InterfaceValue if_value; + for (const auto & joint_interface_handle : joint_handle) { + // check joint handle is valid + if (joint_interface_handle) { + if_value.interface_names.emplace_back(joint_interface_handle->get_interface_name()); + if_value.values.emplace_back(std::numeric_limits::quiet_NaN()); + } + } + dynamic_joint_state_msg_.interface_values.emplace_back(if_value); + } +} + controller_interface::return_type JointStateController::update() { @@ -121,17 +182,36 @@ JointStateController::update() } joint_state_msg_.header.stamp = lifecycle_node_->get_clock()->now(); - size_t i = 0; - for (auto joint_state_handle : registered_joint_handles_) { - joint_state_msg_.position[i] = joint_state_handle->get_position(); - joint_state_msg_.velocity[i] = joint_state_handle->get_velocity(); - joint_state_msg_.effort[i] = joint_state_handle->get_effort(); - - dynamic_joint_state_msg_.interface_values[i].values[0] = joint_state_handle->get_position(); - dynamic_joint_state_msg_.interface_values[i].values[1] = joint_state_handle->get_velocity(); - dynamic_joint_state_msg_.interface_values[i].values[2] = joint_state_handle->get_effort(); - ++i; + // update joint state message and dynamic joint state message + for (auto joint_index = 0ul; joint_index < joint_names_.size(); ++joint_index) { + auto update_value = + [](double & msg_data, const std::shared_ptr & joint_handle) + { + if (joint_handle) { + msg_data = joint_handle->get_value(); + } + }; + + // the interface indexes used here are hardcoded, check init_joint_data() + update_value( + joint_state_msg_.position[joint_index], + joint_handles_[joint_index][kPositionIndex]); + update_value( + joint_state_msg_.velocity[joint_index], + joint_handles_[joint_index][kVelocityIndex]); + update_value( + joint_state_msg_.effort[joint_index], + joint_handles_[joint_index][kEffortIndex]); + + auto interface_index = 0ul; + for (const auto & joint_interface_handle : joint_handles_[joint_index]) { + if (joint_interface_handle) { + dynamic_joint_state_msg_.interface_values[joint_index].values[interface_index] = + joint_interface_handle->get_value(); + ++interface_index; + } + } } // publish diff --git a/joint_state_controller/test/test_joint_state_controller.cpp b/joint_state_controller/test/test_joint_state_controller.cpp index f7c642f9f7..e4042c74d0 100644 --- a/joint_state_controller/test/test_joint_state_controller.cpp +++ b/joint_state_controller/test/test_joint_state_controller.cpp @@ -216,8 +216,8 @@ TEST_F(JointStateControllerTest, JointStatePublishTest) const size_t NUM_JOINTS = 3; ASSERT_EQ(joint_state_msg.position.size(), NUM_JOINTS); ASSERT_EQ(joint_state_msg.position[0], 1.1); - ASSERT_EQ(joint_state_msg.position[1], 2.2); - ASSERT_EQ(joint_state_msg.position[2], 3.3); + ASSERT_EQ(joint_state_msg.position[1], 2.1); + ASSERT_EQ(joint_state_msg.position[2], 3.1); } TEST_F(JointStateControllerTest, DynamicJointStatePublishTest) @@ -255,6 +255,6 @@ TEST_F(JointStateControllerTest, DynamicJointStatePublishTest) const size_t NUM_JOINTS = 3; ASSERT_EQ(dynamic_joint_state_msg.joint_names.size(), NUM_JOINTS); ASSERT_EQ(dynamic_joint_state_msg.interface_values[0].values[0], 1.1); - ASSERT_EQ(dynamic_joint_state_msg.interface_values[1].values[0], 2.2); - ASSERT_EQ(dynamic_joint_state_msg.interface_values[2].values[0], 3.3); + ASSERT_EQ(dynamic_joint_state_msg.interface_values[1].values[0], 2.1); + ASSERT_EQ(dynamic_joint_state_msg.interface_values[2].values[0], 3.1); } 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 6924209ac0..55c62433dc 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 @@ -24,6 +24,7 @@ #include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "control_msgs/action/follow_joint_trajectory.hpp" #include "controller_interface/controller_interface.hpp" +#include "hardware_interface/joint_handle.hpp" #include "hardware_interface/operation_mode_handle.hpp" #include "joint_trajectory_controller/tolerances.hpp" #include "joint_trajectory_controller/visibility_control.h" @@ -44,8 +45,6 @@ namespace hardware_interface { -class JointCommandHandle; -class JointStateHandle; class RobotHardware; } // namespace hardware_interface namespace rclcpp_action @@ -111,8 +110,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::vector joint_names_; std::vector write_op_names_; - std::vector registered_joint_cmd_handles_; - std::vector registered_joint_state_handles_; + std::vector joint_position_command_handles_; + std::vector joint_position_state_handles_; + std::vector joint_velocity_state_handles_; std::vector registered_operation_mode_handles_; // TODO(karsten1987): eventually activate and deactive subscriber directly when its supported diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 10fce2af0c..b8f6504a52 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -26,8 +26,6 @@ #include "angles/angles.h" #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" -#include "hardware_interface/joint_command_handle.hpp" -#include "hardware_interface/joint_state_handle.hpp" #include "hardware_interface/robot_hardware.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "joint_trajectory_controller/trajectory.hpp" @@ -125,14 +123,13 @@ JointTrajectoryController::update() } JointTrajectoryPoint state_current, state_desired, state_error; - const auto joint_num = registered_joint_state_handles_.size(); + const auto joint_num = joint_position_state_handles_.size(); resize_joint_trajectory_point(state_current, joint_num); // current state update for (auto index = 0ul; index < joint_num; ++index) { - auto & joint_state = registered_joint_state_handles_[index]; - state_current.positions[index] = joint_state->get_position(); - state_current.velocities[index] = joint_state->get_velocity(); + state_current.positions[index] = joint_position_state_handles_[index].get_value(); + state_current.velocities[index] = joint_velocity_state_handles_[index].get_value(); state_current.accelerations[index] = 0.0; } state_current.time_from_start.set__sec(0); @@ -158,7 +155,7 @@ JointTrajectoryController::update() const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); for (auto index = 0ul; index < joint_num; ++index) { // set values for next hardware write() - registered_joint_cmd_handles_[index]->set_cmd(state_desired.positions[index]); + joint_position_command_handles_[index].set_value(state_desired.positions[index]); compute_error_for_joint(state_error, index, state_current, state_desired); if (before_last_point && !check_state_tolerance_per_joint( @@ -259,27 +256,33 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State & previous RCLCPP_WARN(logger, "no joint names specified"); } - // register handles - registered_joint_state_handles_.resize(joint_names_.size()); - for (size_t index = 0; index < joint_names_.size(); ++index) { - const auto ret = robot_hardware->get_joint_state_handle( - joint_names_[index].c_str(), ®istered_joint_state_handles_[index]); - if (ret != hardware_interface::return_type::OK) { - RCLCPP_WARN( - logger, "unable to obtain joint state handle for %s", joint_names_[index].c_str()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } - } - registered_joint_cmd_handles_.resize(joint_names_.size()); - for (size_t index = 0; index < joint_names_.size(); ++index) { - const auto ret = robot_hardware->get_joint_command_handle( - joint_names_[index].c_str(), ®istered_joint_cmd_handles_[index]); - if (ret != hardware_interface::return_type::OK) { - RCLCPP_WARN( - logger, "unable to obtain joint command handle for %s", joint_names_[index].c_str()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } + auto get_handles = [&](const std::string & interface_name, + std::vector & dst) + { + for (size_t index = 0; index < joint_names_.size(); ++index) { + hardware_interface::JointHandle joint_handle(joint_names_[index], interface_name); + const auto ret = robot_hardware->get_joint_handle(joint_handle); + if (ret == hardware_interface::return_type::OK) { + dst.emplace_back(joint_handle); + } else { + RCLCPP_WARN( + logger, "unable to obtain joint '%s' with interface '%s'", + joint_names_[index].c_str(), interface_name.c_str()); + return false; + } + } + return true; + }; + + // get joint state and command handles + if (!get_handles("position", joint_position_state_handles_) || + !get_handles("velocity", joint_velocity_state_handles_) || + !get_handles("position_command", joint_position_command_handles_)) + { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } + + // get operation mode handles registered_operation_mode_handles_.resize(write_op_names_.size()); for (size_t index = 0; index < write_op_names_.size(); ++index) { const auto ret = robot_hardware->get_operation_mode_handle( @@ -295,8 +298,9 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State & previous } if ( - registered_joint_cmd_handles_.empty() || - registered_joint_state_handles_.empty() || + joint_position_command_handles_.empty() || + joint_position_state_handles_.empty() || + joint_velocity_state_handles_.empty() || registered_operation_mode_handles_.empty()) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -311,10 +315,10 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State & previous traj_msg_home_ptr_->points.resize(1); traj_msg_home_ptr_->points[0].time_from_start.sec = 0; traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; - traj_msg_home_ptr_->points[0].positions.resize(registered_joint_state_handles_.size()); - for (size_t index = 0; index < registered_joint_state_handles_.size(); ++index) { + traj_msg_home_ptr_->points[0].positions.resize(joint_position_state_handles_.size()); + for (size_t index = 0; index < joint_position_state_handles_.size(); ++index) { traj_msg_home_ptr_->points[0].positions[index] = - registered_joint_state_handles_[index]->get_position(); + joint_position_state_handles_[index].get_value(); } traj_external_point_ptr_ = std::make_shared(); @@ -464,8 +468,9 @@ JointTrajectoryController::reset() // joint_names_.clear(); // write_op_names_.clear(); - registered_joint_cmd_handles_.clear(); - registered_joint_state_handles_.clear(); + joint_position_command_handles_.clear(); + joint_position_state_handles_.clear(); + joint_velocity_state_handles_.clear(); registered_operation_mode_handles_.clear(); subscriber_is_active_ = false; @@ -503,10 +508,10 @@ JointTrajectoryController::set_op_mode(const hardware_interface::OperationMode & void JointTrajectoryController::halt() { - const size_t joint_num = registered_joint_cmd_handles_.size(); + const size_t joint_num = joint_position_command_handles_.size(); for (size_t index = 0; index < joint_num; ++index) { - registered_joint_cmd_handles_[index]->set_cmd( - registered_joint_state_handles_[index]->get_position()); + joint_position_command_handles_[index].set_value( + joint_position_state_handles_[index].get_value()); } set_op_mode(hardware_interface::OperationMode::ACTIVE); } @@ -632,10 +637,10 @@ void JointTrajectoryController::fill_partial_goal( } trajectory_msg->joint_names.push_back(joint_names_[index]); - const auto & joint_state = registered_joint_state_handles_[index]; + const auto & joint_state = joint_position_state_handles_[index]; for (auto & it : trajectory_msg->points) { // Assume hold position with 0 velocity and acceleration for missing joints - it.positions.push_back(joint_state->get_position()); + it.positions.push_back(joint_state.get_value()); if (!it.velocities.empty()) { it.velocities.push_back(0.0); } diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 28ae06d8cb..286e4cf82a 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -67,9 +67,23 @@ class TestTrajectoryActions : public ::testing::Test { test_robot_ = std::make_shared(); test_robot_->init(); - joint_names_ = {{test_robot_->joint_name1, test_robot_->joint_name2, test_robot_->joint_name3}}; + joint_names_ = test_robot_->joint_names; op_mode_ = {{test_robot_->write_op_handle_name1}}; + // create joint handles + auto get_handle = [&](const std::string & joint_name, const std::string & interface_name) + { + auto joint_handle = std::make_shared( + joint_name, + interface_name); + test_robot_->get_joint_handle(*joint_handle); + return joint_handle; + }; + + joint1_pos_handle_ = get_handle("joint1", "position"); + joint2_pos_handle_ = get_handle("joint2", "position"); + joint3_pos_handle_ = get_handle("joint3", "position"); + node_ = std::make_shared("trajectory_test_node"); traj_controller_ = std::make_shared(); @@ -209,6 +223,10 @@ class TestTrajectoryActions : public ::testing::Test std::vector joint_names_; std::vector op_mode_; + std::shared_ptr joint1_pos_handle_; + std::shared_ptr joint2_pos_handle_; + std::shared_ptr joint3_pos_handle_; + rclcpp::Node::SharedPtr node_; std::shared_ptr traj_controller_; rclcpp_lifecycle::LifecycleNode::SharedPtr traj_lifecycle_node_; @@ -290,9 +308,9 @@ TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) { EXPECT_TRUE(common_goal_accepted_); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_EQ(1.0, test_robot_->pos1); - EXPECT_EQ(2.0, test_robot_->pos2); - EXPECT_EQ(3.0, test_robot_->pos3); + EXPECT_EQ(1.0, joint1_pos_handle_->get_value()); + EXPECT_EQ(2.0, joint2_pos_handle_->get_value()); + EXPECT_EQ(3.0, joint3_pos_handle_->get_value()); } TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) { @@ -338,9 +356,9 @@ TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) { EXPECT_TRUE(common_goal_accepted_); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_NEAR(7.0, test_robot_->pos1, COMMON_THRESHOLD); - EXPECT_NEAR(8.0, test_robot_->pos2, COMMON_THRESHOLD); - EXPECT_NEAR(9.0, test_robot_->pos3, COMMON_THRESHOLD); + EXPECT_NEAR(7.0, joint1_pos_handle_->get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(8.0, joint2_pos_handle_->get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(9.0, joint3_pos_handle_->get_value(), COMMON_THRESHOLD); } TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) { @@ -374,9 +392,9 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) { control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - EXPECT_NEAR(1.0, test_robot_->pos1, COMMON_THRESHOLD); - EXPECT_NEAR(2.0, test_robot_->pos2, COMMON_THRESHOLD); - EXPECT_NEAR(3.0, test_robot_->pos3, COMMON_THRESHOLD); + EXPECT_NEAR(1.0, joint1_pos_handle_->get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(2.0, joint2_pos_handle_->get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(3.0, joint3_pos_handle_->get_value(), COMMON_THRESHOLD); } TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) { @@ -430,9 +448,9 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) { control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - EXPECT_NEAR(7.0, test_robot_->pos1, COMMON_THRESHOLD); - EXPECT_NEAR(8.0, test_robot_->pos2, COMMON_THRESHOLD); - EXPECT_NEAR(9.0, test_robot_->pos3, COMMON_THRESHOLD); + EXPECT_NEAR(7.0, joint1_pos_handle_->get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(8.0, joint2_pos_handle_->get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(9.0, joint3_pos_handle_->get_value(), COMMON_THRESHOLD); } TEST_F(TestTrajectoryActions, test_state_tolerances_fail) { @@ -504,15 +522,15 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position) { control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - const double prev_pos1 = test_robot_->pos1; - const double prev_pos2 = test_robot_->pos2; - const double prev_pos3 = test_robot_->pos3; + const double prev_pos1 = joint1_pos_handle_->get_value(); + const double prev_pos2 = joint2_pos_handle_->get_value(); + const double prev_pos3 = joint3_pos_handle_->get_value(); // run an update, it should be holding traj_controller_->update(); test_robot_->write(); - EXPECT_EQ(prev_pos1, test_robot_->pos1); - EXPECT_EQ(prev_pos2, test_robot_->pos2); - EXPECT_EQ(prev_pos3, test_robot_->pos3); + EXPECT_EQ(prev_pos1, joint1_pos_handle_->get_value()); + EXPECT_EQ(prev_pos2, joint2_pos_handle_->get_value()); + EXPECT_EQ(prev_pos3, joint3_pos_handle_->get_value()); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index ef95039d7e..b6dde7cce8 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -111,9 +111,26 @@ class TestTrajectoryController : public ::testing::Test { test_robot_ = std::make_shared(); test_robot_->init(); - joint_names_ = {{test_robot_->joint_name1, test_robot_->joint_name2, test_robot_->joint_name3}}; + joint_names_ = test_robot_->joint_names; op_mode_ = {{test_robot_->write_op_handle_name1}}; + // create joint handles + auto get_handle = [&](const std::string & joint_name, const std::string & interface_name) + { + auto joint_handle = std::make_shared( + joint_name, + interface_name); + test_robot_->get_joint_handle(*joint_handle); + return joint_handle; + }; + + joint1_pos_handle_ = get_handle("joint1", "position"); + joint2_pos_handle_ = get_handle("joint2", "position"); + joint3_pos_handle_ = get_handle("joint3", "position"); + joint1_pos_cmd_handle_ = get_handle("joint1", "position_command"); + joint2_pos_cmd_handle_ = get_handle("joint2", "position_command"); + joint3_pos_cmd_handle_ = get_handle("joint3", "position_command"); + pub_node_ = std::make_shared("trajectory_publisher_"); trajectory_publisher_ = pub_node_->create_publisher( controller_name_ + "/joint_trajectory", rclcpp::SystemDefaultsQoS()); @@ -284,6 +301,13 @@ class TestTrajectoryController : public ::testing::Test std::vector joint_names_; std::vector op_mode_; + std::shared_ptr joint1_pos_handle_; + std::shared_ptr joint2_pos_handle_; + std::shared_ptr joint3_pos_handle_; + std::shared_ptr joint1_pos_cmd_handle_; + std::shared_ptr joint2_pos_cmd_handle_; + std::shared_ptr joint3_pos_cmd_handle_; + rclcpp::Node::SharedPtr pub_node_; rclcpp::Publisher::SharedPtr trajectory_publisher_; @@ -318,11 +342,18 @@ TEST_F(TestTrajectoryController, correct_initialization) { FAIL(); } + auto get_handle = [&](const std::string & joint_name, const std::string & interface_name) + { + hardware_interface::JointHandle joint_handle(joint_name, interface_name); + initialized_robot->get_joint_handle(joint_handle); + return joint_handle; + }; + const auto inactive_state = traj_controller->get_lifecycle_node()->configure(); EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, inactive_state.id()); - EXPECT_EQ(1.1, initialized_robot->pos1); - EXPECT_EQ(2.2, initialized_robot->pos2); - EXPECT_EQ(3.3, initialized_robot->pos3); + EXPECT_EQ(1.1, get_handle("joint1", "position").get_value()); + EXPECT_EQ(2.1, get_handle("joint2", "position").get_value()); + EXPECT_EQ(3.1, get_handle("joint3", "position").get_value()); } TEST_F(TestTrajectoryController, configuration) { @@ -347,9 +378,9 @@ TEST_F(TestTrajectoryController, configuration) { test_robot_->write(); // no change in hw position - EXPECT_NE(3.3, test_robot_->pos1); - EXPECT_NE(4.4, test_robot_->pos2); - EXPECT_NE(5.5, test_robot_->pos3); + EXPECT_NE(3.3, joint1_pos_handle_->get_value()); + EXPECT_NE(4.4, joint2_pos_handle_->get_value()); + EXPECT_NE(5.5, joint3_pos_handle_->get_value()); executor.cancel(); } @@ -389,9 +420,9 @@ TEST_F(TestTrajectoryController, configuration) { // test_robot_->write(); // // // change in hw position -// EXPECT_EQ(3.3, test_robot_->pos1); -// EXPECT_EQ(4.4, test_robot_->pos2); -// EXPECT_EQ(5.5, test_robot_->pos3); +// EXPECT_EQ(3.3, joint1_pos_handle_->get_value()); +// EXPECT_EQ(4.4, joint2_pos_handle_->get_value()); +// EXPECT_EQ(5.5, joint3_pos_handle_->get_value()); // // executor.cancel(); // } @@ -446,9 +477,9 @@ TEST_F(TestTrajectoryController, configuration) { // test_robot_->write(); // // // no change in hw position -// EXPECT_EQ(3.3, test_robot_->pos1); -// EXPECT_EQ(4.4, test_robot_->pos2); -// EXPECT_EQ(5.5, test_robot_->pos3); +// EXPECT_EQ(3.3, joint1_pos_handle_->get_value()); +// EXPECT_EQ(4.4, joint2_pos_handle_->get_value()); +// EXPECT_EQ(5.5, joint3_pos_handle_->get_value()); // // // reactivated // // wait so controller process the third point when reactivated @@ -460,9 +491,9 @@ TEST_F(TestTrajectoryController, configuration) { // test_robot_->write(); // // // change in hw position to 3rd point -// EXPECT_EQ(10.10, test_robot_->pos1); -// EXPECT_EQ(11.11, test_robot_->pos2); -// EXPECT_EQ(12.12, test_robot_->pos3); +// EXPECT_EQ(10.10, joint1_pos_handle_->get_value()); +// EXPECT_EQ(11.11, joint2_pos_handle_->get_value()); +// EXPECT_EQ(12.12, joint3_pos_handle_->get_value()); // // executor.cancel(); // } @@ -502,9 +533,9 @@ TEST_F(TestTrajectoryController, cleanup) { updateController(rclcpp::Duration::from_seconds(0.25)); // should be home pose again - EXPECT_NEAR(1.1, test_robot_->pos1, COMMON_THRESHOLD); - EXPECT_NEAR(2.2, test_robot_->pos2, COMMON_THRESHOLD); - EXPECT_NEAR(3.3, test_robot_->pos3, COMMON_THRESHOLD); + EXPECT_NEAR(1.1, joint1_pos_handle_->get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(2.1, joint2_pos_handle_->get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(3.1, joint3_pos_handle_->get_value(), COMMON_THRESHOLD); } TEST_F(TestTrajectoryController, correct_initialization_using_parameters) { @@ -525,9 +556,9 @@ TEST_F(TestTrajectoryController, correct_initialization_using_parameters) { auto state = traj_lifecycle_node->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(1.1, test_robot_->pos1); - EXPECT_EQ(2.2, test_robot_->pos2); - EXPECT_EQ(3.3, test_robot_->pos3); + EXPECT_EQ(1.1, joint1_pos_handle_->get_value()); + EXPECT_EQ(2.1, joint2_pos_handle_->get_value()); + EXPECT_EQ(3.1, joint3_pos_handle_->get_value()); state = traj_lifecycle_node->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -559,9 +590,10 @@ TEST_F(TestTrajectoryController, correct_initialization_using_parameters) { ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); const auto allowed_delta = 0.05; - EXPECT_NEAR(3.3, test_robot_->pos1, allowed_delta); - EXPECT_NEAR(4.4, test_robot_->pos2, allowed_delta); - EXPECT_NEAR(5.5, test_robot_->pos3, allowed_delta); + + EXPECT_NEAR(3.3, joint1_pos_handle_->get_value(), allowed_delta); + EXPECT_NEAR(4.4, joint2_pos_handle_->get_value(), allowed_delta); + EXPECT_NEAR(5.5, joint3_pos_handle_->get_value(), allowed_delta); // cleanup state = traj_lifecycle_node->cleanup(); @@ -576,9 +608,9 @@ TEST_F(TestTrajectoryController, correct_initialization_using_parameters) { test_robot_->write(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_NEAR(1.1, test_robot_->pos1, allowed_delta); - EXPECT_NEAR(2.2, test_robot_->pos2, allowed_delta); - EXPECT_NEAR(3.3, test_robot_->pos3, allowed_delta); + EXPECT_NEAR(1.1, joint1_pos_handle_->get_value(), allowed_delta); + EXPECT_NEAR(2.1, joint2_pos_handle_->get_value(), allowed_delta); + EXPECT_NEAR(3.1, joint3_pos_handle_->get_value(), allowed_delta); state = traj_lifecycle_node->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); @@ -676,7 +708,7 @@ TEST_F(TestTrajectoryController, test_jumbled_joint_order) { { trajectory_msgs::msg::JointTrajectory traj_msg; const std::vector jumbled_joint_names { - test_robot_->joint_name2, test_robot_->joint_name3, test_robot_->joint_name1 + test_robot_->joint_names[1], test_robot_->joint_names[2], test_robot_->joint_names[0] }; traj_msg.joint_names = jumbled_joint_names; traj_msg.header.stamp = rclcpp::Time(0); @@ -695,9 +727,9 @@ TEST_F(TestTrajectoryController, test_jumbled_joint_order) { // update for 0.25 seconds updateController(rclcpp::Duration::from_seconds(0.25)); - EXPECT_NEAR(1.0, test_robot_->pos1, COMMON_THRESHOLD); - EXPECT_NEAR(2.0, test_robot_->pos2, COMMON_THRESHOLD); - EXPECT_NEAR(3.0, test_robot_->pos3, COMMON_THRESHOLD); + EXPECT_NEAR(1.0, joint1_pos_handle_->get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(2.0, joint2_pos_handle_->get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(3.0, joint3_pos_handle_->get_value(), COMMON_THRESHOLD); } /** @@ -709,12 +741,12 @@ TEST_F(TestTrajectoryController, test_partial_joint_list) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); - const double initial_joint3_cmd = test_robot_->cmd3; + const double initial_joint3_cmd = joint3_pos_cmd_handle_->get_value(); trajectory_msgs::msg::JointTrajectory traj_msg; { std::vector partial_joint_names { - test_robot_->joint_name2, test_robot_->joint_name1 + test_robot_->joint_names[1], test_robot_->joint_names[0] }; traj_msg.joint_names = partial_joint_names; traj_msg.header.stamp = rclcpp::Time(0); @@ -736,10 +768,10 @@ TEST_F(TestTrajectoryController, test_partial_joint_list) { updateController(rclcpp::Duration::from_seconds(0.25)); double threshold = 0.001; - EXPECT_NEAR(traj_msg.points[0].positions[1], test_robot_->pos1, threshold); - EXPECT_NEAR(traj_msg.points[0].positions[0], test_robot_->pos2, threshold); + EXPECT_NEAR(traj_msg.points[0].positions[1], joint1_pos_handle_->get_value(), threshold); + EXPECT_NEAR(traj_msg.points[0].positions[0], joint2_pos_handle_->get_value(), threshold); EXPECT_NEAR( - initial_joint3_cmd, test_robot_->pos3, + initial_joint3_cmd, joint3_pos_handle_->get_value(), threshold) << "Joint 3 command should be current position"; // Velocity commands are not sent yet @@ -761,14 +793,14 @@ TEST_F(TestTrajectoryController, test_partial_joint_list_not_allowed) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); - const double initial_joint1_cmd = test_robot_->cmd1; - const double initial_joint2_cmd = test_robot_->cmd2; - const double initial_joint3_cmd = test_robot_->cmd3; + const double initial_joint1_cmd = joint1_pos_cmd_handle_->get_value(); + const double initial_joint2_cmd = joint2_pos_cmd_handle_->get_value(); + const double initial_joint3_cmd = joint3_pos_cmd_handle_->get_value(); trajectory_msgs::msg::JointTrajectory traj_msg; { std::vector partial_joint_names { - test_robot_->joint_name2, test_robot_->joint_name1 + test_robot_->joint_names[1], test_robot_->joint_names[0] }; traj_msg.joint_names = partial_joint_names; traj_msg.header.stamp = rclcpp::Time(0); @@ -791,13 +823,13 @@ TEST_F(TestTrajectoryController, test_partial_joint_list_not_allowed) { double threshold = 0.001; EXPECT_NEAR( - initial_joint1_cmd, test_robot_->pos1, + initial_joint1_cmd, joint1_pos_cmd_handle_->get_value(), threshold) << "All joints command should be current position because goal was rejected"; EXPECT_NEAR( - initial_joint2_cmd, test_robot_->pos2, + initial_joint2_cmd, joint2_pos_cmd_handle_->get_value(), threshold) << "All joints command should be current position because goal was rejected"; EXPECT_NEAR( - initial_joint3_cmd, test_robot_->pos3, + initial_joint3_cmd, joint3_pos_cmd_handle_->get_value(), threshold) << "All joints command should be current position because goal was rejected"; // Velocity commands are not sent yet