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,
|
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
||||||
|
|
||||||
|
std::string getPlannerId() const override { return "CartesianPath"; }
|
||||||
};
|
};
|
||||||
} // namespace solvers
|
} // namespace solvers
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
|
|||||||
@ -66,6 +66,8 @@ public:
|
|||||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
||||||
|
|
||||||
|
std::string getPlannerId() const override { return "JointInterpolationPlanner"; }
|
||||||
};
|
};
|
||||||
} // namespace solvers
|
} // namespace solvers
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
|
|||||||
@ -137,6 +137,8 @@ public:
|
|||||||
robot_trajectory::RobotTrajectoryPtr& result,
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
||||||
|
|
||||||
|
std::string getPlannerId() const override { return last_successful_planner_; }
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
/** \brief Actual plan() implementation, targeting the given goal_constraints.
|
/** \brief Actual plan() implementation, targeting the given goal_constraints.
|
||||||
* \param [in] planning_scene Scene for which the planning should be solved
|
* \param [in] planning_scene Scene for which the planning should be solved
|
||||||
@ -155,6 +157,8 @@ protected:
|
|||||||
|
|
||||||
rclcpp::Node::SharedPtr node_;
|
rclcpp::Node::SharedPtr node_;
|
||||||
|
|
||||||
|
std::string last_successful_planner_;
|
||||||
|
|
||||||
/** \brief Map of instantiated (and named) planning pipelines. */
|
/** \brief Map of instantiated (and named) planning pipelines. */
|
||||||
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
|
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
|
||||||
|
|
||||||
|
|||||||
@ -99,6 +99,9 @@ public:
|
|||||||
const moveit::core::JointModelGroup* jmg, double timeout,
|
const moveit::core::JointModelGroup* jmg, double timeout,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result,
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
|
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 solvers
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
|
|||||||
@ -73,7 +73,13 @@ public:
|
|||||||
WAYPOINTS = 1
|
WAYPOINTS = 1
|
||||||
};
|
};
|
||||||
|
|
||||||
using GroupPlannerVector = std::vector<std::pair<std::string, solvers::PlannerInterfacePtr> >;
|
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 = {});
|
Connect(const std::string& name = "connect", const GroupPlannerVector& planners = {});
|
||||||
|
|
||||||
void setPathConstraints(moveit_msgs::msg::Constraints path_constraints) {
|
void setPathConstraints(moveit_msgs::msg::Constraints path_constraints) {
|
||||||
@ -85,10 +91,10 @@ public:
|
|||||||
void compute(const InterfaceState& from, const InterfaceState& to) override;
|
void compute(const InterfaceState& from, const InterfaceState& to) override;
|
||||||
|
|
||||||
protected:
|
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 std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
|
||||||
const InterfaceState& from, const InterfaceState& to);
|
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 std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
|
||||||
const moveit::core::RobotState& state);
|
const moveit::core::RobotState& state);
|
||||||
|
|
||||||
|
|||||||
@ -309,6 +309,9 @@ public:
|
|||||||
const std::string& comment() const { return comment_; }
|
const std::string& comment() const { return comment_; }
|
||||||
void setComment(const std::string& comment) { comment_ = 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_; }
|
auto& markers() { return markers_; }
|
||||||
const auto& markers() const { return markers_; }
|
const auto& markers() const { return markers_; }
|
||||||
|
|
||||||
@ -326,8 +329,8 @@ public:
|
|||||||
bool operator<(const SolutionBase& other) const { return this->cost_ < other.cost_; }
|
bool operator<(const SolutionBase& other) const { return this->cost_ < other.cost_; }
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
SolutionBase(Stage* creator = nullptr, double cost = 0.0, std::string comment = "")
|
SolutionBase(Stage* creator = nullptr, double cost = 0.0, std::string comment = "", std::string planner_id = "")
|
||||||
: creator_(creator), cost_(cost), comment_(std::move(comment)) {}
|
: creator_(creator), cost_(cost), comment_(std::move(comment)), planner_id_(std::move(planner_id)) {}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// back-pointer to creating stage, allows to access sub-solutions
|
// back-pointer to creating stage, allows to access sub-solutions
|
||||||
@ -336,6 +339,8 @@ private:
|
|||||||
double cost_;
|
double cost_;
|
||||||
// comment for this solution, e.g. explanation of failure
|
// comment for this solution, e.g. explanation of failure
|
||||||
std::string comment_;
|
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
|
// markers for this solution, e.g. target frame or collision indicators
|
||||||
std::deque<visualization_msgs::msg::Marker> markers_;
|
std::deque<visualization_msgs::msg::Marker> markers_;
|
||||||
|
|
||||||
@ -351,8 +356,8 @@ class SubTrajectory : public SolutionBase
|
|||||||
public:
|
public:
|
||||||
SubTrajectory(
|
SubTrajectory(
|
||||||
const robot_trajectory::RobotTrajectoryConstPtr& trajectory = robot_trajectory::RobotTrajectoryConstPtr(),
|
const robot_trajectory::RobotTrajectoryConstPtr& trajectory = robot_trajectory::RobotTrajectoryConstPtr(),
|
||||||
double cost = 0.0, std::string comment = "")
|
double cost = 0.0, std::string comment = "", std::string planner_id = "")
|
||||||
: SolutionBase(nullptr, cost, std::move(comment)), trajectory_(trajectory) {}
|
: SolutionBase(nullptr, cost, std::move(comment), std::move(planner_id)), trajectory_(trajectory) {}
|
||||||
|
|
||||||
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
|
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
|
||||||
void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; }
|
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,
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::msg::Constraints& path_constraints) {
|
const moveit_msgs::msg::Constraints& path_constraints) {
|
||||||
const auto& map = properties().get<PipelineMap>("pipeline_id_planner_id_map");
|
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
|
// Create a request for every planning pipeline that should run in parallel
|
||||||
std::vector<moveit_msgs::msg::MotionPlanRequest> requests;
|
std::vector<moveit_msgs::msg::MotionPlanRequest> requests;
|
||||||
@ -205,6 +206,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning
|
|||||||
if (solution) {
|
if (solution) {
|
||||||
// Choose the first solution trajectory as response
|
// Choose the first solution trajectory as response
|
||||||
result = solution.trajectory;
|
result = solution.trajectory;
|
||||||
|
last_successful_planner_ = solution.planner_id;
|
||||||
return bool(result);
|
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 auto& path_constraints = props.get<moveit_msgs::msg::Constraints>("path_constraints");
|
||||||
|
|
||||||
const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState();
|
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;
|
std::vector<planning_scene::PlanningSceneConstPtr> intermediate_scenes;
|
||||||
planning_scene::PlanningSceneConstPtr start = from.scene();
|
planning_scene::PlanningSceneConstPtr start = from.scene();
|
||||||
@ -161,7 +161,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
|||||||
|
|
||||||
robot_trajectory::RobotTrajectoryPtr trajectory;
|
robot_trajectory::RobotTrajectoryPtr trajectory;
|
||||||
success = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints);
|
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)
|
if (!success)
|
||||||
break;
|
break;
|
||||||
@ -181,7 +181,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
SolutionSequencePtr
|
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 std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
|
||||||
const InterfaceState& from, const InterfaceState& to) {
|
const InterfaceState& from, const InterfaceState& to) {
|
||||||
assert(!sub_trajectories.empty());
|
assert(!sub_trajectories.empty());
|
||||||
@ -195,9 +195,10 @@ Connect::makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConst
|
|||||||
SolutionSequence::container_type sub_solutions;
|
SolutionSequence::container_type sub_solutions;
|
||||||
for (const auto& sub : sub_trajectories) {
|
for (const auto& sub : sub_trajectories) {
|
||||||
// persistently store sub solution
|
// 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);
|
inserted->setCreator(this);
|
||||||
if (!sub) // a null RobotTrajectoryPtr indicates a failure
|
if (!sub.trajectory) // a null RobotTrajectoryPtr indicates a failure
|
||||||
inserted->markAsFailure();
|
inserted->markAsFailure();
|
||||||
// push back solution pointer
|
// push back solution pointer
|
||||||
sub_solutions.push_back(&*inserted);
|
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));
|
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 std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
|
||||||
const moveit::core::RobotState& state) {
|
const moveit::core::RobotState& state) {
|
||||||
// no need to merge if there is only a single sub trajectory
|
// no need to merge if there is only a single sub trajectory
|
||||||
if (sub_trajectories.size() == 1)
|
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();
|
auto jmg = merged_jmg_.get();
|
||||||
assert(jmg);
|
assert(jmg);
|
||||||
auto timing = properties().get<TimeParameterizationPtr>("merge_time_parameterization");
|
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)
|
if (!trajectory)
|
||||||
return SubTrajectoryPtr();
|
return SubTrajectoryPtr();
|
||||||
|
|
||||||
@ -237,6 +250,7 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject
|
|||||||
return SubTrajectoryPtr();
|
return SubTrajectoryPtr();
|
||||||
|
|
||||||
return std::make_shared<SubTrajectory>(trajectory);
|
return std::make_shared<SubTrajectory>(trajectory);
|
||||||
|
return std::make_shared<SubTrajectory>(trajectory, 0.0, std::string(""), planner_ids);
|
||||||
}
|
}
|
||||||
} // namespace stages
|
} // namespace stages
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
|
|||||||
@ -199,6 +199,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
|||||||
if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) {
|
if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) {
|
||||||
// plan to joint-space target
|
// plan to joint-space target
|
||||||
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
||||||
|
solution.setPlannerId(planner_->getPlannerId());
|
||||||
} else {
|
} else {
|
||||||
// Cartesian targets require an IK reference frame
|
// Cartesian targets require an IK reference frame
|
||||||
const moveit::core::LinkModel* link;
|
const moveit::core::LinkModel* link;
|
||||||
@ -287,6 +288,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
|||||||
|
|
||||||
success =
|
success =
|
||||||
planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
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
|
if (robot_trajectory) { // the following requires a robot_trajectory returned from planning
|
||||||
moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
|
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())) {
|
if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) {
|
||||||
// plan to joint-space target
|
// plan to joint-space target
|
||||||
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
||||||
|
solution.setPlannerId(planner_->getPlannerId());
|
||||||
} else { // Cartesian goal
|
} else { // Cartesian goal
|
||||||
// Where to go?
|
// Where to go?
|
||||||
Eigen::Isometry3d target;
|
Eigen::Isometry3d target;
|
||||||
@ -241,6 +242,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
|||||||
|
|
||||||
// plan to Cartesian target
|
// plan to Cartesian target
|
||||||
success = planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints);
|
success = planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints);
|
||||||
|
solution.setPlannerId(planner_->getPlannerId());
|
||||||
}
|
}
|
||||||
|
|
||||||
// store result
|
// store result
|
||||||
|
|||||||
@ -215,6 +215,7 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& inf
|
|||||||
info.id = introspection ? introspection->solutionId(*this) : 0;
|
info.id = introspection ? introspection->solutionId(*this) : 0;
|
||||||
info.cost = this->cost();
|
info.cost = this->cost();
|
||||||
info.comment = this->comment();
|
info.comment = this->comment();
|
||||||
|
info.planner_id = this->plannerId();
|
||||||
const Introspection* ci = introspection;
|
const Introspection* ci = introspection;
|
||||||
info.stage_id = ci ? ci->stageId(this->creator()) : 0;
|
info.stage_id = ci ? ci->stageId(this->creator()) : 0;
|
||||||
|
|
||||||
|
|||||||
@ -23,6 +23,7 @@ TEST_F(PipelinePlannerTest, testInitialization) {
|
|||||||
// WHEN a PipelinePlanner instance is initialized
|
// WHEN a PipelinePlanner instance is initialized
|
||||||
// THEN it does not throw
|
// THEN it does not throw
|
||||||
EXPECT_NO_THROW(pipeline_planner.init(robot_model));
|
EXPECT_NO_THROW(pipeline_planner.init(robot_model));
|
||||||
|
EXPECT_EQ(pipeline_planner.getPlannerId(), "Unknown");
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(PipelinePlannerTest, testWithoutPlanningPipelines) {
|
TEST_F(PipelinePlannerTest, testWithoutPlanningPipelines) {
|
||||||
@ -44,6 +45,7 @@ TEST_F(PipelinePlannerTest, testValidPlan) {
|
|||||||
std::make_shared<robot_trajectory::RobotTrajectory>(robot_model, robot_model->getJointModelGroup("group"));
|
std::make_shared<robot_trajectory::RobotTrajectory>(robot_model, robot_model->getJointModelGroup("group"));
|
||||||
// THEN it returns true
|
// THEN it returns true
|
||||||
EXPECT_TRUE(pipeline_planner.plan(scene, scene, robot_model->getJointModelGroup("group"), 1.0, result));
|
EXPECT_TRUE(pipeline_planner.plan(scene, scene, robot_model->getJointModelGroup("group"), 1.0, result));
|
||||||
|
EXPECT_EQ(pipeline_planner.getPlannerId(), "stomp");
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(PipelinePlannerTest, testInvalidPipelineID) {
|
TEST_F(PipelinePlannerTest, testInvalidPipelineID) {
|
||||||
|
|||||||
@ -10,5 +10,8 @@ string comment
|
|||||||
# id of stage that created this trajectory
|
# id of stage that created this trajectory
|
||||||
uint32 stage_id
|
uint32 stage_id
|
||||||
|
|
||||||
|
# name of the planner that created this solution
|
||||||
|
string planner_id
|
||||||
|
|
||||||
# markers, e.g. providing additional hints or illustrating failure
|
# markers, e.g. providing additional hints or illustrating failure
|
||||||
visualization_msgs/Marker[] markers
|
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) {
|
DisplaySolutionPtr TaskListModel::processSolutionMessage(const moveit_task_constructor_msgs::msg::Solution& msg) {
|
||||||
auto it = remote_tasks_.find(msg.task_id);
|
auto it = remote_tasks_.find(msg.task_id);
|
||||||
if (it == remote_tasks_.cend())
|
if (it == remote_tasks_.cend())
|
||||||
return DisplaySolutionPtr(); // unkown task
|
return DisplaySolutionPtr(); // unknown task
|
||||||
|
|
||||||
RemoteTaskModel* remote_task = it->second;
|
RemoteTaskModel* remote_task = it->second;
|
||||||
if (!remote_task)
|
if (!remote_task)
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user