ompl_ros_planning_group.cpp
Go to the documentation of this file.
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   return true;
00193 }
00194 
00195 bool OmplRosPlanningGroup::initializeRRTConnectPlanner()
00196 {
00197   ompl_planner_.reset(new ompl::geometric::RRTConnect(planner_->getSpaceInformation()));
00198   ompl::geometric::RRTConnect* new_planner = dynamic_cast<ompl::geometric::RRTConnect*>(ompl_planner_.get());
00199   if (planner_config_->hasParam("range"))
00200   {
00201     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00202     ROS_DEBUG("RRTConnectPlanner::Range is set to %g", new_planner->getRange());
00203   }  
00204   return true;
00205 }
00206 
00207 bool OmplRosPlanningGroup::initializepRRTPlanner()
00208 {
00209   ompl_planner_.reset(new ompl::geometric::pRRT(planner_->getSpaceInformation()));
00210   ompl::geometric::pRRT* new_planner = dynamic_cast<ompl::geometric::pRRT*>(ompl_planner_.get());
00211   if (planner_config_->hasParam("range"))
00212   {
00213     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00214     ROS_DEBUG("pRRTPlanner::Range is set to %g", new_planner->getRange());
00215   }  
00216   if (planner_config_->hasParam("goal_bias"))
00217   {
00218     new_planner->setGoalBias(planner_config_->getParamDouble("goal_bias",new_planner->getGoalBias()));
00219     ROS_DEBUG("pRRTPlanner::Goal bias is set to %g", new_planner->getGoalBias());
00220   }  
00221   if (planner_config_->hasParam("thread_count"))
00222   {
00223     new_planner->setThreadCount(planner_config_->getParamDouble("thread_count",new_planner->getThreadCount()));
00224     ROS_DEBUG("pRRTPlanner::Thread count is set to %d", (int) new_planner->getThreadCount());
00225   }  
00226   return true;
00227 }
00228 
00229 bool OmplRosPlanningGroup::initializeLazyRRTPlanner()
00230 {
00231   ompl_planner_.reset(new ompl::geometric::LazyRRT(planner_->getSpaceInformation()));
00232   ompl::geometric::LazyRRT* new_planner = dynamic_cast<ompl::geometric::LazyRRT*>(ompl_planner_.get());
00233   if (planner_config_->hasParam("range"))
00234   {
00235     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00236     ROS_DEBUG("LazyRRTPlanner::Range is set to %g", new_planner->getRange());
00237   }  
00238   if (planner_config_->hasParam("goal_bias"))
00239   {
00240     new_planner->setGoalBias(planner_config_->getParamDouble("goal_bias",new_planner->getGoalBias()));
00241     ROS_DEBUG("LazyRRTPlanner::Goal bias is set to %g", new_planner->getGoalBias());
00242   }  
00243   return true;
00244 }
00245 
00246 bool OmplRosPlanningGroup::initializeESTPlanner()
00247 {
00248   ompl_planner_.reset(new ompl::geometric::EST(planner_->getSpaceInformation()));
00249   ompl::geometric::EST* new_planner = dynamic_cast<ompl::geometric::EST*>(ompl_planner_.get());
00250   if (planner_config_->hasParam("range"))
00251   {
00252     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00253     ROS_DEBUG("ESTPlanner::Range is set to %g", new_planner->getRange());
00254   }  
00255   if (planner_config_->hasParam("goal_bias"))
00256   {
00257     new_planner->setGoalBias(planner_config_->getParamDouble("goal_bias",new_planner->getGoalBias()));
00258     ROS_DEBUG("ESTPlanner::Goal bias is set to %g", new_planner->getGoalBias());
00259   }  
00260   return true;
00261 }
00262 
00263 bool OmplRosPlanningGroup::initializeSBLPlanner()
00264 {
00265   ompl_planner_.reset(new ompl::geometric::SBL(planner_->getSpaceInformation()));
00266   ompl::geometric::SBL* new_planner = dynamic_cast<ompl::geometric::SBL*>(ompl_planner_.get());
00267   if (planner_config_->hasParam("range"))
00268   {
00269     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00270     ROS_DEBUG("SBLPlanner::Range is set to %g", new_planner->getRange());
00271   }  
00272   return true;
00273 }
00274 
00275 bool OmplRosPlanningGroup::initializepSBLPlanner()
00276 {
00277   ompl_planner_.reset(new ompl::geometric::pSBL(planner_->getSpaceInformation()));
00278   ompl::geometric::pSBL* new_planner = dynamic_cast<ompl::geometric::pSBL*>(ompl_planner_.get());
00279   if (planner_config_->hasParam("range"))
00280   {
00281     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00282     ROS_DEBUG("pSBLPlanner::Range is set to %g", new_planner->getRange());
00283   }  
00284   if (planner_config_->hasParam("thread_count"))
00285   {
00286     new_planner->setThreadCount(planner_config_->getParamDouble("thread_count",new_planner->getThreadCount()));
00287     ROS_DEBUG("pSBLPlanner::Thread count is set to %d", (int) new_planner->getThreadCount());
00288   }  
00289   return true;
00290 }
00291 
00292 bool OmplRosPlanningGroup::initializeKPIECEPlanner()
00293 {
00294   ompl_planner_.reset(new ompl::geometric::KPIECE1(planner_->getSpaceInformation()));
00295   ompl::geometric::KPIECE1* new_planner = dynamic_cast<ompl::geometric::KPIECE1*>(ompl_planner_.get());
00296   if (planner_config_->hasParam("range"))
00297   {
00298     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00299     ROS_DEBUG("KPIECEPlanner::Range is set to %g", new_planner->getRange());
00300   }  
00301   if (planner_config_->hasParam("goal_bias"))
00302   {
00303     new_planner->setGoalBias(planner_config_->getParamDouble("goal_bias",new_planner->getGoalBias()));
00304     ROS_DEBUG("KPIECEPlanner::Goal bias is set to %g", new_planner->getGoalBias());
00305   }  
00306   if (planner_config_->hasParam("min_valid_path_fraction"))
00307   {
00308     new_planner->setMinValidPathFraction(planner_config_->getParamDouble("min_valid_path_fraction",new_planner->getMinValidPathFraction()));
00309     ROS_DEBUG("KPIECEPlanner::Min valid path fraction is set to %g", new_planner->getMinValidPathFraction());
00310   }  
00311   if (planner_config_->hasParam("failed_expansion_cell_score_factor"))
00312   {
00313     new_planner->setFailedExpansionCellScoreFactor(planner_config_->getParamDouble("failed_expansion_cell_score_factor",
00314                                                                                    new_planner->getFailedExpansionCellScoreFactor()));
00315     ROS_DEBUG("KPIECEPlanner:: Filed expansion cell score factor is %g", new_planner->getFailedExpansionCellScoreFactor());
00316   }  
00317   return true;
00318 }
00319 
00320 bool OmplRosPlanningGroup::initializeBKPIECEPlanner()
00321 {
00322   ompl_planner_.reset(new ompl::geometric::BKPIECE1(planner_->getSpaceInformation()));
00323   ompl::geometric::BKPIECE1* new_planner = dynamic_cast<ompl::geometric::BKPIECE1*>(ompl_planner_.get());
00324   if (planner_config_->hasParam("range"))
00325   {
00326     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00327     ROS_DEBUG("BKPIECEPlanner::Range is set to %g", new_planner->getRange());
00328   }  
00329   if (planner_config_->hasParam("border_fraction"))
00330   {
00331     new_planner->setBorderFraction(planner_config_->getParamDouble("border_fraction",new_planner->getBorderFraction()));
00332     ROS_DEBUG("BKPIECEPlanner::Range is set to %g", new_planner->getBorderFraction());
00333   }  
00334   if (planner_config_->hasParam("failed_expansion_cell_score_factor"))
00335   {
00336     new_planner->setFailedExpansionCellScoreFactor(planner_config_->getParamDouble("failed_expansion_cell_score_factor",
00337                                                                                    new_planner->getFailedExpansionCellScoreFactor()));
00338     ROS_DEBUG("KPIECEPlanner:: Filed expansion cell score factor is %g", new_planner->getFailedExpansionCellScoreFactor());
00339   }  
00340   if (planner_config_->hasParam("min_valid_path_fraction"))
00341   {
00342     new_planner->setMinValidPathFraction(planner_config_->getParamDouble("min_valid_path_fraction",new_planner->getMinValidPathFraction()));
00343     ROS_DEBUG("BKPIECEPlanner::Min valid path fraction is set to %g", new_planner->getMinValidPathFraction());
00344   }  
00345   return true;
00346 }
00347 
00348 bool OmplRosPlanningGroup::initializeLBKPIECEPlanner()
00349 {
00350   ompl_planner_.reset(new ompl::geometric::LBKPIECE1(planner_->getSpaceInformation()));
00351   ompl::geometric::LBKPIECE1* new_planner = dynamic_cast<ompl::geometric::LBKPIECE1*>(ompl_planner_.get());
00352   if (planner_config_->hasParam("range"))
00353   {
00354     new_planner->setRange(planner_config_->getParamDouble("range",new_planner->getRange()));
00355     ROS_DEBUG("LBKPIECEPlanner::Range is set to %g", new_planner->getRange());
00356   }  
00357   if (planner_config_->hasParam("border_fraction"))
00358   {
00359     new_planner->setBorderFraction(planner_config_->getParamDouble("border_fraction",new_planner->getBorderFraction()));
00360     ROS_DEBUG("LBKPIECEPlanner::Border fraction is set to %g", new_planner->getBorderFraction());
00361   }  
00362   if (planner_config_->hasParam("min_valid_path_fraction"))
00363   {
00364     new_planner->setMinValidPathFraction(planner_config_->getParamDouble("min_valid_path_fraction",new_planner->getMinValidPathFraction()));
00365     ROS_DEBUG("BKPIECEPlanner::Min valid path fraction is set to %g", new_planner->getMinValidPathFraction());
00366   }  
00367   return true;
00368 }
00369 
00370 bool OmplRosPlanningGroup::transformConstraints(arm_navigation_msgs::GetMotionPlan::Request &request, 
00371                                                 arm_navigation_msgs::GetMotionPlan::Response &response)
00372 {
00373   if(!collision_models_interface_->convertConstraintsGivenNewWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00374                                                                             request.motion_plan_request.goal_constraints))
00375   {
00376     response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00377     return false;
00378   }
00379 
00380   if(!collision_models_interface_->convertConstraintsGivenNewWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00381                                                                             request.motion_plan_request.path_constraints))
00382   {
00383     response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00384     return false;
00385   }
00386   return true;
00387 }
00388 
00389 bool OmplRosPlanningGroup::omplPathGeometricToRobotTrajectory(const ompl::geometric::PathGeometric &path, 
00390                                                               arm_navigation_msgs::RobotTrajectory &robot_trajectory)
00391 {
00392   if(!ompl_ros_interface::jointStateGroupToRobotTrajectory(physical_joint_state_group_,robot_trajectory))
00393     return false;
00394   if(!ompl_ros_interface::omplPathGeometricToRobotTrajectory(path,state_space_,robot_trajectory))
00395     return false;
00396   return true;
00397 }
00398 
00399 bool OmplRosPlanningGroup::computePlan(arm_navigation_msgs::GetMotionPlan::Request &request, 
00400                                        arm_navigation_msgs::GetMotionPlan::Response &response)
00401 {
00402   planner_->clear();
00403   planning_models::KinematicState* kinematic_state = collision_models_interface_->getPlanningSceneState();
00404   if(kinematic_state == NULL) {
00405     ROS_ERROR_STREAM("Planning scene hasn't been set");
00406     return finish(false);
00407   }
00408 
00409   //updating for new start state
00410   planning_environment::setRobotStateAndComputeTransforms(request.motion_plan_request.start_state,
00411                                                           *kinematic_state);
00412 
00413   physical_joint_state_group_ = kinematic_state->getJointStateGroup(physical_joint_group_->getName());
00414   if(!physical_joint_state_group_)
00415   {
00416     ROS_ERROR("Could not find physical joint state group");
00417     response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::PLANNING_FAILED;
00418     return finish(false);
00419   }
00420 
00421   //disabling collisions that don't affect this group
00422   collision_models_interface_->disableCollisionsForNonUpdatedLinks(physical_joint_group_->getName());
00423 
00424   if (!isRequestValid(request,response))
00425     return finish(false);
00426 
00427   if(!configureStateValidityChecker(request,response,kinematic_state))
00428     return finish(false);
00429   
00430   if(!transformConstraints(request,response))
00431     return finish(false);
00432   
00433   if(!setStartAndGoalStates(request,response))
00434     return finish(false);
00435   
00436   bool solved = planner_->solve(request.motion_plan_request.allowed_planning_time.toSec());
00437   
00438   if(solved)
00439   {
00440     ROS_DEBUG("Found solution for request in %f seconds",planner_->getLastPlanComputationTime());
00441     response.planning_time = ros::Duration(planner_->getLastPlanComputationTime());
00442     planner_->getPathSimplifier()->reduceVertices(planner_->getSolutionPath());
00443     planner_->getPathSimplifier()->collapseCloseVertices(planner_->getSolutionPath());
00444     
00445     try
00446     {
00447       response.trajectory = getSolutionPath();
00448       response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS;
00449       return finish(true);
00450     }
00451     catch(...)
00452     {
00453       ROS_ERROR("Could not find solution");
00454       response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::PLANNING_FAILED;
00455       return finish(false);
00456     }
00457   }
00458   else
00459   {
00460     ROS_ERROR("Could not find solution for request");
00461     response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::PLANNING_FAILED;
00462     return finish(false);
00463   }  
00464 }
00465 
00466 
00467 bool OmplRosPlanningGroup::finish(const bool &result)
00468 {
00469   if(collision_models_interface_->getPlanningSceneState() != NULL) {
00470     collision_models_interface_->resetToStartState(*collision_models_interface_->getPlanningSceneState());
00471   }
00472   return result;
00473 }
00474 
00475 bool OmplRosPlanningGroup::configureStateValidityChecker(arm_navigation_msgs::GetMotionPlan::Request &request,
00476                                                          arm_navigation_msgs::GetMotionPlan::Response &response,
00477                                                          planning_models::KinematicState *kinematic_state)
00478 {
00479   /* set the pose of the whole robot */
00480   /* set the kinematic state for the state validator */
00481   ompl_ros_interface::OmplRosStateValidityChecker *my_checker = dynamic_cast<ompl_ros_interface::OmplRosStateValidityChecker*>(state_validity_checker_.get());
00482   my_checker->configureOnRequest(kinematic_state,
00483                                  physical_joint_state_group_,
00484                                  request);
00485   return true;
00486 }
00487 
00488 bool OmplRosPlanningGroup::setStartAndGoalStates(arm_navigation_msgs::GetMotionPlan::Request &request,
00489                                                  arm_navigation_msgs::GetMotionPlan::Response &response)
00490 {
00491   if(!setStart(request,response))
00492     return false;
00493   if(!setGoal(request,response))
00494     return false;
00495   return true;
00496 }
00497 
00498 }


ompl_ros_interface
Author(s): Sachin Chitta
autogenerated on Thu Dec 12 2013 11:10:58