Add planner name to trajectory info (#5)

Author: sjahr

core/include/moveit/task_constructor/solvers/cartesian_path.h

@@ -69,6 +69,8 @@ class CartesianPath : public PlannerInterface
 	          const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
 	          double timeout, robot_trajectory::RobotTrajectoryPtr& result,
 	          const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
+
+	std::string getPlannerId() const override { return std::string("CartesianPath"); }
 };
 }  // namespace solvers
 }  // namespace task_constructor

core/include/moveit/task_constructor/solvers/joint_interpolation.h

@@ -66,6 +66,8 @@ class JointInterpolationPlanner : public PlannerInterface
 	          const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
 	          double timeout, robot_trajectory::RobotTrajectoryPtr& result,
 	          const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
+
+	std::string getPlannerId() const override { return std::string("JointInterpolationPlanner"); }
 };
 }  // namespace solvers
 }  // namespace task_constructor

core/include/moveit/task_constructor/solvers/pipeline_planner.h

@@ -148,6 +148,12 @@ class PipelinePlanner : public PlannerInterface
 	          robot_trajectory::RobotTrajectoryPtr& result,
 	          const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 
+	/**
+	 * \brief Get planner name
+	 * \return Name of the last successful planner
+	 */
+	std::string getPlannerId() const override;
+
 protected:
 	/** \brief Function that actually uses the planning pipelines to solve the given planning problem. It is called by
 	 * the public plan function after the goal constraints are generated. This function uses a predefined number of
@@ -173,6 +179,8 @@ class PipelinePlanner : public PlannerInterface
 
 	rclcpp::Node::SharedPtr node_;
 
+	std::string last_successful_planner_;
+
 	/** \brief Map of pipeline names (ids) and their corresponding planner ids. The planning problem is solved for every
 	 * pipeline-planner pair in this map. If no pipelines are passed via constructor argument, the pipeline names are
 	 * used to initialize a set of planning pipelines.  */

core/include/moveit/task_constructor/solvers/planner_interface.h

@@ -90,6 +90,9 @@ class PlannerInterface
 	                  const moveit::core::JointModelGroup* jmg, double timeout,
 	                  robot_trajectory::RobotTrajectoryPtr& result,
 	                  const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
+
+	// get name of the planner
+	virtual std::string getPlannerId() const = 0;
 };
 }  // namespace solvers
 }  // namespace task_constructor

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

@@ -73,7 +73,13 @@ class Connect : public Connecting
 		WAYPOINTS = 1
 	};
 
