Reorder incoming out of order joint_names in trajectory messages (#53)

* incoming action messages are now remapped based on initialized order
* shift remapping code to remap_msg_trajectories function, applied it to subscriber as well
* added jumbled_joint_order test
* error message for incoming goal with unmatched joint name
Co-authored-by: Karsten Knese <[email protected]>
Co-authored-by: Bence Magyar <[email protected]>

Author: 3 people

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -144,6 +144,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   void feedback_setup_callback(
     std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle);
 
+  // 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);
+
   SegmentTolerances default_tolerances_;
 
   void preempt_active_goal();

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

@@ -121,6 +121,31 @@ class Trajectory
   bool sampled_already_ = false;
 };
 
+/**
+ * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2 indices.
+ * If \p t1 is <tt>"{C, B}"</tt> and \p t2 is <tt>"{A, B, C, D}"</tt>, the associated mapping vector is
+ * <tt>"{2, 1}"</tt>.
+ */
+template<class T>
+inline std::vector<size_t> mapping(const T & t1, const T & t2)
+{
+  // t1 must be a subset of t2
+  if (t1.size() > t2.size()) {return std::vector<size_t>();}
+
+  std::vector<size_t> mapping_vector(t1.size());  // Return value
+  for (auto t1_it = t1.begin(); t1_it != t1.end(); ++t1_it) {
+    auto t2_it = std::find(t2.begin(), t2.end(), *t1_it);
+    if (t2.end() == t2_it) {
+      return std::vector<size_t>();
+    } else {
+      const size_t t1_dist = std::distance(t1.begin(), t1_it);
+      const size_t t2_dist = std::distance(t2.begin(), t2_it);
+      mapping_vector[t1_dist] = t2_dist;
+    }
+  }
+  return mapping_vector;
+}
+
 }  // namespace joint_trajectory_controller
 
 #endif  // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -322,6 +322,7 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State & previous
       // 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);
       }
     };
@@ -554,6 +555,18 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_callback(
     }
   }
 
+  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;
 }
@@ -593,6 +606,7 @@ void JointTrajectoryController::feedback_setup_callback(
     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);
 
     RealtimeGoalHandlePtr rt_goal = std::make_shared<RealtimeGoalHandle>(goal_handle);
@@ -607,6 +621,47 @@ void JointTrajectoryController::feedback_setup_callback(
     std::bind(&RealtimeGoalHandle::runNonRealtime, rt_active_goal_));
 }
 
+void JointTrajectoryController::sort_to_local_joint_order(
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg)
+{
+  // rearrange all points in the trajectory message based on mapping
+  std::vector<size_t> mapping_vector = mapping(trajectory_msg->joint_names, joint_names_);
+  auto remap = [this](const std::vector<double> & to_remap, const std::vector<size_t> & mapping)
+    -> std::vector<double>
+    {
+      if (to_remap.empty()) {
+        return to_remap;
+      }
+      if (to_remap.size() != mapping.size()) {
+        RCLCPP_WARN(
+          lifecycle_node_->get_logger(),
+          "Invalid input size (%d) for sorting", to_remap.size());
+        return to_remap;
+      }
+      std::vector<double> output;
+      output.resize(mapping.size(), 0.0);
+      for (auto index = 0ul; index < mapping.size(); ++index) {
+        auto map_index = mapping[index];
+        output[map_index] = to_remap[index];
+      }
+      return output;
+    };
+
+  for (auto index = 0ul; index < trajectory_msg->points.size(); ++index) {
+    trajectory_msg->points[index].positions =
+      remap(trajectory_msg->points[index].positions, mapping_vector);
+
+    trajectory_msg->points[index].velocities =
+      remap(trajectory_msg->points[index].velocities, mapping_vector);
+
+    trajectory_msg->points[index].accelerations =
+      remap(trajectory_msg->points[index].accelerations, mapping_vector);
+
+    trajectory_msg->points[index].effort =
+      remap(trajectory_msg->points[index].effort, mapping_vector);
+  }
+}
+
 void JointTrajectoryController::preempt_active_goal()
 {
   if (rt_active_goal_) {

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -526,3 +526,68 @@ TEST_F(TestTrajectoryController, zero_state_publish_rate) {
 
   test_state_publish_rate_target(traj_controller, 0);
 }
+
+TEST_F(TestTrajectoryController, test_jumbled_joint_order) {
+  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::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));
+
+  {
+    trajectory_msgs::msg::JointTrajectory traj_msg;
+    std::vector<std::string> jumbled_joint_names {
+      test_robot->joint_name2, test_robot->joint_name3, test_robot->joint_name1
+    };
+    traj_msg.joint_names = jumbled_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(3);
+    traj_msg.points[0].positions[0] = 2.0;
+    traj_msg.points[0].positions[1] = 3.0;
+    traj_msg.points[0].positions[2] = 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(1.0, test_robot->pos1, threshold);
+  EXPECT_NEAR(2.0, test_robot->pos2, threshold);
+  EXPECT_NEAR(3.0, test_robot->pos3, threshold);
+
+  executor.cancel();
+}