Rework PipelinePlanner creation (#249)

- Moved Task::createPlanner into PipelinePlanner::create
- Handle mutiple planner pipeline configs as introduced in https://github.com/ros-planning/moveit/pull/2127
This commit is contained in:
Michael Görner 2021-03-29 12:00:28 +02:00 committed by GitHub
parent 4ee71881ae
commit 36d9d3e8c0
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 76 additions and 53 deletions

View File

@ -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 @@ public:
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
protected:
std::string pipeline_name_;
planning_pipeline::PlanningPipelinePtr planner_;
};
} // namespace solvers

View File

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

View File

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

View File

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