ompl_ros_interface: ompl_ros_task_space

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