From 5c4ef60525dd1402ed04064960694ccd8bc9d768 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 9 Oct 2023 15:12:58 +0200 Subject: [PATCH] Add planner name to trajectory info (#490) * Add planner name to trajectory info * Extend unittest --- .../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 | 12 +++- .../include/moveit/task_constructor/storage.h | 28 +++++--- core/src/solvers/pipeline_planner.cpp | 7 ++ core/src/stages/connect.cpp | 68 +++++++++++++------ core/src/stages/move_relative.cpp | 2 + core/src/stages/move_to.cpp | 2 + core/src/storage.cpp | 1 + core/test/test_pipeline_planner.cpp | 2 + msgs/msg/SolutionInfo.msg | 3 + .../src/task_list_model.cpp | 2 +- 14 files changed, 106 insertions(+), 36 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 230cb7b53..8dfa3ea2b 100644 --- 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 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 4415123c5..f024da201 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 @@ -172,6 +178,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 760d1155e..ff9948414 100644 --- 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 index c8cf883d5..94dfde045 100644 --- 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 >; + 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 = {}); 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& sub_trajectories, + SolutionSequencePtr makeSequential(const std::vector& trajectory_planner_vector, const std::vector& intermediate_scenes, const InterfaceState& from, const InterfaceState& to); - SubTrajectoryPtr merge(const std::vector& sub_trajectories, + SubTrajectoryPtr merge(const std::vector& trajectory_planner_vector, const std::vector& 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 index bc75bcc07..5bff53d74 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& 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 index 702cd44cd..0c2435320 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -67,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) { @@ -167,6 +168,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(); @@ -230,11 +233,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 index e013a180a..d4591f5c6 100644 --- 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("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(); @@ -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,20 +172,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, +Connect::makeSequential(const std::vector& trajectory_planner_vector, const std::vector& 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. */ @@ -193,18 +194,19 @@ Connect::makeSequential(const std::vectorsetCreator(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(std::move(sub_solutions)); } -SubTrajectoryPtr Connect::merge(const std::vector& sub_trajectories, +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).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("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_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("path_constraints"))) { return SubTrajectoryPtr(); + } - // check merged trajectory for collisions - if (!intermediate_scenes.front()->isPathValid(*trajectory, - properties().get("path_constraints"))) - return SubTrajectoryPtr(); + // 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.planner_name; }); - return std::make_shared(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(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 index 7052d6c4c..62450fa77 100644 --- 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 index e7257d447..1a44a197a 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; @@ -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 index 16485371c..f08859baf 100644 --- 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 index 347d0b572..6f19a18b8 100644 --- 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_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 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 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)