robot_models.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 "planning_environment/models/robot_models.h"
00038 #include <ros/console.h>
00039 
00040 #include <boost/algorithm/string.hpp>
00041 #include <sstream>
00042 
00043 planning_environment::RobotModels::RobotModels(const std::string &description) : priv_nh_("~")
00044 {
00045   description_ = nh_.resolveName(description);
00046   loaded_models_ = false;
00047   loadRobotFromParamServer();
00048 }
00049 
00050 planning_environment::RobotModels::RobotModels(boost::shared_ptr<urdf::Model> urdf,
00051                                                planning_models::KinematicModel* kmodel) {
00052   urdf_ = urdf;
00053   kmodel_ = kmodel;
00054   loaded_models_ = true;
00055 }
00056 
00057 
00058 void planning_environment::RobotModels::reload(void)
00059 {
00060   urdf_.reset();
00061   delete kmodel_;
00062   loadRobotFromParamServer();
00063 }
00064 
00065 void planning_environment::RobotModels::loadRobotFromParamServer(void)
00066 {
00067   std::string content;
00068   if (nh_.getParam(description_, content))
00069   {
00070     urdf_ = boost::shared_ptr<urdf::Model>(new urdf::Model());
00071     if (urdf_->initString(content))
00072     {
00073       loaded_models_ = true;
00074       std::vector<planning_models::KinematicModel::GroupConfig> group_configs;
00075       std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
00076       bool hasMulti = loadMultiDofConfigsFromParamServer(multi_dof_configs);
00077       loadGroupConfigsFromParamServer(multi_dof_configs, group_configs);
00078       if(hasMulti) {
00079         kmodel_ = new planning_models::KinematicModel(*urdf_, group_configs, multi_dof_configs);
00080       } else {
00081         ROS_WARN("Can't do anything without a root transform");
00082       }
00083     }
00084     else
00085     {
00086       urdf_.reset();
00087       ROS_ERROR("Unable to parse URDF description!");
00088     }
00089   }
00090   else
00091     ROS_ERROR("Robot model '%s' not found! Did you remap 'robot_description'?", description_.c_str());
00092 }
00093 
00094 bool planning_environment::RobotModels::loadMultiDofConfigsFromParamServer(std::vector<planning_models::KinematicModel::MultiDofConfig>& configs) 
00095 {
00096   configs.clear();
00097   
00098   XmlRpc::XmlRpcValue multi_dof_joints;
00099   
00100   std::string multi_dof_name = description_ + "_planning/multi_dof_joints";
00101   
00102   if(!nh_.hasParam(multi_dof_name)) {
00103     ROS_WARN_STREAM("No multi dof joints specified, including root to world conversion");
00104     return false;
00105   }
00106  
00107   nh_.getParam(multi_dof_name, multi_dof_joints);
00108   
00109   if(multi_dof_joints.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00110     ROS_WARN("Multi-dof joints is not an array");
00111     return false;
00112   }
00113   
00114   for(int i = 0; i < multi_dof_joints.size(); i++) {
00115     if(!multi_dof_joints[i].hasMember("name")) {
00116       ROS_WARN("Multi dof joint must have name");
00117       continue;
00118     }
00119     if(!multi_dof_joints[i].hasMember("parent_frame_id")) {
00120       ROS_WARN("Multi dof joint must have parent frame id");
00121       continue;
00122     }
00123     if(!multi_dof_joints[i].hasMember("child_frame_id")) {
00124       ROS_WARN("Multi dof joint must have child frame id");
00125       continue;
00126     }
00127     if(!multi_dof_joints[i].hasMember("type")) {
00128       ROS_WARN("Multi dof joint must have a type");
00129       continue;
00130     }
00131     std::string joint_name = multi_dof_joints[i]["name"];
00132     planning_models::KinematicModel::MultiDofConfig mdc(joint_name);
00133     for(XmlRpc::XmlRpcValue::iterator it = multi_dof_joints[i].begin();
00134         it != multi_dof_joints[i].end();
00135         it++) {
00136       if(it->first == "parent_frame_id") {
00137         mdc.parent_frame_id = std::string(it->second);
00138       } else if(it->first == "child_frame_id") {
00139         mdc.child_frame_id = std::string(it->second);
00140       } else if(it->first == "type") {
00141         mdc.type = std::string(it->second);
00142       } else if(it->first != "name") {
00143         mdc.name_equivalents[it->first] = std::string(it->second);
00144       }
00145     }
00146     configs.push_back(mdc);
00147   }
00148   if(configs.empty()) {
00149     return false;
00150   }
00151   return true;
00152 }
00153 
00154 void planning_environment::RobotModels::loadGroupConfigsFromParamServer(const std::vector<planning_models::KinematicModel::MultiDofConfig>& multi_dof_configs,
00155                                                                         std::vector<planning_models::KinematicModel::GroupConfig>& configs) {
00156   
00157   configs.clear();
00158 
00159   XmlRpc::XmlRpcValue all_groups;
00160   
00161   std::string group_name = description_ + "_planning/groups";
00162 
00163   if(!nh_.hasParam(group_name)) {
00164     ROS_WARN_STREAM("No groups for planning specified in " << group_name);
00165     return;
00166   } 
00167   
00168   nh_.getParam(group_name, all_groups);
00169   
00170   if(all_groups.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00171     ROS_WARN("Groups is not an array");
00172     return;
00173   }
00174   
00175   if(all_groups.size() == 0) {
00176     ROS_WARN("No groups in groups");
00177     return;
00178   }
00179  
00180   for(int i = 0; i < all_groups.size(); i++) {
00181     if(!all_groups[i].hasMember("name")) {
00182       ROS_WARN("All groups must have a name");
00183       continue;
00184     }
00185     std::string gname = all_groups[i]["name"];
00186     bool already_have = false;
00187     for(unsigned int j = 0; j < configs.size(); j++) {
00188       if(configs[j].name_ == gname) {
00189         ROS_WARN_STREAM("Already have group name " << gname); 
00190         already_have = true;
00191         break;
00192       }
00193     }
00194     if(already_have) continue;
00195     if((all_groups[i].hasMember("base_link") && !all_groups[i].hasMember("tip_link")) ||
00196        (!all_groups[i].hasMember("base_link") && all_groups[i].hasMember("tip_link"))) {
00197       ROS_WARN_STREAM("If using chain definition must have both base_link and tip_link defined");
00198       continue;
00199     }
00200     //only need to test one condition
00201     if(all_groups[i].hasMember("base_link")) {
00202       configs.push_back(planning_models::KinematicModel::GroupConfig(gname,
00203                                                                      all_groups[i]["base_link"],
00204                                                                      all_groups[i]["tip_link"])); 
00205     } else {
00206       if(!all_groups[i].hasMember("subgroups") && !all_groups[i].hasMember("joints")) {
00207         ROS_WARN_STREAM("Group " << gname << " is not a valid chain and thus must have one or more joint or subgroups defined");
00208         continue;
00209       }
00210 
00211       std::vector<std::string> subgroups;
00212       if(all_groups[i].hasMember("subgroups")) {
00213         XmlRpc::XmlRpcValue subgroups_seq = all_groups[i]["subgroups"];
00214         if(subgroups_seq.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00215           ROS_WARN_STREAM("Group " << gname << " subgroups not an array");
00216           continue;
00217         }
00218         for(unsigned int j = 0; j < (unsigned int) subgroups_seq.size(); j++) {
00219           subgroups.push_back(std::string(subgroups_seq[j]));
00220         }
00221       }
00222 
00223       std::vector<std::string> joints;
00224       if(all_groups[i].hasMember("joints")) {
00225         XmlRpc::XmlRpcValue joints_seq = all_groups[i]["joints"];
00226         if(joints_seq.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00227           ROS_WARN_STREAM("Group " << gname << " joints not an array");
00228           continue;
00229         }
00230         for(unsigned int j = 0; j < (unsigned int) joints_seq.size(); j++) {
00231           std::string jname = std::string(joints_seq[j]);
00232           if (urdf_->getJoint(jname)) {
00233             joints.push_back(jname);
00234           } else {
00235             bool have_multi = false;
00236             for(unsigned int i = 0; i < multi_dof_configs.size(); i++) {
00237               if(jname == multi_dof_configs[i].name) {
00238                 joints.push_back(jname);
00239                 have_multi = true;
00240                 break;
00241               }
00242             }
00243             if(!have_multi) {
00244               ROS_WARN_STREAM("Urdf doesn't have joint " << jname << " and no multi-dof joint of that name");
00245             }
00246           }
00247         }
00248       }
00249       configs.push_back(planning_models::KinematicModel::GroupConfig(gname,
00250                                                                      joints,
00251                                                                      subgroups));
00252     }
00253   }
00254 }
00255 
00256 


planning_environment
Author(s): Ioan Sucan
autogenerated on Thu Dec 12 2013 11:09:24