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