rm unnessary time offset in pilz

moveit · Dec 17, 2024 · 27cdc4f · 27cdc4f
1 parent f91a650
commit 27cdc4f
Showing 1 changed file with 13 additions and 1 deletion.
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
@@ -54,6 +54,11 @@ std::vector<robot_trajectory::RobotTrajectoryPtr> PlanComponentsBuilder::build()
 void PlanComponentsBuilder::appendWithStrictTimeIncrease(robot_trajectory::RobotTrajectory& result,
                                                          const robot_trajectory::RobotTrajectory& source)
 {
+  if (!source.getWayPointCount())
+  {
+    return;
+  }
+
   if (result.empty())
   {
     result.append(source, 0.0);
@@ -62,7 +67,14 @@ void PlanComponentsBuilder::appendWithStrictTimeIncrease(robot_trajectory::Robot
   if (!pilz_industrial_motion_planner::isRobotStateEqual(result.getLastWayPoint(), source.getFirstWayPoint(),
                                                          result.getGroupName(), ROBOT_STATE_EQUALITY_EPSILON))
   {
-    result.append(source, source.getWayPointDurationFromStart(0));
+    if (source.getWayPointDurationFromPrevious(0) == 0)
+    {
+      result.append(source, source.getWayPointDurationFromStart(0));
+    }
+    else
+    {
+      result.append(source, 0.0);
+    }
     return;
   }