ompl_ros_interface: ompl_ros_interface: ompl

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.h>
00038 
00039 namespace ompl_ros_interface
00040 {
00041 
00042   OmplRos::OmplRos(void): node_handle_("~")
00043 {
00044   collision_models_interface_ = new planning_environment::CollisionModelsInterface("robot_description");
00045 }
00046 
00048 OmplRos::~OmplRos(void)
00049 {
00050   delete collision_models_interface_;
00051 }
00052 
00053 void OmplRos::run(void)
00054 {
00055   if(!initialize(node_handle_.getNamespace()))
00056     return;
00057   if (collision_models_interface_->loadedModels())
00058   {
00059     plan_path_service_ = node_handle_.advertiseService("plan_kinematic_path", &OmplRos::computePlan, this);
00060     node_handle_.param<bool>("publish_diagnostics", publish_diagnostics_,false);
00061     if(publish_diagnostics_)
00062       diagnostic_publisher_ = node_handle_.advertise<ompl_ros_interface::OmplPlannerDiagnostics>("diagnostics", 1);
00063   }
00064   else
00065     ROS_ERROR("Collision models not loaded.");
00066 }
00067 
00068 bool OmplRos::initialize(const std::string &param_server_prefix)
00069 {
00070   std::vector<std::string> group_names;
00071   if(!getGroupNamesFromParamServer(param_server_prefix,group_names))
00072   {
00073     ROS_ERROR("Could not find groups for planning under %s",param_server_prefix.c_str());
00074     return false;
00075   }
00076   if(!initializePlanningMap(param_server_prefix,group_names))
00077   {
00078     ROS_ERROR("Could not initialize planning groups from the param server");
00079     return false;
00080   }
00081 
00082   if(!node_handle_.hasParam("default_planner_config"))
00083   {
00084     ROS_ERROR("No default planner configuration defined under 'default_planner_config'. A default planner must be defined from among the configured planners");
00085     return false;
00086   }
00087 
00088   node_handle_.param<std::string>("default_planner_config",default_planner_config_,"SBLkConfig1");
00089   for(unsigned int i=0; i < group_names.size(); i++)
00090   {
00091     std::string location = default_planner_config_ + "[" + group_names[i] + "]";
00092     if(planner_map_.find(location) == planner_map_.end())
00093     {
00094       ROS_ERROR("The default planner configuration %s has not been defined for group %s. The default planner must be configured for every group in your ompl_planning.yaml file", default_planner_config_.c_str(), group_names[i].c_str());
00095       return false;
00096     }
00097   }
00098 
00099   return true;
00100 };
00101 
00102 bool OmplRos::getGroupNamesFromParamServer(const std::string &param_server_prefix, 
00103                                            std::vector<std::string> &group_names)
00104 {
00105   XmlRpc::XmlRpcValue group_list;
00106   if(!node_handle_.getParam(param_server_prefix+"/groups", group_list))
00107   {
00108     ROS_ERROR("Could not find parameter %s on param server",(param_server_prefix+"/groups").c_str());
00109     return false;
00110   }
00111   if(group_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
00112   {
00113     ROS_ERROR("Group list should be of XmlRpc Array type");
00114     return false;
00115   } 
00116   for (int32_t i = 0; i < group_list.size(); ++i) 
00117   {
00118     if(group_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
00119     {
00120       ROS_ERROR("Group names should be strings");
00121       return false;
00122     }
00123     group_names.push_back(static_cast<std::string>(group_list[i]));
00124     ROS_DEBUG("Adding group: %s",group_names.back().c_str());
00125   }
00126   return true;
00127 };
00128 
00129 bool OmplRos::initializePlanningMap(const std::string &param_server_prefix,
00130                                     const std::vector<std::string> &group_names)
00131 {
00132   for(unsigned int i=0; i < group_names.size(); i++)
00133   {
00134     XmlRpc::XmlRpcValue planner_list;
00135     if(!node_handle_.getParam(param_server_prefix+"/"+group_names[i]+"/planner_configs", planner_list))
00136     {
00137       ROS_ERROR("Could not find parameter %s on param server",(param_server_prefix+"/"+group_names[i]+"/planner_configs").c_str());
00138       return false;
00139     }
00140     ROS_ASSERT(planner_list.getType() == XmlRpc::XmlRpcValue::TypeArray);    
00141     for (int32_t j = 0; j < planner_list.size(); ++j) 
00142     {
00143       if(planner_list[j].getType() != XmlRpc::XmlRpcValue::TypeString)
00144       {
00145         ROS_ERROR("Planner names must be of type string");
00146         return false;
00147       }
00148       std::string planner_config = static_cast<std::string>(planner_list[j]);
00149       if(!initializePlanningInstance(param_server_prefix,group_names[i],planner_config))
00150       {
00151         ROS_ERROR("Could not add planner for group %s and planner_config %s",group_names[i].c_str(),planner_config.c_str());
00152         return false;
00153       }
00154       else
00155       {
00156         ROS_DEBUG("Adding planning group config: %s",(planner_config+"["+group_names[i]+"]").c_str());
00157       }
00158     }    
00159   } 
00160   return true;
00161 };
00162 
00163 bool OmplRos::initializePlanningInstance(const std::string &param_server_prefix,
00164                                          const std::string &group_name,
00165                                          const std::string &planner_config_name)
00166 {
00167   std::string location = planner_config_name+"["+group_name+"]";
00168   if (planner_map_.find(location) != planner_map_.end())
00169   {
00170     ROS_WARN("Re-definition of '%s'", location.c_str());
00171     return true;
00172   }
00173 
00174   if(!node_handle_.hasParam(param_server_prefix+"/"+group_name+"/planner_type"))
00175   {
00176     ROS_ERROR_STREAM("Planner type not defined for group " << group_name << " param name " << param_server_prefix+"/"+group_name+"/planner_type");
00177     return false;
00178   }
00179 
00180   std::string planner_type;
00181   node_handle_.getParam(param_server_prefix+"/"+group_name+"/planner_type",planner_type);
00182   if(planner_type == "JointPlanner")
00183   {
00184     boost::shared_ptr<ompl_ros_interface::OmplRosJointPlanner> new_planner;
00185     new_planner.reset(new ompl_ros_interface::OmplRosJointPlanner());
00186     if(!new_planner->initialize(ros::NodeHandle(param_server_prefix),group_name,planner_config_name,collision_models_interface_))
00187     {
00188       new_planner.reset();
00189       ROS_ERROR("Could not configure planner for group %s with config %s",group_name.c_str(),planner_config_name.c_str());
00190       return false;
00191     }
00192     planner_map_[location] = new_planner;
00193   }
00194   else if(planner_type == "RPYIKTaskSpacePlanner")
00195   {
00196     boost::shared_ptr<ompl_ros_interface::OmplRosRPYIKTaskSpacePlanner> new_planner;
00197     new_planner.reset(new ompl_ros_interface::OmplRosRPYIKTaskSpacePlanner());
00198     if(!new_planner->initialize(ros::NodeHandle(param_server_prefix),group_name,planner_config_name,collision_models_interface_))
00199     {
00200       new_planner.reset();
00201       ROS_ERROR("Could not configure planner for group %s with config %s",group_name.c_str(),planner_config_name.c_str());
00202       return false;
00203     }
00204     planner_map_[location] = new_planner;
00205   }
00206   else
00207   {
00208     ROS_ERROR("No planner type %s available",planner_type.c_str());
00209     std::string cast;
00210     node_handle_.getParam(param_server_prefix+"/"+group_name, cast);
00211     ROS_ERROR_STREAM("Here " << cast);
00212     return false;
00213   }
00214 
00215   return true;
00216 };
00217 
00218 bool OmplRos::computePlan(arm_navigation_msgs::GetMotionPlan::Request &request, 
00219                           arm_navigation_msgs::GetMotionPlan::Response &response)
00220 {
00221   std::string location;
00222   std::string planner_id;
00223   if(request.motion_plan_request.planner_id == "")
00224     planner_id = default_planner_config_;
00225   else
00226     planner_id = request.motion_plan_request.planner_id; 
00227   location = planner_id + "[" +request.motion_plan_request.group_name + "]";
00228   if(planner_map_.find(location) == planner_map_.end())
00229   {
00230     ROS_ERROR("Could not find requested planner %s", location.c_str());
00231     response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::INVALID_PLANNER_ID;
00232     return true;
00233   }
00234   else
00235   {
00236     ROS_DEBUG("Using planner config %s",location.c_str());
00237   }
00238   planner_map_[location]->computePlan(request,response);
00239   if(publish_diagnostics_)
00240   {
00241     ompl_ros_interface::OmplPlannerDiagnostics msg;
00242     if(response.error_code.val != arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS) {
00243       msg.summary = "Planning Failed";
00244       std::string filename = "planning_failure_";
00245       std::string str = boost::lexical_cast<std::string>(ros::Time::now().toSec());
00246       filename += str;
00247       collision_models_interface_->writePlanningSceneBag(filename,
00248                                                          collision_models_interface_->getLastPlanningScene());
00249       collision_models_interface_->appendMotionPlanRequestToPlanningSceneBag(filename,
00250                                                                              "motion_plan_request",
00251                                                                              request.motion_plan_request);
00252     }
00253     else
00254       msg.summary = "Planning Succeeded";
00255 
00256     msg.group = request.motion_plan_request.group_name;
00257     msg.planner = planner_id;
00258     msg.result =  arm_navigation_msgs::armNavigationErrorCodeToString(response.error_code);
00259     if(response.error_code.val == arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS)
00260     {
00261       msg.planning_time = response.planning_time.toSec();
00262       msg.trajectory_size = response.trajectory.joint_trajectory.points.size();
00263       msg.trajectory_duration = response.trajectory.joint_trajectory.points.back().time_from_start.toSec()-response.trajectory.joint_trajectory.points.front().time_from_start.toSec();    
00264       //      msg.state_allocator_size = planner_map_[location]->planner_->getSpaceInformation()->getStateAllocator().sizeInUse();
00265     }
00266     diagnostic_publisher_.publish(msg);
00267   }
00268   return true;
00269 };
00270 
00271 boost::shared_ptr<ompl_ros_interface::OmplRosPlanningGroup>& OmplRos::getPlanner(const std::string &group_name,
00272                                                                                  const std::string &planner_config_name)
00273 {
00274   std::string location = planner_config_name + "[" + group_name + "]";
00275   if(planner_map_.find(location) == planner_map_.end())
00276   {
00277     ROS_ERROR("Could not find requested planner %s", location.c_str());
00278     return empty_ptr;
00279   }
00280   else
00281   {
00282     ROS_DEBUG("Using planner config %s",location.c_str());
00283     return planner_map_[location];
00284   }
00285 };
00286 
00287 
00288 }
ompl_ros.cpp
