planning_context_manager.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
40 #include <algorithm>
41 #include <set>
42 
43 #include <ompl/geometric/planners/rrt/RRT.h>
44 #include <ompl/geometric/planners/rrt/pRRT.h>
45 #include <ompl/geometric/planners/rrt/RRTConnect.h>
46 #include <ompl/geometric/planners/rrt/TRRT.h>
47 #include <ompl/geometric/planners/rrt/LazyRRT.h>
48 #include <ompl/geometric/planners/est/EST.h>
49 #include <ompl/geometric/planners/sbl/SBL.h>
50 #include <ompl/geometric/planners/sbl/pSBL.h>
51 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
52 #include <ompl/geometric/planners/kpiece/BKPIECE1.h>
53 #include <ompl/geometric/planners/kpiece/LBKPIECE1.h>
54 #include <ompl/geometric/planners/rrt/RRTstar.h>
55 #include <ompl/geometric/planners/prm/PRM.h>
56 #include <ompl/geometric/planners/prm/PRMstar.h>
57 #include <ompl/geometric/planners/fmt/FMT.h>
58 #include <ompl/geometric/planners/fmt/BFMT.h>
59 #include <ompl/geometric/planners/pdst/PDST.h>
60 #include <ompl/geometric/planners/stride/STRIDE.h>
61 #include <ompl/geometric/planners/rrt/BiTRRT.h>
62 #include <ompl/geometric/planners/rrt/LBTRRT.h>
63 #include <ompl/geometric/planners/est/BiEST.h>
64 #include <ompl/geometric/planners/est/ProjEST.h>
65 #include <ompl/geometric/planners/prm/LazyPRM.h>
66 #include <ompl/geometric/planners/prm/LazyPRMstar.h>
67 #include <ompl/geometric/planners/prm/SPARS.h>
68 #include <ompl/geometric/planners/prm/SPARStwo.h>
69 
73 
74 namespace ompl_interface
75 {
77 {
78 public:
79  ModelBasedPlanningContextPtr getContext()
80  {
81  boost::mutex::scoped_lock slock(lock_);
83  }
84 
85  void setContext(const ModelBasedPlanningContextPtr& context)
86  {
87  boost::mutex::scoped_lock slock(lock_);
89  }
90 
91  void clear()
92  {
93  boost::mutex::scoped_lock slock(lock_);
95  }
96 
97 private:
98  /* The planning group for which solve() was called last */
99  ModelBasedPlanningContextPtr last_planning_context_solve_;
100  boost::mutex lock_;
101 };
102 
104 {
105  std::map<std::pair<std::string, std::string>, std::vector<ModelBasedPlanningContextPtr> > contexts_;
106  boost::mutex lock_;
107 };
108 
109 } // namespace ompl_interface
110 
112  const robot_model::RobotModelConstPtr& kmodel, const constraint_samplers::ConstraintSamplerManagerPtr& csm)
113  : kmodel_(kmodel)
115  , max_goal_samples_(10)
121 {
123  cached_contexts_.reset(new CachedContexts());
126 }
127 
129 {
130 }
131 
132 namespace
133 {
134 using namespace ompl_interface;
135 
136 template <typename T>
137 static ompl::base::PlannerPtr allocatePlanner(const ob::SpaceInformationPtr& si, const std::string& new_name,
139 {
140  ompl::base::PlannerPtr planner(new T(si));
141  if (!new_name.empty())
142  planner->setName(new_name);
143  planner->params().setParams(spec.config_, true);
144  planner->setup();
145  return planner;
146 }
147 }
148 
151 {
152  std::map<std::string, ConfiguredPlannerAllocator>::const_iterator it = known_planners_.find(planner);
153  if (it != known_planners_.end())
154  return it->second;
155  else
156  {
157  logError("Unknown planner: '%s'", planner.c_str());
159  }
160 }
161 
163 {
164  registerPlannerAllocator("geometric::RRT", boost::bind(&allocatePlanner<og::RRT>, _1, _2, _3));
165  registerPlannerAllocator("geometric::RRTConnect", boost::bind(&allocatePlanner<og::RRTConnect>, _1, _2, _3));
166  registerPlannerAllocator("geometric::LazyRRT", boost::bind(&allocatePlanner<og::LazyRRT>, _1, _2, _3));
167  registerPlannerAllocator("geometric::TRRT", boost::bind(&allocatePlanner<og::TRRT>, _1, _2, _3));
168  registerPlannerAllocator("geometric::EST", boost::bind(&allocatePlanner<og::EST>, _1, _2, _3));
169  registerPlannerAllocator("geometric::SBL", boost::bind(&allocatePlanner<og::SBL>, _1, _2, _3));
170  registerPlannerAllocator("geometric::KPIECE", boost::bind(&allocatePlanner<og::KPIECE1>, _1, _2, _3));
171  registerPlannerAllocator("geometric::BKPIECE", boost::bind(&allocatePlanner<og::BKPIECE1>, _1, _2, _3));
172  registerPlannerAllocator("geometric::LBKPIECE", boost::bind(&allocatePlanner<og::LBKPIECE1>, _1, _2, _3));
173  registerPlannerAllocator("geometric::RRTstar", boost::bind(&allocatePlanner<og::RRTstar>, _1, _2, _3));
174  registerPlannerAllocator("geometric::PRM", boost::bind(&allocatePlanner<og::PRM>, _1, _2, _3));
175  registerPlannerAllocator("geometric::PRMstar", boost::bind(&allocatePlanner<og::PRMstar>, _1, _2, _3));
176  registerPlannerAllocator("geometric::FMT", boost::bind(&allocatePlanner<og::FMT>, _1, _2, _3));
177  registerPlannerAllocator("geometric::BFMT", boost::bind(&allocatePlanner<og::BFMT>, _1, _2, _3));
178  registerPlannerAllocator("geometric::PDST", boost::bind(&allocatePlanner<og::PDST>, _1, _2, _3));
179  registerPlannerAllocator("geometric::STRIDE", boost::bind(&allocatePlanner<og::STRIDE>, _1, _2, _3));
180  registerPlannerAllocator("geometric::BiTRRT", boost::bind(&allocatePlanner<og::BiTRRT>, _1, _2, _3));
181  registerPlannerAllocator("geometric::LBTRRT", boost::bind(&allocatePlanner<og::LBTRRT>, _1, _2, _3));
182  registerPlannerAllocator("geometric::BiEST", boost::bind(&allocatePlanner<og::BiEST>, _1, _2, _3));
183  registerPlannerAllocator("geometric::ProjEST", boost::bind(&allocatePlanner<og::ProjEST>, _1, _2, _3));
184  registerPlannerAllocator("geometric::LazyPRM", boost::bind(&allocatePlanner<og::LazyPRM>, _1, _2, _3));
185  registerPlannerAllocator("geometric::LazyPRMstar", boost::bind(&allocatePlanner<og::LazyPRMstar>, _1, _2, _3));
186  registerPlannerAllocator("geometric::SPARS", boost::bind(&allocatePlanner<og::SPARS>, _1, _2, _3));
187  registerPlannerAllocator("geometric::SPARStwo", boost::bind(&allocatePlanner<og::SPARStwo>, _1, _2, _3));
188 }
189 
191 {
192  registerStateSpaceFactory(ModelBasedStateSpaceFactoryPtr(new JointModelStateSpaceFactory()));
193  registerStateSpaceFactory(ModelBasedStateSpaceFactoryPtr(new PoseModelStateSpaceFactory()));
194 }
195 
197 {
198  return boost::bind(&PlanningContextManager::plannerSelector, this, _1);
199 }
200 
203 {
204  planner_configs_ = pconfig;
205 }
206 
207 ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext(
208  const std::string& config, const std::string& factory_type) const
209 {
210  planning_interface::PlannerConfigurationMap::const_iterator pc = planner_configs_.find(config);
211 
212  if (pc != planner_configs_.end())
213  {
214  moveit_msgs::MotionPlanRequest req; // dummy request with default values
215  return getPlanningContext(pc->second,
216  boost::bind(&PlanningContextManager::getStateSpaceFactory1, this, _1, factory_type), req);
217  }
218  else
219  {
220  logError("Planning configuration '%s' was not found", config.c_str());
221  return ModelBasedPlanningContextPtr();
222  }
223 }
224 
225 ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext(
227  const StateSpaceFactoryTypeSelector& factory_selector, const moveit_msgs::MotionPlanRequest& req) const
228 {
229  const ompl_interface::ModelBasedStateSpaceFactoryPtr& factory = factory_selector(config.group);
230 
231  // Check for a cached planning context
232  ModelBasedPlanningContextPtr context;
233 
234  {
235  boost::mutex::scoped_lock slock(cached_contexts_->lock_);
236  std::map<std::pair<std::string, std::string>, std::vector<ModelBasedPlanningContextPtr> >::const_iterator cc =
237  cached_contexts_->contexts_.find(std::make_pair(config.name, factory->getType()));
238  if (cc != cached_contexts_->contexts_.end())
239  {
240  for (std::size_t i = 0; i < cc->second.size(); ++i)
241  if (cc->second[i].unique())
242  {
243  logDebug("Reusing cached planning context");
244  context = cc->second[i];
245  break;
246  }
247  }
248  }
249 
250  // Create a new planning context
251  if (!context)
252  {
253  ModelBasedStateSpaceSpecification space_spec(kmodel_, config.group);
255  context_spec.config_ = config.config;
256  context_spec.planner_selector_ = getPlannerSelector();
258  context_spec.state_space_ = factory->getNewStateSpace(space_spec);
259 
260  // Choose the correct simple setup type to load
261  context_spec.ompl_simple_setup_.reset(new ompl::geometric::SimpleSetup(context_spec.state_space_));
262 
263  bool state_validity_cache = true;
264  if (config.config.find("subspaces") != config.config.end())
265  {
266  context_spec.config_.erase("subspaces");
267  // if the planner operates at subspace level the cache may be unsafe
268  state_validity_cache = false;
269  boost::char_separator<char> sep(" ");
270  boost::tokenizer<boost::char_separator<char> > tok(config.config.at("subspaces"), sep);
271  for (boost::tokenizer<boost::char_separator<char> >::iterator beg = tok.begin(); beg != tok.end(); ++beg)
272  {
273  const ompl_interface::ModelBasedStateSpaceFactoryPtr& sub_fact = factory_selector(*beg);
274  if (sub_fact)
275  {
276  ModelBasedStateSpaceSpecification sub_space_spec(kmodel_, *beg);
277  context_spec.subspaces_.push_back(sub_fact->getNewStateSpace(sub_space_spec));
278  }
279  }
280  }
281 
282  logDebug("Creating new planning context");
283  context.reset(new ModelBasedPlanningContext(config.name, context_spec));
284  context->useStateValidityCache(state_validity_cache);
285  {
286  boost::mutex::scoped_lock slock(cached_contexts_->lock_);
287  cached_contexts_->contexts_[std::make_pair(config.name, factory->getType())].push_back(context);
288  }
289  }
290 
291  context->setMaximumPlanningThreads(max_planning_threads_);
292  context->setMaximumGoalSamples(max_goal_samples_);
293  context->setMaximumStateSamplingAttempts(max_state_sampling_attempts_);
294  context->setMaximumGoalSamplingAttempts(max_goal_sampling_attempts_);
295  if (max_solution_segment_length_ <= std::numeric_limits<double>::epsilon())
296  context->setMaximumSolutionSegmentLength(context->getOMPLSimpleSetup()->getStateSpace()->getMaximumExtent() /
297  100.0);
298  else
299  context->setMaximumSolutionSegmentLength(max_solution_segment_length_);
300  context->setMinimumWaypointCount(minimum_waypoint_count_);
301 
302  context->setSpecificationConfig(config.config);
303 
304  last_planning_context_->setContext(context);
305  return context;
306 }
307 
308 const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningContextManager::getStateSpaceFactory1(
309  const std::string& /* dummy */, const std::string& factory_type) const
310 {
311  std::map<std::string, ModelBasedStateSpaceFactoryPtr>::const_iterator f =
312  factory_type.empty() ? state_space_factories_.begin() : state_space_factories_.find(factory_type);
313  if (f != state_space_factories_.end())
314  return f->second;
315  else
316  {
317  logError("Factory of type '%s' was not found", factory_type.c_str());
318  static const ModelBasedStateSpaceFactoryPtr empty;
319  return empty;
320  }
321 }
322 
323 const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningContextManager::getStateSpaceFactory2(
324  const std::string& group, const moveit_msgs::MotionPlanRequest& req) const
325 {
326  // find the problem representation to use
327  std::map<std::string, ModelBasedStateSpaceFactoryPtr>::const_iterator best = state_space_factories_.end();
328  int prev_priority = -1;
329  for (std::map<std::string, ModelBasedStateSpaceFactoryPtr>::const_iterator it = state_space_factories_.begin();
330  it != state_space_factories_.end(); ++it)
331  {
332  int priority = it->second->canRepresentProblem(group, req, kmodel_);
333  if (priority > 0)
334  if (best == state_space_factories_.end() || priority > prev_priority)
335  {
336  best = it;
337  prev_priority = priority;
338  }
339  }
340 
341  if (best == state_space_factories_.end())
342  {
343  logError("There are no known state spaces that can represent the given planning problem");
344  static const ModelBasedStateSpaceFactoryPtr empty;
345  return empty;
346  }
347  else
348  {
349  logDebug("Using '%s' parameterization for solving problem", best->first.c_str());
350  return best->second;
351  }
352 }
353 
354 ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext(
355  const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::MotionPlanRequest& req,
356  moveit_msgs::MoveItErrorCodes& error_code) const
357 {
358  if (req.group_name.empty())
359  {
360  logError("No group specified to plan for");
361  error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
362  return ModelBasedPlanningContextPtr();
363  }
364 
365  error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
366 
367  if (!planning_scene)
368  {
369  logError("No planning scene supplied as input");
370  return ModelBasedPlanningContextPtr();
371  }
372 
373  // identify the correct planning configuration
374  planning_interface::PlannerConfigurationMap::const_iterator pc = planner_configs_.end();
375  if (!req.planner_id.empty())
376  {
377  pc = planner_configs_.find(req.planner_id.find(req.group_name) == std::string::npos ?
378  req.group_name + "[" + req.planner_id + "]" :
379  req.planner_id);
380  if (pc == planner_configs_.end())
381  logWarn("Cannot find planning configuration for group '%s' using planner '%s'. Will use defaults instead.",
382  req.group_name.c_str(), req.planner_id.c_str());
383  }
384 
385  if (pc == planner_configs_.end())
386  {
387  pc = planner_configs_.find(req.group_name);
388  if (pc == planner_configs_.end())
389  {
390  logError("Cannot find planning configuration for group '%s'", req.group_name.c_str());
391  return ModelBasedPlanningContextPtr();
392  }
393  }
394 
395  // Check if sampling in JointModelStateSpace is enforced for this group by user.
396  // This is done by setting 'enforce_joint_model_state_space' to 'true' for the desired group in ompl_planning.yaml.
397  //
398  // Some planning problems like orientation path constraints are represented in PoseModelStateSpace and sampled via IK.
399  // However consecutive IK solutions are not checked for proximity at the moment and sometimes happen to be flipped,
400  // leading to invalid trajectories. This workaround lets the user prevent this problem by forcing rejection sampling
401  // in JointModelStateSpace.
402  StateSpaceFactoryTypeSelector factory_selector;
403  std::map<std::string, std::string>::const_iterator it = pc->second.config.find("enforce_joint_model_state_space");
404 
405  if (it != pc->second.config.end() && boost::lexical_cast<bool>(it->second))
406  factory_selector = boost::bind(&PlanningContextManager::getStateSpaceFactory1, this, _1,
408  else
409  factory_selector = boost::bind(&PlanningContextManager::getStateSpaceFactory2, this, _1, req);
410 
411  ModelBasedPlanningContextPtr context = getPlanningContext(pc->second, factory_selector, req);
412 
413  if (context)
414  {
415  context->clear();
416 
417  robot_state::RobotStatePtr start_state = planning_scene->getCurrentStateUpdated(req.start_state);
418 
419  // Setup the context
420  context->setPlanningScene(planning_scene);
421  context->setMotionPlanRequest(req);
422  context->setCompleteInitialState(*start_state);
423 
424  context->setPlanningVolume(req.workspace_parameters);
425  if (!context->setPathConstraints(req.path_constraints, &error_code))
426  return ModelBasedPlanningContextPtr();
427 
428  if (!context->setGoalConstraints(req.goal_constraints, req.path_constraints, &error_code))
429  return ModelBasedPlanningContextPtr();
430 
431  try
432  {
433  context->configure();
434  logDebug("%s: New planning context is set.", context->getName().c_str());
435  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
436  }
437  catch (ompl::Exception& ex)
438  {
439  logError("OMPL encountered an error: %s", ex.what());
440  context.reset();
441  }
442  }
443 
444  return context;
445 }
446 
447 ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getLastPlanningContext() const
448 {
449  return last_planning_context_->getContext();
450 }
boost::function< ConfiguredPlannerAllocator(const std::string &planner_type)> ConfiguredPlannerSelector
robot_model::RobotModelConstPtr kmodel_
The kinematic model for which motion plans are computed.
ModelBasedPlanningContextPtr getPlanningContext(const std::string &config, const std::string &factory_type="") const
void setContext(const ModelBasedPlanningContextPtr &context)
f
static const std::string PARAMETERIZATION_TYPE
std::map< std::string, PlannerConfigurationSettings > PlannerConfigurationMap
std::map< std::pair< std::string, std::string >, std::vector< ModelBasedPlanningContextPtr > > contexts_
std::map< std::string, std::string > config
The MoveIt interface to OMPL.
PlanningContextManager(const robot_model::RobotModelConstPtr &kmodel, const constraint_samplers::ConstraintSamplerManagerPtr &csm)
std::map< std::string, ModelBasedStateSpaceFactoryPtr > state_space_factories_
const ModelBasedStateSpaceFactoryPtr & getStateSpaceFactory2(const std::string &group_name, const moveit_msgs::MotionPlanRequest &req) const
boost::function< ob::PlannerPtr(const ompl::base::SpaceInformationPtr &si, const std::string &name, const ModelBasedPlanningContextSpecification &spec)> ConfiguredPlannerAllocator
ConfiguredPlannerSelector getPlannerSelector() const
void registerPlannerAllocator(const std::string &planner_id, const ConfiguredPlannerAllocator &pa)
constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_
planning_interface::PlannerConfigurationMap planner_configs_
All the existing planning configurations. The name of the configuration is the key of the map...
unsigned int max_planning_threads_
when planning in parallel, this is the maximum number of threads to use at one time ...
ConfiguredPlannerAllocator plannerSelector(const std::string &planner) const
void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap &pconfig)
Specify configurations for the planners.
unsigned int max_goal_sampling_attempts_
maximum number of attempts to be made at sampling goals
constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_
void registerStateSpaceFactory(const ModelBasedStateSpaceFactoryPtr &factory)
const ModelBasedStateSpaceFactoryPtr & getStateSpaceFactory1(const std::string &group_name, const std::string &factory_type) const
unsigned int max_goal_samples_
maximum number of states to sample in the goal region for any planning request (when such sampling is...
ModelBasedPlanningContextPtr getLastPlanningContext() const
boost::function< const ModelBasedStateSpaceFactoryPtr &(const std::string &)> StateSpaceFactoryTypeSelector
std::map< std::string, ConfiguredPlannerAllocator > known_planners_


ompl
Author(s): Ioan Sucan
autogenerated on Sun Jan 21 2018 03:55:31