From 1e058598c74a4bcce3ba9525223592eb377967bb Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 9 Oct 2023 15:12:58 +0200 Subject: [PATCH] Add planner_id to SubTrajectory info (#490) --- .../task_constructor/solvers/cartesian_path.h | 2 ++ .../solvers/joint_interpolation.h | 2 ++ .../solvers/pipeline_planner.h | 4 +++ .../solvers/planner_interface.h | 3 ++ .../moveit/task_constructor/stages/connect.h | 12 ++++++-- .../include/moveit/task_constructor/storage.h | 13 +++++--- core/src/solvers/pipeline_planner.cpp | 2 ++ core/src/stages/connect.cpp | 30 ++++++++++++++----- 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, 64 insertions(+), 16 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 230cb7b5..7aad5051 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 @@ public: 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 index 4e080c56..e4f65a15 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 @@ public: 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 index ddf4568c..e796e026 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -137,6 +137,8 @@ public: 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 @@ protected: rclcpp::Node::SharedPtr node_; + std::string last_successful_planner_; + /** \brief Map of instantiated (and named) planning pipelines. */ std::unordered_map 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 760d1155..ff994841 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 @@ public: 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 c8cf883d..9ede3ac1 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -73,7 +73,13 @@ public: WAYPOINTS = 1 }; - using GroupPlannerVector = std::vector >; + struct PlannerIdTrajectoryPair + { + std::string planner_id; + robot_trajectory::RobotTrajectoryConstPtr trajectory; + }; + + 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 @@ public: void compute(const InterfaceState& from, const InterfaceState& to) override; protected: - SolutionSequencePtr makeSequential(const std::vector& sub_trajectories, + 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, + SubTrajectoryPtr merge(const std::vector& sub_trajectories, 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 bc75bcc0..0b7f5b6f 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -309,6 +309,9 @@ public: 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 @@ public: 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 @@ private: 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 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 index 80d232ef..d9337ca4 100644 --- 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("pipeline_id_planner_id_map"); + last_successful_planner_ = "Unknown"; // Create a request for every planning pipeline that should run in parallel std::vector 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 index e013a180..db2c600a 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 sub_trajectories; 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 + 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& sub_trajectories, +Connect::makeSequential(const std::vector& sub_trajectories, const std::vector& intermediate_scenes, const InterfaceState& from, const InterfaceState& to) { assert(!sub_trajectories.empty()); @@ -195,9 +195,10 @@ Connect::makeSequential(const std::vectorsetCreator(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(std::move(sub_solutions)); } -SubTrajectoryPtr Connect::merge(const std::vector& sub_trajectories, +SubTrajectoryPtr Connect::merge(const std::vector& sub_trajectories, 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]); + return std::make_shared(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 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("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(trajectory); + return std::make_shared(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 index 7052d6c4..62450fa7 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 e7257d44..1a44a197 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 16485371..f08859ba 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 347d0b57..6f19a18b 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 c3346ab5..5689205c 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 21a54261..3cd64669 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)