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

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

@ -48,161 +48,161 @@
namespace srdf namespace srdf
{ {
/** \brief Representation of semantic information about the robot */ /** \brief Representation of semantic information about the robot */
class Model class Model
{ {
public: public:
Model(void) Model(void)
{ {
} }
~Model(void) ~Model(void)
{ {
} }
/// \brief Load Model from TiXMLElement /// \brief Load Model from TiXMLElement
bool initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *xml); bool initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *xml);
/// \brief Load Model from TiXMLDocument /// \brief Load Model from TiXMLDocument
bool initXml(const urdf::ModelInterface &urdf_model, TiXmlDocument *xml); bool initXml(const urdf::ModelInterface &urdf_model, TiXmlDocument *xml);
/// \brief Load Model given a filename /// \brief Load Model given a filename
bool initFile(const urdf::ModelInterface &urdf_model, const std::string& filename); bool initFile(const urdf::ModelInterface &urdf_model, const std::string& filename);
/// \brief Load Model from a XML-string /// \brief Load Model from a XML-string
bool initString(const urdf::ModelInterface &urdf_model, const std::string& xmlstring); bool initString(const urdf::ModelInterface &urdf_model, const std::string& xmlstring);
/** \brief A group consists of a set of joints and the /** \brief A group consists of a set of joints and the
corresponding descendant links. There are multiple ways to corresponding descendant links. There are multiple ways to
specify a group. Directly specifying joints, links or specify a group. Directly specifying joints, links or
chains, or referring to other defined groups. */ chains, or referring to other defined groups. */
struct Group struct Group
{ {
/// The name of the group /// The name of the group
std::string name_; std::string name_;
/// Directly specified joints to be included in the /// Directly specified joints to be included in the
/// group. Descendent links should be implicitly /// group. Descendent links should be implicitly
/// considered to be part of the group, although this /// considered to be part of the group, although this
/// parsed does not add them to links_. The joints are /// parsed does not add them to links_. The joints are
/// checked to be in the corresponding URDF. /// checked to be in the corresponding URDF.
std::vector<std::string> joints_; std::vector<std::string> joints_;
/// Directly specified links to be included in the /// Directly specified links to be included in the
/// group. Parent joints should be implicitly considered /// group. Parent joints should be implicitly considered
/// to be part of the group. The links are checked to be /// to be part of the group. The links are checked to be
/// in the corresponding URDF. /// in the corresponding URDF.
std::vector<std::string> links_; std::vector<std::string> links_;
/// Specify a chain of links (and the implicit joints) to /// Specify a chain of links (and the implicit joints) to
/// be added to the group. Each chain is specified as a /// be added to the group. Each chain is specified as a
/// pair of base link and tip link. It is checked that the /// pair of base link and tip link. It is checked that the
/// chain is indeed a chain in the specified URDF. /// chain is indeed a chain in the specified URDF.
std::vector<std::pair<std::string, std::string> > chains_; std::vector<std::pair<std::string, std::string> > chains_;
/// It is sometimes convenient to refer to the content of /// It is sometimes convenient to refer to the content of
/// another group. A group can include the content of the /// another group. A group can include the content of the
/// referenced groups /// referenced groups
std::vector<std::string> subgroups_; std::vector<std::string> subgroups_;
}; };
/// In addition to the joints specified in the URDF it is /// In addition to the joints specified in the URDF it is
/// sometimes convenient to add special (virtual) joints. For /// sometimes convenient to add special (virtual) joints. For
/// example, to connect the robot to the environment in a /// example, to connect the robot to the environment in a
/// meaningful way. /// meaningful way.
struct VirtualJoint struct VirtualJoint
{ {
/// The name of the new joint /// The name of the new joint
std::string name_; std::string name_;
/// The type of this new joint. This can be "fixed" (0 DOF), "planar" (3 DOF: x,y,yaw) or "floating" (6DOF) /// The type of this new joint. This can be "fixed" (0 DOF), "planar" (3 DOF: x,y,yaw) or "floating" (6DOF)
std::string type_; std::string type_;
/// The transform applied by this joint to the robot model brings that model to a particular frame. /// The transform applied by this joint to the robot model brings that model to a particular frame.
std::string parent_frame_; std::string parent_frame_;
/// The link this joint applies to /// The link this joint applies to
std::string child_link_; std::string child_link_;
}; };
/// Representation of an end effector /// Representation of an end effector
struct EndEffector struct EndEffector
{ {
/// The name of the end effector /// The name of the end effector
std::string name_; std::string name_;
/// The name of the link this end effector connects to /// The name of the link this end effector connects to
std::string parent_link_; std::string parent_link_;
/// The name of the group that includes the joints & links this end effector consists of /// The name of the group that includes the joints & links this end effector consists of
std::string component_group_; std::string component_group_;
}; };
/// A named state for a particular group /// A named state for a particular group
struct GroupState struct GroupState
{ {
/// The name of the state /// The name of the state
std::string name_; std::string name_;
/// The name of the group this state is specified for /// The name of the group this state is specified for
std::string group_; std::string group_;
/// The values of joints for this state. Each joint can have a value. We use a vector for the 'value' to support multi-DOF joints /// The values of joints for this state. Each joint can have a value. We use a vector for the 'value' to support multi-DOF joints
std::map<std::string, std::vector<double> > joint_values_; std::map<std::string, std::vector<double> > joint_values_;
}; };
/// Get the name of this model /// Get the name of this model
const std::string& getName(void) const const std::string& getName(void) const
{ {
return name_; return name_;
} }
/// Get the list of pairs of links that need not be checked for collisions (because they can never touch given the geometry and kinematics of the robot) /// Get the list of pairs of links that need not be checked for collisions (because they can never touch given the geometry and kinematics of the robot)
const std::vector<std::pair<std::string, std::string> >& getDisabledCollisions(void) const const std::vector<std::pair<std::string, std::string> >& getDisabledCollisions(void) const
{ {
return disabled_collisions_; return disabled_collisions_;
} }
/// Get the list of groups defined for this model /// Get the list of groups defined for this model
const std::vector<Group>& getGroups(void) const const std::vector<Group>& getGroups(void) const
{ {
return groups_; return groups_;
} }
/// Get the list of virtual joints defined for this model /// Get the list of virtual joints defined for this model
const std::vector<VirtualJoint>& getVirtualJoints(void) const const std::vector<VirtualJoint>& getVirtualJoints(void) const
{ {
return virtual_joints_; return virtual_joints_;
} }
/// Get the list of end effectors defined for this model /// Get the list of end effectors defined for this model
const std::vector<EndEffector>& getEndEffectors(void) const const std::vector<EndEffector>& getEndEffectors(void) const
{ {
return end_effectors_; return end_effectors_;
} }
/// Get the list of group states defined for this model /// Get the list of group states defined for this model
const std::vector<GroupState>& getGroupStates(void) const const std::vector<GroupState>& getGroupStates(void) const
{ {
return group_states_; return group_states_;
} }
/// Clear the model /// Clear the model
void clear(void); void clear(void);
private: private:
void loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml); void loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
void loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml); void loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
void loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml); void loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
void loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml); void loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
void loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml); void loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
std::string name_; std::string name_;
std::vector<Group> groups_; std::vector<Group> groups_;
std::vector<GroupState> group_states_; std::vector<GroupState> group_states_;
std::vector<VirtualJoint> virtual_joints_; std::vector<VirtualJoint> virtual_joints_;
std::vector<EndEffector> end_effectors_; std::vector<EndEffector> end_effectors_;
std::vector<std::pair<std::string, std::string> > disabled_collisions_; std::vector<std::pair<std::string, std::string> > disabled_collisions_;
}; };
} }
#endif #endif

