From 4fad0d0de6b2b5b17ecdca21923203d2b902369c Mon Sep 17 00:00:00 2001
From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
Date: Sun, 11 Aug 2024 10:23:26 -0400
Subject: [PATCH] Fix Pilz blending times... the right way (#2961)

* Fix Pilz blending times... the right way

* Remove more debugs

* Format
---
 .../src/command_list_manager.cpp              | 20 +------------------
 .../src/plan_components_builder.cpp           | 13 ++++++++----
 .../trajectory_blender_transition_window.cpp  |  3 ++-
 3 files changed, 12 insertions(+), 24 deletions(-)

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 de829de34a..49e58df4bd 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
@@ -113,25 +113,7 @@ RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConst
                               (i > 0 ? radii.at(i - 1) : 0.));
   }
 
-  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;
+  return plan_comp_builder_.build();
 }
 
 bool CommandListManager::checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A, const double radii_A,
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp
index 39c9d1e06c..0513d22b49 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp
@@ -54,13 +54,17 @@ std::vector<robot_trajectory::RobotTrajectoryPtr> PlanComponentsBuilder::build()
 void PlanComponentsBuilder::appendWithStrictTimeIncrease(robot_trajectory::RobotTrajectory& result,
                                                          const robot_trajectory::RobotTrajectory& source)
 {
-  if (result.empty() ||
-      !pilz_industrial_motion_planner::isRobotStateEqual(result.getLastWayPoint(), source.getFirstWayPoint(),
-                                                         result.getGroupName(), ROBOT_STATE_EQUALITY_EPSILON))
+  if (result.empty())
   {
     result.append(source, 0.0);
     return;
   }
+  if (!pilz_industrial_motion_planner::isRobotStateEqual(result.getLastWayPoint(), source.getFirstWayPoint(),
+                                                         result.getGroupName(), ROBOT_STATE_EQUALITY_EPSILON))
+  {
+    result.append(source, source.getWayPointDurationFromStart(0));
+    return;
+  }
 
   for (size_t i = 1; i < source.getWayPointCount(); ++i)
   {
@@ -94,7 +98,8 @@ void PlanComponentsBuilder::blend(const planning_scene::PlanningSceneConstPtr& p
 
   // Append the new trajectory elements
   appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.first_trajectory);
-  traj_cont_.back()->append(*blend_response.blend_trajectory, 0.0);
+  appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.blend_trajectory);
+
   // Store the last new trajectory element for future processing
   traj_tail_ = blend_response.second_trajectory;  // first for next blending segment
 }
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp
index 159640da14..ebdc3d3ee7 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp
@@ -70,7 +70,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend(
   std::size_t second_intersection_index;
   if (!searchIntersectionPoints(req, first_intersection_index, second_intersection_index))
   {
-    RCLCPP_ERROR(getLogger(), "Blend radius to large.");
+    RCLCPP_ERROR(getLogger(), "Blend radius too large.");
     res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
     return false;
   }
@@ -127,6 +127,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend(
 
   // append the blend trajectory
   res.blend_trajectory->setRobotTrajectoryMsg(req.first_trajectory->getFirstWayPoint(), blend_joint_trajectory);
+
   // copy the points [second_intersection_index, len] from the second trajectory
   for (size_t i = second_intersection_index + 1; i < req.second_trajectory->getWayPointCount(); ++i)
   {