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 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
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
