Rework PipelinePlanner creation (#249)

- Moved Task::createPlanner into PipelinePlanner::create
- Handle mutiple planner pipeline configs as introduced in moveit/moveit#2127

Author: v4hn

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

@@ -55,7 +55,23 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner)
 class PipelinePlanner : public PlannerInterface
 {
 public:
-	PipelinePlanner();
+	struct Specification
+	{
+		moveit::core::RobotModelConstPtr model;
+		std::string ns{ "move_group" };
+		std::string pipeline{ "ompl" };
+		std::string adapter_param{ "request_adapters" };
+	};
+
+	static planning_pipeline::PlanningPipelinePtr create(const moveit::core::RobotModelConstPtr& model) {
+		Specification spec;
+		spec.model = model;
+		return create(spec);
+	}
+
+	static planning_pipeline::PlanningPipelinePtr create(const Specification& spec);
+
+	PipelinePlanner(const std::string& pipeline = "ompl");
 
 	PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline);
 
@@ -73,6 +89,7 @@ class PipelinePlanner : public PlannerInterface
 	          const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
 
 protected:
+	std::string pipeline_name_;
 	planning_pipeline::PlanningPipelinePtr planner_;
 };
 }  // namespace solvers

core/include/moveit/task_constructor/task.h

@@ -73,11 +73,6 @@ class Task : protected WrapperBase
 public:
 	PRIVATE_CLASS(Task)
 
-	// +1 TODO: move into MoveIt core
-	static planning_pipeline::PlanningPipelinePtr
-	createPlanner(const moveit::core::RobotModelConstPtr& model, const std::string& ns = "move_group",
-	              const std::string& planning_plugin_param_name = "planning_plugin",
-	              const std::string& adapter_plugins_param_name = "request_adapters");
 	Task(const std::string& ns = "", bool introspection = true,
 	     ContainerBase::pointer&& container = std::make_unique<SerialContainer>("task pipeline"));
 	Task(Task&& other);  // NOLINT(performance-noexcept-move-constructor)

core/src/solvers/pipeline_planner.cpp

@@ -48,7 +48,60 @@ namespace moveit {
 namespace task_constructor {
 namespace solvers {
 
-PipelinePlanner::PipelinePlanner() {
+struct PlannerCache
+{
+	using PlannerID = std::tuple<std::string, std::string>;
+	using PlannerMap = std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline> >;
+	using ModelList = std::list<std::pair<std::weak_ptr<const robot_model::RobotModel>, PlannerMap> >;
+	ModelList cache_;
+
+	PlannerMap::mapped_type& retrieve(const robot_model::RobotModelConstPtr& model, const PlannerID& id) {
+		// find model in cache_ and remove expired entries while doing so
+		ModelList::iterator model_it = cache_.begin();
+		while (model_it != cache_.end()) {
+			if (model_it->first.expired()) {
+				model_it = cache_.erase(model_it);
+				continue;
+			}
+			if (model_it->first.lock() == model)
+				break;
+			++model_it;
+		}
+		if (model_it == cache_.end())  // if not found, create a new PlannerMap for this model
+			model_it = cache_.insert(cache_.begin(), std::make_pair(model, PlannerMap()));
+
+		return model_it->second.insert(std::make_pair(id, PlannerMap::mapped_type())).first->second;
+	}
+};
+
+planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const PipelinePlanner::Specification& spec) {
+	static PlannerCache cache;
+
+	constexpr char const* PLUGIN_PARAMETER_NAME = "planning_plugin";
+
+	std::string pipeline_ns = spec.ns + "/planning_pipelines/" + spec.pipeline;
+	// fallback to old structure for pipeline parameters in MoveIt
+	if (!ros::NodeHandle(pipeline_ns).hasParam(PLUGIN_PARAMETER_NAME)) {
+		ROS_WARN("Failed to find '%s/%s'. %s", pipeline_ns.c_str(), PLUGIN_PARAMETER_NAME,
+		         "Attempting to load pipeline from old parameter structure. Please update your MoveIt config.");
+		pipeline_ns = spec.ns;
+	}
+
+	PlannerCache::PlannerID id(pipeline_ns, spec.adapter_param);
+
+	std::weak_ptr<planning_pipeline::PlanningPipeline>& entry = cache.retrieve(spec.model, id);
+	planning_pipeline::PlanningPipelinePtr planner = entry.lock();
+	if (!planner) {
+		// create new entry
+		planner = std::make_shared<planning_pipeline::PlanningPipeline>(spec.model, ros::NodeHandle(pipeline_ns),
+		                                                                PLUGIN_PARAMETER_NAME, spec.adapter_param);
+		// store in cache
+		entry = planner;
+	}
+	return planner;
+}
+
+PipelinePlanner::PipelinePlanner(const std::string& pipeline_name) : pipeline_name_{ pipeline_name } {
 	auto& p = properties();
 	p.declare<std::string>("planner", "", "planner id");
 
@@ -73,7 +126,10 @@ PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& p
 
 void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
 	if (!planner_) {
-		planner_ = Task::createPlanner(robot_model);
+		Specification spec;
+		spec.model = robot_model;
+		spec.pipeline = pipeline_name_;
+		planner_ = create(spec);
 	} else if (robot_model != planner_->getRobotModel()) {
 		throw std::runtime_error(
 		    "The robot model of the planning pipeline isn't the same as the task's robot model -- "

core/src/task.cpp

@@ -126,51 +126,6 @@ Task& Task::operator=(Task&& other) {  // NOLINT(performance-noexcept-move-const
 	return *this;
 }
 
-struct PlannerCache
-{
-	using PlannerID = std::tuple<std::string, std::string, std::string>;
-	using PlannerMap = std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline> >;
-	using ModelList = std::list<std::pair<std::weak_ptr<const robot_model::RobotModel>, PlannerMap> >;
-	ModelList cache_;
-
-	PlannerMap::mapped_type& retrieve(const robot_model::RobotModelConstPtr& model, const PlannerID& id) {
-		// find model in cache_ and remove expired entries while doing so
-		ModelList::iterator model_it = cache_.begin();
-		while (model_it != cache_.end()) {
-			if (model_it->first.expired()) {
-				model_it = cache_.erase(model_it);
-				continue;
-			}
-			if (model_it->first.lock() == model)
-				break;
-			++model_it;
-		}
-		if (model_it == cache_.end())  // if not found, create a new PlannerMap for this model
-			model_it = cache_.insert(cache_.begin(), std::make_pair(model, PlannerMap()));
-
-		return model_it->second.insert(std::make_pair(id, PlannerMap::mapped_type())).first->second;
-	}
-};
-
-planning_pipeline::PlanningPipelinePtr Task::createPlanner(const robot_model::RobotModelConstPtr& model,
-                                                           const std::string& ns,
-                                                           const std::string& planning_plugin_param_name,
-                                                           const std::string& adapter_plugins_param_name) {
-	static PlannerCache cache;
-	PlannerCache::PlannerID id(ns, planning_plugin_param_name, adapter_plugins_param_name);
-
-	std::weak_ptr<planning_pipeline::PlanningPipeline>& entry = cache.retrieve(model, id);
-	planning_pipeline::PlanningPipelinePtr planner = entry.lock();
-	if (!planner) {
-		// create new entry
-		planner = std::make_shared<planning_pipeline::PlanningPipeline>(
-		    model, ros::NodeHandle(ns), planning_plugin_param_name, adapter_plugins_param_name);
-		// store in cache
-		entry = planner;
-	}
-	return planner;
-}
-
 Task::~Task() {
 	auto impl = pimpl();
 	impl->introspection_.reset();  // stop introspection