mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Add planner_id to SubTrajectory info (#490)
This commit is contained in:
parent
ed70497d33
commit
1e058598c7
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -73,6 +73,12 @@ public:
|
||||
WAYPOINTS = 1
|
||||
};
|
||||
|
||||
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 = {});
|
||||
|
||||
@ -85,10 +91,10 @@ public:
|
||||
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);
|
||||
|
||||
|
||||
@ -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<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; }
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user