Fix Pilz blending times... the right way (#2961)

* Fix Pilz blending times... the right way * Remove more debugs * Format (cherry picked from commit 4fad0d0) # Conflicts: # moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp # moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp
moveit · Sep 14, 2024 · 4488894 · 4488894
1 parent 0a4f1ff
commit 4488894
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 4 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
@@ -105,6 +105,10 @@ RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConst
                               // therefore: "i-1".
                               (i > 0 ? radii.at(i - 1) : 0.));
   }
+<<<<<<< HEAD
+=======
+
+>>>>>>> 4fad0d0de (Fix Pilz blending times... the right way (#2961))
   return plan_comp_builder_.build();
 }
 

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,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
@@ -67,7 +67,11 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend(
   std::size_t second_intersection_index;
   if (!searchIntersectionPoints(req, first_intersection_index, second_intersection_index))
   {
+<<<<<<< HEAD
     RCLCPP_ERROR(LOGGER, "Blend radius to large.");
+=======
+    RCLCPP_ERROR(getLogger(), "Blend radius too large.");
+>>>>>>> 4fad0d0de (Fix Pilz blending times... the right way (#2961))
     res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
     return false;
   }
@@ -124,6 +128,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)
   {