ompl_ros_interface: 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 #include <planning_environment/models/model_utils.h>
00039 
00040 namespace ompl_ros_interface
00041 {
00042 bool OmplRosPlanningGroup::initialize(const ros::NodeHandle &node_handle,
00043                                       const std::string &group_name,
00044                                       const std::string &planner_config_name,
00045                                       planning_environment::CollisionModelsInterface *cmi)
00046 {
00047   collision_models_interface_    = cmi;
00048   group_name_          = group_name;
00049   node_handle_         = node_handle;
00050   planner_config_name_ = planner_config_name;
00051     
00052   if(!initializePhysicalGroup())
00053     return false;
00054 
00055   if(!initializePlanningStateSpace(state_space_))
00056     return false;
00057 
00058   double longest_valid_segment_fraction;
00059   node_handle_.param(group_name_+"/longest_valid_segment_fraction",longest_valid_segment_fraction,0.005);
00060   state_space_->setLongestValidSegmentFraction(longest_valid_segment_fraction);
00061 
00062   //Setup the projection evaluator for this group
00063   if(!initializeProjectionEvaluator())
00064   {
00065     ROS_ERROR("Could not setup the projection evaluator");
00066     return false;
00067   }
00068 
00069   planner_.reset(new ompl::geometric::SimpleSetup(state_space_));
00070   
00071   if(!initializePlanner())
00072     return false;
00073 
00074   if(!initializeStateValidityChecker(state_validity_checker_))
00075     return false;
00076 
00077   planner_->setStateValidityChecker(static_cast<ompl::base::StateValidityCheckerPtr> (state_validity_checker_));
00078   planner_->setPlanner(ompl_planner_);
00079 
00080   return true;
00081 };
00082 
00083 bool OmplRosPlanningGroup::initializePhysicalGroup()
00084 {
00085   std::string physical_group_name;
00086   if(!collision_models_interface_->getKinematicModel()->hasModelGroup(group_name_))
00087   {
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_ = collision_models_interface_->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   try
00116   {
00117     ompl_projection_evaluator.reset(new ompl_ros_interface::OmplRosProjectionEvaluator(state_space_.get(),
00118                                                                                        projection_evaluator));
00119   }
00120   catch(...)
00121   {
00122     return false;
00123   }
00124   state_space_->registerDefaultProjection(ompl_projection_evaluator);
00125   return true;
00126 }
00127 
00128 bool OmplRosPlanningGroup::initializePlanner()
00129 {
00130   planner_config_.reset(new ompl_ros_interface::PlannerConfig(node_handle_.getNamespace(),planner_config_name_));
00131   std::string planner_type = planner_config_->getParamString("type");
00132   if(planner_type == "kinematic::RRT")
00133     return initializeRRTPlanner();
00134   else if(planner_type == "kinematic::RRTConnect")
00135     return initializeRRTConnectPlanner();
00136   else if(planner_type == "kinematic::pRRT")
00137     return initializepRRTPlanner();
00138   else if(planner_type == "kinematic::LazyRRT")
00139     return initializeLazyRRTPlanner();
00140   else if(planner_type == "kinematic::EST")
00141     return initializeESTPlanner();
00142   else if(planner_type == "kinematic::SBL")
00143     return initializeSBLPlanner();
00144   else if(planner_type == "kinematic::pSBL")
00145     return initializepSBLPlanner();
00146   else if(planner_type == "kinematic::KPIECE")
00147     return initializeKPIECEPlanner();
00148   else if(planner_type == "kinematic::LBKPIECE")
00149     return initializeLBKPIECEPlanner();
00150   else if(planner_type == "kinematic::RRTStar")
00151     return initializeRRTStarPlanner();
00152   else if(planner_type == "kinematic::BKPIECE")
00153     return initializeBKPIECEPlanner();
00154         else
00155   {
00156     ROS_WARN("Unknown planner type: %s", planner_type.c_str());
00157     return false;
00158   }
00159 }
00160 
00161 bool OmplRosPlanningGroup::initializeRRTPlanner()
00162 {
00163   ompl_planner_.reset(new ompl::geometric::RRT(planner_->getSpaceInformation()));
00164   ompl::geometric::RRT* new_planner = dynamic_cast<ompl::geometric::RRT*>(ompl_planner_.get());
00165   if (planner_config_->hasParam("goal_bias"))
00166   {
00167     new_planner->setGoalBias(planner_config_->getParamDouble("goal_bias",new_planner->getGoalBias()));
00168     ROS_DEBUG("RRTPlanner::Goal bias is set to %g", new_planner->getGoalBias());
00169   }  
00170   if (planner_config_->hasParam("range"))
00171   {
00172     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00173     ROS_DEBUG("RRTPlanner::Range is set to %g", new_planner->getRange());
00174   }  
00175   return true;
00176 }
00177 
00178 bool OmplRosPlanningGroup::initializeRRTStarPlanner()
00179 {
00180   ompl_planner_.reset(new ompl::geometric::RRTstar(planner_->getSpaceInformation()));
00181   ompl::geometric::RRTstar* new_planner = dynamic_cast<ompl::geometric::RRTstar*>(ompl_planner_.get());
00182   if (planner_config_->hasParam("goal_bias"))
00183   {
00184     new_planner->setGoalBias(planner_config_->getParamDouble("goal_bias",new_planner->getGoalBias()));
00185     ROS_DEBUG("RRTStarPlanner::Goal bias is set to %g", new_planner->getGoalBias());
00186   }  
00187   if (planner_config_->hasParam("range"))
00188   {
00189     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00190     ROS_DEBUG("RRTStarPlanner::Range is set to %g", new_planner->getRange());
00191   }  
00192   if (planner_config_->hasParam("ball_radius_constant"))
00193   {
00194     new_planner->setBallRadiusConstant(planner_config_->getParamDouble("ball_radius_constant",new_planner->getBallRadiusConstant()));
00195     ROS_DEBUG("RRTStarPlanner::Ball radius constant is set to %g", new_planner->getBallRadiusConstant());
00196   }  
00197   if (planner_config_->hasParam("max_ball_radius"))
00198   {
00199     new_planner->setMaxBallRadius(planner_config_->getParamDouble("max_ball_radius",new_planner->getMaxBallRadius()));
00200     ROS_DEBUG("RRTStarPlanner::Ball radius constant is set to %g", new_planner->getMaxBallRadius());
00201   }  
00202   return true;
00203 }
00204 
00205 bool OmplRosPlanningGroup::initializeRRTConnectPlanner()
00206 {
00207   ompl_planner_.reset(new ompl::geometric::RRTConnect(planner_->getSpaceInformation()));
00208   ompl::geometric::RRTConnect* new_planner = dynamic_cast<ompl::geometric::RRTConnect*>(ompl_planner_.get());
00209   if (planner_config_->hasParam("range"))
00210   {
00211     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00212     ROS_DEBUG("RRTConnectPlanner::Range is set to %g", new_planner->getRange());
00213   }  
00214   return true;
00215 }
00216 
00217 bool OmplRosPlanningGroup::initializepRRTPlanner()
00218 {
00219   ompl_planner_.reset(new ompl::geometric::pRRT(planner_->getSpaceInformation()));
00220   ompl::geometric::pRRT* new_planner = dynamic_cast<ompl::geometric::pRRT*>(ompl_planner_.get());
00221   if (planner_config_->hasParam("range"))
00222   {
00223     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00224     ROS_DEBUG("pRRTPlanner::Range is set to %g", new_planner->getRange());
00225   }  
00226   if (planner_config_->hasParam("goal_bias"))
00227   {
00228     new_planner->setGoalBias(planner_config_->getParamDouble("goal_bias",new_planner->getGoalBias()));
00229     ROS_DEBUG("pRRTPlanner::Goal bias is set to %g", new_planner->getGoalBias());
00230   }  
00231   if (planner_config_->hasParam("thread_count"))
00232   {
00233     new_planner->setThreadCount(planner_config_->getParamDouble("thread_count",new_planner->getThreadCount()));
00234     ROS_DEBUG("pRRTPlanner::Thread count is set to %d", (int) new_planner->getThreadCount());
00235   }  
00236   return true;
00237 }
00238 
00239 bool OmplRosPlanningGroup::initializeLazyRRTPlanner()
00240 {
00241   ompl_planner_.reset(new ompl::geometric::LazyRRT(planner_->getSpaceInformation()));
00242   ompl::geometric::LazyRRT* new_planner = dynamic_cast<ompl::geometric::LazyRRT*>(ompl_planner_.get());
00243   if (planner_config_->hasParam("range"))
00244   {
00245     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00246     ROS_DEBUG("LazyRRTPlanner::Range is set to %g", new_planner->getRange());
00247   }  
00248   if (planner_config_->hasParam("goal_bias"))
00249   {
00250     new_planner->setGoalBias(planner_config_->getParamDouble("goal_bias",new_planner->getGoalBias()));
00251     ROS_DEBUG("LazyRRTPlanner::Goal bias is set to %g", new_planner->getGoalBias());
00252   }  
00253   return true;
00254 }
00255 
00256 bool OmplRosPlanningGroup::initializeESTPlanner()
00257 {
00258   ompl_planner_.reset(new ompl::geometric::EST(planner_->getSpaceInformation()));
00259   ompl::geometric::EST* new_planner = dynamic_cast<ompl::geometric::EST*>(ompl_planner_.get());
00260   if (planner_config_->hasParam("range"))
00261   {
00262     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00263     ROS_DEBUG("ESTPlanner::Range is set to %g", new_planner->getRange());
00264   }  
00265   if (planner_config_->hasParam("goal_bias"))
00266   {
00267     new_planner->setGoalBias(planner_config_->getParamDouble("goal_bias",new_planner->getGoalBias()));
00268     ROS_DEBUG("ESTPlanner::Goal bias is set to %g", new_planner->getGoalBias());
00269   }  
00270   return true;
00271 }
00272 
00273 bool OmplRosPlanningGroup::initializeSBLPlanner()
00274 {
00275   ompl_planner_.reset(new ompl::geometric::SBL(planner_->getSpaceInformation()));
00276   ompl::geometric::SBL* new_planner = dynamic_cast<ompl::geometric::SBL*>(ompl_planner_.get());
00277   if (planner_config_->hasParam("range"))
00278   {
00279     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00280     ROS_DEBUG("SBLPlanner::Range is set to %g", new_planner->getRange());
00281   }  
00282   return true;
00283 }
00284 
00285 bool OmplRosPlanningGroup::initializepSBLPlanner()
00286 {
00287   ompl_planner_.reset(new ompl::geometric::pSBL(planner_->getSpaceInformation()));
00288   ompl::geometric::pSBL* new_planner = dynamic_cast<ompl::geometric::pSBL*>(ompl_planner_.get());
00289   if (planner_config_->hasParam("range"))
00290   {
00291     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00292     ROS_DEBUG("pSBLPlanner::Range is set to %g", new_planner->getRange());
00293   }  
00294   if (planner_config_->hasParam("thread_count"))
00295   {
00296     new_planner->setThreadCount(planner_config_->getParamDouble("thread_count",new_planner->getThreadCount()));
00297     ROS_DEBUG("pSBLPlanner::Thread count is set to %d", (int) new_planner->getThreadCount());
00298   }  
00299   return true;
00300 }
00301 
00302 bool OmplRosPlanningGroup::initializeKPIECEPlanner()
00303 {
00304   ompl_planner_.reset(new ompl::geometric::KPIECE1(planner_->getSpaceInformation()));
00305   ompl::geometric::KPIECE1* new_planner = dynamic_cast<ompl::geometric::KPIECE1*>(ompl_planner_.get());
00306   if (planner_config_->hasParam("range"))
00307   {
00308     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00309     ROS_DEBUG("KPIECEPlanner::Range is set to %g", new_planner->getRange());
00310   }  
00311   if (planner_config_->hasParam("goal_bias"))
00312   {
00313     new_planner->setGoalBias(planner_config_->getParamDouble("goal_bias",new_planner->getGoalBias()));
00314     ROS_DEBUG("KPIECEPlanner::Goal bias is set to %g", new_planner->getGoalBias());
00315   }  
00316   if (planner_config_->hasParam("min_valid_path_fraction"))
00317   {
00318     new_planner->setMinValidPathFraction(planner_config_->getParamDouble("min_valid_path_fraction",new_planner->getMinValidPathFraction()));
00319     ROS_DEBUG("KPIECEPlanner::Min valid path fraction is set to %g", new_planner->getMinValidPathFraction());
00320   }  
00321   if (planner_config_->hasParam("good_cell_score_factor") && planner_config_->hasParam("bad_cell_score_factor"))
00322   {
00323     new_planner->setCellScoreFactor(planner_config_->getParamDouble("cell_score_factor",new_planner->getBorderFraction()),
00324                                     planner_config_->getParamDouble("good_cell_score_factor",new_planner->getBorderFraction()));
00325     ROS_DEBUG("KPIECEPlanner::Border score factor is set to (good,bad):(%g,%g)", new_planner->getGoodCellScoreFactor(),new_planner->getBadCellScoreFactor());
00326   }  
00327   return true;
00328 }
00329 
00330 bool OmplRosPlanningGroup::initializeBKPIECEPlanner()
00331 {
00332   ompl_planner_.reset(new ompl::geometric::BKPIECE1(planner_->getSpaceInformation()));
00333   ompl::geometric::BKPIECE1* new_planner = dynamic_cast<ompl::geometric::BKPIECE1*>(ompl_planner_.get());
00334   if (planner_config_->hasParam("range"))
00335   {
00336     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00337     ROS_DEBUG("BKPIECEPlanner::Range is set to %g", new_planner->getRange());
00338   }  
00339   if (planner_config_->hasParam("border_fraction"))
00340   {
00341     new_planner->setBorderFraction(planner_config_->getParamDouble("border_fraction",new_planner->getBorderFraction()));
00342     ROS_DEBUG("BKPIECEPlanner::Range is set to %g", new_planner->getBorderFraction());
00343   }  
00344   if (planner_config_->hasParam("good_cell_score_factor") && planner_config_->hasParam("bad_cell_score_factor"))
00345   {
00346     new_planner->setCellScoreFactor(planner_config_->getParamDouble("cell_score_factor",new_planner->getBorderFraction()),
00347                                     planner_config_->getParamDouble("good_cell_score_factor",new_planner->getBorderFraction()));
00348     ROS_DEBUG("BKPIECEPlanner::Border score factor is set to (good,bad):(%g,%g)", new_planner->getGoodCellScoreFactor(),new_planner->getBadCellScoreFactor());
00349   }  
00350   if (planner_config_->hasParam("min_valid_path_fraction"))
00351   {
00352     new_planner->setMinValidPathFraction(planner_config_->getParamDouble("min_valid_path_fraction",new_planner->getMinValidPathFraction()));
00353     ROS_DEBUG("BKPIECEPlanner::Min valid path fraction is set to %g", new_planner->getMinValidPathFraction());
00354   }  
00355   return true;
00356 }
00357 
00358 bool OmplRosPlanningGroup::initializeLBKPIECEPlanner()
00359 {
00360   ompl_planner_.reset(new ompl::geometric::LBKPIECE1(planner_->getSpaceInformation()));
00361   ompl::geometric::LBKPIECE1* new_planner = dynamic_cast<ompl::geometric::LBKPIECE1*>(ompl_planner_.get());
00362   if (planner_config_->hasParam("range"))
00363   {
00364     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00365     ROS_DEBUG("LBKPIECEPlanner::Range is set to %g", new_planner->getRange());
00366   }  
00367   if (planner_config_->hasParam("border_fraction"))
00368   {
00369     new_planner->setBorderFraction(planner_config_->getParamDouble("border_fraction",new_planner->getBorderFraction()));
00370     ROS_DEBUG("LBKPIECEPlanner::Border fraction is set to %g", new_planner->getBorderFraction());
00371   }  
00372   if (planner_config_->hasParam("min_valid_path_fraction"))
00373   {
00374     new_planner->setMinValidPathFraction(planner_config_->getParamDouble("min_valid_path_fraction",new_planner->getMinValidPathFraction()));
00375     ROS_DEBUG("BKPIECEPlanner::Min valid path fraction is set to %g", new_planner->getMinValidPathFraction());
00376   }  
00377   return true;
00378 }
00379 
00380 bool OmplRosPlanningGroup::transformConstraints(arm_navigation_msgs::GetMotionPlan::Request &request, 
00381                                                 arm_navigation_msgs::GetMotionPlan::Response &response)
00382 {
00383   if(!collision_models_interface_->convertConstraintsGivenNewWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00384                                                                             request.motion_plan_request.goal_constraints))
00385   {
00386     response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00387     return false;
00388   }
00389 
00390   if(!collision_models_interface_->convertConstraintsGivenNewWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00391                                                                             request.motion_plan_request.path_constraints))
00392   {
00393     response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00394     return false;
00395   }
00396   return true;
00397 }
00398 
00399 bool OmplRosPlanningGroup::omplPathGeometricToRobotTrajectory(const ompl::geometric::PathGeometric &path, 
00400                                                               arm_navigation_msgs::RobotTrajectory &robot_trajectory)
00401 {
00402   if(!ompl_ros_interface::jointStateGroupToRobotTrajectory(physical_joint_state_group_,robot_trajectory))
00403     return false;
00404   if(!ompl_ros_interface::omplPathGeometricToRobotTrajectory(path,state_space_,robot_trajectory))
00405     return false;
00406   return true;
00407 }
00408 
00409 bool OmplRosPlanningGroup::computePlan(arm_navigation_msgs::GetMotionPlan::Request &request, 
00410                                        arm_navigation_msgs::GetMotionPlan::Response &response)
00411 {
00412   planner_->clear();
00413   planning_models::KinematicState* kinematic_state = collision_models_interface_->getPlanningSceneState();
00414   if(kinematic_state == NULL) {
00415     ROS_ERROR_STREAM("Planning scene hasn't been set");
00416     return finish(false);
00417   }
00418 
00419   //updating for new start state
00420   planning_environment::setRobotStateAndComputeTransforms(request.motion_plan_request.start_state,
00421                                                           *kinematic_state);
00422 
00423   physical_joint_state_group_ = kinematic_state->getJointStateGroup(physical_joint_group_->getName());
00424   if(!physical_joint_state_group_)
00425   {
00426     ROS_ERROR("Could not find physical joint state group");
00427     response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::PLANNING_FAILED;
00428     return finish(false);
00429   }
00430 
00431   //disabling collisions that don't affect this group
00432   collision_models_interface_->disableCollisionsForNonUpdatedLinks(physical_joint_group_->getName());
00433 
00434   if (!isRequestValid(request,response))
00435     return finish(false);
00436 
00437   if(!configureStateValidityChecker(request,response,kinematic_state))
00438     return finish(false);
00439   
00440   if(!transformConstraints(request,response))
00441     return finish(false);
00442   
00443   if(!setStartAndGoalStates(request,response))
00444     return finish(false);
00445   
00446   bool solved = planner_->solve(request.motion_plan_request.allowed_planning_time.toSec());
00447   
00448   if(solved)
00449   {
00450     ROS_DEBUG("Found solution for request in %f seconds",planner_->getLastPlanComputationTime());
00451     response.planning_time = ros::Duration(planner_->getLastPlanComputationTime());
00452     planner_->getPathSimplifier()->reduceVertices(planner_->getSolutionPath());
00453     planner_->getPathSimplifier()->collapseCloseVertices(planner_->getSolutionPath());
00454     
00455     try
00456     {
00457       response.trajectory = getSolutionPath();
00458       response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS;
00459       return finish(true);
00460     }
00461     catch(...)
00462     {
00463       ROS_ERROR("Could not find solution");
00464       response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::PLANNING_FAILED;
00465       return finish(false);
00466     }
00467   }
00468   else
00469   {
00470     ROS_ERROR("Could not find solution for request");
00471     response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::PLANNING_FAILED;
00472     return finish(false);
00473   }  
00474 }
00475 
00476 
00477 bool OmplRosPlanningGroup::finish(const bool &result)
00478 {
00479   if(collision_models_interface_->getPlanningSceneState() != NULL) {
00480     collision_models_interface_->resetToStartState(*collision_models_interface_->getPlanningSceneState());
00481   }
00482   return result;
00483 }
00484 
00485 bool OmplRosPlanningGroup::configureStateValidityChecker(arm_navigation_msgs::GetMotionPlan::Request &request,
00486                                                          arm_navigation_msgs::GetMotionPlan::Response &response,
00487                                                          planning_models::KinematicState *kinematic_state)
00488 {
00489   /* set the pose of the whole robot */
00490   /* set the kinematic state for the state validator */
00491   ompl_ros_interface::OmplRosStateValidityChecker *my_checker = dynamic_cast<ompl_ros_interface::OmplRosStateValidityChecker*>(state_validity_checker_.get());
00492   my_checker->configureOnRequest(kinematic_state,
00493                                  physical_joint_state_group_,
00494                                  request);
00495   return true;
00496 }
00497 
00498 bool OmplRosPlanningGroup::setStartAndGoalStates(arm_navigation_msgs::GetMotionPlan::Request &request,
00499                                                  arm_navigation_msgs::GetMotionPlan::Response &response)
00500 {
00501   if(!setStart(request,response))
00502     return false;
00503   if(!setGoal(request,response))
00504     return false;
00505   return true;
00506 }
00507 
00508 }
ompl_ros_planning_group.cpp
