bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

make parsing more robust to white space in attribute values

This commit is contained in:
Ioan Sucan 2012-04-10 20:45:22 +03:00
parent 9d368b1a42
commit e29657e464
2 changed files with 549 additions and 548 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
srdf/include/srdf/model.h
View File

@ -48,10 +48,10 @@
namespace srdf
{
/** \brief Representation of semantic information about the robot */
class Model
{
public:
/** \brief Representation of semantic information about the robot */
class Model
{
public:
Model(void)
{
@ -188,7 +188,7 @@ namespace srdf
/// Clear the model
void clear(void);
private:
private:
void loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
void loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
@ -202,7 +202,7 @@ namespace srdf
std::vector<VirtualJoint> virtual_joints_;
std::vector<EndEffector> end_effectors_;
std::vector<std::pair<std::string, std::string> > disabled_collisions_;
};
};
}
#endif

41
srdf/src/model.cpp
View File

@ -37,6 +37,7 @@
#include "srdf/model.h"
#include <ros/console.h>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <algorithm>
#include <fstream>
#include <sstream>
@ -60,7 +61,7 @@ void srdf::Model::loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXm
ROS_ERROR("Child link of virtual joint is not specified");
continue;
}
if (!urdf_model.getLink(std::string(child)))
if (!urdf_model.getLink(boost::trim_copy(std::string(child))))
{
ROS_ERROR("Virtual joint does not attach to a link on the robot (link '%s' is not known)", child);
continue;
@ -76,16 +77,16 @@ void srdf::Model::loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXm
continue;
}
VirtualJoint vj;
vj.type_ = std::string(type);
vj.type_ = std::string(type); boost::trim(vj.type_);
std::transform(vj.type_.begin(), vj.type_.end(), vj.type_.begin(), ::tolower);
if (vj.type_ != "planar" && vj.type_ != "floating" && vj.type_ != "fixed")
{
ROS_ERROR("Unknown type of joint: '%s'. Assuming 'fixed' instead. Other known types are 'planar' and 'floating'.", type);
vj.type_ = "fixed";
}
vj.name_ = std::string(jname);
vj.child_link_ = std::string(child);
vj.parent_frame_ = std::string(parent);
vj.name_ = std::string(jname); boost::trim(vj.name_);
vj.child_link_ = std::string(child); boost::trim(vj.child_link_);
vj.parent_frame_ = std::string(parent); boost::trim(vj.parent_frame_);
virtual_joints_.push_back(vj);
}
}
@ -101,7 +102,7 @@ void srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElemen
continue;
}
Group g;
g.name_ = std::string(gname);
g.name_ = std::string(gname); boost::trim(g.name_);
// get the links in the groups
for (TiXmlElement* link_xml = group_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
@ -112,7 +113,7 @@ void srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElemen
ROS_ERROR("Link name not specified");
continue;
}
std::string lname_str = std::string(lname);
std::string lname_str = boost::trim_copy(std::string(lname));
if (!urdf_model.getLink(lname_str))
{
ROS_ERROR("Link '%s' declared as part of group '%s' is not known to the URDF", lname, gname);
@ -130,7 +131,7 @@ void srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElemen
ROS_ERROR("Joint name not specified");
continue;
}
std::string jname_str = std::string(jname);
std::string jname_str = boost::trim_copy(std::string(jname));
if (!urdf_model.getJoint(jname_str))
{
bool missing = true;
@ -164,8 +165,8 @@ void srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElemen
ROS_ERROR("Tip link name not specified for chain");
continue;
}
std::string base_str = std::string(base);
std::string tip_str = std::string(tip);
std::string base_str = boost::trim_copy(std::string(base));
std::string tip_str = boost::trim_copy(std::string(tip));
if (!urdf_model.getLink(base_str))
{
ROS_ERROR("Link '%s' declared as part of a chain in group '%s' is not known to the URDF", base, gname);
@ -200,7 +201,7 @@ void srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElemen
ROS_ERROR("Group name not specified when included as subgroup");
continue;
}
g.subgroups_.push_back(std::string(sub));
g.subgroups_.push_back(boost::trim_copy(std::string(sub)));
}
if (g.links_.empty() && g.joints_.empty() && g.chains_.empty() && g.subgroups_.empty())
ROS_WARN("Group '%s' is empty.", gname);
@ -268,8 +269,8 @@ void srdf::Model::loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlE
}
GroupState gs;
gs.name_ = std::string(sname);
gs.group_ = std::string(gname);
gs.name_ = boost::trim_copy(std::string(sname));
gs.group_ = boost::trim_copy(std::string(gname));
bool found = false;
for (std::size_t k = 0 ; k < groups_.size() ; ++k)
@ -299,7 +300,7 @@ void srdf::Model::loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlE
ROS_ERROR("Joint name not specified for joint '%s' in group state '%s'", jname, sname);
continue;
}
std::string jname_str = std::string(jname);
std::string jname_str = boost::trim_copy(std::string(jname));
if (!urdf_model.getJoint(jname_str))
{
bool missing = true;
@ -355,8 +356,8 @@ void srdf::Model::loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXml
continue;
}
EndEffector e;
e.name_ = std::string(ename);
e.component_group_ = std::string(gname);
e.name_ = std::string(ename); boost::trim(e.name_);
e.component_group_ = std::string(gname); boost::trim(e.component_group_);
bool found = false;
for (std::size_t k = 0 ; k < groups_.size() ; ++k)
if (groups_[k].name_ == e.component_group_)
@ -374,7 +375,7 @@ void srdf::Model::loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXml
ROS_ERROR("Parent link not specified for end effector '%s'", ename);
continue;
}
e.parent_link_ = std::string(parent);
e.parent_link_ = std::string(parent); boost::trim(e.parent_link_);
if (!urdf_model.getLink(e.parent_link_))
{
ROS_ERROR("Link '%s' specified as parent for end effector '%s' is not known to the URDF", parent, ename);
@ -395,8 +396,8 @@ void srdf::Model::loadDisabledCollisions(const urdf::ModelInterface &urdf_model,
ROS_ERROR("A pair of links needs to be specified to disable collisions");
continue;
}
std::string link1_str = std::string(link1);
std::string link2_str = std::string(link2);
std::string link1_str = boost::trim_copy(std::string(link1));
std::string link2_str = boost::trim_copy(std::string(link2));
if (!urdf_model.getLink(link1_str))
{
ROS_ERROR("Link '%s' is not known to URDF. Cannot disable collisons.", link1);
@ -426,7 +427,7 @@ bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *
ROS_ERROR("No name given for the robot.");
else
{
name_ = std::string(name);
name_ = std::string(name); boost::trim(name_);
if (name_ != urdf_model.getName())
ROS_ERROR("Semantic description is not specified for the same robot as the URDF");
}