From d7d8c5804cdeef92930e08d579727b621ef1693e Mon Sep 17 00:00:00 2001
From: Abishalini Sivaraman <abi.gpuram@gmail.com>
Date: Fri, 17 Mar 2023 09:21:28 -0600
Subject: [PATCH] Handle failed trajectory in connect stage (#1)

* Handle failed trajectory in connect stage

* Add some comments

---------

Co-authored-by: Abishalini <abishalini.sivaraman@picknik.ai>
---
 core/src/stages/connect.cpp | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp
index e013a180a..eaf628fea 100644
--- 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;