[JTC] Cleanup API

joint_trajectory_controller/CMakeLists.txt

@@ -59,7 +59,9 @@ if(BUILD_TESTING)
 
   ament_add_gmock(test_trajectory test/test_trajectory.cpp)
   target_link_libraries(test_trajectory joint_trajectory_controller)
-  target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES)
+
+  ament_add_gmock(test_trajectory_operations test/test_trajectory_operations.cpp)
+  target_link_libraries(test_trajectory_operations joint_trajectory_controller)
 
   ament_add_gmock(test_trajectory_controller
     test/test_trajectory_controller.cpp)

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -30,6 +30,7 @@
 #include "joint_trajectory_controller/interpolation_methods.hpp"
 #include "joint_trajectory_controller/tolerances.hpp"
 #include "joint_trajectory_controller/trajectory.hpp"
+#include "joint_trajectory_controller/trajectory_operations.hpp"
 #include "joint_trajectory_controller/visibility_control.h"
 #include "rclcpp/duration.hpp"
 #include "rclcpp/subscription.hpp"
@@ -225,24 +226,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   void compute_error_for_joint(
     JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current,
     const JointTrajectoryPoint & desired) const;
-  // fill trajectory_msg so it matches joints controlled by this controller
-  // positions set to current position, velocities, accelerations and efforts to 0.0
-  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
-  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
-  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
-  void sort_to_local_joint_order(
-    std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg) const;
-  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
-  bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const;
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   void add_new_trajectory_msg(
     const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg);
-  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
-  bool validate_trajectory_point_field(
-    size_t joint_names_size, const std::vector<double> & vector_field,
-    const std::string & string_for_vector_field, size_t i, bool allow_empty) const;
 
   SegmentTolerances default_tolerances_;
 

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

@@ -162,38 +162,6 @@ 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;
-}
-
 /**
  * \param current_position The current position given from the controller, which will be adapted.
  * \param next_position Next position from which to compute the wraparound offset, i.e.,