fixup! Add planner name to trajectory info (moveit#490)

Author: rhaschke

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

@@ -72,7 +72,7 @@ class CartesianPath : public PlannerInterface
 	          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"); }
+	std::string getPlannerId() const override { return "CartesianPath"; }
 };
 }  // namespace solvers
 }  // namespace task_constructor

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

@@ -67,7 +67,7 @@ class JointInterpolationPlanner : public PlannerInterface
 	          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"); }
+	std::string getPlannerId() const override { return "JointInterpolationPlanner"; }
 };
 }  // namespace solvers
 }  // namespace task_constructor

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

@@ -137,11 +137,7 @@ 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;
+	std::string getPlannerId() const override { return last_successful_planner_; }
 
 protected:
 	/** \brief Actual plan() implementation, targeting the given goal_constraints.

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

@@ -75,8 +75,8 @@ class Connect : public Connecting
 
 	struct PlannerIdTrajectoryPair
 	{
-		std::string planner_name;
-		robot_trajectory::RobotTrajectoryConstPtr robot_trajectory_ptr;
+		std::string planner_id;
+		robot_trajectory::RobotTrajectoryConstPtr trajectory;
 	};
 
 	using GroupPlannerVector = std::vector<std::pair<std::string, solvers::PlannerInterfacePtr>>;
@@ -91,10 +91,10 @@ class Connect : public Connecting
 	void compute(const InterfaceState& from, const InterfaceState& to) override;
 
 protected:
-	SolutionSequencePtr makeSequential(const std::vector<PlannerIdTrajectoryPair>& trajectory_planner_vector,
+	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<PlannerIdTrajectoryPair>& trajectory_planner_vector,
+	SubTrajectoryPtr merge(const std::vector<PlannerIdTrajectoryPair>& sub_trajectories,
 	                       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;
 
-	[[nodiscard]] inline const InterfaceState* start() const { return start_; }
-	[[nodiscard]] inline const InterfaceState* end() const { return end_; }
+	inline const InterfaceState* start() const { return start_; }
+	inline const InterfaceState* end() const { return end_; }
 
 	/** Set the solution's start_state_
 	 *
@@ -298,21 +298,21 @@ class SolutionBase
 		const_cast<InterfaceState&>(state).addIncoming(this);
 	}
 
-	[[nodiscard]] inline const Stage* creator() const { return creator_; }
+	inline const Stage* creator() const { return creator_; }
 	void setCreator(Stage* creator);
 
-	[[nodiscard]] inline double cost() const { return cost_; }
+	inline double cost() const { return cost_; }
 	void setCost(double cost);
 	void markAsFailure(const std::string& msg = std::string());
-	[[nodiscard]] inline bool isFailure() const { return !std::isfinite(cost_); }
+	inline bool isFailure() const { return !std::isfinite(cost_); }
 
-	[[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_; }
+	const std::string& comment() const { return comment_; }
 	void setComment(const std::string& comment) { comment_ = comment; }
 
-	[[nodiscard]] auto& markers() { return markers_; }
+	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_; }
 
 	/// convert solution to message
@@ -329,19 +329,18 @@ 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 = std::string(""),
-	             std::string planner_id = std::string(""))
-	  : creator_(creator), cost_(cost), planner_id_(std::move(planner_id)), 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
 	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_;
+	// 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_;
 
@@ -357,7 +356,7 @@ class SubTrajectory : public SolutionBase
 public:
 	SubTrajectory(
 	    const robot_trajectory::RobotTrajectoryConstPtr& trajectory = robot_trajectory::RobotTrajectoryConstPtr(),
-	    double cost = 0.0, std::string comment = std::string(""), std::string planner_id = std::string(""))
+	    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_; }

core/src/solvers/pipeline_planner.cpp

@@ -60,7 +60,6 @@ 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
@@ -148,8 +147,6 @@ 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();
@@ -168,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;
@@ -214,9 +212,6 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning
 	}
 	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

@@ -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<PlannerIdTrajectoryPair> trajectory_planner_vector;
+	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);
-		trajectory_planner_vector.push_back(PlannerIdTrajectoryPair({ pair.second->getPlannerId(), trajectory }));
+		sub_trajectories.push_back({ pair.second->getPlannerId(), trajectory });
 
 		if (!success)
 			break;
@@ -172,41 +172,40 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 
 	SolutionBasePtr solution;
 	if (success && mode != SEQUENTIAL)  // try to merge
-		solution = merge(trajectory_planner_vector, intermediate_scenes, from.scene()->getCurrentState());
+		solution = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
 	if (!solution)  // success == false or merging failed: store sequentially
-		solution = makeSequential(trajectory_planner_vector, intermediate_scenes, from, to);
+		solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
 	if (!success)  // error during sequential planning
 		solution->markAsFailure();
-
 	connect(from, to, solution);
 }
 
 SolutionSequencePtr
-Connect::makeSequential(const std::vector<PlannerIdTrajectoryPair>& trajectory_planner_vector,
+Connect::makeSequential(const std::vector<PlannerIdTrajectoryPair>& sub_trajectories,
                         const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
                         const InterfaceState& from, const InterfaceState& to) {
-	assert(!trajectory_planner_vector.empty());
-	assert(trajectory_planner_vector.size() + 1 == intermediate_scenes.size());
+	assert(!sub_trajectories.empty());
+	assert(sub_trajectories.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& pair : trajectory_planner_vector) {
+	for (const auto& sub : sub_trajectories) {
 		// persistently store sub solution
-		auto inserted = subsolutions_.insert(
-		    subsolutions_.end(), SubTrajectory(pair.robot_trajectory_ptr, 0.0, std::string(""), pair.planner_name));
+		auto inserted = subsolutions_.insert(subsolutions_.end(),
+		                                     SubTrajectory(sub.trajectory, 0.0, std::string(""), sub.planner_id));
 		inserted->setCreator(this);
-		if (!pair.robot_trajectory_ptr)  // 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);
 
 		// create a new end state, either from intermediate or final planning scene
 		planning_scene::PlanningSceneConstPtr end_ps =
-		    (sub_solutions.size() < trajectory_planner_vector.size()) ? *++scene_it : to.scene();
+		    (sub_solutions.size() < sub_trajectories.size()) ? *++scene_it : to.scene();
 		const InterfaceState* end = &*states_.insert(states_.end(), InterfaceState(end_ps));
 
 		// provide newly created start/end states
@@ -219,50 +218,39 @@ Connect::makeSequential(const std::vector<PlannerIdTrajectoryPair>& trajectory_p
 	return std::make_shared<SolutionSequence>(std::move(sub_solutions));
 }
 
-SubTrajectoryPtr Connect::merge(const std::vector<PlannerIdTrajectoryPair>& trajectory_planner_vector,
+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 (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);
+	if (sub_trajectories.size() == 1)
+		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 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"))) {
+	robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(subs, state, jmg, *timing);
+	if (!trajectory)
 		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; });
+	// check merged trajectory for collisions
+	if (!intermediate_scenes.front()->isPathValid(*trajectory,
+	                                              properties().get<moveit_msgs::msg::Constraints>("path_constraints")))
+		return SubTrajectoryPtr();
 
-	// 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);
+	return std::make_shared<SubTrajectory>(trajectory);
+	return std::make_shared<SubTrajectory>(trajectory, 0.0, std::string(""), planner_ids);
 }
 }  // namespace stages
 }  // namespace task_constructor