787
srdf/src/model.cpp
View File

@ -37,6 +37,7 @@
#include "srdf/model.h" #include "srdf/model.h"
#include <ros/console.h> #include <ros/console.h>
#include <boost/lexical_cast.hpp> #include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <algorithm> #include <algorithm>
#include <fstream> #include <fstream>
#include <sstream> #include <sstream>
@ -44,451 +45,451 @@
void srdf::Model::loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) void srdf::Model::loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{ {
for (TiXmlElement* vj_xml = robot_xml->FirstChildElement("virtual_joint"); vj_xml; vj_xml = vj_xml->NextSiblingElement("virtual_joint")) for (TiXmlElement* vj_xml = robot_xml->FirstChildElement("virtual_joint"); vj_xml; vj_xml = vj_xml->NextSiblingElement("virtual_joint"))
{
const char *jname = vj_xml->Attribute("name");
const char *child = vj_xml->Attribute("child_link");
const char *parent = vj_xml->Attribute("parent_frame");
const char *type = vj_xml->Attribute("type");
if (!jname)
{ {
const char *jname = vj_xml->Attribute("name"); ROS_ERROR("Name of virtual joint is not specified");
const char *child = vj_xml->Attribute("child_link"); continue;
const char *parent = vj_xml->Attribute("parent_frame");
const char *type = vj_xml->Attribute("type");
if (!jname)
{
ROS_ERROR("Name of virtual joint is not specified");
continue;
}
if (!child)
{
ROS_ERROR("Child link of virtual joint is not specified");
continue;
}
if (!urdf_model.getLink(std::string(child)))
{
ROS_ERROR("Virtual joint does not attach to a link on the robot (link '%s' is not known)", child);
continue;
}
if (!parent)
{
ROS_ERROR("Parent frame of virtual joint is not specified");
continue;
}
if (!type)
{
ROS_ERROR("Type of virtual joint is not specified");
continue;
}
VirtualJoint vj;
vj.type_ = std::string(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);
virtual_joints_.push_back(vj);
} }
if (!child)
{
ROS_ERROR("Child link of virtual joint is not specified");
continue;
}
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;
}
if (!parent)
{
ROS_ERROR("Parent frame of virtual joint is not specified");
continue;
}
if (!type)
{
ROS_ERROR("Type of virtual joint is not specified");
continue;
}
VirtualJoint vj;
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); 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);
}
} }
void srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) void srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{ {
for (TiXmlElement* group_xml = robot_xml->FirstChildElement("group"); group_xml; group_xml = group_xml->NextSiblingElement("group")) for (TiXmlElement* group_xml = robot_xml->FirstChildElement("group"); group_xml; group_xml = group_xml->NextSiblingElement("group"))
{
const char *gname = group_xml->Attribute("name");
if (!gname)
{ {
const char *gname = group_xml->Attribute("name"); ROS_ERROR("Group name not specified");
if (!gname) continue;
{
ROS_ERROR("Group name not specified");
continue;
}
Group g;
g.name_ = std::string(gname);
// get the links in the groups
for (TiXmlElement* link_xml = group_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{
const char *lname = link_xml->Attribute("name");
if (!lname)
{
ROS_ERROR("Link name not specified");
continue;
}
std::string lname_str = 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);
continue;
}
g.links_.push_back(lname_str);
}
// get the joints in the groups
for (TiXmlElement* joint_xml = group_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
const char *jname = joint_xml->Attribute("name");
if (!jname)
{
ROS_ERROR("Joint name not specified");
continue;
}
std::string jname_str = std::string(jname);
if (!urdf_model.getJoint(jname_str))
{
bool missing = true;
for (std::size_t k = 0 ; k < virtual_joints_.size() ; ++k)
if (virtual_joints_[k].name_ == jname_str)
{
missing = false;
break;
}
if (missing)
{
ROS_ERROR("Joint '%s' declared as part of group '%s' is not known to the URDF", jname, gname);
continue;
}
}
g.joints_.push_back(jname_str);
}
// get the chains in the groups
for (TiXmlElement* chain_xml = group_xml->FirstChildElement("chain"); chain_xml; chain_xml = chain_xml->NextSiblingElement("chain"))
{
const char *base = chain_xml->Attribute("base_link");
const char *tip = chain_xml->Attribute("tip_link");
if (!base)
{
ROS_ERROR("Base link name not specified for chain");
continue;
}
if (!tip)
{
ROS_ERROR("Tip link name not specified for chain");
continue;
}
std::string base_str = std::string(base);
std::string tip_str = 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);
continue;
}
if (!urdf_model.getLink(tip_str))
{
ROS_ERROR("Link '%s' declared as part of a chain in group '%s' is not known to the URDF", tip, gname);
continue;
}
bool found = false;
boost::shared_ptr<const urdf::Link> l = urdf_model.getLink(tip_str);
while (!found && l)
{
if (l->name == base_str)
found = true;
else
l = l->getParent();
}
if (found)
g.chains_.push_back(std::make_pair(base_str, tip_str));
else
ROS_ERROR("Links '%s' and '%s' do not form a chain. Not included in group '%s'", base, tip, gname);
}
// get the subgroups in the groups
for (TiXmlElement* subg_xml = group_xml->FirstChildElement("group"); subg_xml; subg_xml = subg_xml->NextSiblingElement("group"))
{
const char *sub = subg_xml->Attribute("name");
if (!sub)
{
ROS_ERROR("Group name not specified when included as subgroup");
continue;
}
g.subgroups_.push_back(std::string(sub));
}
if (g.links_.empty() && g.joints_.empty() && g.chains_.empty() && g.subgroups_.empty())
ROS_WARN("Group '%s' is empty.", gname);
groups_.push_back(g);
} }
Group g;
// check the subgroups g.name_ = std::string(gname); boost::trim(g.name_);
std::set<std::string> known_groups;
bool update = true; // get the links in the groups
while (update) for (TiXmlElement* link_xml = group_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{ {
update = false; const char *lname = link_xml->Attribute("name");
for (std::size_t i = 0 ; i < groups_.size() ; ++i) if (!lname)
{ {
if (known_groups.find(groups_[i].name_) != known_groups.end()) ROS_ERROR("Link name not specified");
continue; continue;
if (groups_[i].subgroups_.empty()) }
{ std::string lname_str = boost::trim_copy(std::string(lname));
known_groups.insert(groups_[i].name_); if (!urdf_model.getLink(lname_str))
update = true; {
} ROS_ERROR("Link '%s' declared as part of group '%s' is not known to the URDF", lname, gname);
else continue;
{ }
bool ok = true; g.links_.push_back(lname_str);
for (std::size_t j = 0 ; ok && j < groups_[i].subgroups_.size() ; ++j)
if (known_groups.find(groups_[i].subgroups_[j]) == known_groups.end())
ok = false;
if (ok)
{
known_groups.insert(groups_[i].name_);
update = true;
}
}
}
} }
// if there are erroneous groups, keep only the valid ones // get the joints in the groups
if (known_groups.size() != groups_.size()) for (TiXmlElement* joint_xml = group_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{ {
std::vector<Group> correct; const char *jname = joint_xml->Attribute("name");
for (std::size_t i = 0 ; i < groups_.size() ; ++i) if (!jname)
if (known_groups.find(groups_[i].name_) != known_groups.end()) {
correct.push_back(groups_[i]); ROS_ERROR("Joint name not specified");
else continue;
ROS_ERROR("Group '%s' has unsatisfied subgroups", groups_[i].name_.c_str()); }
groups_.swap(correct); std::string jname_str = boost::trim_copy(std::string(jname));
if (!urdf_model.getJoint(jname_str))
{
bool missing = true;
for (std::size_t k = 0 ; k < virtual_joints_.size() ; ++k)
if (virtual_joints_[k].name_ == jname_str)
{
missing = false;
break;
}
if (missing)
{
ROS_ERROR("Joint '%s' declared as part of group '%s' is not known to the URDF", jname, gname);
continue;
}
}
g.joints_.push_back(jname_str);
} }
// get the chains in the groups
for (TiXmlElement* chain_xml = group_xml->FirstChildElement("chain"); chain_xml; chain_xml = chain_xml->NextSiblingElement("chain"))
{
const char *base = chain_xml->Attribute("base_link");
const char *tip = chain_xml->Attribute("tip_link");
if (!base)
{
ROS_ERROR("Base link name not specified for chain");
continue;
}
if (!tip)
{
ROS_ERROR("Tip link name not specified for chain");
continue;
}
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);
continue;
}
if (!urdf_model.getLink(tip_str))
{
ROS_ERROR("Link '%s' declared as part of a chain in group '%s' is not known to the URDF", tip, gname);
continue;
}
bool found = false;
boost::shared_ptr<const urdf::Link> l = urdf_model.getLink(tip_str);
while (!found && l)
{
if (l->name == base_str)
found = true;
else
l = l->getParent();
}
if (found)
g.chains_.push_back(std::make_pair(base_str, tip_str));
else
ROS_ERROR("Links '%s' and '%s' do not form a chain. Not included in group '%s'", base, tip, gname);
}
// get the subgroups in the groups
for (TiXmlElement* subg_xml = group_xml->FirstChildElement("group"); subg_xml; subg_xml = subg_xml->NextSiblingElement("group"))
{
const char *sub = subg_xml->Attribute("name");
if (!sub)
{
ROS_ERROR("Group name not specified when included as subgroup");
continue;
}
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);
groups_.push_back(g);
}
// check the subgroups
std::set<std::string> known_groups;
bool update = true;
while (update)
{
update = false;
for (std::size_t i = 0 ; i < groups_.size() ; ++i)
{
if (known_groups.find(groups_[i].name_) != known_groups.end())
continue;
if (groups_[i].subgroups_.empty())
{
known_groups.insert(groups_[i].name_);
update = true;
}
else
{
bool ok = true;
for (std::size_t j = 0 ; ok && j < groups_[i].subgroups_.size() ; ++j)
if (known_groups.find(groups_[i].subgroups_[j]) == known_groups.end())
ok = false;
if (ok)
{
known_groups.insert(groups_[i].name_);
update = true;
}
}
}
}
// if there are erroneous groups, keep only the valid ones
if (known_groups.size() != groups_.size())
{
std::vector<Group> correct;
for (std::size_t i = 0 ; i < groups_.size() ; ++i)
if (known_groups.find(groups_[i].name_) != known_groups.end())
correct.push_back(groups_[i]);
else
ROS_ERROR("Group '%s' has unsatisfied subgroups", groups_[i].name_.c_str());
groups_.swap(correct);
}
} }
void srdf::Model::loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) void srdf::Model::loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{ {
for (TiXmlElement* gstate_xml = robot_xml->FirstChildElement("group_state"); gstate_xml; gstate_xml = gstate_xml->NextSiblingElement("group_state")) for (TiXmlElement* gstate_xml = robot_xml->FirstChildElement("group_state"); gstate_xml; gstate_xml = gstate_xml->NextSiblingElement("group_state"))
{
const char *sname = gstate_xml->Attribute("name");
const char *gname = gstate_xml->Attribute("group");
if (!sname)
{ {
const char *sname = gstate_xml->Attribute("name"); ROS_ERROR("Name of group state is not specified");
const char *gname = gstate_xml->Attribute("group"); continue;
if (!sname)
{
ROS_ERROR("Name of group state is not specified");
continue;
}
if (!gname)
{
ROS_ERROR("Name of group for state '%s' is not specified", sname);
continue;
}
GroupState gs;
gs.name_ = std::string(sname);
gs.group_ = std::string(gname);
bool found = false;
for (std::size_t k = 0 ; k < groups_.size() ; ++k)
if (groups_[k].name_ == gs.group_)
{
found = true;
break;
}
if (!found)
{
ROS_ERROR("Group state '%s' specified for group '%s', but that group is not known", sname, gname);
continue;
}
// get the joint values in the group state
for (TiXmlElement* joint_xml = gstate_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
const char *jname = joint_xml->Attribute("name");
const char *jval = joint_xml->Attribute("value");
if (!jname)
{
ROS_ERROR("Joint name not specified in group state '%s'", sname);
continue;
}
if (!jval)
{
ROS_ERROR("Joint name not specified for joint '%s' in group state '%s'", jname, sname);
continue;
}
std::string jname_str = std::string(jname);
if (!urdf_model.getJoint(jname_str))
{
bool missing = true;
for (std::size_t k = 0 ; k < virtual_joints_.size() ; ++k)
if (virtual_joints_[k].name_ == jname_str)
{
missing = false;
break;
}
if (missing)
{
ROS_ERROR("Joint '%s' declared as part of group state '%s' is not known to the URDF", jname, sname);
continue;
}
}
try
{
std::string jval_str = std::string(jval);
std::stringstream ss(jval_str);
while (ss.good() && !ss.eof())
{
std::string val; ss >> val >> std::ws;
gs.joint_values_[jname_str].push_back(boost::lexical_cast<double>(val));
}
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("Unable to parse joint value '%s'", jval);
}
if (gs.joint_values_.empty())
ROS_ERROR("Unable to parse joint value ('%s') for joint '%s' in group state '%s'", jval, jname, sname);
}
group_states_.push_back(gs);
} }
if (!gname)
{
ROS_ERROR("Name of group for state '%s' is not specified", sname);
continue;
}
GroupState gs;
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)
if (groups_[k].name_ == gs.group_)
{
found = true;
break;
}
if (!found)
{
ROS_ERROR("Group state '%s' specified for group '%s', but that group is not known", sname, gname);
continue;
}
// get the joint values in the group state
for (TiXmlElement* joint_xml = gstate_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
const char *jname = joint_xml->Attribute("name");
const char *jval = joint_xml->Attribute("value");
if (!jname)
{
ROS_ERROR("Joint name not specified in group state '%s'", sname);
continue;
}
if (!jval)
{
ROS_ERROR("Joint name not specified for joint '%s' in group state '%s'", jname, sname);
continue;
}
std::string jname_str = boost::trim_copy(std::string(jname));
if (!urdf_model.getJoint(jname_str))
{
bool missing = true;
for (std::size_t k = 0 ; k < virtual_joints_.size() ; ++k)
if (virtual_joints_[k].name_ == jname_str)
{
missing = false;
break;
}
if (missing)
{
ROS_ERROR("Joint '%s' declared as part of group state '%s' is not known to the URDF", jname, sname);
continue;
}
}
try
{
std::string jval_str = std::string(jval);
std::stringstream ss(jval_str);
while (ss.good() && !ss.eof())
{
std::string val; ss >> val >> std::ws;
gs.joint_values_[jname_str].push_back(boost::lexical_cast<double>(val));
}
}
catch (boost::bad_lexical_cast &e)
{
ROS_ERROR("Unable to parse joint value '%s'", jval);
}
if (gs.joint_values_.empty())
ROS_ERROR("Unable to parse joint value ('%s') for joint '%s' in group state '%s'", jval, jname, sname);
}
group_states_.push_back(gs);
}
} }
void srdf::Model::loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) void srdf::Model::loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{ {
for (TiXmlElement* eef_xml = robot_xml->FirstChildElement("end_effector"); eef_xml; eef_xml = eef_xml->NextSiblingElement("end_effector")) for (TiXmlElement* eef_xml = robot_xml->FirstChildElement("end_effector"); eef_xml; eef_xml = eef_xml->NextSiblingElement("end_effector"))
{
const char *ename = eef_xml->Attribute("name");
const char *gname = eef_xml->Attribute("group");
const char *parent = eef_xml->Attribute("parent_link");
if (!ename)
{ {
const char *ename = eef_xml->Attribute("name"); ROS_ERROR("Name of end effector is not specified");
const char *gname = eef_xml->Attribute("group"); continue;
const char *parent = eef_xml->Attribute("parent_link");
if (!ename)
{
ROS_ERROR("Name of end effector is not specified");
continue;
}
if (!gname)
{
ROS_ERROR("Group not specified for end effector '%s'", ename);
continue;
}
EndEffector e;
e.name_ = std::string(ename);
e.component_group_ = std::string(gname);
bool found = false;
for (std::size_t k = 0 ; k < groups_.size() ; ++k)
if (groups_[k].name_ == e.component_group_)
{
found = true;
break;
}
if (!found)
{
ROS_ERROR("End effector '%s' specified for group '%s', but that group is not known", ename, gname);
continue;
}
if (!parent)
{
ROS_ERROR("Parent link not specified for end effector '%s'", ename);
continue;
}
e.parent_link_ = std::string(parent);
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);
continue;
}
end_effectors_.push_back(e);
} }
if (!gname)
{
ROS_ERROR("Group not specified for end effector '%s'", ename);
continue;
}
EndEffector e;
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_)
{
found = true;
break;
}
if (!found)
{
ROS_ERROR("End effector '%s' specified for group '%s', but that group is not known", ename, gname);
continue;
}
if (!parent)
{
ROS_ERROR("Parent link not specified for end effector '%s'", ename);
continue;
}
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);
continue;
}
end_effectors_.push_back(e);
}
} }
void srdf::Model::loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) void srdf::Model::loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{ {
for (TiXmlElement* c_xml = robot_xml->FirstChildElement("disable_collisions"); c_xml; c_xml = c_xml->NextSiblingElement("disable_collisions")) for (TiXmlElement* c_xml = robot_xml->FirstChildElement("disable_collisions"); c_xml; c_xml = c_xml->NextSiblingElement("disable_collisions"))
{
const char *link1 = c_xml->Attribute("link1");
const char *link2 = c_xml->Attribute("link2");
if (!link1 || !link2)
{ {
const char *link1 = c_xml->Attribute("link1"); ROS_ERROR("A pair of links needs to be specified to disable collisions");
const char *link2 = c_xml->Attribute("link2"); continue;
if (!link1 || !link2)
{
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);
if (!urdf_model.getLink(link1_str))
{
ROS_ERROR("Link '%s' is not known to URDF. Cannot disable collisons.", link1);
continue;
}
if (!urdf_model.getLink(link2_str))
{
ROS_ERROR("Link '%s' is not known to URDF. Cannot disable collisons.", link2);
continue;
}
disabled_collisions_.push_back(std::make_pair(link1_str, link2_str));
} }
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);
continue;
}
if (!urdf_model.getLink(link2_str))
{
ROS_ERROR("Link '%s' is not known to URDF. Cannot disable collisons.", link2);
continue;
}
disabled_collisions_.push_back(std::make_pair(link1_str, link2_str));
}
} }
bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{ {
clear(); clear();
if (!robot_xml || robot_xml->ValueStr() != "robot") if (!robot_xml || robot_xml->ValueStr() != "robot")
{ {
ROS_ERROR("Could not find the 'robot' element in the xml file"); ROS_ERROR("Could not find the 'robot' element in the xml file");
return false; return false;
} }
// get the robot name // get the robot name
const char *name = robot_xml->Attribute("name"); const char *name = robot_xml->Attribute("name");
if (!name) if (!name)
ROS_ERROR("No name given for the robot."); ROS_ERROR("No name given for the robot.");
else else
{ {
name_ = std::string(name); name_ = std::string(name); boost::trim(name_);
if (name_ != urdf_model.getName()) if (name_ != urdf_model.getName())
ROS_ERROR("Semantic description is not specified for the same robot as the URDF"); ROS_ERROR("Semantic description is not specified for the same robot as the URDF");
} }
loadVirtualJoints(urdf_model, robot_xml); loadVirtualJoints(urdf_model, robot_xml);
loadGroups(urdf_model, robot_xml); loadGroups(urdf_model, robot_xml);
loadGroupStates(urdf_model, robot_xml); loadGroupStates(urdf_model, robot_xml);
loadEndEffectors(urdf_model, robot_xml); loadEndEffectors(urdf_model, robot_xml);
loadDisabledCollisions(urdf_model, robot_xml); loadDisabledCollisions(urdf_model, robot_xml);
return true; return true;
} }
bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlDocument *xml) bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlDocument *xml)
{ {
TiXmlElement *robot_xml = xml ? xml->FirstChildElement("robot") : NULL; TiXmlElement *robot_xml = xml ? xml->FirstChildElement("robot") : NULL;
if (!robot_xml) if (!robot_xml)
{ {
ROS_ERROR("Could not find the 'robot' element in the xml file"); ROS_ERROR("Could not find the 'robot' element in the xml file");
return false; return false;
} }
return initXml(urdf_model, robot_xml); return initXml(urdf_model, robot_xml);
} }
bool srdf::Model::initFile(const urdf::ModelInterface &urdf_model, const std::string& filename) bool srdf::Model::initFile(const urdf::ModelInterface &urdf_model, const std::string& filename)
{ {
// get the entire file // get the entire file
std::string xml_string; std::string xml_string;
std::fstream xml_file(filename.c_str(), std::fstream::in); std::fstream xml_file(filename.c_str(), std::fstream::in);
if (xml_file.is_open()) if (xml_file.is_open())
{
while (xml_file.good())
{ {
while (xml_file.good()) std::string line;
{ std::getline(xml_file, line);
std::string line; xml_string += (line + "\n");
std::getline(xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
return initString(urdf_model, xml_string);
}
else
{
ROS_ERROR("Could not open file [%s] for parsing.", filename.c_str());
return false;
} }
xml_file.close();
return initString(urdf_model, xml_string);
}
else
{
ROS_ERROR("Could not open file [%s] for parsing.", filename.c_str());
return false;
}
} }
bool srdf::Model::initString(const urdf::ModelInterface &urdf_model, const std::string& xmlstring) bool srdf::Model::initString(const urdf::ModelInterface &urdf_model, const std::string& xmlstring)
{ {
TiXmlDocument xml_doc; TiXmlDocument xml_doc;
xml_doc.Parse(xmlstring.c_str()); xml_doc.Parse(xmlstring.c_str());
return initXml(urdf_model, &xml_doc); return initXml(urdf_model, &xml_doc);
} }
void srdf::Model::clear(void) void srdf::Model::clear(void)
{ {
name_ = ""; name_ = "";
groups_.clear(); groups_.clear();
group_states_.clear(); group_states_.clear();
virtual_joints_.clear(); virtual_joints_.clear();
end_effectors_.clear(); end_effectors_.clear();
disabled_collisions_.clear(); disabled_collisions_.clear();
} }