Handle failed trajectory in connect stage (moveit#1)

* Handle failed trajectory in connect stage * Add some comments --------- Co-authored-by: Abishalini <[email protected]>
sjahr · May 3, 2023 · d7d8c58 · d7d8c58
1 parent 1e45e85
commit d7d8c58
Showing 1 changed file with 9 additions and 1 deletion.
diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp
@@ -161,7 +161,15 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 
 		robot_trajectory::RobotTrajectoryPtr trajectory;
 		success = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints);
-		sub_trajectories.push_back(trajectory);  // include failed trajectory
+
+		// Do not push partial solutions
+		if (success) {
+			sub_trajectories.push_back(trajectory);  
+		}
+		else {
+			// Pushing a nullptr instead of a failed trajectory. 
+			sub_trajectories.push_back(nullptr); 
+		}
 
 		if (!success)
 			break;