Debug print function for RobotTrajectory

moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h

@@ -273,11 +273,30 @@ class RobotTrajectory
    */
   bool getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr& output_state) const;
 
+  /** @brief Print information about the trajectory
+   * @param out              Stream to print to
+   * @param variable_indexes The indexes of the variables to print.
+   *                         If empty/not specified and the group is defined, then uses the indexes for the group
+   *                         If empty and the group is not defined, uses ALL variables in robot_model
+   *
+   * e.g.
+   * Trajectory has 13 points over 2.965 seconds
+   *   waypoint   0 time 0.000 pos  0.000 vel  0.000 acc  0.000
+   *   waypoint   1 time 0.067 pos  0.001 vel  0.033 acc  1.000
+   *   waypoint   2 time 0.665 pos  0.200 vel  0.632 acc  1.000
+   *   ...
+   */
+  void print(std::ostream& out, std::vector<int> variable_indexes = std::vector<int>()) const;
+
 private:
   moveit::core::RobotModelConstPtr robot_model_;
   const moveit::core::JointModelGroup* group_;
   std::deque<moveit::core::RobotStatePtr> waypoints_;
   std::deque<double> duration_from_previous_;
   rclcpp::Clock clock_ros_;
 };
+
+/** @brief Operator overload for printing trajectory to a stream */
+std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory);
+
 }  // namespace robot_trajectory

moveit_core/robot_trajectory/src/robot_trajectory.cpp

@@ -504,4 +504,81 @@ bool RobotTrajectory::getStateAtDurationFromStart(const double request_duration,
   return true;
 }
 
+void RobotTrajectory::print(std::ostream& out, std::vector<int> variable_indexes) const
+{
+  size_t num_points = getWayPointCount();
+  if (num_points == 0)
+  {
+    out << "Empty trajectory.";
+    return;
+  }
+
+  std::ios::fmtflags old_settings = out.flags();
+  int old_precision = out.precision();
+  out << std::fixed << std::setprecision(3);
+
+  out << "Trajectory has " << num_points << " points over " << getDuration() << " seconds\n";
+
+  if (variable_indexes.empty())
+  {
+    if (group_)
+    {
+      variable_indexes = group_->getVariableIndexList();
+    }
+    else
+    {
+      // use all variables
+      variable_indexes.resize(robot_model_->getVariableCount());
+      std::iota(variable_indexes.begin(), variable_indexes.end(), 0);
+    }
+  }
+
+  for (size_t p_i = 0; p_i < num_points; ++p_i)
+  {
+    const moveit::core::RobotState& point = getWayPoint(p_i);
+    out << "  waypoint " << std::setw(3) << p_i;
+    out << " time " << std::setw(5) << getWayPointDurationFromStart(p_i);
+    out << " pos ";
+    for (int index : variable_indexes)
+    {
+      out << std::setw(6) << point.getVariablePosition(index) << " ";
+    }
+    if (point.hasVelocities())
+    {
+      out << "vel ";
+      for (int index : variable_indexes)
+      {
+        out << std::setw(6) << point.getVariableVelocity(index) << " ";
+      }
+    }
+    if (point.hasAccelerations())
+    {
+      out << "acc ";
+      for (int index : variable_indexes)
+      {
+        out << std::setw(6) << point.getVariableAcceleration(index) << " ";
+      }
+    }
+    if (point.hasEffort())
+    {
+      out << "eff ";
+      for (int index : variable_indexes)
+      {
+        out << std::setw(6) << point.getVariableEffort(index) << " ";
+      }
+    }
+    out << "\n";
+  }
+
+  out.flags(old_settings);
+  out.precision(old_precision);
+  out.flush();
+}
+
+std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory)
+{
+  trajectory.print(out);
+  return out;
+}
+
 }  // end of namespace robot_trajectory

moveit_core/trajectory_processing/test/test_time_parameterization.cpp

@@ -115,26 +115,7 @@ void printTrajectory(robot_trajectory::RobotTrajectory& trajectory)
 {
   const moveit::core::JointModelGroup* group = trajectory.getGroup();
   const std::vector<int>& idx = group->getVariableIndexList();
-  unsigned int count = trajectory.getWayPointCount();
-
-  std::cout << "trajectory length is " << trajectory.getWayPointDurationFromStart(count - 1) << " seconds."
-            << std::endl;
-  std::cout << "  Trajectory Points" << std::endl;
-  for (unsigned i = 0; i < count; i++)
-  {
-    moveit::core::RobotStatePtr point = trajectory.getWayPointPtr(i);
-    printf("  waypoint %2d time %6.2f pos %6.2f vel %6.2f acc %6.2f ", i, trajectory.getWayPointDurationFromStart(i),
-           point->getVariablePosition(idx[0]), point->getVariableVelocity(idx[0]),
-           point->getVariableAcceleration(idx[0]));
-    if (i > 0)
-    {
-      moveit::core::RobotStatePtr prev = trajectory.getWayPointPtr(i - 1);
-      printf("jrk %6.2f",
-             (point->getVariableAcceleration(idx[0]) - prev->getVariableAcceleration(idx[0])) /
-                 (trajectory.getWayPointDurationFromStart(i) - trajectory.getWayPointDurationFromStart(i - 1)));
-    }
-    printf("\n");
-  }
+  trajectory.print(std::cout, { idx[0] });
 }
 
 TEST(TestTimeParameterization, TestIterativeParabolic)