Partial joints

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -155,9 +155,14 @@ 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;
 
   SegmentTolerances default_tolerances_;
 

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -320,17 +320,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_) {
+        fill_partial_goal(msg);
         sort_to_local_joint_order(msg);
         traj_external_point_ptr_->update(msg);
       }
@@ -380,8 +377,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")
@@ -554,28 +550,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;
 }
@@ -615,6 +593,7 @@ void JointTrajectoryController::feedback_setup_callback(
     preempt_active_goal();
     auto traj_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>(
       goal_handle->get_goal()->trajectory);
+    fill_partial_goal(traj_msg);
     sort_to_local_joint_order(traj_msg);
     traj_external_point_ptr_->update(traj_msg);
 
@@ -630,6 +609,47 @@ 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);
+        }
+        // TODO(v-lopez) is effort 0.0 valid here? Is it worse than joint_state->get_effort()?
+        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)
 {
@@ -671,6 +691,41 @@ 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::preempt_active_goal()
 {
   if (rt_active_goal_) {

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();
+}