bool loadRobotModel(ros::NodeHandle node_handle, urdf::Model &robot_model, std::string &root_name, std::string &tip_name, std::string &xml_string)
 {
   std::string urdf_xml,full_urdf_xml;
   node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
   node_handle.searchParam(urdf_xml,full_urdf_xml);
   TiXmlDocument xml;
   ROS_DEBUG("Reading xml file from parameter server\n");
   std::string result;
   if (node_handle.getParam(full_urdf_xml, result))
     xml.Parse(result.c_str());
   else
   {
     ROS_FATAL("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
     return false;
   }
   xml_string = result;
   TiXmlElement *root_element = xml.RootElement();
   TiXmlElement *root = xml.FirstChildElement("robot");
   if (!root || !root_element)
   {
     ROS_FATAL("Could not parse the xml from %s\n", urdf_xml.c_str());
     exit(1);
   }
   robot_model.initXml(root);
   if (!node_handle.getParam("root_name", root_name)){
     ROS_FATAL("No root name found on parameter server");
     return false;
   }
   if (!node_handle.getParam("tip_name", tip_name)){
     ROS_FATAL("No tip name found on parameter server");
     return false;
   }
   return true;
 }
コード例 #2
0
bool get_model(urdf::Model& robot)
{
  ros::NodeHandle nh;
  std::string file;
  nh.getParam("urdf_file_path", file);
  TiXmlDocument robot_model_xml;
  robot_model_xml.LoadFile(file);
  TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
  if (!robot_xml){
    std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl;
    return false;
  }

  if (!robot.initXml(robot_xml)){
    std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
    return false;
  }
  return true;
}