ompl_ros_task_space_planner.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/planners/ompl_ros_task_space_planner.h>
00038 #include <planning_environment/models/model_utils.h>
00039 
00040 namespace ompl_ros_interface
00041 {
00042 bool OmplRosTaskSpacePlanner::initializePlanningStateSpace(ompl::base::StateSpacePtr &state_space)
00043 {
00044   if(!node_handle_.hasParam(group_name_))
00045   {
00046     ROS_ERROR("Could not find description of planning state space %s",group_name_.c_str());
00047     return false;
00048   }
00049   if(!node_handle_.getParam(group_name_+"/parent_frame", planning_frame_id_))
00050   {
00051     ROS_ERROR("Could not find parent frame for group %ss",group_name_.c_str());
00052     return false;
00053   }
00054 
00055   XmlRpc::XmlRpcValue space_list;
00056   std::vector<std::string> space_names;
00057   if(!node_handle_.getParam(group_name_+"/state_spaces", space_list))
00058   {
00059     ROS_ERROR("Could not find parameter %s on param server",(group_name_+"/state_spaces").c_str());
00060     return false;
00061   }
00062   if(space_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
00063   {
00064     ROS_ERROR("State space list should be of XmlRpc Array type");
00065     return false;
00066   }
00067 
00068   int real_vector_index = -1;
00069   state_space.reset(new ompl::base::CompoundStateSpace());
00070   state_space->setName(group_name_);
00071   for (unsigned int i = 0; i < (unsigned int) space_list.size(); ++i) 
00072   {
00073     if(space_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
00074     {
00075       ROS_ERROR("State space names should be strings");
00076       return false;
00077     }
00078     space_names.push_back(static_cast<std::string>(space_list[i]));
00079     ROS_INFO("Adding state space: %s",space_names.back().c_str());
00080 
00081     if(collision_models_interface_->getKinematicModel()->hasJointModel(space_names.back()))
00082     {
00083       addToOmplStateSpace(collision_models_interface_->getKinematicModel(),
00084                           space_names.back(),
00085                           state_space);
00086       continue;
00087     }
00088 
00089     if(!getSpaceFromParamServer(ros::NodeHandle(node_handle_,group_name_+"/"+space_names.back()),
00090                                    space_names.back(),
00091                                    state_space,
00092                                    real_vector_index))
00093       return false;
00094   }
00095   if(!state_space)
00096   {
00097     ROS_ERROR("Could not set up the ompl state space from group %s",group_name_.c_str());
00098     return false;
00099   }
00100   return true;
00101 }
00102 
00103 bool OmplRosTaskSpacePlanner::getSpaceFromParamServer(const ros::NodeHandle &node_handle,
00104                                                          const std::string &space_name,
00105                                                          ompl::base::StateSpacePtr& state_space,
00106                                                          int& real_vector_index)
00107 {
00108   std::string type;
00109   if(!node_handle.hasParam("type"))
00110   {
00111     ROS_ERROR("Could not find type for state space %s",space_name.c_str());
00112     return false;
00113   }
00114   node_handle.getParam("type",type);
00115   if(type == "Revolute" || type == "Linear")
00116   {
00117     if(real_vector_index < 0)
00118     {
00119       real_vector_index = state_space->as<ompl::base::CompoundStateSpace>()->getSubspaceCount();
00120       ompl::base::RealVectorStateSpace *subspace = new ompl::base::RealVectorStateSpace(0);
00121       subspace->setName("real_vector");
00122       state_space->as<ompl::base::CompoundStateSpace>()->addSubspace(ompl::base::StateSpacePtr(subspace),1.0);
00123     }
00124     ompl::base::StateSpacePtr real_vector_state_space = state_space->as<ompl::base::CompoundStateSpace>()->getSubspace("real_vector");
00125     double min_value, max_value;
00126     node_handle.param("min",min_value,-M_PI);
00127     node_handle.param("max",max_value,M_PI);
00128     real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->addDimension(space_name,min_value,max_value);    
00129   }
00130   else if(type == "Planar")
00131   {
00132     ompl::base::SE2StateSpace *subspace = new ompl::base::SE2StateSpace();
00133     subspace->setName(space_name);
00134     if(!node_handle.hasParam("x/min") || !node_handle.hasParam("x/max") || 
00135        !node_handle.hasParam("y/min") || !node_handle.hasParam("y/max"))
00136     {
00137       ROS_ERROR("Could not find bounds for planar state space %s",space_name.c_str());
00138     }
00139     ompl::base::RealVectorBounds real_vector_bounds(2);
00140     node_handle.getParam("x/min",real_vector_bounds.low[0]);
00141     node_handle.getParam("x/max",real_vector_bounds.high[0]);
00142     node_handle.getParam("y/min",real_vector_bounds.low[1]);
00143     node_handle.getParam("y/max",real_vector_bounds.high[1]);
00144     subspace->setBounds(real_vector_bounds);
00145 
00146     state_space->as<ompl::base::CompoundStateSpace>()->addSubspace(ompl::base::StateSpacePtr(subspace), 1.0);
00147   } 
00148   else if(type == "Floating")
00149   {
00150     ompl::base::SE3StateSpace *subspace = new ompl::base::SE3StateSpace();
00151     subspace->setName(space_name);
00152     if(!node_handle.hasParam("x/min") || !node_handle.hasParam("x/max") || 
00153        !node_handle.hasParam("y/min") || !node_handle.hasParam("y/max") ||
00154        !node_handle.hasParam("z/min") || !node_handle.hasParam("z/max"))
00155     {
00156       ROS_ERROR("Could not find bounds for floating state space %s",space_name.c_str());
00157     }
00158     ompl::base::RealVectorBounds real_vector_bounds(3);
00159     node_handle.getParam("x/min",real_vector_bounds.low[0]);
00160     node_handle.getParam("x/max",real_vector_bounds.high[0]);
00161 
00162     node_handle.getParam("y/min",real_vector_bounds.low[1]);
00163     node_handle.getParam("y/max",real_vector_bounds.high[1]);
00164 
00165     node_handle.getParam("z/min",real_vector_bounds.low[2]);
00166     node_handle.getParam("z/max",real_vector_bounds.high[2]);
00167 
00168     subspace->setBounds(real_vector_bounds);
00169     state_space->as<ompl::base::CompoundStateSpace>()->addSubspace(ompl::base::StateSpacePtr(subspace), 1.0);
00170   } 
00171   else if(type == "Continuous")
00172   {
00173     ompl::base::SO2StateSpace *subspace = new ompl::base::SO2StateSpace();
00174     subspace->setName(space_name);
00175     state_space->as<ompl::base::CompoundStateSpace>()->addSubspace(ompl::base::StateSpacePtr(subspace), 1.0);
00176   } 
00177   else
00178   {
00179     ROS_ERROR("Unknown joint type: %s",type.c_str());
00180     return false;
00181   }
00182   return true;
00183 }
00184 
00185 bool OmplRosTaskSpacePlanner::isRequestValid(arm_navigation_msgs::GetMotionPlan::Request &request,
00186                                              arm_navigation_msgs::GetMotionPlan::Response &response)
00187 {
00188   if(request.motion_plan_request.group_name != group_name_)
00189   {
00190     ROS_ERROR("Invalid group name: %s",request.motion_plan_request.group_name.c_str());
00191     response.error_code.val = response.error_code.INVALID_GROUP_NAME;
00192     return false;
00193   }
00194 
00195   if(!collision_models_interface_->convertConstraintsGivenNewWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00196                                                                             request.motion_plan_request.goal_constraints))
00197   {
00198     response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00199     return false;
00200   }
00201   if(!collision_models_interface_->convertConstraintsGivenNewWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00202                                                                             request.motion_plan_request.path_constraints))
00203   {
00204     response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00205     return false;
00206   }
00207   if(request.motion_plan_request.allowed_planning_time.toSec() <= 0.0)
00208   {
00209     response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::INVALID_TIMEOUT;
00210     ROS_ERROR("Request does not specify correct allowed planning time %f",request.motion_plan_request.allowed_planning_time.toSec());
00211     return false;
00212   }
00213   return true;
00214 }
00215 
00216 bool OmplRosTaskSpacePlanner::setGoal(arm_navigation_msgs::GetMotionPlan::Request &request,
00217                                       arm_navigation_msgs::GetMotionPlan::Response &response)
00218 {
00219   ompl::base::ScopedState<ompl::base::CompoundStateSpace> goal(state_space_);
00220   ompl::base::GoalPtr goal_states(new ompl::base::GoalStates(planner_->getSpaceInformation()));
00221   ROS_DEBUG("Setting my goal");
00222   if(!constraintsToOmplState(request.motion_plan_request.goal_constraints,goal))
00223   {
00224     response.error_code.val = response.error_code.PLANNING_FAILED;
00225     ROS_WARN("Problem converting constraints to ompl state");
00226     return false;
00227   }
00228   goal_states->as<ompl::base::GoalStates>()->addState(goal.get());
00229   ompl_ros_interface::OmplRosTaskSpaceValidityChecker *my_checker = dynamic_cast<ompl_ros_interface::OmplRosTaskSpaceValidityChecker*>(state_validity_checker_.get());  
00230   if(!my_checker->isStateValid(goal.get()))
00231   {
00232     response.error_code = my_checker->getLastErrorCode();
00233     if(response.error_code.val == response.error_code.PATH_CONSTRAINTS_VIOLATED)
00234       response.error_code.val = response.error_code.GOAL_VIOLATES_PATH_CONSTRAINTS;
00235     else if(response.error_code.val == response.error_code.COLLISION_CONSTRAINTS_VIOLATED)
00236       response.error_code.val = response.error_code.GOAL_IN_COLLISION;
00237     ROS_ERROR("Goal state is invalid. Reason: %s",arm_navigation_msgs::armNavigationErrorCodeToString(response.error_code).c_str());
00238     return false;
00239   }  
00240   planner_->setGoal(goal_states);    
00241   ROS_DEBUG("Setting goal state successful");
00242   return true;
00243 }
00244 
00245 bool OmplRosTaskSpacePlanner::constraintsToOmplState(const arm_navigation_msgs::Constraints &constraints, 
00246                                                      ompl::base::ScopedState<ompl::base::CompoundStateSpace> &goal)
00247 {
00248   return ompl_ros_interface::constraintsToOmplState(constraints,goal);
00249 }
00250 
00251 bool OmplRosTaskSpacePlanner::setStart(arm_navigation_msgs::GetMotionPlan::Request &request,
00252                                        arm_navigation_msgs::GetMotionPlan::Response &response)
00253 {
00254   ompl::base::ScopedState<ompl::base::CompoundStateSpace> start(state_space_);
00255   arm_navigation_msgs::RobotState cur_state;
00256   planning_environment::convertKinematicStateToRobotState(*collision_models_interface_->getPlanningSceneState(),
00257                                                           ros::Time::now(),
00258                                                           collision_models_interface_->getWorldFrameId(),
00259                                                           cur_state);
00260   
00261   ompl_ros_interface::robotStateToOmplState(cur_state,start,false);
00262 
00263   ompl_ros_interface::OmplRosTaskSpaceValidityChecker *my_checker = dynamic_cast<ompl_ros_interface::OmplRosTaskSpaceValidityChecker*>(state_validity_checker_.get());  
00264   if(!my_checker->isStateValid(start.get()))
00265   {
00266     response.error_code = my_checker->getLastErrorCode();
00267     if(response.error_code.val == response.error_code.PATH_CONSTRAINTS_VIOLATED)
00268       response.error_code.val = response.error_code.START_STATE_VIOLATES_PATH_CONSTRAINTS;
00269     else if(response.error_code.val == response.error_code.COLLISION_CONSTRAINTS_VIOLATED)
00270       response.error_code.val = response.error_code.START_STATE_IN_COLLISION;
00271     return false;
00272   }
00273   planner_->getProblemDefinition()->clearStartStates(); 
00274   planner_->addStartState(start);
00275   return true;
00276 }
00277 
00278 bool OmplRosTaskSpacePlanner::initializeStateValidityChecker(ompl_ros_interface::OmplRosStateValidityCheckerPtr &state_validity_checker)
00279 {
00280   state_validity_checker.reset(new ompl_ros_interface::OmplRosTaskSpaceValidityChecker(planner_->getSpaceInformation().get(),
00281                                                                                        collision_models_interface_,
00282                                                                                        planning_frame_id_));
00283   return true;
00284 }
00285 
00286 
00287 }


ompl_ros_interface
Author(s): Sachin Chitta
autogenerated on Mon Dec 2 2013 12:38:54