ompl_ros_interface: ompl_ros.cpp Source 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.h>
00038 
00039 namespace ompl_ros_interface
00040 {
00041 
00042   OmplRos::OmplRos(void): node_handle_("~")
00043 {
00044   collision_models_ = new planning_environment::CollisionModels("robot_description");
00045   planning_monitor_ = new planning_environment::PlanningMonitor(collision_models_, &tf_);    
00046   planning_monitor_->waitForState();
00047   planning_monitor_->startEnvironmentMonitor();    
00048 }
00049 
00051 OmplRos::~OmplRos(void)
00052 {
00053   delete planning_monitor_;
00054   delete collision_models_;
00055 }
00056 
00057 void OmplRos::run(void)
00058 {
00059   if(!initialize(planning_monitor_,node_handle_.getNamespace()))
00060     return;
00061   if (collision_models_->loadedModels())
00062   {
00063     bool verbose_collisions;    
00064     node_handle_.param("verbose_collisions", verbose_collisions, false);
00065     if (verbose_collisions)
00066     {
00067       planning_monitor_->getEnvironmentModel()->setVerbose(true);
00068       ROS_WARN("Verbose collisions is enabled");
00069     }
00070     else
00071       planning_monitor_->getEnvironmentModel()->setVerbose(false);
00072     node_handle_.param<std::string>("default_planner_id",default_planner_id_,"SBLkConfig1");
00073     plan_path_service_ = node_handle_.advertiseService("plan_kinematic_path", &OmplRos::computePlan, this);
00074     node_handle_.param<bool>("publish_diagnostics", publish_diagnostics_,false);
00075     if(publish_diagnostics_)
00076       diagnostic_publisher_ = node_handle_.advertise<ompl_ros_interface::OmplPlannerDiagnostics>("diagnostics", 1);
00077   }
00078   else
00079     ROS_ERROR("Collision models not loaded.");
00080 }
00081 
00082 bool OmplRos::initialize(planning_environment::PlanningMonitor *planning_monitor,
00083                          const std::string &param_server_prefix)
00084 {
00085   std::vector<std::string> group_names;
00086   if(!getGroupNamesFromParamServer(param_server_prefix,group_names))
00087   {
00088     ROS_ERROR("Could not find groups for planning under %s",param_server_prefix.c_str());
00089     return false;
00090   }
00091 
00092   if(!initializePlanningMap(param_server_prefix,group_names))
00093   {
00094     ROS_ERROR("Could not initialize planning groups from the param server");
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_INFO("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_INFO("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("Planner type not defined for group %s",group_name.c_str());
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,planning_monitor_))
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,planning_monitor_))
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     return false;
00210   }
00211 
00212   return true;
00213 };
00214 
00215 bool OmplRos::computePlan(motion_planning_msgs::GetMotionPlan::Request &request, 
00216                           motion_planning_msgs::GetMotionPlan::Response &response)
00217 {
00218   std::string location;
00219   std::string planner_id;
00220   if(request.motion_plan_request.planner_id == "")
00221     planner_id = default_planner_id_;
00222   else
00223     planner_id = request.motion_plan_request.planner_id; 
00224   location = planner_id + "[" +request.motion_plan_request.group_name + "]";
00225   if(planner_map_.find(location) == planner_map_.end())
00226   {
00227     ROS_ERROR("Could not find requested planner %s", location.c_str());
00228     response.error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::INVALID_PLANNER_ID;
00229     return true;
00230   }
00231   else
00232   {
00233     ROS_INFO("Using planner config %s",location.c_str());
00234   }
00235   planner_map_[location]->computePlan(request,response);
00236   if(publish_diagnostics_)
00237   {
00238     ompl_ros_interface::OmplPlannerDiagnostics msg;
00239     if(response.error_code.val != motion_planning_msgs::ArmNavigationErrorCodes::SUCCESS)
00240       msg.summary = "Planning Failed";
00241     else
00242       msg.summary = "Planning Succeeded";
00243 
00244     msg.group = request.motion_plan_request.group_name;
00245     msg.planner = planner_id;
00246     msg.result =  motion_planning_msgs::armNavigationErrorCodeToString(response.error_code);
00247     if(response.error_code.val == motion_planning_msgs::ArmNavigationErrorCodes::SUCCESS)
00248     {
00249       msg.planning_time = response.planning_time.toSec();
00250       msg.trajectory_size = response.trajectory.joint_trajectory.points.size();
00251       msg.trajectory_duration = response.trajectory.joint_trajectory.points.back().time_from_start.toSec()-response.trajectory.joint_trajectory.points.front().time_from_start.toSec();    
00252       //      msg.state_allocator_size = planner_map_[location]->planner_->getSpaceInformation()->getStateAllocator().sizeInUse();
00253     }
00254     diagnostic_publisher_.publish(msg);
00255   }
00256   return true;
00257 };
00258 
00259 boost::shared_ptr<ompl_ros_interface::OmplRosPlanningGroup>& OmplRos::getPlanner(const std::string &group_name,
00260                                                                                  const std::string &planner_config_name)
00261 {
00262   std::string location = planner_config_name + "[" + group_name + "]";
00263   if(planner_map_.find(location) == planner_map_.end())
00264   {
00265     ROS_ERROR("Could not find requested planner %s", location.c_str());
00266     return empty_ptr;
00267   }
00268   else
00269   {
00270     ROS_INFO("Using planner config %s",location.c_str());
00271     return planner_map_[location];
00272   }
00273 };
00274 
00275 
00276 }