ompl_ros_interface: ompl_ros_planning

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, 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, Inc. 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 
00037 #include <ompl_ros_interface/ompl_ros_planning_group.h>
00038 
00039 namespace ompl_ros_interface
00040 {
00041 bool OmplRosPlanningGroup::initialize(const ros::NodeHandle &node_handle,
00042                                       const std::string &group_name,
00043                                       const std::string &planner_config_name,
00044                                       planning_environment::PlanningMonitor *planning_monitor)
00045 {
00046   planning_monitor_    = planning_monitor;
00047   group_name_          = group_name;
00048   node_handle_         = node_handle;
00049   planner_config_name_ = planner_config_name;
00050     
00051   if(!initializePhysicalGroup())
00052     return false;
00053 
00054   if(!initializePlanningStateSpace(state_space_))
00055     return false;
00056 
00057   double longest_valid_segment_fraction;
00058   node_handle_.param(group_name_+"/longest_valid_segment_fraction",longest_valid_segment_fraction,0.005);
00059   state_space_->setLongestValidSegmentFraction(longest_valid_segment_fraction);
00060 
00061   //Setup the projection evaluator for this group
00062   if(!initializeProjectionEvaluator())
00063   {
00064     ROS_ERROR("Could not setup the projection evaluator");
00065     return false;
00066   }
00067 
00068   planner_.reset(new ompl::geometric::SimpleSetup(state_space_));
00069   
00070   if(!initializePlanner())
00071     return false;
00072 
00073   if(!initializeStateValidityChecker(state_validity_checker_))
00074     return false;
00075 
00076   planner_->setStateValidityChecker(static_cast<ompl::base::StateValidityCheckerPtr> (state_validity_checker_));
00077   planner_->setPlanner(ompl_planner_);
00078 
00079   return true;
00080 };
00081 
00082 bool OmplRosPlanningGroup::initializePhysicalGroup()
00083 {
00084   std::string physical_group_name;
00085   if(!planning_monitor_->getKinematicModel()->hasModelGroup(group_name_))
00086   {
00087     ROS_INFO("Joint group %s is an abstract group",group_name_.c_str());
00088     if(!node_handle_.hasParam(group_name_+"/physical_group"))
00089     {
00090       ROS_ERROR("No physical group specified for %s",group_name_.c_str());
00091       return false;
00092     }
00093     else
00094       node_handle_.getParam(group_name_+"/physical_group",physical_group_name);
00095   }
00096   else
00097     physical_group_name = group_name_;
00098 
00099   //Setup the actual (physical) groups
00100   physical_joint_group_ = planning_monitor_->getKinematicModel()->getModelGroup(physical_group_name);
00101   return true;
00102 }
00103 
00104 
00105 bool OmplRosPlanningGroup::initializeProjectionEvaluator()
00106 {
00107   std::string projection_evaluator;
00108   if(!node_handle_.hasParam(group_name_+"/projection_evaluator"))
00109   {
00110     ROS_ERROR("Projection evaluator not defined for group %s",group_name_.c_str());
00111     return false;
00112   }
00113   node_handle_.getParam(group_name_+"/projection_evaluator",projection_evaluator);
00114   ompl::base::ProjectionEvaluatorPtr ompl_projection_evaluator;
00115   ompl_projection_evaluator.reset(new ompl_ros_interface::OmplRosProjectionEvaluator(state_space_.get(),
00116                                                                                      projection_evaluator));
00117   state_space_->registerDefaultProjection(ompl_projection_evaluator);
00118   return true;
00119 }
00120 
00121 bool OmplRosPlanningGroup::initializePlanner()
00122 {
00123   planner_config_.reset(new ompl_ros_interface::PlannerConfig(node_handle_.getNamespace(),planner_config_name_));
00124   std::string planner_type = planner_config_->getParamString("type");
00125   if(planner_type == "kinematic::RRT")
00126     return initializeRRTPlanner();
00127   else if(planner_type == "kinematic::RRTConnect")
00128     return initializeRRTConnectPlanner();
00129   else if(planner_type == "kinematic::pRRT")
00130     return initializepRRTPlanner();
00131   else if(planner_type == "kinematic::LazyRRT")
00132     return initializeLazyRRTPlanner();
00133   else if(planner_type == "kinematic::EST")
00134     return initializeESTPlanner();
00135   else if(planner_type == "kinematic::SBL")
00136     return initializeSBLPlanner();
00137   else if(planner_type == "kinematic::pSBL")
00138     return initializepSBLPlanner();
00139   else if(planner_type == "kinematic::KPIECE")
00140     return initializeKPIECEPlanner();
00141   else if(planner_type == "kinematic::LBKPIECE")
00142     return initializeLBKPIECEPlanner();
00143         else
00144   {
00145     ROS_WARN("Unknown planner type: %s", planner_type.c_str());
00146     return false;
00147   }
00148 }
00149 
00150 bool OmplRosPlanningGroup::initializeRRTPlanner()
00151 {
00152   ompl_planner_.reset(new ompl::geometric::RRT(planner_->getSpaceInformation()));
00153   ompl::geometric::RRT* new_planner = dynamic_cast<ompl::geometric::RRT*>(ompl_planner_.get());
00154   if (planner_config_->hasParam("goal_bias"))
00155   {
00156     new_planner->setGoalBias(planner_config_->getParamDouble("goal_bias",new_planner->getGoalBias()));
00157     ROS_DEBUG("RRTPlanner::Goal bias is set to %g", new_planner->getGoalBias());
00158   }  
00159   return true;
00160 }
00161 
00162 bool OmplRosPlanningGroup::initializeRRTConnectPlanner()
00163 {
00164   ompl_planner_.reset(new ompl::geometric::RRTConnect(planner_->getSpaceInformation()));
00165   ompl::geometric::RRTConnect* new_planner = dynamic_cast<ompl::geometric::RRTConnect*>(ompl_planner_.get());
00166   if (planner_config_->hasParam("range"))
00167   {
00168     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00169     ROS_DEBUG("RRTConnectPlanner::Range is set to %g", new_planner->getRange());
00170   }  
00171   return true;
00172 }
00173 
00174 bool OmplRosPlanningGroup::initializepRRTPlanner()
00175 {
00176   ompl_planner_.reset(new ompl::geometric::pRRT(planner_->getSpaceInformation()));
00177   ompl::geometric::pRRT* new_planner = dynamic_cast<ompl::geometric::pRRT*>(ompl_planner_.get());
00178   if (planner_config_->hasParam("range"))
00179   {
00180     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00181     ROS_DEBUG("pRRTPlanner::Range is set to %g", new_planner->getRange());
00182   }  
00183   if (planner_config_->hasParam("goal_bias"))
00184   {
00185     new_planner->setGoalBias(planner_config_->getParamDouble("goal_bias",new_planner->getGoalBias()));
00186     ROS_DEBUG("pRRTPlanner::Goal bias is set to %g", new_planner->getGoalBias());
00187   }  
00188   if (planner_config_->hasParam("thread_count"))
00189   {
00190     new_planner->setThreadCount(planner_config_->getParamDouble("thread_count",new_planner->getThreadCount()));
00191     ROS_DEBUG("pRRTPlanner::Thread count is set to %d", (int) new_planner->getThreadCount());
00192   }  
00193   return true;
00194 }
00195 
00196 bool OmplRosPlanningGroup::initializeLazyRRTPlanner()
00197 {
00198   ompl_planner_.reset(new ompl::geometric::LazyRRT(planner_->getSpaceInformation()));
00199   ompl::geometric::LazyRRT* new_planner = dynamic_cast<ompl::geometric::LazyRRT*>(ompl_planner_.get());
00200   if (planner_config_->hasParam("range"))
00201   {
00202     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00203     ROS_DEBUG("LazyRRTPlanner::Range is set to %g", new_planner->getRange());
00204   }  
00205   if (planner_config_->hasParam("goal_bias"))
00206   {
00207     new_planner->setGoalBias(planner_config_->getParamDouble("goal_bias",new_planner->getGoalBias()));
00208     ROS_DEBUG("LazyRRTPlanner::Goal bias is set to %g", new_planner->getGoalBias());
00209   }  
00210   return true;
00211 }
00212 
00213 bool OmplRosPlanningGroup::initializeESTPlanner()
00214 {
00215   ompl_planner_.reset(new ompl::geometric::EST(planner_->getSpaceInformation()));
00216   ompl::geometric::EST* new_planner = dynamic_cast<ompl::geometric::EST*>(ompl_planner_.get());
00217   if (planner_config_->hasParam("range"))
00218   {
00219     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00220     ROS_DEBUG("ESTPlanner::Range is set to %g", new_planner->getRange());
00221   }  
00222   if (planner_config_->hasParam("goal_bias"))
00223   {
00224     new_planner->setGoalBias(planner_config_->getParamDouble("goal_bias",new_planner->getGoalBias()));
00225     ROS_DEBUG("ESTPlanner::Goal bias is set to %g", new_planner->getGoalBias());
00226   }  
00227   return true;
00228 }
00229 
00230 bool OmplRosPlanningGroup::initializeSBLPlanner()
00231 {
00232   ompl_planner_.reset(new ompl::geometric::SBL(planner_->getSpaceInformation()));
00233   ompl::geometric::SBL* new_planner = dynamic_cast<ompl::geometric::SBL*>(ompl_planner_.get());
00234   if (planner_config_->hasParam("range"))
00235   {
00236     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00237     ROS_DEBUG("SBLPlanner::Range is set to %g", new_planner->getRange());
00238   }  
00239   return true;
00240 }
00241 
00242 bool OmplRosPlanningGroup::initializepSBLPlanner()
00243 {
00244   ompl_planner_.reset(new ompl::geometric::pSBL(planner_->getSpaceInformation()));
00245   ompl::geometric::pSBL* new_planner = dynamic_cast<ompl::geometric::pSBL*>(ompl_planner_.get());
00246   if (planner_config_->hasParam("range"))
00247   {
00248     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00249     ROS_DEBUG("pSBLPlanner::Range is set to %g", new_planner->getRange());
00250   }  
00251   if (planner_config_->hasParam("thread_count"))
00252   {
00253     new_planner->setThreadCount(planner_config_->getParamDouble("thread_count",new_planner->getThreadCount()));
00254     ROS_DEBUG("pSBLPlanner::Thread count is set to %d", (int) new_planner->getThreadCount());
00255   }  
00256   return true;
00257 }
00258 
00259 bool OmplRosPlanningGroup::initializeKPIECEPlanner()
00260 {
00261   ompl_planner_.reset(new ompl::geometric::KPIECE1(planner_->getSpaceInformation()));
00262   ompl::geometric::KPIECE1* new_planner = dynamic_cast<ompl::geometric::KPIECE1*>(ompl_planner_.get());
00263   if (planner_config_->hasParam("range"))
00264   {
00265     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00266     ROS_DEBUG("KPIECEPlanner::Range is set to %g", new_planner->getRange());
00267   }  
00268   if (planner_config_->hasParam("goal_bias"))
00269   {
00270     new_planner->setGoalBias(planner_config_->getParamDouble("goal_bias",new_planner->getGoalBias()));
00271     ROS_DEBUG("KPIECEPlanner::Goal bias is set to %g", new_planner->getGoalBias());
00272   }  
00273   return true;
00274 }
00275 
00276 bool OmplRosPlanningGroup::initializeLBKPIECEPlanner()
00277 {
00278   ompl_planner_.reset(new ompl::geometric::LBKPIECE1(planner_->getSpaceInformation()));
00279   ompl::geometric::LBKPIECE1* new_planner = dynamic_cast<ompl::geometric::LBKPIECE1*>(ompl_planner_.get());
00280   if (planner_config_->hasParam("range"))
00281   {
00282     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00283     ROS_DEBUG("LBKPIECEPlanner::Range is set to %g", new_planner->getRange());
00284   }  
00285   return true;
00286 }
00287 
00288 bool OmplRosPlanningGroup::transformConstraints(motion_planning_msgs::GetMotionPlan::Request &request, 
00289                                                 motion_planning_msgs::GetMotionPlan::Response &response)
00290 {
00291   if(!planning_monitor_->transformConstraintsToFrame(request.motion_plan_request.goal_constraints, 
00292                                                      planning_monitor_->getWorldFrameId(),
00293                                                      response.error_code))
00294     return false;
00295 
00296 
00297   if(!planning_monitor_->transformConstraintsToFrame(request.motion_plan_request.goal_constraints, 
00298                                                      planning_monitor_->getWorldFrameId(),
00299                                                      response.error_code))
00300     return false;
00301  
00302   return true;
00303 }
00304 
00305 bool OmplRosPlanningGroup::omplPathGeometricToRobotTrajectory(const ompl::geometric::PathGeometric &path, 
00306                                                               motion_planning_msgs::RobotTrajectory &robot_trajectory)
00307 {
00308   if(!ompl_ros_interface::jointStateGroupToRobotTrajectory(physical_joint_state_group_,robot_trajectory))
00309     return false;
00310   if(!ompl_ros_interface::omplPathGeometricToRobotTrajectory(path,state_space_,robot_trajectory))
00311     return false;
00312   return true;
00313 }
00314 
00315 bool OmplRosPlanningGroup::computePlan(motion_planning_msgs::GetMotionPlan::Request &request, 
00316                                        motion_planning_msgs::GetMotionPlan::Response &response)
00317 {
00318   planner_->clear();
00319   boost::scoped_ptr<planning_models::KinematicState> kinematic_state;
00320   kinematic_state.reset(new planning_models::KinematicState(planning_monitor_->getKinematicModel()));
00321   physical_joint_state_group_ = kinematic_state->getJointStateGroup(physical_joint_group_->getName());
00322   if(!physical_joint_state_group_)
00323   {
00324     ROS_ERROR("Could not find physical joint state group");
00325     response.error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::PLANNING_FAILED;
00326     return finish(false);
00327   }
00328   if (!isRequestValid(request,response))
00329     return finish(false);
00330 
00331   if(!configurePlanningMonitor(request,response,kinematic_state.get()))
00332     return finish(false);
00333   
00334   if(!transformConstraints(request,response))
00335     return finish(false);
00336   
00337   if(!setStartAndGoalStates(request,response))
00338     return finish(false);
00339   
00340   bool solved = planner_->solve(request.motion_plan_request.allowed_planning_time.toSec());
00341   
00342   if(solved)
00343   {
00344     ROS_DEBUG("Found solution for request in %f seconds",planner_->getLastPlanComputationTime());
00345     response.planning_time = ros::Duration(planner_->getLastPlanComputationTime());
00346     planner_->getPathSimplifier()->reduceVertices(planner_->getSolutionPath());
00347     planner_->getPathSimplifier()->collapseCloseVertices(planner_->getSolutionPath());
00348     
00349     try
00350     {
00351       response.trajectory = getSolutionPath();
00352       response.error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::SUCCESS;
00353       return finish(true);
00354     }
00355     catch(...)
00356     {
00357       ROS_ERROR("Could not find solution");
00358       response.error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::PLANNING_FAILED;
00359       return finish(false);
00360     }
00361   }
00362   else
00363   {
00364     ROS_ERROR("Could not find solution for request");
00365     response.error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::PLANNING_FAILED;
00366     return finish(false);
00367   }  
00368 }
00369 
00370 
00371 bool OmplRosPlanningGroup::finish(const bool &result)
00372 {
00373   planning_monitor_->revertToDefaultState();
00374   return result;
00375 }
00376 
00377 bool OmplRosPlanningGroup::configurePlanningMonitor(motion_planning_msgs::GetMotionPlan::Request &request,
00378                                                     motion_planning_msgs::GetMotionPlan::Response &response,
00379                                                     planning_models::KinematicState *kinematic_state)
00380 {
00381   if(!planning_monitor_->prepareForValidityChecks(physical_joint_group_->getJointModelNames(),
00382                                                   request.motion_plan_request.ordered_collision_operations,
00383                                                   request.motion_plan_request.allowed_contacts,
00384                                                   request.motion_plan_request.path_constraints,
00385                                                   request.motion_plan_request.goal_constraints,
00386                                                   request.motion_plan_request.link_padding,
00387                                                   response.error_code))
00388   {
00389     ROS_ERROR("Could not configure planning monitor");
00390     return false;
00391   }
00392 
00393   /* set the pose of the whole robot */
00394   planning_monitor_->setRobotStateAndComputeTransforms(request.motion_plan_request.start_state, *kinematic_state);
00395   planning_monitor_->getEnvironmentModel()->updateRobotModel(kinematic_state);
00396 
00397   /* set the kinematic state for the state validator */
00398   ompl_ros_interface::OmplRosStateValidityChecker *my_checker = dynamic_cast<ompl_ros_interface::OmplRosStateValidityChecker*>(state_validity_checker_.get());
00399   my_checker->configureOnRequest(kinematic_state,
00400                                  physical_joint_state_group_,
00401                                  request);
00402   return true;
00403 }
00404 
00405 bool OmplRosPlanningGroup::setStartAndGoalStates(motion_planning_msgs::GetMotionPlan::Request &request,
00406                                                  motion_planning_msgs::GetMotionPlan::Response &response)
00407 {
00408   if(!setStart(request,response))
00409     return false;
00410   if(!setGoal(request,response))
00411     return false;
00412   return true;
00413 }
00414 
00415 }
ompl_ros_planning_group.cpp
