ompl_interface.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, 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/ompl_interface.h>
00038 #include <moveit/robot_state/conversions.h>
00039 #include <moveit/kinematic_constraints/utils.h>
00040 #include <moveit/ompl_interface/detail/constrained_valid_state_sampler.h>
00041 #include <moveit/profiler/profiler.h>
00042 #include <fstream>
00043 
00044 ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstPtr& kmodel, const ros::NodeHandle& nh)
00045   : nh_(nh)
00046   , kmodel_(kmodel)
00047   , constraint_sampler_manager_(new constraint_samplers::ConstraintSamplerManager())
00048   , context_manager_(kmodel, constraint_sampler_manager_)
00049   , constraints_library_(new ConstraintsLibrary(context_manager_))
00050   , use_constraints_approximations_(true)
00051   , simplify_solutions_(true)
00052 {
00053   ROS_INFO("Initializing OMPL interface using ROS parameters");
00054   loadPlannerConfigurations();
00055   loadConstraintApproximations();
00056   loadConstraintSamplers();
00057 }
00058 
00059 ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstPtr& kmodel,
00060                                              const planning_interface::PlannerConfigurationMap& pconfig,
00061                                              const ros::NodeHandle& nh)
00062   : nh_(nh)
00063   , kmodel_(kmodel)
00064   , constraint_sampler_manager_(new constraint_samplers::ConstraintSamplerManager())
00065   , context_manager_(kmodel, constraint_sampler_manager_)
00066   , constraints_library_(new ConstraintsLibrary(context_manager_))
00067   , use_constraints_approximations_(true)
00068   , simplify_solutions_(true)
00069 {
00070   ROS_INFO("Initializing OMPL interface using specified configuration");
00071   setPlannerConfigurations(pconfig);
00072   loadConstraintApproximations();
00073   loadConstraintSamplers();
00074 }
00075 
00076 ompl_interface::OMPLInterface::~OMPLInterface()
00077 {
00078 }
00079 
00080 void ompl_interface::OMPLInterface::setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig)
00081 {
00082   planning_interface::PlannerConfigurationMap pconfig2 = pconfig;
00083 
00084   // construct default configurations for planning groups that don't have configs already passed in
00085   const std::vector<const robot_model::JointModelGroup*>& groups = kmodel_->getJointModelGroups();
00086   for (std::size_t i = 0; i < groups.size(); ++i)
00087   {
00088     if (pconfig.find(groups[i]->getName()) == pconfig.end())
00089     {
00090       planning_interface::PlannerConfigurationSettings empty;
00091       empty.name = empty.group = groups[i]->getName();
00092       pconfig2[empty.name] = empty;
00093     }
00094   }
00095 
00096   context_manager_.setPlannerConfigurations(pconfig2);
00097 }
00098 
00099 ompl_interface::ModelBasedPlanningContextPtr ompl_interface::OMPLInterface::getPlanningContext(
00100     const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req) const
00101 {
00102   moveit_msgs::MoveItErrorCodes dummy;
00103   return getPlanningContext(planning_scene, req, dummy);
00104 }
00105 
00106 ompl_interface::ModelBasedPlanningContextPtr ompl_interface::OMPLInterface::getPlanningContext(
00107     const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req,
00108     moveit_msgs::MoveItErrorCodes& error_code) const
00109 {
00110   ModelBasedPlanningContextPtr ctx = context_manager_.getPlanningContext(planning_scene, req, error_code);
00111   if (ctx)
00112     configureContext(ctx);
00113   return ctx;
00114 }
00115 
00116 ompl_interface::ModelBasedPlanningContextPtr
00117 ompl_interface::OMPLInterface::getPlanningContext(const std::string& config, const std::string& factory_type) const
00118 {
00119   ModelBasedPlanningContextPtr ctx = context_manager_.getPlanningContext(config, factory_type);
00120   if (ctx)
00121     configureContext(ctx);
00122   return ctx;
00123 }
00124 
00125 void ompl_interface::OMPLInterface::configureContext(const ModelBasedPlanningContextPtr& context) const
00126 {
00127   if (use_constraints_approximations_)
00128     context->setConstraintsApproximations(constraints_library_);
00129   else
00130     context->setConstraintsApproximations(ConstraintsLibraryPtr());
00131   context->simplifySolutions(simplify_solutions_);
00132 }
00133 
00134 void ompl_interface::OMPLInterface::loadConstraintApproximations(const std::string& path)
00135 {
00136   constraints_library_->loadConstraintApproximations(path);
00137   std::stringstream ss;
00138   constraints_library_->printConstraintApproximations(ss);
00139   ROS_INFO_STREAM(ss.str());
00140 }
00141 
00142 void ompl_interface::OMPLInterface::saveConstraintApproximations(const std::string& path)
00143 {
00144   constraints_library_->saveConstraintApproximations(path);
00145 }
00146 
00147 bool ompl_interface::OMPLInterface::saveConstraintApproximations()
00148 {
00149   std::string cpath;
00150   if (nh_.getParam("constraint_approximations_path", cpath))
00151   {
00152     saveConstraintApproximations(cpath);
00153     return true;
00154   }
00155   ROS_WARN("ROS param 'constraint_approximations' not found. Unable to save constraint approximations");
00156   return false;
00157 }
00158 
00159 bool ompl_interface::OMPLInterface::loadConstraintApproximations()
00160 {
00161   std::string cpath;
00162   if (nh_.getParam("constraint_approximations_path", cpath))
00163   {
00164     loadConstraintApproximations(cpath);
00165     return true;
00166   }
00167   return false;
00168 }
00169 
00170 void ompl_interface::OMPLInterface::loadConstraintSamplers()
00171 {
00172   constraint_sampler_manager_loader_.reset(
00173       new constraint_sampler_manager_loader::ConstraintSamplerManagerLoader(constraint_sampler_manager_));
00174 }
00175 
00176 void ompl_interface::OMPLInterface::loadPlannerConfigurations()
00177 {
00178   const std::vector<std::string>& group_names = kmodel_->getJointModelGroupNames();
00179   planning_interface::PlannerConfigurationMap pconfig;
00180 
00181   // read the planning configuration for each group
00182   pconfig.clear();
00183   for (std::size_t i = 0; i < group_names.size(); ++i)
00184   {
00185     // the set of planning parameters that can be specific for the group (inherited by configurations of that group)
00186     static const std::string KNOWN_GROUP_PARAMS[] = { "projection_evaluator", "longest_valid_segment_fraction",
00187                                                       "enforce_joint_model_state_space" };
00188 
00189     // get parameters specific for the robot planning group
00190     std::map<std::string, std::string> specific_group_params;
00191     for (std::size_t k = 0; k < sizeof(KNOWN_GROUP_PARAMS) / sizeof(std::string); ++k)
00192     {
00193       if (nh_.hasParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k]))
00194       {
00195         std::string value;
00196         if (nh_.getParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k], value))
00197         {
00198           if (!value.empty())
00199             specific_group_params[KNOWN_GROUP_PARAMS[k]] = value;
00200         }
00201         else
00202         {
00203           double value_d;
00204           if (nh_.getParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k], value_d))
00205             specific_group_params[KNOWN_GROUP_PARAMS[k]] = boost::lexical_cast<std::string>(value_d);
00206           else
00207           {
00208             int value_i;
00209             if (nh_.getParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k], value_d))
00210               specific_group_params[KNOWN_GROUP_PARAMS[k]] = boost::lexical_cast<std::string>(value_i);
00211             else
00212             {
00213               bool value_b;
00214               if (nh_.getParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k], value_b))
00215                 specific_group_params[KNOWN_GROUP_PARAMS[k]] = boost::lexical_cast<std::string>(value_b);
00216             }
00217           }
00218         }
00219       }
00220     }
00221 
00222     // set the parameters (if any) for the default group configuration;
00223     if (!specific_group_params.empty())
00224     {
00225       planning_interface::PlannerConfigurationSettings pc;
00226       pc.name = group_names[i];
00227       pc.group = group_names[i];
00228       pc.config = specific_group_params;
00229       pconfig[pc.name] = pc;
00230     }
00231 
00232     // get parameters specific to each planner type
00233     XmlRpc::XmlRpcValue config_names;
00234     if (nh_.getParam(group_names[i] + "/planner_configs", config_names))
00235     {
00236       if (config_names.getType() == XmlRpc::XmlRpcValue::TypeArray)
00237       {
00238         for (int32_t j = 0; j < config_names.size(); ++j)
00239           if (config_names[j].getType() == XmlRpc::XmlRpcValue::TypeString)
00240           {
00241             std::string planner_config = static_cast<std::string>(config_names[j]);
00242             XmlRpc::XmlRpcValue xml_config;
00243             if (nh_.getParam("planner_configs/" + planner_config, xml_config))
00244             {
00245               if (xml_config.getType() == XmlRpc::XmlRpcValue::TypeStruct)
00246               {
00247                 planning_interface::PlannerConfigurationSettings pc;
00248                 pc.name = group_names[i] + "[" + planner_config + "]";
00249                 pc.group = group_names[i];
00250                 // inherit parameters from the group (which can be overriden)
00251                 pc.config = specific_group_params;
00252 
00253                 // read parameters specific for this configuration
00254                 for (XmlRpc::XmlRpcValue::iterator it = xml_config.begin(); it != xml_config.end(); ++it)
00255                   if (it->second.getType() == XmlRpc::XmlRpcValue::TypeString)
00256                     pc.config[it->first] = static_cast<std::string>(it->second);
00257                   else if (it->second.getType() == XmlRpc::XmlRpcValue::TypeDouble)
00258                     pc.config[it->first] = boost::lexical_cast<std::string>(static_cast<double>(it->second));
00259                   else if (it->second.getType() == XmlRpc::XmlRpcValue::TypeInt)
00260                     pc.config[it->first] = boost::lexical_cast<std::string>(static_cast<int>(it->second));
00261                   else if (it->second.getType() == XmlRpc::XmlRpcValue::TypeBoolean)
00262                     pc.config[it->first] = boost::lexical_cast<std::string>(static_cast<bool>(it->second));
00263                 pconfig[pc.name] = pc;
00264               }
00265               else
00266                 ROS_ERROR("A planning configuration should be of type XmlRpc Struct type (for configuration '%s')",
00267                           planner_config.c_str());
00268             }
00269             else
00270               ROS_ERROR("Could not find the planner configuration '%s' on the param server", planner_config.c_str());
00271           }
00272           else
00273             ROS_ERROR("Planner configuration names must be of type string (for group '%s')", group_names[i].c_str());
00274       }
00275       else
00276         ROS_ERROR("The planner_configs argument of a group configuration should be an array of strings (for group "
00277                   "'%s')",
00278                   group_names[i].c_str());
00279     }
00280   }
00281 
00282   for (planning_interface::PlannerConfigurationMap::iterator it = pconfig.begin(); it != pconfig.end(); ++it)
00283   {
00284     ROS_DEBUG_STREAM_NAMED("parameters", "Parameters for configuration '" << it->first << "'");
00285     for (std::map<std::string, std::string>::const_iterator config_it = it->second.config.begin();
00286          config_it != it->second.config.end(); ++config_it)
00287       ROS_DEBUG_STREAM_NAMED("parameters", " - " << config_it->first << " = " << config_it->second);
00288   }
00289   setPlannerConfigurations(pconfig);
00290 }
00291 
00292 void ompl_interface::OMPLInterface::printStatus()
00293 {
00294   ROS_INFO("OMPL ROS interface is running.");
00295 }


ompl
Author(s): Ioan Sucan
autogenerated on Wed Jan 17 2018 03:32:20