bool parseSensor(Sensor &sensor, TiXmlElement* config)
{
sensor.clear();
const char *name_char = config->Attribute("name");
if (!name_char)
{
printf("No name given for the sensor. \n");
return false;
}
sensor.name = std::string(name_char);
// parse parent_link_name
const char *parent_link_name_char = config->Attribute("parent_link_name");
if (!parent_link_name_char)
{
printf("No parent_link_name given for the sensor. \n");
return false;
}
sensor.parent_link_name = std::string(parent_link_name_char);
// parse origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parsePose(sensor.origin, o))
return false;
}
// parse sensor
sensor.sensor = parseVisualSensor(config);
return true;
}
bool RPLocaliser::setupRoadmap(std::string filename) {
ros::NodeHandle nh("~");
// clear previous roadmap from knowledge base
ROS_INFO("KCL: (RPLocaliser) Loading roadmap from file %s", filename.c_str());
// load configuration file
std::ifstream infile(filename.c_str());
std::string line;
int curr,next;
while(!infile.eof()) {
// read waypoint
// std::cout << line << std::endl;
std::getline(infile, line);
curr=line.find("[");
std::string name = line.substr(0,curr);
// data
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "map";
parsePose(pose, line);
waypoints[name] = pose;
}
infile.close();
}
bool RPSimpleMapServer::setupRoadmap(std::string filename) {
ros::NodeHandle nh("~");
// clear previous roadmap from knowledge base
ros::service::waitForService("/kcl_rosplan/update_knowledge_base", ros::Duration(20));
ROS_INFO("KCL: (RPSimpleMapServer) Loading roadmap from file");
// load configuration file
std::ifstream infile(filename.c_str());
std::string line;
int curr,next;
while(std::getline(infile, line)) {
// read waypoint
curr=line.find("[");
std::string name = line.substr(0,curr);
// instance
rosplan_knowledge_msgs::KnowledgeUpdateService updateSrv;
updateSrv.request.update_type = rosplan_knowledge_msgs::KnowledgeUpdateService::Request::ADD_KNOWLEDGE;
updateSrv.request.knowledge.knowledge_type = rosplan_knowledge_msgs::KnowledgeItem::INSTANCE;
updateSrv.request.knowledge.instance_type = "waypoint";
updateSrv.request.knowledge.instance_name = name;
update_knowledge_client.call(updateSrv);
// data
geometry_msgs::PoseStamped pose;
pose.header.frame_id = fixed_frame;
parsePose(pose, line);
std::string id(message_store.insertNamed(name, pose));
db_name_map[name] = id;
// save here for viz
Waypoint* wp = new Waypoint(name, pose.pose.position.x, pose.pose.position.y);
waypoints[wp->wpID] = wp;
}
infile.close();
// publish visualization
publishWaypointMarkerArray(nh);
}
bool parseVisual(Visual &vis, TiXmlElement *config)
{
vis.clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o) {
if (!parsePose(vis.origin, o))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
vis.geometry = parseGeometry(geom);
if (!vis.geometry)
return false;
const char *name_char = config->Attribute("name");
if (name_char)
vis.name = name_char;
// Material
TiXmlElement *mat = config->FirstChildElement("material");
if (mat) {
// get material name
if (!mat->Attribute("name")) {
logError("Visual material must contain a name attribute");
return false;
}
vis.material_name = mat->Attribute("name");
// try to parse material element in place
resetPtr(vis.material,new Material());
if (!parseMaterial(*vis.material, mat, true))
{
logDebug("urdfdom: material has only name, actual material definition may be in the model");
}
}
return true;
}
bool parseCollision(Collision &col, TiXmlElement* config)
{
col.clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o) {
if (!parsePose(col.origin, o))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
col.geometry = parseGeometry(geom);
if (!col.geometry)
return false;
const char *name_char = config->Attribute("name");
if (name_char)
col.name = name_char;
return true;
}
bool parseJoint(Joint &joint, TiXmlElement* config)
{
joint.clear();
// Get Joint Name
const char *name = config->Attribute("name");
if (!name)
{
CONSOLE_BRIDGE_logError("unnamed joint found");
return false;
}
joint.name = name;
// Get transform from Parent Link to Joint Frame
TiXmlElement *origin_xml = config->FirstChildElement("origin");
if (!origin_xml)
{
CONSOLE_BRIDGE_logDebug("urdfdom: Joint [%s] missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", joint.name.c_str());
joint.parent_to_joint_origin_transform.clear();
}
else
{
if (!parsePose(joint.parent_to_joint_origin_transform, origin_xml))
{
joint.parent_to_joint_origin_transform.clear();
CONSOLE_BRIDGE_logError("Malformed parent origin element for joint [%s]", joint.name.c_str());
return false;
}
}
// Get Parent Link
TiXmlElement *parent_xml = config->FirstChildElement("parent");
if (parent_xml)
{
const char *pname = parent_xml->Attribute("link");
if (!pname)
{
CONSOLE_BRIDGE_logInform("no parent link name specified for Joint link [%s]. this might be the root?", joint.name.c_str());
}
else
{
joint.parent_link_name = std::string(pname);
}
}
// Get Child Link
TiXmlElement *child_xml = config->FirstChildElement("child");
if (child_xml)
{
const char *pname = child_xml->Attribute("link");
if (!pname)
{
CONSOLE_BRIDGE_logInform("no child link name specified for Joint link [%s].", joint.name.c_str());
}
else
{
joint.child_link_name = std::string(pname);
}
}
// Get Joint type
const char* type_char = config->Attribute("type");
if (!type_char)
{
CONSOLE_BRIDGE_logError("joint [%s] has no type, check to see if it's a reference.", joint.name.c_str());
return false;
}
std::string type_str = type_char;
if (type_str == "planar")
joint.type = Joint::PLANAR;
else if (type_str == "floating")
joint.type = Joint::FLOATING;
else if (type_str == "revolute")
joint.type = Joint::REVOLUTE;
else if (type_str == "continuous")
joint.type = Joint::CONTINUOUS;
else if (type_str == "prismatic")
joint.type = Joint::PRISMATIC;
else if (type_str == "fixed")
joint.type = Joint::FIXED;
else
{
CONSOLE_BRIDGE_logError("Joint [%s] has no known type [%s]", joint.name.c_str(), type_str.c_str());
return false;
}
// Get Joint Axis
if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED)
{
// axis
TiXmlElement *axis_xml = config->FirstChildElement("axis");
if (!axis_xml){
CONSOLE_BRIDGE_logDebug("urdfdom: no axis elemement for Joint link [%s], defaulting to (1,0,0) axis", joint.name.c_str());
joint.axis = Vector3(1.0, 0.0, 0.0);
}
else{
if (axis_xml->Attribute("xyz")){
try {
joint.axis.init(axis_xml->Attribute("xyz"));
}
catch (ParseError &e) {
joint.axis.clear();
CONSOLE_BRIDGE_logError("Malformed axis element for joint [%s]: %s", joint.name.c_str(), e.what());
return false;
}
}
}
}
// Get limit
TiXmlElement *limit_xml = config->FirstChildElement("limit");
if (limit_xml)
{
joint.limits.reset(new JointLimits());
if (!parseJointLimits(*joint.limits, limit_xml))
{
CONSOLE_BRIDGE_logError("Could not parse limit element for joint [%s]", joint.name.c_str());
joint.limits.reset();
return false;
}
}
else if (joint.type == Joint::REVOLUTE)
{
CONSOLE_BRIDGE_logError("Joint [%s] is of type REVOLUTE but it does not specify limits", joint.name.c_str());
return false;
}
else if (joint.type == Joint::PRISMATIC)
{
CONSOLE_BRIDGE_logError("Joint [%s] is of type PRISMATIC without limits", joint.name.c_str());
return false;
}
// Get safety
TiXmlElement *safety_xml = config->FirstChildElement("safety_controller");
if (safety_xml)
{
joint.safety.reset(new JointSafety());
if (!parseJointSafety(*joint.safety, safety_xml))
{
CONSOLE_BRIDGE_logError("Could not parse safety element for joint [%s]", joint.name.c_str());
joint.safety.reset();
return false;
}
}
// Get calibration
TiXmlElement *calibration_xml = config->FirstChildElement("calibration");
if (calibration_xml)
{
joint.calibration.reset(new JointCalibration());
if (!parseJointCalibration(*joint.calibration, calibration_xml))
{
CONSOLE_BRIDGE_logError("Could not parse calibration element for joint [%s]", joint.name.c_str());
joint.calibration.reset();
return false;
}
}
// Get Joint Mimic
TiXmlElement *mimic_xml = config->FirstChildElement("mimic");
if (mimic_xml)
{
joint.mimic.reset(new JointMimic());
if (!parseJointMimic(*joint.mimic, mimic_xml))
{
CONSOLE_BRIDGE_logError("Could not parse mimic element for joint [%s]", joint.name.c_str());
joint.mimic.reset();
return false;
}
}
// Get Dynamics
TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
if (prop_xml)
{
joint.dynamics.reset(new JointDynamics());
if (!parseJointDynamics(*joint.dynamics, prop_xml))
{
CONSOLE_BRIDGE_logError("Could not parse joint_dynamics element for joint [%s]", joint.name.c_str());
joint.dynamics.reset();
return false;
}
}
return true;
}
/**
* @function parseWorldURDF
*/
World* parseWorldURDF(const std::string &_xml_string, std::string _root_to_world_path) {
World* world = new World();
TiXmlDocument xml_doc;
xml_doc.Parse( _xml_string.c_str() );
TiXmlElement *world_xml = xml_doc.FirstChildElement("world");
if( !world_xml ) {
printf ( "[parseWorldURDF] ERROR: Could not find a <world> element in XML, exiting and not loading! \n" );
return NULL;
}
// Get world name
const char *name = world_xml->Attribute("name");
if(!name) {
printf ("[parseWorldURDF] ERROR: World does not have a name specified. Exiting and not loading! \n");
return NULL;
}
world->name = std::string(name);
if(debug) std::cout<< "World name: "<< world->name << std::endl;
// Get all include filenames
int count = 0;
std::map<std::string, std::string> includedFiles;
for( TiXmlElement* include_xml = world_xml->FirstChildElement("include");
include_xml; include_xml = include_xml->NextSiblingElement("include") ) {
count++;
const char *filename = include_xml->Attribute("filename");
const char *model_name = include_xml->Attribute("model_name");
std::string string_filename( filename );
std::string string_model_name( model_name );
includedFiles[string_model_name] = string_filename;
if(debug) std::cout<<"Include: Model name: "<< model_name <<" filename: "<< filename <<std::endl;
}
if(debug) std::cout<<"Found "<<count<<" include filenames "<<std::endl;
// Get all entities
count = 0;
for( TiXmlElement* entity_xml = world_xml->FirstChildElement("entity");
entity_xml; entity_xml = entity_xml->NextSiblingElement("entity") ) {
count++;
Entity entity;
try {
const char* entity_model = entity_xml->Attribute("model");
std::string string_entity_model( entity_model );
// Find the model
if( includedFiles.find( string_entity_model ) == includedFiles.end() ) {
std::cout<<"[parseWorldURDF] ERROR: I cannot find the model you want to use, did you write the name right? Exiting and not loading! \n"<<std::endl;
return NULL;
}
else {
std::string fileName = includedFiles.find( string_entity_model )->second;
std::string fileFullName = _root_to_world_path;
fileFullName.append( fileName );
if(debug) std::cout<< "Entity full filename: "<< fileFullName << std::endl;
// Parse model
std::string xml_model_string;
std::fstream xml_file( fileFullName.c_str(), std::fstream::in );
while( xml_file.good() ) {
std::string line;
std::getline( xml_file, line );
xml_model_string += (line + "\n");
}
xml_file.close();
ModelInterface* model = parseURDF( xml_model_string ).get();
if( !model ) {
std::cout<< "[parseWorldURDF] Model in "<<fileFullName<<" not found. Exiting and not loading!" <<std::endl;
return NULL;
}
else {
entity.model.reset(model);
// Parse location
TiXmlElement *o = entity_xml->FirstChildElement("origin");
if( o ) {
if( !parsePose( entity.origin, o ) ) {
printf ("[ERROR] Write the pose for your entity! \n");
return world;
}
}
// If name is defined
const char* entity_name = entity_xml->Attribute("name");
if( entity_name ) {
std::string string_entity_name( entity_name );
entity.model->name_ = string_entity_name;
}
// Store in world
world->models.push_back( entity );
}
} // end of include read
}
catch( ParseError &e ) {
if(debug) printf ("Entity xml not initialized correctly \n");
//entity->reset();
//world->reset();
return world;
}
} // end for
if(debug) printf ("Found %d entities \n", count);
return world;
}
bool parseInertial(Inertial &i, TiXmlElement *config)
{
i.clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parsePose(i.origin, o))
return false;
}
TiXmlElement *mass_xml = config->FirstChildElement("mass");
if (!mass_xml)
{
logError("Inertial element must have a mass element");
return false;
}
if (!mass_xml->Attribute("value"))
{
logError("Inertial: mass element must have value attribute");
return false;
}
if( !stringToDouble(mass_xml->Attribute("value"),i.mass) )
{
std::stringstream stm;
stm << "Inertial: mass [" << mass_xml->Attribute("value")
<< "] is not a float";
logError(stm.str().c_str());
return false;
}
TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
if (!inertia_xml)
{
logError("Inertial element must have inertia element");
return false;
}
if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
inertia_xml->Attribute("izz")))
{
logError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
return false;
}
if( !stringToDouble(inertia_xml->Attribute("ixx"),i.ixx)
|| !stringToDouble(inertia_xml->Attribute("ixy"),i.ixy)
|| !stringToDouble(inertia_xml->Attribute("ixz"),i.ixz)
|| !stringToDouble(inertia_xml->Attribute("iyy"),i.iyy)
|| !stringToDouble(inertia_xml->Attribute("iyz"),i.iyz)
|| !stringToDouble(inertia_xml->Attribute("izz"),i.izz) )
{
std::stringstream stm;
stm << "Inertial: one of the inertia elements is not a valid double:"
<< " ixx [" << inertia_xml->Attribute("ixx") << "]"
<< " ixy [" << inertia_xml->Attribute("ixy") << "]"
<< " ixz [" << inertia_xml->Attribute("ixz") << "]"
<< " iyy [" << inertia_xml->Attribute("iyy") << "]"
<< " iyz [" << inertia_xml->Attribute("iyz") << "]"
<< " izz [" << inertia_xml->Attribute("izz") << "]";
logError(stm.str().c_str());
return false;
}
return true;
}
bool parseInertial(Inertial &i, TiXmlElement *config)
{
i.clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parsePose(i.origin, o))
return false;
}
TiXmlElement *mass_xml = config->FirstChildElement("mass");
if (!mass_xml)
{
CONSOLE_BRIDGE_logError("Inertial element must have a mass element");
return false;
}
if (!mass_xml->Attribute("value"))
{
CONSOLE_BRIDGE_logError("Inertial: mass element must have value attribute");
return false;
}
try
{
i.mass = boost::lexical_cast<double>(mass_xml->Attribute("value"));
}
catch (boost::bad_lexical_cast &/*e*/)
{
std::stringstream stm;
stm << "Inertial: mass [" << mass_xml->Attribute("value")
<< "] is not a float";
CONSOLE_BRIDGE_logError(stm.str().c_str());
return false;
}
TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
if (!inertia_xml)
{
CONSOLE_BRIDGE_logError("Inertial element must have inertia element");
return false;
}
if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
inertia_xml->Attribute("izz")))
{
CONSOLE_BRIDGE_logError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
return false;
}
try
{
i.ixx = boost::lexical_cast<double>(inertia_xml->Attribute("ixx"));
i.ixy = boost::lexical_cast<double>(inertia_xml->Attribute("ixy"));
i.ixz = boost::lexical_cast<double>(inertia_xml->Attribute("ixz"));
i.iyy = boost::lexical_cast<double>(inertia_xml->Attribute("iyy"));
i.iyz = boost::lexical_cast<double>(inertia_xml->Attribute("iyz"));
i.izz = boost::lexical_cast<double>(inertia_xml->Attribute("izz"));
}
catch (boost::bad_lexical_cast &/*e*/)
{
std::stringstream stm;
stm << "Inertial: one of the inertia elements is not a valid double:"
<< " ixx [" << inertia_xml->Attribute("ixx") << "]"
<< " ixy [" << inertia_xml->Attribute("ixy") << "]"
<< " ixz [" << inertia_xml->Attribute("ixz") << "]"
<< " iyy [" << inertia_xml->Attribute("iyy") << "]"
<< " iyz [" << inertia_xml->Attribute("iyz") << "]"
<< " izz [" << inertia_xml->Attribute("izz") << "]";
CONSOLE_BRIDGE_logError(stm.str().c_str());
return false;
}
return true;
}
