Add planner name to trajectory info (moveit#490)

sjahr · rhaschke · commit 8d74d35eef1b · 2024-03-06T18:22:18.000+01:00
* Add planner name to trajectory info

* Extend unittest
diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h
@@ -71,6 +71,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
diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/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
diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h
@@ -137,6 +137,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 Actual plan() implementation, targeting the given goal_constraints.
 	 * \param [in] planning_scene Scene for which the planning should be solved
@@ -155,6 +161,8 @@ class PipelinePlanner : public PlannerInterface
 
 	rclcpp::Node::SharedPtr node_;
 
+	std::string last_successful_planner_;
+
 	/** \brief Map of instantiated (and named) planning pipelines. */
 	std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
 
diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h
@@ -99,6 +99,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
diff --git a/core/include/moveit/task_constructor/stages/connect.h b/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) {
@@ -85,10 +91,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);
 
diff --git a/core/include/moveit/task_constructor/storage.h b/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_; }
 
 	/// convert solution to message
@@ -326,14 +329,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
@@ -351,8 +357,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; }
diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp
@@ -60,6 +60,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_("")
   , stopping_criterion_callback_(stopping_criterion_callback)
   , solution_selection_function_(solution_selection_function) {
 	// Declare properties of the MotionPlanRequest
@@ -147,6 +148,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();
@@ -205,11 +208,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
diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp
@@ -141,7 +141,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();
@@ -161,7 +161,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);  // include failed trajectory
+		trajectory_planner_vector.push_back(PlannerIdTrajectoryPair({ pair.second->getPlannerId(), trajectory }));
 
 		if (!success)
 			break;
@@ -172,39 +172,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
@@ -217,26 +219,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
diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp
@@ -199,6 +199,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 	if (getJointStateFromOffset(direction, dir, 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;
@@ -287,6 +288,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());
 
 		if (robot_trajectory) {  // the following requires a robot_trajectory returned from planning
 			moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
diff --git a/core/src/stages/move_to.cpp b/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
diff --git a/core/src/storage.cpp b/core/src/storage.cpp
@@ -215,6 +215,7 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& inf
 	info.id = introspection ? introspection->solutionId(*this) : 0;
 	info.cost = this->cost();
 	info.comment = this->comment();
+	info.planner_id = this->plannerId();
 	const Introspection* ci = introspection;
 	info.stage_id = ci ? ci->stageId(this->creator()) : 0;
 
diff --git a/core/test/test_pipeline_planner.cpp b/core/test/test_pipeline_planner.cpp
@@ -23,6 +23,7 @@ TEST_F(PipelinePlannerTest, testInitialization) {
 	// WHEN a PipelinePlanner instance is initialized
 	// THEN it does not throw
 	EXPECT_NO_THROW(pipeline_planner.init(robot_model));
+	EXPECT_EQ(pipeline_planner.getPlannerId(), "Unknown");
 }
 
 TEST_F(PipelinePlannerTest, testWithoutPlanningPipelines) {
@@ -44,6 +45,7 @@ TEST_F(PipelinePlannerTest, testValidPlan) {
 	    std::make_shared<robot_trajectory::RobotTrajectory>(robot_model, robot_model->getJointModelGroup("group"));
 	// THEN it returns true
 	EXPECT_TRUE(pipeline_planner.plan(scene, scene, robot_model->getJointModelGroup("group"), 1.0, result));
+	EXPECT_EQ(pipeline_planner.getPlannerId(), "stomp");
 }
 
 TEST_F(PipelinePlannerTest, testInvalidPipelineID) {
diff --git a/msgs/msg/SolutionInfo.msg b/msgs/msg/SolutionInfo.msg
@@ -10,5 +10,8 @@ string comment
 # id of stage that created this trajectory
 uint32 stage_id
 
+# name of the planner that created this solution
+string planner_id
+
 # markers, e.g. providing additional hints or illustrating failure
 visualization_msgs/Marker[] markers
diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp
@@ -295,7 +295,7 @@ void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_m
 DisplaySolutionPtr TaskListModel::processSolutionMessage(const moveit_task_constructor_msgs::msg::Solution& msg) {
 	auto it = remote_tasks_.find(msg.task_id);
 	if (it == remote_tasks_.cend())
-		return DisplaySolutionPtr();  // unkown task
+		return DisplaySolutionPtr();  // unknown task
 
 	RemoteTaskModel* remote_task = it->second;
 	if (!remote_task)
