00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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 void planning_environment::RobotModels::reload(void)
00044 {
00045 kmodel_.reset();
00046 urdf_.reset();
00047 for(std::map<std::string, GroupConfig*>::iterator it = group_config_map_.begin();
00048 it != group_config_map_.end();
00049 it++) {
00050 delete it->second;
00051 }
00052 group_config_map_.clear();
00053 planning_group_joints_.clear();
00054 planning_group_links_.clear();
00055 group_joint_union_.clear();
00056 group_link_union_.clear();
00057
00058 loadRobot();
00059 }
00060
00061 void planning_environment::RobotModels::loadRobot(void)
00062 {
00063 std::string content;
00064 if (nh_.getParam(description_, content))
00065 {
00066 urdf_ = boost::shared_ptr<urdf::Model>(new urdf::Model());
00067 if (urdf_->initString(content))
00068 {
00069 loaded_models_ = true;
00070 readGroupConfigs();
00071 bool hasMulti = readMultiDofConfigs();
00072 if(hasMulti) {
00073 kmodel_ = boost::shared_ptr<planning_models::KinematicModel>(new planning_models::KinematicModel(*urdf_, planning_group_joints_, multi_dof_configs_));
00074 } else {
00075 ROS_WARN("Can't do anything without a root transform");
00076 }
00077 }
00078 else
00079 {
00080 urdf_.reset();
00081 ROS_ERROR("Unable to parse URDF description!");
00082 }
00083 }
00084 else
00085 ROS_ERROR("Robot model '%s' not found! Did you remap 'robot_description'?", description_.c_str());
00086 }
00087
00088 bool planning_environment::RobotModels::readMultiDofConfigs() {
00089
00090 XmlRpc::XmlRpcValue multi_dof_joints;
00091
00092 std::string multi_dof_name = description_ + "_planning/multi_dof_joints";
00093
00094 if(!nh_.hasParam(multi_dof_name)) {
00095 ROS_WARN_STREAM("No multi dof joints specified, including root to world conversion");
00096 return false;
00097 }
00098
00099 nh_.getParam(multi_dof_name, multi_dof_joints);
00100
00101 if(multi_dof_joints.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00102 ROS_WARN("Multi-dof joints is not an array");
00103 return false;
00104 }
00105
00106 for(int i = 0; i < multi_dof_joints.size(); i++) {
00107 if(!multi_dof_joints[i].hasMember("name")) {
00108 ROS_WARN("Multi dof joint must have name");
00109 continue;
00110 }
00111 if(!multi_dof_joints[i].hasMember("parent_frame_id")) {
00112 ROS_WARN("Multi dof joint must have parent frame id");
00113 continue;
00114 }
00115 if(!multi_dof_joints[i].hasMember("child_frame_id")) {
00116 ROS_WARN("Multi dof joint must have child frame id");
00117 continue;
00118 }
00119 if(!multi_dof_joints[i].hasMember("type")) {
00120 ROS_WARN("Multi dof joint must have a type");
00121 continue;
00122 }
00123 std::string joint_name = multi_dof_joints[i]["name"];
00124 planning_models::KinematicModel::MultiDofConfig mdc(joint_name);
00125 for(XmlRpc::XmlRpcValue::iterator it = multi_dof_joints[i].begin();
00126 it != multi_dof_joints[i].end();
00127 it++) {
00128 if(it->first == "parent_frame_id") {
00129 mdc.parent_frame_id = std::string(it->second);
00130 } else if(it->first == "child_frame_id") {
00131 mdc.child_frame_id = std::string(it->second);
00132 } else if(it->first == "type") {
00133 mdc.type = std::string(it->second);
00134 } else if(it->first != "name") {
00135 mdc.name_equivalents[it->first] = std::string(it->second);
00136 }
00137 }
00138 multi_dof_configs_.push_back(mdc);
00139 }
00140 if(multi_dof_configs_.empty()) {
00141 return false;
00142 }
00143 return true;
00144 }
00145
00146 void planning_environment::RobotModels::readGroupConfigs() {
00147
00148 XmlRpc::XmlRpcValue all_groups;
00149
00150 std::string group_name = description_ + "_planning/groups";
00151
00152 if(!nh_.hasParam(group_name)) {
00153 ROS_WARN_STREAM("No groups for planning specified in " << group_name);
00154 return;
00155 }
00156
00157 nh_.getParam(group_name, all_groups);
00158
00159 if(all_groups.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00160 ROS_WARN("Groups is not an array");
00161 return;
00162 }
00163
00164 if(all_groups.size() == 0) {
00165 ROS_WARN("No groups in groups");
00166 return;
00167 }
00168
00169 for(int i = 0; i < all_groups.size(); i++) {
00170 if(!all_groups[i].hasMember("name")) {
00171 ROS_WARN("All groups must have a name");
00172 continue;
00173 }
00174 std::string gname = all_groups[i]["name"];
00175 GroupConfig* gc = new GroupConfig(gname);
00176 std::map< std::string, GroupConfig*>::iterator group_iterator = group_config_map_.find(gname);
00177 if(group_iterator != group_config_map_.end()) {
00178 ROS_WARN_STREAM("Already have group name " << gname);
00179 delete gc;
00180 continue;
00181 }
00182 group_config_map_[gname] = gc;
00183 if(all_groups[i].hasMember("joints")) {
00184 std::string joint_list = std::string(all_groups[i]["joints"]);
00185 std::stringstream joint_name_stream(joint_list);
00186 while(joint_name_stream.good() && !joint_name_stream.eof()){
00187 std::string jname;
00188 joint_name_stream >> jname;
00189 if(jname.size() == 0) continue;
00190 if (urdf_->getJoint(jname)) {
00191 group_config_map_[gname]->joint_names_.push_back(jname);
00192 } else {
00193 ROS_DEBUG_STREAM("Urdf doesn't have joint " << jname);
00194 }
00195 }
00196 }
00197 if(all_groups[i].hasMember("links")) {
00198 std::string link_list = std::string(all_groups[i]["links"]);
00199 std::stringstream link_name_stream(link_list);
00200 while(link_name_stream.good() && !link_name_stream.eof()){
00201 std::string lname;
00202 link_name_stream >> lname;
00203 if(lname.size() == 0) continue;
00204 if (urdf_->getLink(lname)) {
00205 group_config_map_[gname]->link_names_.push_back(lname);
00206 ROS_DEBUG_STREAM("Urdf has link " << lname);
00207 } else {
00208 ROS_INFO_STREAM("Urdf doesn't have link " << lname);
00209 }
00210 }
00211 }
00212 planning_group_joints_[gname] = group_config_map_[gname]->getJointNames();
00213 for(std::vector<std::string>::iterator it = planning_group_joints_[gname].begin();
00214 it != planning_group_joints_[gname].end();
00215 it++) {
00216 ROS_DEBUG_STREAM("Group " << gname << " joint " << (*it));
00217 }
00218
00219 planning_group_links_[gname] = group_config_map_[gname]->getLinkNames();
00220 }
00221
00222
00223 std::map<std::string, bool> pmap;
00224 group_joint_union_.clear();
00225 for(std::map< std::string, std::vector<std::string> >::iterator it1 = planning_group_joints_.begin();
00226 it1 != planning_group_joints_.end();
00227 it1++) {
00228 for(std::vector<std::string>::iterator it2 = it1->second.begin();
00229 it2 != it1->second.end();
00230 it2++) {
00231 pmap[*it2] = true;
00232 }
00233 }
00234 for(std::map<std::string, bool>::iterator it = pmap.begin();
00235 it != pmap.end();
00236 it++) {
00237 group_joint_union_.push_back(it->first);
00238 }
00239
00240 pmap.clear();
00241 group_link_union_.clear();
00242 for(std::map< std::string, std::vector<std::string> >::iterator it1 = planning_group_links_.begin();
00243 it1 != planning_group_links_.end();
00244 it1++) {
00245 for(std::vector<std::string>::iterator it2 = it1->second.begin();
00246 it2 != it1->second.end();
00247 it2++) {
00248 pmap[*it2] = true;
00249 }
00250 }
00251 for(std::map<std::string, bool>::iterator it = pmap.begin();
00252 it != pmap.end();
00253 it++) {
00254 group_link_union_.push_back(it->first);
00255 }
00256 }
00257
00258
