mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
4ee71881ae
commit
36d9d3e8c0
@ -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
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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 -- "
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user