Add planner_id to SubTrajectory info (moveit#490)

sjahr · rhaschke · commit 1e058598c74a · 2024-03-06T18:11:57.000+01:00
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 "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 "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,8 @@ class PipelinePlanner : public PlannerInterface
 	          robot_trajectory::RobotTrajectoryPtr& result,
 	          const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 
+	std::string getPlannerId() const override { return last_successful_planner_; }
+
 protected:
 	/** \brief Actual plan() implementation, targeting the given goal_constraints.
 	 * \param [in] planning_scene Scene for which the planning should be solved
@@ -155,6 +157,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_id;
+		robot_trajectory::RobotTrajectoryConstPtr trajectory;
+	};
+
+	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>& sub_trajectories,
 	                                   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>& sub_trajectories,
 	                       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
@@ -309,6 +309,9 @@ class SolutionBase
 	const std::string& comment() const { return comment_; }
 	void setComment(const std::string& comment) { comment_ = comment; }
 
+	const std::string& plannerId() const { return planner_id_; }
+	void setPlannerId(const std::string& planner_id) { planner_id_ = planner_id; }
+
 	auto& markers() { return markers_; }
 	const auto& markers() const { return markers_; }
 
@@ -326,8 +329,8 @@ 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 planner_id = "")
+	  : creator_(creator), cost_(cost), comment_(std::move(comment)), planner_id_(std::move(planner_id)) {}
 
 private:
 	// back-pointer to creating stage, allows to access sub-solutions
@@ -336,6 +339,8 @@ class SolutionBase
 	double cost_;
 	// comment for this solution, e.g. explanation of failure
 	std::string comment_;
+	// name of the planner used to create this solution
+	std::string planner_id_;
 	// markers for this solution, e.g. target frame or collision indicators
 	std::deque<visualization_msgs::msg::Marker> markers_;
 
@@ -351,8 +356,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 planner_id = "")
+	  : 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
@@ -165,6 +165,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning
                            robot_trajectory::RobotTrajectoryPtr& result,
                            const moveit_msgs::msg::Constraints& path_constraints) {
 	const auto& map = properties().get<PipelineMap>("pipeline_id_planner_id_map");
+	last_successful_planner_ = "Unknown";
 
 	// Create a request for every planning pipeline that should run in parallel
 	std::vector<moveit_msgs::msg::MotionPlanRequest> requests;
@@ -205,6 +206,7 @@ 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);
 		}
 	}
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> sub_trajectories;
 
 	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
+		sub_trajectories.push_back({ pair.second->getPlannerId(), trajectory });
 
 		if (!success)
 			break;
@@ -181,7 +181,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 }
 
 SolutionSequencePtr
-Connect::makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
+Connect::makeSequential(const std::vector<PlannerIdTrajectoryPair>& sub_trajectories,
                         const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
                         const InterfaceState& from, const InterfaceState& to) {
 	assert(!sub_trajectories.empty());
@@ -195,9 +195,10 @@ Connect::makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConst
 	SolutionSequence::container_type sub_solutions;
 	for (const auto& sub : sub_trajectories) {
 		// persistently store sub solution
-		auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(sub));
+		auto inserted = subsolutions_.insert(subsolutions_.end(),
+		                                     SubTrajectory(sub.trajectory, 0.0, std::string(""), sub.planner_id));
 		inserted->setCreator(this);
-		if (!sub)  // a null RobotTrajectoryPtr indicates a failure
+		if (!sub.trajectory)  // a null RobotTrajectoryPtr indicates a failure
 			inserted->markAsFailure();
 		// push back solution pointer
 		sub_solutions.push_back(&*inserted);
@@ -217,17 +218,29 @@ 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>& sub_trajectories,
                                 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]);
+		return std::make_shared<SubTrajectory>(sub_trajectories.at(0).trajectory, 0.0, std::string(""),
+		                                       sub_trajectories.at(0).planner_id);
+
+	// split sub_trajectories into trajectories and joined planner_ids
+	std::string planner_ids;
+	std::vector<robot_trajectory::RobotTrajectoryConstPtr> subs;
+	subs.reserve(sub_trajectories.size());
+	for (auto it = sub_trajectories.begin(); it != sub_trajectories.end(); ++it) {
+		subs.push_back(it->trajectory);
+		if (it != sub_trajectories.begin())
+			planner_ids += ", ";
+		planner_ids += it->planner_id;
+	}
 
 	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);
+	robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(subs, state, jmg, *timing);
 	if (!trajectory)
 		return SubTrajectoryPtr();
 
@@ -237,6 +250,7 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject
 		return SubTrajectoryPtr();
 
 	return std::make_shared<SubTrajectory>(trajectory);
+	return std::make_shared<SubTrajectory>(trajectory, 0.0, std::string(""), planner_ids);
 }
 }  // 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)

Original file line number	Diff line number	Diff line change
`@@ -165,6 +165,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning`
`165`	`165`	`robot_trajectory::RobotTrajectoryPtr& result,`
`166`	`166`	`const moveit_msgs::msg::Constraints& path_constraints) {`
`167`	`167`	`const auto& map = properties().get<PipelineMap>("pipeline_id_planner_id_map");`
	`168`	`+ last_successful_planner_ = "Unknown";`
`168`	`169`
`169`	`170`	`// Create a request for every planning pipeline that should run in parallel`
`170`	`171`	`std::vector<moveit_msgs::msg::MotionPlanRequest> requests;`
`@@ -205,6 +206,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning`
`205`	`206`	`if (solution) {`
`206`	`207`	`// Choose the first solution trajectory as response`
`207`	`208`	`result = solution.trajectory;`
	`209`	`+ last_successful_planner_ = solution.planner_id;`
`208`	`210`	`return bool(result);`
`209`	`211`	`}`
`210`	`212`	`}`