ompl_ros_planner_config.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 the Willow Garage 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_planner_config.h"
00038 #include <boost/algorithm/string.hpp>
00039 #include <sstream>
00040 
00041 const std::string& ompl_ros_interface::PlannerConfig::getName(void)
00042 {
00043     return config_;
00044 }
00045 
00046 bool ompl_ros_interface::PlannerConfig::hasParam(const std::string &param)
00047 {
00048     return nh_.hasParam(description_ + "/planner_configs/" + config_ + "/" + param);
00049 }
00050 
00051 std::string ompl_ros_interface::PlannerConfig::getParamString(const std::string &param, const std::string& def)
00052 {
00053     std::string value;
00054     nh_.param(description_ + "/planner_configs/" + config_ + "/" + param, value, def);
00055     boost::trim(value);
00056     return value;
00057 }
00058 
00059 double ompl_ros_interface::PlannerConfig::getParamDouble(const std::string &param, double def)
00060 {
00061     double value;
00062     nh_.param(description_ + "/planner_configs/" + config_ + "/" + param, value, def);
00063     return value;
00064 }
00065 
00066 int ompl_ros_interface::PlannerConfig::getParamInt(const std::string &param, int def)
00067 {
00068     int value;
00069     nh_.param(description_ + "/planner_configs/" + config_ + "/" + param, value, def);
00070     return value;
00071 }
00072 
00073 void ompl_ros_interface::PlannerConfigMap::loadPlannerConfigs() {
00074 
00075   ros::NodeHandle nh("~");
00076 
00077   //first we load the group_list
00078   std::string group_list;
00079   nh.param(description_ + "/group_list", group_list, std::string(""));
00080   std::stringstream group_list_stream(group_list);
00081   std::map<std::string, bool> plan_group_map;
00082   while (group_list_stream.good() && !group_list_stream.eof())
00083   {
00084     std::string g;
00085     group_list_stream >> g;
00086     if(g.size() == 0) continue;
00087     planning_group_names_.push_back(g);
00088   }
00089 
00090   //now we go through the groups
00091   XmlRpc::XmlRpcValue all_groups;
00092   
00093   if(!nh.hasParam(description_+"/groups")) {
00094     ROS_WARN("No groups for planning specified");
00095     return;
00096   } 
00097   
00098   nh.getParam(description_+"/groups", all_groups);
00099   
00100   if(all_groups.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00101     ROS_WARN("Groups is not an array");
00102     return;
00103   }
00104   
00105   if(all_groups.size() == 0) {
00106     ROS_WARN("No groups in groups");
00107     return;
00108   }
00109   
00110   for(int i = 0; i < all_groups.size(); i++) {
00111     if(!all_groups[i].hasMember("name")) {
00112       ROS_WARN("All groups must have a name");
00113       continue;
00114     }
00115     std::string gname = all_groups[i]["name"];
00116     std::vector<std::string> configs;
00117     
00118     std::string plan_list = std::string(all_groups[i]["planner_configs"]);
00119     std::stringstream planner_name_stream(plan_list);
00120     while(planner_name_stream.good() && !planner_name_stream.eof()){
00121       std::string pname; 
00122       planner_name_stream >> pname;
00123       if(pname.size() == 0) continue;
00124       configs.push_back(pname);
00125     }
00126     group_to_planner_string_config_map_[gname] = configs;
00127   }
00128 }
00129 
00130 std::vector< boost::shared_ptr<ompl_ros_interface::PlannerConfig> > ompl_ros_interface::PlannerConfigMap::getGroupPlannersConfig(const std::string &group) const
00131 {
00132   std::vector< boost::shared_ptr<ompl_ros_interface::PlannerConfig> > ret;
00133   if(group_to_planner_string_config_map_.find(group) != group_to_planner_string_config_map_.end()) {
00134     //planners expect their own shared pointers
00135     for(std::vector<std::string>::const_iterator it = group_to_planner_string_config_map_.find(group)->second.begin();
00136         it != group_to_planner_string_config_map_.find(group)->second.end();
00137         it++) {
00138       ret.push_back(boost::shared_ptr<PlannerConfig>(new ompl_ros_interface::PlannerConfig(description_, (*it))));
00139     }
00140   }
00141   return ret;
00142 }


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