43 #include <ompl/config.h>
45 #include <ompl/geometric/planners/AnytimePathShortening.h>
46 #include <ompl/geometric/planners/rrt/RRT.h>
47 #include <ompl/geometric/planners/rrt/pRRT.h>
48 #include <ompl/geometric/planners/rrt/RRTConnect.h>
49 #include <ompl/geometric/planners/rrt/TRRT.h>
50 #include <ompl/geometric/planners/rrt/LazyRRT.h>
51 #include <ompl/geometric/planners/est/EST.h>
52 #include <ompl/geometric/planners/sbl/SBL.h>
53 #include <ompl/geometric/planners/sbl/pSBL.h>
54 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
55 #include <ompl/geometric/planners/kpiece/BKPIECE1.h>
56 #include <ompl/geometric/planners/kpiece/LBKPIECE1.h>
57 #include <ompl/geometric/planners/rrt/RRTstar.h>
58 #include <ompl/geometric/planners/prm/PRM.h>
59 #include <ompl/geometric/planners/prm/PRMstar.h>
60 #include <ompl/geometric/planners/fmt/FMT.h>
61 #include <ompl/geometric/planners/fmt/BFMT.h>
62 #include <ompl/geometric/planners/pdst/PDST.h>
63 #include <ompl/geometric/planners/stride/STRIDE.h>
64 #include <ompl/geometric/planners/rrt/BiTRRT.h>
65 #include <ompl/geometric/planners/rrt/LBTRRT.h>
66 #include <ompl/geometric/planners/est/BiEST.h>
67 #include <ompl/geometric/planners/est/ProjEST.h>
68 #include <ompl/geometric/planners/prm/LazyPRM.h>
69 #include <ompl/geometric/planners/prm/LazyPRMstar.h>
70 #include <ompl/geometric/planners/prm/SPARS.h>
71 #include <ompl/geometric/planners/prm/SPARStwo.h>
73 #if OMPL_VERSION_VALUE >= 1005000
74 #include <ompl/geometric/planners/informedtrees/AITstar.h>
75 #include <ompl/geometric/planners/informedtrees/ABITstar.h>
76 #include <ompl/geometric/planners/informedtrees/BITstar.h>
83 using namespace std::placeholders;
87 constexpr
char LOGNAME[] =
"planning_context_manager";
91 std::map<std::pair<std::string, std::string>, std::vector<ModelBasedPlanningContextPtr> >
contexts_;
100 for (
const auto& entry : planner_data_storage_paths_)
103 ob::PlannerData data(planners_[entry.first]->getSpaceInformation());
104 planners_[entry.first]->getPlannerData(data);
105 storage_.store(data, entry.second.c_str());
109 template <
typename T>
115 auto it = cfg.find(
"multi_query_planning_enabled");
116 bool multi_query_planning_enabled =
false;
119 multi_query_planning_enabled = boost::lexical_cast<bool>(it->second);
122 if (multi_query_planning_enabled)
125 auto planner_map_it = planners_.find(new_name);
126 if (planner_map_it != planners_.end())
127 return planner_map_it->second;
134 it = cfg.find(
"load_planner_data");
135 bool load_planner_data =
false;
138 load_planner_data = boost::lexical_cast<bool>(it->second);
141 it = cfg.find(
"store_planner_data");
142 bool store_planner_data =
false;
145 store_planner_data = boost::lexical_cast<bool>(it->second);
148 it = cfg.find(
"planner_data_path");
149 std::string planner_data_path;
152 planner_data_path = it->second;
156 planners_[new_name] =
157 allocatePlannerImpl<T>(si, new_name, spec, load_planner_data, store_planner_data, planner_data_path);
158 return planners_[new_name];
163 return allocatePlannerImpl<T>(si, new_name, spec);
167 template <
typename T>
170 bool load_planner_data,
bool store_planner_data,
const std::string& file_path)
172 ob::PlannerPtr planner;
174 if (load_planner_data)
177 ob::PlannerData data(si);
178 storage_.load(file_path.c_str(), data);
179 planner = std::shared_ptr<ob::Planner>{ allocatePersistentPlanner<T>(data) };
182 "Creating a '%s' planner from persistent data is not supported. Going to create a new instance.",
186 planner = std::make_shared<T>(si);
187 if (!new_name.empty())
188 planner->setName(new_name);
189 planner->params().setParams(spec.
config_,
true);
191 if (store_planner_data)
192 planner_data_storage_paths_[new_name] = file_path;
197 template <
typename T>
198 inline ompl::base::Planner*
205 #if OMPL_VERSION_VALUE >= 1005000
209 inline ompl::base::Planner*
210 MultiQueryPlannerAllocator::allocatePersistentPlanner<ompl::geometric::PRM>(
const ob::PlannerData& data)
212 return new og::PRM(data);
215 inline ompl::base::Planner*
216 MultiQueryPlannerAllocator::allocatePersistentPlanner<ompl::geometric::PRMstar>(
const ob::PlannerData& data)
218 return new og::PRMstar(data);
221 inline ompl::base::Planner*
222 MultiQueryPlannerAllocator::allocatePersistentPlanner<ompl::geometric::LazyPRM>(
const ob::PlannerData& data)
224 return new og::LazyPRM(data);
227 inline ompl::base::Planner*
228 MultiQueryPlannerAllocator::allocatePersistentPlanner<ompl::geometric::LazyPRMstar>(
const ob::PlannerData& data)
230 return new og::LazyPRMstar(data);
236 constraint_samplers::ConstraintSamplerManagerPtr csm)
237 : robot_model_(
std::move(robot_model))
238 , constraint_sampler_manager_(
std::move(csm))
239 , max_goal_samples_(10)
240 , max_state_sampling_attempts_(4)
241 , max_goal_sampling_attempts_(1000)
242 , max_planning_threads_(4)
243 , max_solution_segment_length_(0.0)
244 , minimum_waypoint_count_(2)
256 auto it = known_planners_.find(planner);
257 if (it != known_planners_.end())
266 template <
typename T>
269 registerPlannerAllocator(planner_id, [&](
const ob::SpaceInformationPtr& si,
const std::string& new_name,
271 return planner_allocator_.allocatePlanner<T>(si, new_name, spec);
277 registerPlannerAllocatorHelper<og::AnytimePathShortening>(
"geometric::AnytimePathShortening");
278 registerPlannerAllocatorHelper<og::BFMT>(
"geometric::BFMT");
279 registerPlannerAllocatorHelper<og::BiEST>(
"geometric::BiEST");
280 registerPlannerAllocatorHelper<og::BiTRRT>(
"geometric::BiTRRT");
281 registerPlannerAllocatorHelper<og::BKPIECE1>(
"geometric::BKPIECE");
282 registerPlannerAllocatorHelper<og::EST>(
"geometric::EST");
283 registerPlannerAllocatorHelper<og::FMT>(
"geometric::FMT");
284 registerPlannerAllocatorHelper<og::KPIECE1>(
"geometric::KPIECE");
285 registerPlannerAllocatorHelper<og::LazyPRM>(
"geometric::LazyPRM");
286 registerPlannerAllocatorHelper<og::LazyPRMstar>(
"geometric::LazyPRMstar");
287 registerPlannerAllocatorHelper<og::LazyRRT>(
"geometric::LazyRRT");
288 registerPlannerAllocatorHelper<og::LBKPIECE1>(
"geometric::LBKPIECE");
289 registerPlannerAllocatorHelper<og::LBTRRT>(
"geometric::LBTRRT");
290 registerPlannerAllocatorHelper<og::PDST>(
"geometric::PDST");
291 registerPlannerAllocatorHelper<og::PRM>(
"geometric::PRM");
292 registerPlannerAllocatorHelper<og::PRMstar>(
"geometric::PRMstar");
293 registerPlannerAllocatorHelper<og::ProjEST>(
"geometric::ProjEST");
294 registerPlannerAllocatorHelper<og::RRT>(
"geometric::RRT");
295 registerPlannerAllocatorHelper<og::RRTConnect>(
"geometric::RRTConnect");
296 registerPlannerAllocatorHelper<og::RRTstar>(
"geometric::RRTstar");
297 registerPlannerAllocatorHelper<og::SBL>(
"geometric::SBL");
298 registerPlannerAllocatorHelper<og::SPARS>(
"geometric::SPARS");
299 registerPlannerAllocatorHelper<og::SPARStwo>(
"geometric::SPARStwo");
300 registerPlannerAllocatorHelper<og::STRIDE>(
"geometric::STRIDE");
301 registerPlannerAllocatorHelper<og::TRRT>(
"geometric::TRRT");
303 #if OMPL_VERSION_VALUE >= 1005000
304 registerPlannerAllocatorHelper<og::AITstar>(
"geometric::AITstar");
305 registerPlannerAllocatorHelper<og::ABITstar>(
"geometric::ABITstar");
306 registerPlannerAllocatorHelper<og::BITstar>(
"geometric::BITstar");
318 return [
this](
const std::string& planner) {
return plannerSelector(planner); };
324 planner_configs_ = pconfig;
331 ModelBasedPlanningContextPtr context;
334 std::unique_lock<std::mutex> slock(cached_contexts_->lock_);
335 auto cached_contexts = cached_contexts_->contexts_.find(std::make_pair(config.
name, factory->getType()));
336 if (cached_contexts != cached_contexts_->contexts_.end())
338 for (
const ModelBasedPlanningContextPtr& cached_context : cached_contexts->second)
339 if (cached_context.unique())
342 context = cached_context;
356 context_spec.
state_space_ = factory->getNewStateSpace(space_spec);
362 context = std::make_shared<ModelBasedPlanningContext>(config.
name, context_spec);
364 std::unique_lock<std::mutex> slock(cached_contexts_->lock_);
365 cached_contexts_->contexts_[std::make_pair(config.
name, factory->getType())].push_back(context);
369 context->setMaximumPlanningThreads(max_planning_threads_);
370 context->setMaximumGoalSamples(max_goal_samples_);
371 context->setMaximumStateSamplingAttempts(max_state_sampling_attempts_);
372 context->setMaximumGoalSamplingAttempts(max_goal_sampling_attempts_);
373 if (max_solution_segment_length_ > std::numeric_limits<double>::epsilon())
374 context->setMaximumSolutionSegmentLength(max_solution_segment_length_);
375 context->setMinimumWaypointCount(minimum_waypoint_count_);
377 context->setSpecificationConfig(config.
config);
382 const ompl_interface::ModelBasedStateSpaceFactoryPtr&
385 auto f = factory_type.empty() ? state_space_factories_.begin() : state_space_factories_.find(factory_type);
386 if (
f != state_space_factories_.end())
391 static const ModelBasedStateSpaceFactoryPtr EMPTY;
396 const ompl_interface::ModelBasedStateSpaceFactoryPtr&
398 const moveit_msgs::MotionPlanRequest& req)
const
401 auto best = state_space_factories_.end();
402 int prev_priority = 0;
403 for (
auto it = state_space_factories_.begin(); it != state_space_factories_.end(); ++it)
405 int priority = it->second->canRepresentProblem(group, req, robot_model_);
406 if (priority > prev_priority)
409 prev_priority = priority;
413 if (best == state_space_factories_.end())
417 static const ModelBasedStateSpaceFactoryPtr EMPTY;
428 const planning_scene::PlanningSceneConstPtr&
planning_scene,
const moveit_msgs::MotionPlanRequest& req,
429 moveit_msgs::MoveItErrorCodes& error_code,
const ros::NodeHandle& nh,
bool use_constraints_approximation)
const
431 if (req.group_name.empty())
434 error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
435 return ModelBasedPlanningContextPtr();
438 error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
443 return ModelBasedPlanningContextPtr();
447 auto pc = planner_configs_.end();
448 if (!req.planner_id.empty())
450 pc = planner_configs_.find(req.planner_id.find(req.group_name) == std::string::npos ?
451 req.group_name +
"[" + req.planner_id +
"]" :
453 if (pc == planner_configs_.end())
455 "Cannot find planning configuration for group '%s' using planner '%s'. Will use defaults instead.",
456 req.group_name.c_str(), req.planner_id.c_str());
459 if (pc == planner_configs_.end())
461 pc = planner_configs_.find(req.group_name);
462 if (pc == planner_configs_.end())
465 return ModelBasedPlanningContextPtr();
479 ModelBasedStateSpaceFactoryPtr factory;
480 auto it = pc->second.config.find(
"enforce_joint_model_state_space");
482 auto type_it = pc->second.config.find(
"type");
483 std::string planner_type;
484 if (type_it != pc->second.config.end())
485 planner_type = type_it->second;
487 if (it != pc->second.config.end() && boost::lexical_cast<bool>(it->second))
489 else if (planner_type ==
"geometric::AITstar")
491 else if (planner_type ==
"geometric::ABITstar")
493 else if (planner_type ==
"geometric::BITstar")
496 factory = getStateSpaceFactory(pc->second.group, req);
498 ModelBasedPlanningContextPtr context = getPlanningContext(pc->second, factory);
504 moveit::core::RobotStatePtr start_state =
planning_scene->getCurrentStateUpdated(req.start_state);
508 context->setMotionPlanRequest(req);
509 context->setCompleteInitialState(*start_state);
511 context->setPlanningVolume(req.workspace_parameters);
512 if (!context->setPathConstraints(req.path_constraints, &error_code))
513 return ModelBasedPlanningContextPtr();
515 if (!context->setGoalConstraints(req.goal_constraints, req.path_constraints, &error_code))
516 return ModelBasedPlanningContextPtr();
520 context->configure(nh, use_constraints_approximation);
522 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
524 catch (ompl::Exception& ex)