-	using GroupPlannerVector = std::vector<std::pair<std::string, solvers::PlannerInterfacePtr> >;
+	struct PlannerIdTrajectoryPair
+	{
+		std::string planner_name;
+		robot_trajectory::RobotTrajectoryConstPtr robot_trajectory_ptr;
+	};
+
+	using GroupPlannerVector = std::vector<std::pair<std::string, solvers::PlannerInterfacePtr>>;
 	Connect(const std::string& name = "connect", const GroupPlannerVector& planners = {});
 
 	void setPathConstraints(moveit_msgs::msg::Constraints path_constraints) {
@@ -89,10 +95,10 @@ class Connect : public Connecting
 	void compute(const InterfaceState& from, const InterfaceState& to) override;
 
 protected:
-	SolutionSequencePtr makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
+	SolutionSequencePtr makeSequential(const std::vector<PlannerIdTrajectoryPair>& trajectory_planner_vector,
 	                                   const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
 	                                   const InterfaceState& from, const InterfaceState& to);
-	SubTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
+	SubTrajectoryPtr merge(const std::vector<PlannerIdTrajectoryPair>& trajectory_planner_vector,
 	                       const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
 	                       const moveit::core::RobotState& state);
 

core/include/moveit/task_constructor/storage.h

@@ -275,8 +275,8 @@ class SolutionBase
 public:
 	virtual ~SolutionBase() = default;
 
-	inline const InterfaceState* start() const { return start_; }
-	inline const InterfaceState* end() const { return end_; }
+	[[nodiscard]] inline const InterfaceState* start() const { return start_; }
+	[[nodiscard]] inline const InterfaceState* end() const { return end_; }
 
 	/** Set the solution's start_state_
 	 *
@@ -298,18 +298,21 @@ class SolutionBase
 		const_cast<InterfaceState&>(state).addIncoming(this);
 	}
 
-	inline const Stage* creator() const { return creator_; }
+	[[nodiscard]] inline const Stage* creator() const { return creator_; }
 	void setCreator(Stage* creator);
 
-	inline double cost() const { return cost_; }
+	[[nodiscard]] inline double cost() const { return cost_; }
 	void setCost(double cost);
 	void markAsFailure(const std::string& msg = std::string());
-	inline bool isFailure() const { return !std::isfinite(cost_); }
+	[[nodiscard]] inline bool isFailure() const { return !std::isfinite(cost_); }
 
-	const std::string& comment() const { return comment_; }
+	[[nodiscard]] const std::string& plannerId() const { return planner_id_; }
+	void setPlannerId(const std::string& planner_id) { planner_id_ = planner_id; }
+
+	[[nodiscard]] const std::string& comment() const { return comment_; }
 	void setComment(const std::string& comment) { comment_ = comment; }
 
-	auto& markers() { return markers_; }
+	[[nodiscard]] auto& markers() { return markers_; }
 	const auto& markers() const { return markers_; }
 
 	/// append this solution to Solution msg
@@ -324,14 +327,17 @@ class SolutionBase
 	bool operator<(const SolutionBase& other) const { return this->cost_ < other.cost_; }
 
 protected:
-	SolutionBase(Stage* creator = nullptr, double cost = 0.0, std::string comment = "")
-	  : creator_(creator), cost_(cost), comment_(std::move(comment)) {}
+	SolutionBase(Stage* creator = nullptr, double cost = 0.0, std::string comment = std::string(""),
+	             std::string planner_id = std::string(""))
+	  : creator_(creator), cost_(cost), planner_id_(std::move(planner_id)), comment_(std::move(comment)) {}
 
 private:
 	// back-pointer to creating stage, allows to access sub-solutions
 	Stage* creator_;
 	// associated cost
 	double cost_;
+	// name of the planner used to create this solution
+	std::string planner_id_;
 	// comment for this solution, e.g. explanation of failure
 	std::string comment_;
 	// markers for this solution, e.g. target frame or collision indicators
@@ -349,8 +355,8 @@ class SubTrajectory : public SolutionBase
 public:
 	SubTrajectory(
 	    const robot_trajectory::RobotTrajectoryConstPtr& trajectory = robot_trajectory::RobotTrajectoryConstPtr(),
-	    double cost = 0.0, std::string comment = "")
-	  : SolutionBase(nullptr, cost, std::move(comment)), trajectory_(trajectory) {}
+	    double cost = 0.0, std::string comment = std::string(""), std::string planner_id = std::string(""))
+	  : SolutionBase(nullptr, cost, std::move(comment), std::move(planner_id)), trajectory_(trajectory) {}
 
 	robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
 	void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; }

core/src/solvers/pipeline_planner.cpp

@@ -49,10 +49,6 @@
 #include <tf2_eigen/tf2_eigen.h>
 #endif
 
-namespace {
-const std::pair<std::string, std::string> DEFAULT_REQUESTED_PIPELINE =
-    std::pair<std::string, std::string>("ompl", "RRTConnect");
-}
 namespace moveit {
 namespace task_constructor {
 namespace solvers {
@@ -71,6 +67,7 @@ PipelinePlanner::PipelinePlanner(
     const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback,
     const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function)
   : node_(node)
+  , last_successful_planner_("")
   , pipeline_id_planner_id_map_(pipeline_id_planner_id_map)
   , stopping_criterion_callback_(stopping_criterion_callback)
   , solution_selection_function_(solution_selection_function) {
@@ -156,6 +153,8 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co
                            const moveit::core::JointModelGroup* joint_model_group, double timeout,
                            robot_trajectory::RobotTrajectoryPtr& result,
                            const moveit_msgs::msg::Constraints& path_constraints) {
+	last_successful_planner_.clear();
+
 	// Construct a Cartesian target pose from the given target transform and offset
 	geometry_msgs::msg::PoseStamped target;
 	target.header.frame_id = from->getPlanningFrame();
@@ -215,11 +214,15 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning
 		if (solution) {
 			// Choose the first solution trajectory as response
 			result = solution.trajectory;
+			last_successful_planner_ = solution.planner_id;
 			return bool(result);
 		}
 	}
 	return false;
 }
+std::string PipelinePlanner::getPlannerId() const {
+	return last_successful_planner_.empty() ? std::string("Unknown") : last_successful_planner_;
+}
 }  // namespace solvers
 }  // namespace task_constructor
 }  // namespace moveit

core/src/stages/connect.cpp

@@ -145,7 +145,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 	const auto& path_constraints = props.get<moveit_msgs::msg::Constraints>("path_constraints");
 
 	const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState();
-	std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;
+	std::vector<PlannerIdTrajectoryPair> trajectory_planner_vector;
 
 	std::vector<planning_scene::PlanningSceneConstPtr> intermediate_scenes;
 	planning_scene::PlanningSceneConstPtr start = from.scene();
@@ -166,7 +166,7 @@ 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);
+		trajectory_planner_vector.push_back(PlannerIdTrajectoryPair({ pair.second->getPlannerId(), trajectory }));
 
 		// Check if the end goal and the last waypoint are close. The trajectory should be marked as failure otherwise.
 		if (success) {
@@ -189,39 +189,41 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 
 	SolutionBasePtr solution;
 	if (success && mode != SEQUENTIAL)  // try to merge
-		solution = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
+		solution = merge(trajectory_planner_vector, intermediate_scenes, from.scene()->getCurrentState());
 	if (!solution)  // success == false or merging failed: store sequentially
-		solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
+		solution = makeSequential(trajectory_planner_vector, intermediate_scenes, from, to);
 	if (!success)  // error during sequential planning
 		solution->markAsFailure();
+
 	connect(from, to, solution);
 }
 
 SolutionSequencePtr
-Connect::makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
+Connect::makeSequential(const std::vector<PlannerIdTrajectoryPair>& trajectory_planner_vector,
                         const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
                         const InterfaceState& from, const InterfaceState& to) {
-	assert(!sub_trajectories.empty());
-	assert(sub_trajectories.size() + 1 == intermediate_scenes.size());
+	assert(!trajectory_planner_vector.empty());
+	assert(trajectory_planner_vector.size() + 1 == intermediate_scenes.size());
 
 	/* We need to decouple the sequence of subsolutions, created here, from the external from and to states.
 	   Hence, we create new interface states for all subsolutions. */
 	const InterfaceState* start = &*states_.insert(states_.end(), InterfaceState(from.scene()));
 
 	auto scene_it = intermediate_scenes.begin();
 	SolutionSequence::container_type sub_solutions;
-	for (const auto& sub : sub_trajectories) {
+	for (const auto& pair : trajectory_planner_vector) {
 		// persistently store sub solution
-		auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(sub));
+		auto inserted = subsolutions_.insert(
+		    subsolutions_.end(), SubTrajectory(pair.robot_trajectory_ptr, 0.0, std::string(""), pair.planner_name));
 		inserted->setCreator(this);
-		if (!sub)  // a null RobotTrajectoryPtr indicates a failure
+		if (!pair.robot_trajectory_ptr)  // a null RobotTrajectoryPtr indicates a failure
 			inserted->markAsFailure();
 		// push back solution pointer
 		sub_solutions.push_back(&*inserted);
 
 		// create a new end state, either from intermediate or final planning scene
 		planning_scene::PlanningSceneConstPtr end_ps =
-		    (sub_solutions.size() < sub_trajectories.size()) ? *++scene_it : to.scene();
+		    (sub_solutions.size() < trajectory_planner_vector.size()) ? *++scene_it : to.scene();
 		const InterfaceState* end = &*states_.insert(states_.end(), InterfaceState(end_ps));
 
 		// provide newly created start/end states
@@ -234,26 +236,50 @@ Connect::makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConst
 	return std::make_shared<SolutionSequence>(std::move(sub_solutions));
 }
 
-SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
+SubTrajectoryPtr Connect::merge(const std::vector<PlannerIdTrajectoryPair>& trajectory_planner_vector,
                                 const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
                                 const moveit::core::RobotState& state) {
 	// no need to merge if there is only a single sub trajectory
-	if (sub_trajectories.size() == 1)
-		return std::make_shared<SubTrajectory>(sub_trajectories[0]);
+	if (trajectory_planner_vector.size() == 1)
+		return std::make_shared<SubTrajectory>(trajectory_planner_vector.at(0).robot_trajectory_ptr, 0.0, std::string(""),
+		                                       trajectory_planner_vector.at(0).planner_name);
 
 	auto jmg = merged_jmg_.get();
 	assert(jmg);
 	auto timing = properties().get<TimeParameterizationPtr>("merge_time_parameterization");
-	robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg, *timing);
-	if (!trajectory)
+	robot_trajectory::RobotTrajectoryPtr merged_trajectory = task_constructor::merge(
+	    [&]() {
+		    // Move trajectories into single vector
+		    std::vector<robot_trajectory::RobotTrajectoryConstPtr> robot_traj_vector;
+		    robot_traj_vector.reserve(trajectory_planner_vector.size());
+
+		    // Copy second elements of robot planner vector into separate vector
+		    std::transform(begin(trajectory_planner_vector), end(trajectory_planner_vector),
+		                   std::back_inserter(robot_traj_vector),
+		                   [](auto const& pair) { return pair.robot_trajectory_ptr; });
+		    return robot_traj_vector;
+	    }(),
+	    state, jmg, *timing);
+
+	// check merged trajectory is empty or has collisions
+	if (!merged_trajectory ||
+	    !intermediate_scenes.front()->isPathValid(*merged_trajectory,
+	                                              properties().get<moveit_msgs::msg::Constraints>("path_constraints"))) {
 		return SubTrajectoryPtr();
+	}
 
-	// check merged trajectory for collisions
-	if (!intermediate_scenes.front()->isPathValid(*trajectory,
-	                                              properties().get<moveit_msgs::msg::Constraints>("path_constraints")))
-		return SubTrajectoryPtr();
+	// Copy first elements of robot planner vector into separate vector
+	std::vector<std::string> planner_names;
+	planner_names.reserve(trajectory_planner_vector.size());
+	std::transform(begin(trajectory_planner_vector), end(trajectory_planner_vector), std::back_inserter(planner_names),
+	               [](auto const& pair) { return pair.planner_name; });
 
-	return std::make_shared<SubTrajectory>(trajectory);
+	// Create a sequence of planner names
+	std::string planner_name_sequence;
+	for (auto const& name : planner_names) {
+		planner_name_sequence += std::string(", " + name);
+	}
+	return std::make_shared<SubTrajectory>(merged_trajectory, 0.0, std::string(""), planner_name_sequence);
 }
 }  // namespace stages
 }  // namespace task_constructor

core/src/stages/move_relative.cpp

@@ -198,6 +198,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 	if (getJointStateFromOffset(direction, jmg, scene->getCurrentStateNonConst())) {
 		// plan to joint-space target
 		success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
+		solution.setPlannerId(planner_->getPlannerId());
 	} else {
 		// Cartesian targets require an IK reference frame
 		const moveit::core::LinkModel* link;
@@ -286,6 +287,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 
 		success =
 		    planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
+		solution.setPlannerId(planner_->getPlannerId());
 
 		moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
 		reached_state->updateLinkTransforms();

core/src/stages/move_to.cpp

@@ -209,6 +209,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
 	if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) {
 		// plan to joint-space target
 		success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
+		solution.setPlannerId(planner_->getPlannerId());
 	} else {  // Cartesian goal
 		// Where to go?
 		Eigen::Isometry3d target;
@@ -241,6 +242,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
 
 		// plan to Cartesian target
 		success = planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints);
+		solution.setPlannerId(planner_->getPlannerId());
 	}
 
 	// store result