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 dd963149b5..b40db66b6f 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 @@ -36,6 +36,7 @@ #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rcutils/time.h" +#include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "realtime_tools/realtime_server_goal_handle.h" #include "trajectory_msgs/msg/joint_trajectory.hpp" @@ -123,6 +124,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::shared_ptr traj_external_point_ptr_ = nullptr; std::shared_ptr traj_home_point_ptr_ = nullptr; std::shared_ptr traj_msg_home_ptr_ = nullptr; + realtime_tools::RealtimeBuffer> + traj_msg_external_point_ptr_{nullptr}; bool is_halted = false; @@ -144,7 +147,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa RealtimeGoalHandlePtr rt_active_goal_; ///< Currently active action goal, if any. rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(RCUTILS_MS_TO_NS(50)); - std::mutex trajectory_mtx_; // callbacks for action_server_ rclcpp_action::GoalResponse goal_callback( @@ -155,9 +157,16 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void feedback_setup_callback( std::shared_ptr> goal_handle); + // fill trajectory_msg so it matches joints controlled by this controller + // positions set to current position, velocities, accelerations and efforts to 0.0 + void fill_partial_goal( + std::shared_ptr trajectory_msg) const; // sorts the joints of the incoming message to our local order void sort_to_local_joint_order( std::shared_ptr trajectory_msg); + bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; + void add_new_trajectory_msg( + const std::shared_ptr & traj_msg); SegmentTolerances default_tolerances_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 7106b0a83d..1190be3fea 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -121,6 +121,15 @@ JointTrajectoryController::update() error.accelerations[index] = 0.0; }; + // Check if a new external message has been received from nonRT threads + auto current_external_msg = traj_external_point_ptr_->get_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); + traj_external_point_ptr_->update(*new_external_msg); + } + JointTrajectoryPoint state_current, state_desired, state_error; const auto joint_num = registered_joint_state_handles_.size(); resize_joint_trajectory_point(state_current, joint_num); @@ -134,8 +143,6 @@ JointTrajectoryController::update() } state_current.time_from_start.set__sec(0); - std::lock_guard guard(trajectory_mtx_); - // currently carrying out a trajectory if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg() == false) { // if sampling the first time, set the point before you sample @@ -320,19 +327,14 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State & previous auto callback = [this, &logger](const std::shared_ptr msg) -> void { - if (registered_joint_cmd_handles_.size() != msg->joint_names.size()) { - RCLCPP_ERROR( - logger, - "number of joints in joint trajectory msg (%d) " - "does not match number of joint command handles (%d)\n", - msg->joint_names.size(), registered_joint_cmd_handles_.size()); + if (!validate_trajectory_msg(*msg)) { + return; } // http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement // always replace old msg with new one for now if (subscriber_is_active_) { - sort_to_local_joint_order(msg); - traj_external_point_ptr_->update(msg); + add_new_trajectory_msg(msg); } }; @@ -380,8 +382,7 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State & previous allow_partial_joints_goal_ = lifecycle_node_->get_parameter("allow_partial_joints_goal") .get_value(); if (allow_partial_joints_goal_) { - // TODO(ddengster): implement partial joints, log an enabled partial joints goal message https://github.com/ros-controls/ros2_controllers/issues/37 - RCLCPP_WARN(logger, "Warning: Goals with partial set of joints not implemented yet."); + RCLCPP_INFO(logger, "Goals with partial set of joints are allowed"); } const double action_monitor_rate = lifecycle_node_->get_parameter("action_monitor_rate") @@ -554,28 +555,10 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( return rclcpp_action::GoalResponse::REJECT; } - // If partial joints goals are not allowed, goal should specify all controller joints - if (!allow_partial_joints_goal_) { - if (goal->trajectory.joint_names.size() != joint_names_.size()) { - RCLCPP_ERROR( - lifecycle_node_->get_logger(), - "Joints on incoming goal don't match the controller joints."); - return rclcpp_action::GoalResponse::REJECT; - } + if (!validate_trajectory_msg(goal->trajectory)) { + return rclcpp_action::GoalResponse::REJECT; } - for (auto i = 0ul; i < goal->trajectory.joint_names.size(); ++i) { - const std::string & incoming_joint_name = goal->trajectory.joint_names[i]; - - auto it = std::find(joint_names_.begin(), joint_names_.end(), incoming_joint_name); - if (it == joint_names_.end()) { - RCLCPP_ERROR( - lifecycle_node_->get_logger(), - "Incoming joint %s doesn't match the controller's joints.", - incoming_joint_name.c_str()); - return rclcpp_action::GoalResponse::REJECT; - } - } RCLCPP_INFO(lifecycle_node_->get_logger(), "Accepted new action goal"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -611,12 +594,11 @@ void JointTrajectoryController::feedback_setup_callback( { // Update new trajectory { - std::lock_guard guard(trajectory_mtx_); preempt_active_goal(); auto traj_msg = std::make_shared( goal_handle->get_goal()->trajectory); - sort_to_local_joint_order(traj_msg); - traj_external_point_ptr_->update(traj_msg); + + add_new_trajectory_msg(traj_msg); RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); rt_goal->preallocated_feedback_->joint_names = joint_names_; @@ -630,6 +612,46 @@ void JointTrajectoryController::feedback_setup_callback( std::bind(&RealtimeGoalHandle::runNonRealtime, rt_active_goal_)); } +void JointTrajectoryController::fill_partial_goal( + std::shared_ptr trajectory_msg) const +{ + // joint names in the goal are a subset of existing joints, as checked in goal_callback + // so if the size matches, the goal contains all controller joints + if (joint_names_.size() == trajectory_msg->joint_names.size()) { + return; + } + + trajectory_msg->joint_names.reserve(joint_names_.size()); + + for (auto index = 0ul; index < joint_names_.size(); ++index) { + { + if (std::find( + trajectory_msg->joint_names.begin(), trajectory_msg->joint_names.end(), + joint_names_[index]) != trajectory_msg->joint_names.end()) + { + // joint found on msg + continue; + } + trajectory_msg->joint_names.push_back(joint_names_[index]); + + const auto & joint_state = registered_joint_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()); + if (!it.velocities.empty()) { + it.velocities.push_back(0.0); + } + if (!it.accelerations.empty()) { + it.accelerations.push_back(0.0); + } + if (!it.effort.empty()) { + it.effort.push_back(0.0); + } + } + } + } +} + void JointTrajectoryController::sort_to_local_joint_order( std::shared_ptr trajectory_msg) { @@ -671,6 +693,47 @@ void JointTrajectoryController::sort_to_local_joint_order( } } +bool JointTrajectoryController::validate_trajectory_msg( + const trajectory_msgs::msg::JointTrajectory & trajectory) const +{ + // If partial joints goals are not allowed, goal should specify all controller joints + if (!allow_partial_joints_goal_) { + if (trajectory.joint_names.size() != joint_names_.size()) { + RCLCPP_ERROR( + lifecycle_node_->get_logger(), + "Joints on incoming trajectory don't match the controller joints."); + return false; + } + } + + if (trajectory.joint_names.empty()) { + RCLCPP_ERROR( + lifecycle_node_->get_logger(), + "Empty joint names on incoming trajectory."); + return false; + } + + for (auto i = 0ul; i < trajectory.joint_names.size(); ++i) { + const std::string & incoming_joint_name = trajectory.joint_names[i]; + + auto it = std::find(joint_names_.begin(), joint_names_.end(), incoming_joint_name); + if (it == joint_names_.end()) { + RCLCPP_ERROR( + lifecycle_node_->get_logger(), + "Incoming joint %s doesn't match the controller's joints.", + incoming_joint_name.c_str()); + return false; + } + } + return true; +} + +void JointTrajectoryController::add_new_trajectory_msg( + const std::shared_ptr & traj_msg) +{ + traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); +} + void JointTrajectoryController::preempt_active_goal() { if (rt_active_goal_) { @@ -684,13 +747,12 @@ void JointTrajectoryController::preempt_active_goal() void JointTrajectoryController::set_hold_position() { - std::lock_guard guard(trajectory_mtx_); trajectory_msgs::msg::JointTrajectory empty_msg; empty_msg.header.stamp = rclcpp::Time(0); auto traj_msg = std::make_shared( empty_msg); - traj_external_point_ptr_->update(traj_msg); + add_new_trajectory_msg(traj_msg); } } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 9ed1ec89c9..05e4bf873e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -597,3 +597,84 @@ TEST_F(TestTrajectoryController, test_jumbled_joint_order) { executor.cancel(); } + +TEST_F(TestTrajectoryController, test_partial_joint_list) { + auto traj_controller = std::make_shared(); + auto ret = traj_controller->init(test_robot_, controller_name_); + if (ret != controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) { + FAIL(); + } + + auto traj_lifecycle_node = traj_controller->get_lifecycle_node(); + std::vector joint_names = {"joint1", "joint2", "joint3"}; + rclcpp::Parameter joint_parameters("joints", joint_names); + traj_lifecycle_node->set_parameter(joint_parameters); + + std::vector operation_mode_names = {"write1", "write2"}; + rclcpp::Parameter operation_mode_parameters("write_op_modes", operation_mode_names); + traj_lifecycle_node->set_parameter(operation_mode_parameters); + + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); + traj_lifecycle_node->set_parameter(partial_joints_parameters); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(traj_lifecycle_node->get_node_base_interface()); + + traj_controller->on_configure(traj_lifecycle_node->get_current_state()); + traj_controller->on_activate(traj_lifecycle_node->get_current_state()); + + auto future_handle = std::async( + std::launch::async, [&executor]() -> void { + executor.spin(); + }); + // wait for things to setup + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + const double initial_joint3_cmd = test_robot_->cmd3; + trajectory_msgs::msg::JointTrajectory traj_msg; + + { + std::vector partial_joint_names { + test_robot_->joint_name2, test_robot_->joint_name1 + }; + traj_msg.joint_names = partial_joint_names; + traj_msg.header.stamp = rclcpp::Time(0); + traj_msg.points.resize(1); + + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].positions.resize(2); + traj_msg.points[0].positions[0] = 2.0; + traj_msg.points[0].positions[1] = 1.0; + traj_msg.points[0].velocities.resize(2); + traj_msg.points[0].velocities[0] = 2.0; + traj_msg.points[0].velocities[1] = 1.0; + + trajectory_publisher_->publish(traj_msg); + } + + // update for 0.5 seconds + auto start_time = rclcpp::Clock().now(); + rclcpp::Duration wait = rclcpp::Duration::from_seconds(0.5); + auto end_time = start_time + wait; + while (rclcpp::Clock().now() < end_time) { + test_robot_->read(); + traj_controller->update(); + test_robot_->write(); + } + + 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( + initial_joint3_cmd, test_robot_->pos3, + threshold) << "Joint 3 command should be current position"; + +// Velocity commands are not sent yet +// EXPECT_NEAR(traj_msg.points[0].velocities[1], test_robot_->vel1, threshold); +// EXPECT_NEAR(traj_msg.points[0].velocities[0], test_robot_->vel2, threshold); +// EXPECT_NEAR( +// 0.0, test_robot_->vel3, +// threshold) << "Joint 3 velocity should be 0.0 since it's not in the goal"; + + executor.cancel(); +}