Mark Incomplete trajectories as failure

.github/workflows/ci.yaml

@@ -43,7 +43,7 @@ jobs:
       # TODO: Port to ROS2
       # DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master"
       # Silent googletest warnings: https://github.com/ament/ament_cmake/pull/408#issuecomment-1306004975
-      UPSTREAM_WORKSPACE: github:ament/googletest#clalancette/update-to-gtest-1.11
+      UPSTREAM_WORKSPACE: github:ament/googletest#rolling
       AFTER_SETUP_UPSTREAM_WORKSPACE: dpkg --purge --force-all ros-rolling-gtest-vendor ros-rolling-gmock-vendor
       TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release
       CCACHE_DIR: ${{ github.workspace }}/.ccache

core/include/moveit/task_constructor/stages/connect.h

@@ -80,6 +80,10 @@ class Connect : public Connecting
 		setProperty("path_constraints", std::move(path_constraints));
 	}
 
+	// To make sure the L1 distance between the goal state and the last waypoint in the planned trajectory
+	// are close enough.
+	void setGoalTolerance(float goal_tolerance) { setProperty("goal_tolerance", std::move(goal_tolerance)); }
+
 	void reset() override;
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 	void compute(const InterfaceState& from, const InterfaceState& to) override;
@@ -97,6 +101,8 @@ class Connect : public Connecting
 	moveit::core::JointModelGroupPtr merged_jmg_;
 	std::list<SubTrajectory> subsolutions_;
 	std::list<InterfaceState> states_;
+
+	std::string name_;
 };
 }  // namespace stages
 }  // namespace task_constructor

core/src/stages/connect.cpp

@@ -59,8 +59,12 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) :
 	p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
 	p.declare<moveit_msgs::msg::Constraints>("path_constraints", moveit_msgs::msg::Constraints(),
 	                                         "constraints to maintain during trajectory");
+	p.declare<float>("goal_tolerance", 0.02,
+	                 "Distance between goal state and end of trajectory state should be within this tolerance");
 	properties().declare<TimeParameterizationPtr>("merge_time_parameterization",
 	                                              std::make_shared<TimeOptimalTrajectoryGeneration>());
+
+	name_ = name;
 }
 
 void Connect::reset() {
@@ -149,6 +153,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 
 	bool success = false;
 	std::vector<double> positions;
+
 	for (const GroupPlannerVector::value_type& pair : planner_) {
 		// set intermediate goal state
 		planning_scene::PlanningScenePtr end = start->diff();
@@ -161,13 +166,18 @@ 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);
 
-		// Do not push partial solutions
+		// Check if the end goal and the last waypoint are close. The trajectory should be marked as failure otherwise.
 		if (success) {
-			sub_trajectories.push_back(trajectory);
-		} else {
-			// Pushing a nullptr instead of a failed trajectory.
-			sub_trajectories.push_back(nullptr);
+			const auto distance = end->getCurrentState().distance(trajectory->getLastWayPoint());
+			const auto& goal_tolerance = props.get<float>("goal_tolerance");
+			if (distance > goal_tolerance) {
+				RCLCPP_INFO_STREAM(LOGGER, "Stage Name : " << name_);
+				RCLCPP_INFO_STREAM(LOGGER, "The trajectory given by the plan function does not satisfy the goal state. "
+				                               << "Marking it as a failure");
+				success = false;
+			}
 		}
 
 		if (!success)