Partial joints (#68)

* Move trajectory validation methods to separate function
* Add partial joints handling
* Add realtime buffer to update trajectory msg from auxiliary threads

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<Trajectory> traj_external_point_ptr_ = nullptr;
   std::shared_ptr<Trajectory> traj_home_point_ptr_ = nullptr;
   std::shared_ptr<trajectory_msgs::msg::JointTrajectory> traj_msg_home_ptr_ = nullptr;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectory>>
+  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<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> 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_msgs::msg::JointTrajectory> trajectory_msg) const;
   // sorts the joints of the incoming message to our local order
   void sort_to_local_joint_order(
     std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg);
+  bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const;
+  void add_new_trajectory_msg(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg);
 
   SegmentTolerances default_tolerances_;
 

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -120,6 +120,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);
@@ -133,8 +142,6 @@ JointTrajectoryController::update()
   }
   state_current.time_from_start.set__sec(0);
 
-  std::lock_guard<std::mutex> 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
@@ -319,19 +326,14 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State & previous
   auto callback = [this, &logger](const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> 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);
       }
     };
 
@@ -379,8 +381,7 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State & previous
   allow_partial_joints_goal_ = lifecycle_node_->get_parameter("allow_partial_joints_goal")
     .get_value<bool>();
   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")
@@ -553,28 +554,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;
 }
@@ -610,12 +593,11 @@ void JointTrajectoryController::feedback_setup_callback(
 {
   // Update new trajectory
   {
-    std::lock_guard<std::mutex> guard(trajectory_mtx_);
     preempt_active_goal();
     auto traj_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>(
       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<RealtimeGoalHandle>(goal_handle);
     rt_goal->preallocated_feedback_->joint_names = joint_names_;
@@ -629,6 +611,46 @@ void JointTrajectoryController::feedback_setup_callback(
     std::bind(&RealtimeGoalHandle::runNonRealtime, rt_active_goal_));
 }
 
+void JointTrajectoryController::fill_partial_goal(
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> 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_msgs::msg::JointTrajectory> trajectory_msg)
 {
@@ -670,6 +692,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<trajectory_msgs::msg::JointTrajectory> & traj_msg)
+{
+  traj_msg_external_point_ptr_.writeFromNonRT(traj_msg);
+}
+
 void JointTrajectoryController::preempt_active_goal()
 {
   if (rt_active_goal_) {
@@ -683,13 +746,12 @@ void JointTrajectoryController::preempt_active_goal()
 
 void JointTrajectoryController::set_hold_position()
 {
-  std::lock_guard<std::mutex> guard(trajectory_mtx_);
   trajectory_msgs::msg::JointTrajectory empty_msg;
   empty_msg.header.stamp = rclcpp::Time(0);
 
   auto traj_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>(
     empty_msg);
-  traj_external_point_ptr_->update(traj_msg);
+  add_new_trajectory_msg(traj_msg);
 }
 
 }  // namespace joint_trajectory_controller

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<joint_trajectory_controller::JointTrajectoryController>();
+  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<std::string> joint_names = {"joint1", "joint2", "joint3"};
+  rclcpp::Parameter joint_parameters("joints", joint_names);
+  traj_lifecycle_node->set_parameter(joint_parameters);
+
+  std::vector<std::string> 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<std::string> 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();
+}