From 00f6c6fa6aad0000f279f25901660ccbf82482d1 Mon Sep 17 00:00:00 2001
From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
Date: Fri, 2 Aug 2024 10:26:48 -0400
Subject: [PATCH] Deduplicate joint trajectory points in Pilz Move Group
 Sequence capability (#2943)

* Deduplicate joint trajectory points before sending them to controllers

* Fix max loop value

* Move deduplication code to Pilz

* Clean up

* Add todo
---
 .../robot_trajectory/robot_trajectory.h       | 11 ++++++++++
 .../cartesian_trajectory_point.h              |  1 -
 .../src/command_list_manager.cpp              | 21 ++++++++++++++++++-
 3 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h
index 1f77946234..ed2887e2a2 100644
--- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h
+++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h
@@ -244,6 +244,17 @@ class RobotTrajectory
 
   void swap(robot_trajectory::RobotTrajectory& other) noexcept;
 
+  /**
+   * \brief Remove a point from the trajectory
+   * \param index - the index to remove
+   */
+  RobotTrajectory& removeWayPoint(std::size_t index)
+  {
+    waypoints_.erase(waypoints_.begin() + index);
+    duration_from_previous_.erase(duration_from_previous_.begin() + index);
+    return *this;
+  }
+
   RobotTrajectory& clear()
   {
     waypoints_.clear();
diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h
index 4835dc8876..e810ae4726 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h
@@ -38,7 +38,6 @@
 
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/twist.hpp>
-#include <geometry_msgs/msg/twist.hpp>
 
 namespace pilz_industrial_motion_planner
 {
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp
index be11aa84da..de829de34a 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp
@@ -112,7 +112,26 @@ RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConst
                               // therefore: "i-1".
                               (i > 0 ? radii.at(i - 1) : 0.));
   }
-  return plan_comp_builder_.build();
+
+  const auto res_vec = plan_comp_builder_.build();
+
+  // De-duplicate trajectory points with the same time value.
+  // This is necessary since some controllers do not allow times that are not monotonically increasing.
+  // TODO: Ideally, we would not need this code if the trajectory segments were created without
+  // duplicate time points in the first place. Leaving this note to revisit this with a more principled fix.
+  for (const auto& traj : res_vec)
+  {
+    for (size_t i = 0; i < traj->size() - 1; ++i)
+    {
+      if (traj->getWayPointDurationFromStart(i) == traj->getWayPointDurationFromStart(i + 1))
+      {
+        RCLCPP_WARN(getLogger(), "Removed duplicate point at time=%f", traj->getWayPointDurationFromStart(i));
+        traj->removeWayPoint(i + 1);
+      }
+    }
+  }
+
+  return res_vec;
 }
 
 bool CommandListManager::checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A, const double radii_A,