Skip to content

Commit de2440d

Browse files
sjahrsjahr
committed
Make usable with parallel planning
1 parent 5de1af6 commit de2440d

File tree

2 files changed

+84
-82
lines changed

2 files changed

+84
-82
lines changed

‎core/include/moveit/task_constructor/solvers/pipeline_planner.h‎

Lines changed: 17 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -56,32 +56,36 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner);
5656
class PipelinePlanner : public PlannerInterface
5757
{
5858
public:
59+
/*
5960
struct Specification
6061
{
61-
moveit::core::RobotModelConstPtr model;
62-
std::string ns{ "ompl" };
63-
std::string pipeline{ "ompl" };
64-
std::string adapter_param{ "request_adapters" };
65-
};
62+
moveit::core::RobotModelConstPtr model;
63+
std::string ns{ "ompl" };
64+
std::string pipeline{ "ompl" };
65+
std::string adapter_param{ "request_adapters" };
66+
};*/
6667

68+
/*
6769
static planning_pipeline::PlanningPipelinePtr
6870
createPlanningPipelines(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& model) {
69-
Specification spec;
70-
spec.model = model;
71-
return create(node, spec);
72-
}
71+
Specification spec;
72+
spec.model = model;
73+
return create(node, spec);
74+
}*/
7375

7476
/**
7577
*
7678
* @param node used to load the parameters for the planning pipeline
7779
*/
7880
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_namespace = "ompl");
7981

80-
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::vector<std::string>& pipeline_namespaces);
82+
// PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::vector<std::string>& pipeline_namespaces);
8183

82-
PipelinePlanner(const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& planning_pipelines);
84+
// PipelinePlanner(const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>&
85+
// planning_pipelines);
8386

84-
PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline, const std::string& pipeline = "");
87+
// PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline, const std::string& pipeline =
88+
// "");
8589

8690
void setPlannerId(const std::string& planner) { setProperty("planner", planner); }
8791

@@ -106,8 +110,7 @@ class PipelinePlanner : public PlannerInterface
106110
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints());
107111

108112
std::vector<std::string> pipeline_names_ = std::vector<std::string>(1);
109-
std::vector<planning_pipeline::PlanningPipelinePtr> planning_pipelines_ =
110-
std::vector<planning_pipeline::PlanningPipelinePtr>(1, nullptr);
113+
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
111114
rclcpp::Node::SharedPtr node_;
112115
};
113116
} // namespace solvers

‎core/src/solvers/pipeline_planner.cpp‎

