planning_context_manager.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/ompl_interface/planning_context_manager.h>
00038 #include <moveit/robot_state/conversions.h>
00039 #include <moveit/profiler/profiler.h>
00040 #include <algorithm>
00041 #include <set>
00042 
00043 #include <ompl/geometric/planners/rrt/RRT.h>
00044 #include <ompl/geometric/planners/rrt/pRRT.h>
00045 #include <ompl/geometric/planners/rrt/RRTConnect.h>
00046 #include <ompl/geometric/planners/rrt/TRRT.h>
00047 #include <ompl/geometric/planners/rrt/LazyRRT.h>
00048 #include <ompl/geometric/planners/est/EST.h>
00049 #include <ompl/geometric/planners/sbl/SBL.h>
00050 #include <ompl/geometric/planners/sbl/pSBL.h>
00051 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
00052 #include <ompl/geometric/planners/kpiece/BKPIECE1.h>
00053 #include <ompl/geometric/planners/kpiece/LBKPIECE1.h>
00054 #include <ompl/geometric/planners/rrt/RRTstar.h>
00055 #include <ompl/geometric/planners/prm/PRM.h>
00056 #include <ompl/geometric/planners/prm/PRMstar.h>
00057 
00058 #include <moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h>
00059 #include <moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h>
00060 #include <moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h>
00061 
00062 namespace ompl_interface
00063 {
00064 class PlanningContextManager::LastPlanningContext
00065 {
00066 public:
00067   ModelBasedPlanningContextPtr getContext()
00068   {
00069     boost::mutex::scoped_lock slock(lock_);
00070     return last_planning_context_solve_;
00071   }
00072 
00073   void setContext(const ModelBasedPlanningContextPtr& context)
00074   {
00075     boost::mutex::scoped_lock slock(lock_);
00076     last_planning_context_solve_ = context;
00077   }
00078 
00079   void clear()
00080   {
00081     boost::mutex::scoped_lock slock(lock_);
00082     last_planning_context_solve_.reset();
00083   }
00084 
00085 private:
00086   /* The planning group for which solve() was called last */
00087   ModelBasedPlanningContextPtr last_planning_context_solve_;
00088   boost::mutex lock_;
00089 };
00090 
00091 struct PlanningContextManager::CachedContexts
00092 {
00093   std::map<std::pair<std::string, std::string>, std::vector<ModelBasedPlanningContextPtr> > contexts_;
00094   boost::mutex lock_;
00095 };
00096 
00097 }  // namespace ompl_interface
00098 
00099 ompl_interface::PlanningContextManager::PlanningContextManager(
00100     const robot_model::RobotModelConstPtr& kmodel, const constraint_samplers::ConstraintSamplerManagerPtr& csm)
00101   : kmodel_(kmodel)
00102   , constraint_sampler_manager_(csm)
00103   , max_goal_samples_(10)
00104   , max_state_sampling_attempts_(4)
00105   , max_goal_sampling_attempts_(1000)
00106   , max_planning_threads_(4)
00107   , max_solution_segment_length_(0.0)
00108   , minimum_waypoint_count_(2)
00109 {
00110   last_planning_context_.reset(new LastPlanningContext());
00111   cached_contexts_.reset(new CachedContexts());
00112   registerDefaultPlanners();
00113   registerDefaultStateSpaces();
00114 }
00115 
00116 ompl_interface::PlanningContextManager::~PlanningContextManager()
00117 {
00118 }
00119 
00120 namespace
00121 {
00122 using namespace ompl_interface;
00123 
00124 template <typename T>
00125 static ompl::base::PlannerPtr allocatePlanner(const ob::SpaceInformationPtr& si, const std::string& new_name,
00126                                               const ModelBasedPlanningContextSpecification& spec)
00127 {
00128   ompl::base::PlannerPtr planner(new T(si));
00129   if (!new_name.empty())
00130     planner->setName(new_name);
00131   planner->params().setParams(spec.config_, true);
00132   planner->setup();
00133   return planner;
00134 }
00135 }
00136 
00137 ompl_interface::ConfiguredPlannerAllocator
00138 ompl_interface::PlanningContextManager::plannerSelector(const std::string& planner) const
00139 {
00140   std::map<std::string, ConfiguredPlannerAllocator>::const_iterator it = known_planners_.find(planner);
00141   if (it != known_planners_.end())
00142     return it->second;
00143   else
00144   {
00145     logError("Unknown planner: '%s'", planner.c_str());
00146     return ConfiguredPlannerAllocator();
00147   }
00148 }
00149 
00150 void ompl_interface::PlanningContextManager::registerDefaultPlanners()
00151 {
00152   registerPlannerAllocator("geometric::RRT", boost::bind(&allocatePlanner<og::RRT>, _1, _2, _3));
00153   registerPlannerAllocator("geometric::RRTConnect", boost::bind(&allocatePlanner<og::RRTConnect>, _1, _2, _3));
00154   registerPlannerAllocator("geometric::LazyRRT", boost::bind(&allocatePlanner<og::LazyRRT>, _1, _2, _3));
00155   registerPlannerAllocator("geometric::TRRT", boost::bind(&allocatePlanner<og::TRRT>, _1, _2, _3));
00156   registerPlannerAllocator("geometric::EST", boost::bind(&allocatePlanner<og::EST>, _1, _2, _3));
00157   registerPlannerAllocator("geometric::SBL", boost::bind(&allocatePlanner<og::SBL>, _1, _2, _3));
00158   registerPlannerAllocator("geometric::KPIECE", boost::bind(&allocatePlanner<og::KPIECE1>, _1, _2, _3));
00159   registerPlannerAllocator("geometric::BKPIECE", boost::bind(&allocatePlanner<og::BKPIECE1>, _1, _2, _3));
00160   registerPlannerAllocator("geometric::LBKPIECE", boost::bind(&allocatePlanner<og::LBKPIECE1>, _1, _2, _3));
00161   registerPlannerAllocator("geometric::RRTstar", boost::bind(&allocatePlanner<og::RRTstar>, _1, _2, _3));
00162   registerPlannerAllocator("geometric::PRM", boost::bind(&allocatePlanner<og::PRM>, _1, _2, _3));
00163   registerPlannerAllocator("geometric::PRMstar", boost::bind(&allocatePlanner<og::PRMstar>, _1, _2, _3));
00164 }
00165 
00166 void ompl_interface::PlanningContextManager::registerDefaultStateSpaces()
00167 {
00168   registerStateSpaceFactory(ModelBasedStateSpaceFactoryPtr(new JointModelStateSpaceFactory()));
00169   registerStateSpaceFactory(ModelBasedStateSpaceFactoryPtr(new PoseModelStateSpaceFactory()));
00170 }
00171 
00172 ompl_interface::ConfiguredPlannerSelector ompl_interface::PlanningContextManager::getPlannerSelector() const
00173 {
00174   return boost::bind(&PlanningContextManager::plannerSelector, this, _1);
00175 }
00176 
00177 void ompl_interface::PlanningContextManager::setPlannerConfigurations(
00178     const planning_interface::PlannerConfigurationMap& pconfig)
00179 {
00180   planner_configs_ = pconfig;
00181 }
00182 
00183 ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext(
00184     const std::string& config, const std::string& factory_type) const
00185 {
00186   planning_interface::PlannerConfigurationMap::const_iterator pc = planner_configs_.find(config);
00187 
00188   if (pc != planner_configs_.end())
00189   {
00190     moveit_msgs::MotionPlanRequest req;  // dummy request with default values
00191     return getPlanningContext(pc->second,
00192                               boost::bind(&PlanningContextManager::getStateSpaceFactory1, this, _1, factory_type), req);
00193   }
00194   else
00195   {
00196     logError("Planning configuration '%s' was not found", config.c_str());
00197     return ModelBasedPlanningContextPtr();
00198   }
00199 }
00200 
00201 ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext(
00202     const planning_interface::PlannerConfigurationSettings& config,
00203     const StateSpaceFactoryTypeSelector& factory_selector, const moveit_msgs::MotionPlanRequest& req) const
00204 {
00205   const ompl_interface::ModelBasedStateSpaceFactoryPtr& factory = factory_selector(config.group);
00206 
00207   // Check for a cached planning context
00208   ModelBasedPlanningContextPtr context;
00209 
00210   {
00211     boost::mutex::scoped_lock slock(cached_contexts_->lock_);
00212     std::map<std::pair<std::string, std::string>, std::vector<ModelBasedPlanningContextPtr> >::const_iterator cc =
00213         cached_contexts_->contexts_.find(std::make_pair(config.name, factory->getType()));
00214     if (cc != cached_contexts_->contexts_.end())
00215     {
00216       for (std::size_t i = 0; i < cc->second.size(); ++i)
00217         if (cc->second[i].unique())
00218         {
00219           logDebug("Reusing cached planning context");
00220           context = cc->second[i];
00221           break;
00222         }
00223     }
00224   }
00225 
00226   // Create a new planning context
00227   if (!context)
00228   {
00229     ModelBasedStateSpaceSpecification space_spec(kmodel_, config.group);
00230     ModelBasedPlanningContextSpecification context_spec;
00231     context_spec.config_ = config.config;
00232     context_spec.planner_selector_ = getPlannerSelector();
00233     context_spec.constraint_sampler_manager_ = constraint_sampler_manager_;
00234     context_spec.state_space_ = factory->getNewStateSpace(space_spec);
00235 
00236     // Choose the correct simple setup type to load
00237     context_spec.ompl_simple_setup_.reset(new ompl::geometric::SimpleSetup(context_spec.state_space_));
00238 
00239     bool state_validity_cache = true;
00240     if (config.config.find("subspaces") != config.config.end())
00241     {
00242       context_spec.config_.erase("subspaces");
00243       // if the planner operates at subspace level the cache may be unsafe
00244       state_validity_cache = false;
00245       boost::char_separator<char> sep(" ");
00246       boost::tokenizer<boost::char_separator<char> > tok(config.config.at("subspaces"), sep);
00247       for (boost::tokenizer<boost::char_separator<char> >::iterator beg = tok.begin(); beg != tok.end(); ++beg)
00248       {
00249         const ompl_interface::ModelBasedStateSpaceFactoryPtr& sub_fact = factory_selector(*beg);
00250         if (sub_fact)
00251         {
00252           ModelBasedStateSpaceSpecification sub_space_spec(kmodel_, *beg);
00253           context_spec.subspaces_.push_back(sub_fact->getNewStateSpace(sub_space_spec));
00254         }
00255       }
00256     }
00257 
00258     logDebug("Creating new planning context");
00259     context.reset(new ModelBasedPlanningContext(config.name, context_spec));
00260     context->useStateValidityCache(state_validity_cache);
00261     {
00262       boost::mutex::scoped_lock slock(cached_contexts_->lock_);
00263       cached_contexts_->contexts_[std::make_pair(config.name, factory->getType())].push_back(context);
00264     }
00265   }
00266 
00267   context->setMaximumPlanningThreads(max_planning_threads_);
00268   context->setMaximumGoalSamples(max_goal_samples_);
00269   context->setMaximumStateSamplingAttempts(max_state_sampling_attempts_);
00270   context->setMaximumGoalSamplingAttempts(max_goal_sampling_attempts_);
00271   if (max_solution_segment_length_ <= std::numeric_limits<double>::epsilon())
00272     context->setMaximumSolutionSegmentLength(context->getOMPLSimpleSetup()->getStateSpace()->getMaximumExtent() /
00273                                              100.0);
00274   else
00275     context->setMaximumSolutionSegmentLength(max_solution_segment_length_);
00276   context->setMinimumWaypointCount(minimum_waypoint_count_);
00277 
00278   context->setSpecificationConfig(config.config);
00279 
00280   last_planning_context_->setContext(context);
00281   return context;
00282 }
00283 
00284 const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningContextManager::getStateSpaceFactory1(
00285     const std::string& /* dummy */, const std::string& factory_type) const
00286 {
00287   std::map<std::string, ModelBasedStateSpaceFactoryPtr>::const_iterator f =
00288       factory_type.empty() ? state_space_factories_.begin() : state_space_factories_.find(factory_type);
00289   if (f != state_space_factories_.end())
00290     return f->second;
00291   else
00292   {
00293     logError("Factory of type '%s' was not found", factory_type.c_str());
00294     static const ModelBasedStateSpaceFactoryPtr empty;
00295     return empty;
00296   }
00297 }
00298 
00299 const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningContextManager::getStateSpaceFactory2(
00300     const std::string& group, const moveit_msgs::MotionPlanRequest& req) const
00301 {
00302   // find the problem representation to use
00303   std::map<std::string, ModelBasedStateSpaceFactoryPtr>::const_iterator best = state_space_factories_.end();
00304   int prev_priority = -1;
00305   for (std::map<std::string, ModelBasedStateSpaceFactoryPtr>::const_iterator it = state_space_factories_.begin();
00306        it != state_space_factories_.end(); ++it)
00307   {
00308     int priority = it->second->canRepresentProblem(group, req, kmodel_);
00309     if (priority > 0)
00310       if (best == state_space_factories_.end() || priority > prev_priority)
00311       {
00312         best = it;
00313         prev_priority = priority;
00314       }
00315   }
00316 
00317   if (best == state_space_factories_.end())
00318   {
00319     logError("There are no known state spaces that can represent the given planning problem");
00320     static const ModelBasedStateSpaceFactoryPtr empty;
00321     return empty;
00322   }
00323   else
00324   {
00325     logDebug("Using '%s' parameterization for solving problem", best->first.c_str());
00326     return best->second;
00327   }
00328 }
00329 
00330 ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext(
00331     const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::MotionPlanRequest& req,
00332     moveit_msgs::MoveItErrorCodes& error_code) const
00333 {
00334   if (req.group_name.empty())
00335   {
00336     logError("No group specified to plan for");
00337     error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00338     return ModelBasedPlanningContextPtr();
00339   }
00340 
00341   error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
00342 
00343   if (!planning_scene)
00344   {
00345     logError("No planning scene supplied as input");
00346     return ModelBasedPlanningContextPtr();
00347   }
00348 
00349   // identify the correct planning configuration
00350   planning_interface::PlannerConfigurationMap::const_iterator pc = planner_configs_.end();
00351   if (!req.planner_id.empty())
00352   {
00353     pc = planner_configs_.find(req.planner_id.find(req.group_name) == std::string::npos ?
00354                                    req.group_name + "[" + req.planner_id + "]" :
00355                                    req.planner_id);
00356     if (pc == planner_configs_.end())
00357       logWarn("Cannot find planning configuration for group '%s' using planner '%s'. Will use defaults instead.",
00358               req.group_name.c_str(), req.planner_id.c_str());
00359   }
00360   if (pc == planner_configs_.end())
00361   {
00362     pc = planner_configs_.find(req.group_name);
00363     if (pc == planner_configs_.end())
00364     {
00365       logError("Cannot find planning configuration for group '%s'", req.group_name.c_str());
00366       return ModelBasedPlanningContextPtr();
00367     }
00368   }
00369 
00370   // Check if sampling in JointModelStateSpace is enforced for this group by user.
00371   // This is done by setting 'enforce_joint_model_state_space' to 'true' for the desired group in ompl_planning.yaml.
00372   //
00373   // Some planning problems like orientation path constraints are represented in PoseModelStateSpace and sampled via IK.
00374   // However consecutive IK solutions are not checked for proximity at the moment and sometimes happen to be flipped,
00375   // leading to invalid trajectories. This workaround lets the user prevent this problem by forcing rejection sampling
00376   // in JointModelStateSpace.
00377   StateSpaceFactoryTypeSelector factory_selector;
00378   std::map<std::string, std::string>::const_iterator it = pc->second.config.find("enforce_joint_model_state_space");
00379 
00380   if (it != pc->second.config.end() && boost::lexical_cast<bool>(it->second))
00381     factory_selector = boost::bind(&PlanningContextManager::getStateSpaceFactory1, this, _1,
00382                                    JointModelStateSpace::PARAMETERIZATION_TYPE);
00383   else
00384     factory_selector = boost::bind(&PlanningContextManager::getStateSpaceFactory2, this, _1, req);
00385 
00386   ModelBasedPlanningContextPtr context = getPlanningContext(pc->second, factory_selector, req);
00387 
00388   if (context)
00389   {
00390     context->clear();
00391 
00392     robot_state::RobotStatePtr start_state = planning_scene->getCurrentStateUpdated(req.start_state);
00393 
00394     // Setup the context
00395     context->setPlanningScene(planning_scene);
00396     context->setMotionPlanRequest(req);
00397     context->setCompleteInitialState(*start_state);
00398 
00399     context->setPlanningVolume(req.workspace_parameters);
00400     if (!context->setPathConstraints(req.path_constraints, &error_code))
00401       return ModelBasedPlanningContextPtr();
00402 
00403     if (!context->setGoalConstraints(req.goal_constraints, req.path_constraints, &error_code))
00404       return ModelBasedPlanningContextPtr();
00405 
00406     try
00407     {
00408       context->configure();
00409       logDebug("%s: New planning context is set.", context->getName().c_str());
00410       error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00411     }
00412     catch (ompl::Exception& ex)
00413     {
00414       logError("OMPL encountered an error: %s", ex.what());
00415       context.reset();
00416     }
00417   }
00418 
00419   return context;
00420 }
00421 
00422 ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getLastPlanningContext() const
00423 {
00424   return last_planning_context_->getContext();
00425 }


ompl
Author(s): Ioan Sucan
autogenerated on Fri Oct 26 2018 02:26:38