From 7c28c6333963cfd977656add4a5e3b6f8c101741 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 12 Jul 2023 18:41:43 +0200 Subject: [PATCH 1/7] Add planner ID to SolutionInfo of SubTrajectories --- .../task_constructor/solvers/cartesian_path.h | 2 + .../solvers/joint_interpolation.h | 2 + .../solvers/pipeline_planner.h | 8 ++ .../solvers/planner_interface.h | 3 + .../moveit/task_constructor/stages/connect.h | 16 ++-- .../include/moveit/task_constructor/storage.h | 28 ++++--- core/src/solvers/pipeline_planner.cpp | 5 ++ core/src/stages/connect.cpp | 84 +++++++++++++------ msgs/msg/SolutionInfo.msg | 3 + 9 files changed, 106 insertions(+), 45 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 41473e368..f6fa0e7ae 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/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 diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/core/include/moveit/task_constructor/solvers/joint_interpolation.h index 4e080c56f..66df1e510 100644 --- 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 index 6609957ed..76fca8ae8 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/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. */ diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index 93ef7772e..0101ce70a 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/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 diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 5a3868e8d..31933e531 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -73,7 +73,7 @@ class Connect : public Connecting WAYPOINTS = 1 }; - using GroupPlannerVector = std::vector >; + using GroupPlannerVector = std::vector>; Connect(const std::string& name = "connect", const GroupPlannerVector& planners = {}); void setPathConstraints(moveit_msgs::msg::Constraints path_constraints) { @@ -89,12 +89,14 @@ class Connect : public Connecting void compute(const InterfaceState& from, const InterfaceState& to) override; protected: - SolutionSequencePtr makeSequential(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes, - const InterfaceState& from, const InterfaceState& to); - SubTrajectoryPtr merge(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes, - const moveit::core::RobotState& state); + SolutionSequencePtr makeSequential( + const std::vector>& trajectory_planner_vector, + const std::vector& intermediate_scenes, const InterfaceState& from, + const InterfaceState& to); + SubTrajectoryPtr merge( + const std::vector>& trajectory_planner_vector, + const std::vector& intermediate_scenes, + const moveit::core::RobotState& state); protected: GroupPlannerVector planner_; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index a3dd3b576..75cb81555 100644 --- 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(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& planner_id() 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; } diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index acfc9e453..c7d07e667 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -71,6 +71,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) { @@ -215,11 +216,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_; +} } // namespace solvers } // namespace task_constructor } // namespace moveit diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 0628dc514..2fd197b54 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -145,7 +145,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { const auto& path_constraints = props.get("path_constraints"); const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState(); - std::vector sub_trajectories; + std::vector> trajectory_planner_vector; std::vector intermediate_scenes; planning_scene::PlanningSceneConstPtr start = from.scene(); @@ -166,7 +166,8 @@ 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( + std::pair(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,20 +190,21 @@ 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& sub_trajectories, - const std::vector& intermediate_scenes, - const InterfaceState& from, const InterfaceState& to) { - assert(!sub_trajectories.empty()); - assert(sub_trajectories.size() + 1 == intermediate_scenes.size()); +SolutionSequencePtr Connect::makeSequential( + const std::vector>& trajectory_planner_vector, + const std::vector& intermediate_scenes, const InterfaceState& from, + const InterfaceState& to) { + 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. */ @@ -210,18 +212,20 @@ Connect::makeSequential(const std::vectorsetCreator(this); - if (!sub) // a null RobotTrajectoryPtr indicates a failure + if (!pair.second) // 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 +238,52 @@ Connect::makeSequential(const std::vector(std::move(sub_solutions)); } -SubTrajectoryPtr Connect::merge(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes, - const moveit::core::RobotState& state) { +SubTrajectoryPtr Connect::merge( + const std::vector>& trajectory_planner_vector, + const std::vector& 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(sub_trajectories[0]); + if (trajectory_planner_vector.size() == 1) + return std::make_shared(trajectory_planner_vector.at(0).second /* robot trajectory */, 0.0, + std::string(""), trajectory_planner_vector.at(0).first /* planner id */); auto jmg = merged_jmg_.get(); assert(jmg); auto timing = properties().get("merge_time_parameterization"); - robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg, *timing); - if (!trajectory) - return SubTrajectoryPtr(); - - // check merged trajectory for collisions - if (!intermediate_scenes.front()->isPathValid(*trajectory, - properties().get("path_constraints"))) + robot_trajectory::RobotTrajectoryPtr merged_trajectory = task_constructor::merge( + [&]() { + // Move trajectories into single vector + std::vector 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.second; }); + 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("path_constraints"))) { return SubTrajectoryPtr(); + } - return std::make_shared(trajectory); + // Copy first elements of robot planner vector into separate vector + std::vector 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.first; /* planner name */ + }); + + // 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(merged_trajectory, 0.0, std::string(""), planner_name_sequence); } } // namespace stages } // namespace task_constructor diff --git a/msgs/msg/SolutionInfo.msg b/msgs/msg/SolutionInfo.msg index c3346ab53..5689205c2 100644 --- 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 From 2ae376560fa3aa28fdb5512433e8f10599a32962 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 13 Jul 2023 12:03:17 +0200 Subject: [PATCH 2/7] Add planner id to fillInfo method --- core/include/moveit/task_constructor/storage.h | 2 +- core/src/storage.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 75cb81555..84217eac2 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -306,7 +306,7 @@ class SolutionBase void markAsFailure(const std::string& msg = std::string()); [[nodiscard]] inline bool isFailure() const { return !std::isfinite(cost_); } - [[nodiscard]] const std::string& planner_id() const { return planner_id_; } + [[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_; } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 1d399375e..2e210c7fe 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -210,6 +210,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; From 1553120a15d56c5771510402d8af649616a76134 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 19 Jul 2023 15:51:37 +0200 Subject: [PATCH 3/7] Set planner id in MoveRelative and MoveTo too --- core/src/solvers/pipeline_planner.cpp | 4 ---- core/src/stages/move_relative.cpp | 1 + core/src/stages/move_to.cpp | 1 + 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index c7d07e667..36485c291 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -49,10 +49,6 @@ #include #endif -namespace { -const std::pair DEFAULT_REQUESTED_PIPELINE = - std::pair("ompl", "RRTConnect"); -} namespace moveit { namespace task_constructor { namespace solvers { diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 447eb7b15..d6bac4327 100644 --- a/core/src/stages/move_relative.cpp +++ b/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; diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index e7257d447..cd889ab3d 100644 --- 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; From babbbc6a6ddaacc208831586e2bf185a53115d1e Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 25 Jul 2023 10:02:52 +0200 Subject: [PATCH 4/7] Convert pair to struct --- .../moveit/task_constructor/stages/connect.h | 20 ++++++---- core/src/stages/connect.cpp | 38 +++++++++---------- 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 31933e531..d8fe98f76 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -73,6 +73,12 @@ class Connect : public Connecting WAYPOINTS = 1 }; + struct PlannerIdTrajectoryPair + { + std::string planner_name; + robot_trajectory::RobotTrajectoryConstPtr robot_trajectory_ptr; + }; + using GroupPlannerVector = std::vector>; Connect(const std::string& name = "connect", const GroupPlannerVector& planners = {}); @@ -89,14 +95,12 @@ class Connect : public Connecting void compute(const InterfaceState& from, const InterfaceState& to) override; protected: - SolutionSequencePtr makeSequential( - const std::vector>& trajectory_planner_vector, - const std::vector& intermediate_scenes, const InterfaceState& from, - const InterfaceState& to); - SubTrajectoryPtr merge( - const std::vector>& trajectory_planner_vector, - const std::vector& intermediate_scenes, - const moveit::core::RobotState& state); + SolutionSequencePtr makeSequential(const std::vector& trajectory_planner_vector, + const std::vector& intermediate_scenes, + const InterfaceState& from, const InterfaceState& to); + SubTrajectoryPtr merge(const std::vector& trajectory_planner_vector, + const std::vector& intermediate_scenes, + const moveit::core::RobotState& state); protected: GroupPlannerVector planner_; diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 2fd197b54..331901e3f 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -145,7 +145,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { const auto& path_constraints = props.get("path_constraints"); const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState(); - std::vector> trajectory_planner_vector; + std::vector trajectory_planner_vector; std::vector intermediate_scenes; planning_scene::PlanningSceneConstPtr start = from.scene(); @@ -166,8 +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); - trajectory_planner_vector.push_back( - std::pair(pair.second->getPlannerId(), 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) { @@ -199,10 +198,10 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { connect(from, to, solution); } -SolutionSequencePtr Connect::makeSequential( - const std::vector>& trajectory_planner_vector, - const std::vector& intermediate_scenes, const InterfaceState& from, - const InterfaceState& to) { +SolutionSequencePtr +Connect::makeSequential(const std::vector& trajectory_planner_vector, + const std::vector& intermediate_scenes, + const InterfaceState& from, const InterfaceState& to) { assert(!trajectory_planner_vector.empty()); assert(trajectory_planner_vector.size() + 1 == intermediate_scenes.size()); @@ -214,11 +213,10 @@ SolutionSequencePtr Connect::makeSequential( SolutionSequence::container_type sub_solutions; for (const auto& pair : trajectory_planner_vector) { // persistently store sub solution - auto inserted = - subsolutions_.insert(subsolutions_.end(), SubTrajectory(pair.second /* robot trajectory */, 0.0, - std::string(""), pair.first /* planner id */)); + auto inserted = subsolutions_.insert( + subsolutions_.end(), SubTrajectory(pair.robot_trajectory_ptr, 0.0, std::string(""), pair.planner_name)); inserted->setCreator(this); - if (!pair.second) // 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); @@ -238,14 +236,13 @@ SolutionSequencePtr Connect::makeSequential( return std::make_shared(std::move(sub_solutions)); } -SubTrajectoryPtr Connect::merge( - const std::vector>& trajectory_planner_vector, - const std::vector& intermediate_scenes, - const moveit::core::RobotState& state) { +SubTrajectoryPtr Connect::merge(const std::vector& trajectory_planner_vector, + const std::vector& 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(trajectory_planner_vector.at(0).second /* robot trajectory */, 0.0, - std::string(""), trajectory_planner_vector.at(0).first /* planner id */); + return std::make_shared(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); @@ -258,7 +255,8 @@ SubTrajectoryPtr Connect::merge( // 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.second; }); + std::back_inserter(robot_traj_vector), + [](auto const& pair) { return pair.robot_trajectory_ptr; }); return robot_traj_vector; }(), state, jmg, *timing); @@ -274,9 +272,7 @@ SubTrajectoryPtr Connect::merge( std::vector 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.first; /* planner name */ - }); + [](auto const& pair) { return pair.planner_name; }); // Create a sequence of planner names std::string planner_name_sequence; From ee65d3ec66bf3223bfa655d9e027f539b2cb9b57 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 26 Jul 2023 11:13:55 +0200 Subject: [PATCH 5/7] Add missing planner_->getPlannerId() calls --- core/src/stages/move_relative.cpp | 1 + core/src/stages/move_to.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index d6bac4327..c152ad7a8 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -287,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(); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index cd889ab3d..1a44a197a 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -242,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 From 42a3c33ff3c3d28219c27acb6430c569b3ef0158 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 26 Jul 2023 11:30:48 +0200 Subject: [PATCH 6/7] Return "Unkown" if last_successful_planner_ is not set --- core/src/solvers/pipeline_planner.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 36485c291..71c56de50 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -153,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(); @@ -219,7 +221,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning return false; } std::string PipelinePlanner::getPlannerId() const { - return last_successful_planner_; + return last_successful_planner_.empty() ? std::string("Unkown") : last_successful_planner_; } } // namespace solvers } // namespace task_constructor From b16b438f3df1db850517615f468decf585f89d13 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 27 Jul 2023 09:03:10 +0200 Subject: [PATCH 7/7] Unkown -> Unknown --- core/src/solvers/pipeline_planner.cpp | 2 +- visualization/motion_planning_tasks/src/task_list_model.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 71c56de50..c08cb097c 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -221,7 +221,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning return false; } std::string PipelinePlanner::getPlannerId() const { - return last_successful_planner_.empty() ? std::string("Unkown") : last_successful_planner_; + return last_successful_planner_.empty() ? std::string("Unknown") : last_successful_planner_; } } // namespace solvers } // namespace task_constructor diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index 21a542612..3cd646690 100644 --- 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)