Edge case fixup

moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp

@@ -156,25 +156,17 @@ bool RuckigSmoothing::runRuckigInBatches(const size_t num_waypoints, robot_traje
   // But we need at least 2 waypoints
   size_t waypoint_batch_size = std::max(size_t(2), temp_batch_size);
   size_t batch_start_idx = 0;
-  size_t batch_end_idx = batch_start_idx + waypoint_batch_size;
+  size_t batch_end_idx = waypoint_batch_size - 1;
   const size_t full_traj_final_idx = num_waypoints - 1;
   // A deep copy is not needed since the waypoints are cleared immediately
   robot_trajectory::RobotTrajectory sub_trajectory =
       robot_trajectory::RobotTrajectory(trajectory, false /* deep copy */);
-  sub_trajectory.clear();
   robot_trajectory::RobotTrajectory output_trajectory =
       robot_trajectory::RobotTrajectory(trajectory, false /* deep copy */);
   output_trajectory.clear();
 
-  while (batch_end_idx < full_traj_final_idx)
+  while (batch_end_idx <= full_traj_final_idx)
   {
-    // We cannot have just 1 waypoint remaining
-    if ((full_traj_final_idx - batch_end_idx) < (waypoint_batch_size + 2))
-    {
-      waypoint_batch_size = full_traj_final_idx - batch_end_idx;
-      batch_end_idx = batch_start_idx + waypoint_batch_size;
-    }
-
     sub_trajectory.clear();
     for (size_t waypoint_idx = batch_start_idx; waypoint_idx <= batch_end_idx; ++waypoint_idx)
     {
@@ -201,7 +193,7 @@ bool RuckigSmoothing::runRuckigInBatches(const size_t num_waypoints, robot_traje
     size_t first_new_waypoint = first_point_previously_smoothed ? 1 : 0;
 
     // Add smoothed waypoints to the output
-    for (size_t waypoint_idx = first_new_waypoint; waypoint_idx < sub_trajectory.getWayPointCount() - 1; ++waypoint_idx)
+    for (size_t waypoint_idx = first_new_waypoint; waypoint_idx < sub_trajectory.getWayPointCount(); ++waypoint_idx)
     {
       output_trajectory.addSuffixWayPoint(sub_trajectory.getWayPoint(waypoint_idx),
                                           sub_trajectory.getWayPointDurationFromPrevious(waypoint_idx));