Add planner_id to SubTrajectory info (#490)

This commit is contained in:
Sebastian Jahr 2023-10-09 15:12:58 +02:00 committed by Robert Haschke
parent ed70497d33
commit 1e058598c7
14 changed files with 64 additions and 16 deletions

View File

@ -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

View File

@ -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

View File

@ -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_;

View File

@ -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

View File

@ -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);

View File

@ -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; }

View File

@ -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);
}
}

View File

@ -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

View File

@ -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();

View File

@ -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

View File

@ -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;

View File

@ -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) {

View File

@ -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

View File

@ -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)