incoming action messages are now remapped based on initialized order

Author: ddengster

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

@@ -118,6 +118,34 @@ class Trajectory
   bool sampled_already_ = false;
 };
 
+/**
+ * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \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<unsigned int> mapping(const T& t1, const T& t2)
+{
+  typedef unsigned int SizeType;
+
+  // t1 must be a subset of t2
+  if (t1.size() > t2.size()) {return std::vector<SizeType>();}
+
+  std::vector<SizeType> mapping_vector(t1.size()); // Return value
+  for (typename T::const_iterator t1_it = t1.begin(); t1_it != t1.end(); ++t1_it)
+  {
+    typename T::const_iterator t2_it = std::find(t2.begin(), t2.end(), *t1_it);
+    if (t2.end() == t2_it) {return std::vector<SizeType>();}
+    else
+    {
+      const SizeType t1_dist = std::distance(t1.begin(), t1_it);
+      const SizeType 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

@@ -611,6 +611,17 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_callback(
         "Joints on incoming goal don't match the controller joints.");
       return rclcpp_action::GoalResponse::REJECT;
     }
+    
+    std::vector<unsigned int> mapping_vector = mapping(goal->trajectory.joint_names, joint_names_);
+    if (mapping_vector.empty())
+    {
+      RCLCPP_ERROR(
+        lifecycle_node_->get_logger(),
+        "Joints on incoming goal don't match the controller joints.");
+      return rclcpp_action::GoalResponse::REJECT;
+    }
+    
+    goal->trajectory.joint_names.size() != joint_names_.size();
   }
 
   RCLCPP_ERROR(lifecycle_node_->get_logger(), "Accepted new action goal");
@@ -653,6 +664,38 @@ void JointTrajectoryController::feedback_setup_callback(
     rt_active_goal_.reset();
     auto traj_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>(
       goal_handle->get_goal()->trajectory);
+    
+    // rearrange all points in the trajectory message based on mapping
+    std::vector<unsigned int> mapping_vector = mapping(traj_msg->joint_names, joint_names_);
+    auto remap = [](const std::vector<double>& to_remap, const std::vector<unsigned int>& mapping)
+      -> std::vector<double>
+    {
+      if (to_remap.size() != mapping.size())
+        return to_remap;
+      std::vector<double> output;
+      output.resize(mapping.size(), 0.0);
+      for (auto index = 0ul; index<mapping.size(); ++index)
+      {
+        unsigned int map_index = mapping[index];
+        output[map_index] = to_remap[index];
+      }
+      return output;
+    };
+    for (auto index = 0ul; index < traj_msg->points.size(); ++index)
+    {
+      traj_msg->points[index].positions =
+        remap(traj_msg->points[index].positions, mapping_vector);
+
+      traj_msg->points[index].velocities =
+        remap(traj_msg->points[index].velocities, mapping_vector);
+
+      traj_msg->points[index].accelerations =
+        remap(traj_msg->points[index].accelerations, mapping_vector);
+
+      traj_msg->points[index].effort =
+        remap(traj_msg->points[index].effort, mapping_vector);
+    }
+
     traj_external_point_ptr_->update(traj_msg);
 
     RealtimeGoalHandlePtr rt_goal = std::make_shared<RealtimeGoalHandle>(goal_handle);