Lines changed: 67 additions & 68 deletions
Original file line numberDiff line numberDiff line change
@@ -54,63 +54,65 @@ namespace moveit {
5454
namespace task_constructor {
5555
namespace solvers {
5656

57+
/*
5758
static constexpr char const* PLUGIN_PARAMETER_NAME = "planning_plugin";
5859
5960
struct PlannerCache
6061
{
61-
using PlannerID = std::tuple<std::string, std::string>;
62-
using PlannerMap = std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline>>;
63-
using ModelList = std::list<std::pair<std::weak_ptr<const moveit::core::RobotModel>, PlannerMap>>;
64-
ModelList cache_;
65-
66-
PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& planner_id) {
67-
// find model in cache_ and remove expired entries while doing so
68-
ModelList::iterator model_it = cache_.begin();
69-
while (model_it != cache_.end()) {
70-
if (model_it->first.expired()) {
71-
model_it = cache_.erase(model_it);
72-
continue;
73-
}
74-
if (model_it->first.lock() == model)
75-
break;
76-
++model_it;
77-
}
78-
if (model_it == cache_.end()) // if not found, create a new PlannerMap for this model
79-
model_it = cache_.insert(cache_.begin(), std::make_pair(model, PlannerMap()));
80-
81-
return model_it->second.insert(std::make_pair(planner_id, PlannerMap::mapped_type())).first->second;
82-
}
83-
};
84-
62+
using PlannerID = std::tuple<std::string, std::string>;
63+
using PlannerMap = std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline>>;
64+
using ModelList = std::list<std::pair<std::weak_ptr<const moveit::core::RobotModel>, PlannerMap>>;
65+
ModelList cache_;
66+
67+
PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& planner_id) {
68+
// find model in cache_ and remove expired entries while doing so
69+
ModelList::iterator model_it = cache_.begin();
70+
while (model_it != cache_.end()) {
71+
if (model_it->first.expired()) {
72+
model_it = cache_.erase(model_it);
73+
continue;
74+
}
75+
if (model_it->first.lock() == model)
76+
break;
77+
++model_it;
78+
}
79+
if (model_it == cache_.end()) // if not found, create a new PlannerMap for this model
80+
model_it = cache_.insert(cache_.begin(), std::make_pair(model, PlannerMap()));
81+
82+
return model_it->second.insert(std::make_pair(planner_id, PlannerMap::mapped_type())).first->second;
83+
}
84+
};*/
85+
86+
/*
8587
planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Node::SharedPtr& node,
8688
const PipelinePlanner::Specification& specification) {
87-
static PlannerCache cache;
88-
89-
std::string pipeline_ns = specification.ns;
90-
const std::string parameter_name = pipeline_ns + "." + PLUGIN_PARAMETER_NAME;
91-
// fallback to old structure for pipeline parameters in MoveIt
92-
if (!node->has_parameter(parameter_name)) {
93-
node->declare_parameter(parameter_name, rclcpp::ParameterType::PARAMETER_STRING);
94-
}
95-
if (std::string parameter; !node->get_parameter(parameter_name, parameter)) {
96-
RCLCPP_WARN(node->get_logger(), "Failed to find '%s.%s'. %s", pipeline_ns.c_str(), PLUGIN_PARAMETER_NAME,
97-
"Attempting to load pipeline from old parameter structure. Please update your MoveIt config.");
98-
pipeline_ns = "move_group";
99-
}
100-
101-
PlannerCache::PlannerID id(pipeline_ns, specification.adapter_param);
102-
103-
std::weak_ptr<planning_pipeline::PlanningPipeline>& entry = cache.retrieve(specification.model, id);
104-
planning_pipeline::PlanningPipelinePtr planner = entry.lock();
105-
if (!planner) {
106-
// create new entry
107-
planner = std::make_shared<planning_pipeline::PlanningPipeline>(
108-
specification.model, node, pipeline_ns, PLUGIN_PARAMETER_NAME, specification.adapter_param);
109-
// store in cache
110-
entry = planner;
111-
}
112-
return planner;
113-
}
89+
static PlannerCache cache;
90+
91+
std::string pipeline_ns = specification.ns;
92+
const std::string parameter_name = pipeline_ns + "." + PLUGIN_PARAMETER_NAME;
93+
// fallback to old structure for pipeline parameters in MoveIt
94+
if (!node->has_parameter(parameter_name)) {
95+
node->declare_parameter(parameter_name, rclcpp::ParameterType::PARAMETER_STRING);
96+
}
97+
if (std::string parameter; !node->get_parameter(parameter_name, parameter)) {
98+
RCLCPP_WARN(node->get_logger(), "Failed to find '%s.%s'. %s", pipeline_ns.c_str(), PLUGIN_PARAMETER_NAME,
99+
"Attempting to load pipeline from old parameter structure. Please update your MoveIt config.");
100+
pipeline_ns = "move_group";
101+
}
102+
103+
PlannerCache::PlannerID id(pipeline_ns, specification.adapter_param);
104+
105+
std::weak_ptr<planning_pipeline::PlanningPipeline>& entry = cache.retrieve(specification.model, id);
106+
planning_pipeline::PlanningPipelinePtr planner = entry.lock();
107+
if (!planner) {
108+
// create new entry
109+
planner = std::make_shared<planning_pipeline::PlanningPipeline>(
110+
specification.model, node, pipeline_ns, PLUGIN_PARAMETER_NAME, specification.adapter_param);
111+
// store in cache
112+
entry = planner;
113+
}
114+
return planner;
115+
}*/
114116

115117
PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name) : node_(node) {
116118
pipeline_names_.at(0) = pipeline_name;
@@ -132,25 +134,19 @@ PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std:
132134
planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC);
133135
}
134136

135-
PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline)
136-
: PipelinePlanner(rclcpp::Node::SharedPtr()) {
137-
planning_pipelines_.at(0) = planning_pipeline;
138-
}
137+
// PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline)
138+
// : PipelinePlanner(rclcpp::Node::SharedPtr()) {
139+
// planning_pipelines_.at(0) = planning_pipeline;
140+
//}
139141

140142
void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
141-
if (!planning_pipelines_.at(0)) {
142-
Specification specification;
143-
specification.model = robot_model;
144-
specification.pipeline = pipeline_names_.at(0);
145-
specification.ns = pipeline_names_.at(0);
146-
planning_pipelines_.at(0) = create(node_, specification);
147-
} else if (robot_model != planning_pipelines_.at(0)->getRobotModel()) {
148-
throw std::runtime_error(
149-
"The robot model of the planning pipeline isn't the same as the task's robot model -- "
150-
"use Task::setRobotModel for setting the robot model when using custom planning pipeline");
143+
planning_pipelines_ =
144+
moveit::planning_pipeline_interfaces::createPlanningPipelineMap(pipeline_names_, robot_model, node_);
145+
146+
for (auto const& name_pipeline_pair : planning_pipelines_) {
147+
name_pipeline_pair.second->displayComputedMotionPlans(properties().get<bool>("display_motion_plans"));
148+
name_pipeline_pair.second->publishReceivedRequests(properties().get<bool>("publish_planning_requests"));
151149
}
152-
planning_pipelines_.at(0)->displayComputedMotionPlans(properties().get<bool>("display_motion_plans"));
153-
planning_pipelines_.at(0)->publishReceivedRequests(properties().get<bool>("publish_planning_requests"));
154150
}
155151

156152
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
@@ -189,6 +185,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning
189185

190186
for (auto const& pipeline_name : pipeline_names_) {
191187
moveit_msgs::msg::MotionPlanRequest request;
188+
request.pipeline_id = pipeline_name;
192189
request.group_name = joint_model_group->getName();
193190
request.planner_id = properties().get<std::string>("planner");
194191
request.allowed_planning_time = timeout;
@@ -205,8 +202,10 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning
205202

206203
std::vector<::planning_interface::MotionPlanResponse> responses =
207204
moveit::planning_pipeline_interfaces::planWithParallelPipelines(requests, planning_scene, planning_pipelines_);
205+
206+
// Just choose first result
208207
result = responses.at(0).trajectory;
209-
return bool(responses);
208+
return bool(result);
210209
}
211210
} // namespace solvers
212211
} // namespace task_constructor

0 commit comments

Comments
 (0)