PipelinePlanner: Truncate trajectory to valid part

Fixes moveit/moveit_task_constructor#538

moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h

@@ -227,6 +227,9 @@ class RobotTrajectory
   RobotTrajectory& append(const RobotTrajectory& source, double dt, size_t start_index = 0,
                           size_t end_index = std::numeric_limits<std::size_t>::max());
 
+  /** Truncate trajectory to given size */
+  void truncate(size_t size);
+
   void swap(robot_trajectory::RobotTrajectory& other);
 
   RobotTrajectory& clear()

moveit_core/robot_trajectory/src/robot_trajectory.cpp

@@ -117,6 +117,13 @@ RobotTrajectory& RobotTrajectory::append(const RobotTrajectory& source, double d
   return *this;
 }
 
+void RobotTrajectory::truncate(size_t size)
+{
+  size = std::min(size, waypoints_.size());
+  waypoints_.resize(size);
+  duration_from_previous_.resize(size);
+}
+
 RobotTrajectory& RobotTrajectory::reverse()
 {
   std::reverse(waypoints_.begin(), waypoints_.end());

moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp

@@ -337,6 +337,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
               }
             }
             ROS_ERROR_STREAM("Completed listing of explanations for invalid states.");
+            res.trajectory_->truncate(index[0]);  // truncate returned trajectory to valid part
           }
         }
         else