make parsing more robust to white space in attribute values
This commit is contained in:
Ioan Sucan
parent
9d368b1a42
commit
e29657e464
|
|
@ -48,161 +48,161 @@
|
|||
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)
|
||||
{
|
||||
}
|
||||
Model(void)
|
||||
{
|
||||
}
|
||||
|
||||
~Model(void)
|
||||
{
|
||||
}
|
||||
~Model(void)
|
||||
{
|
||||
}
|
||||
|
||||
/// \brief Load Model from TiXMLElement
|
||||
bool initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *xml);
|
||||
/// \brief Load Model from TiXMLDocument
|
||||
bool initXml(const urdf::ModelInterface &urdf_model, TiXmlDocument *xml);
|
||||
/// \brief Load Model given a filename
|
||||
bool initFile(const urdf::ModelInterface &urdf_model, const std::string& filename);
|
||||
/// \brief Load Model from a XML-string
|
||||
bool initString(const urdf::ModelInterface &urdf_model, const std::string& xmlstring);
|
||||
/// \brief Load Model from TiXMLElement
|
||||
bool initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *xml);
|
||||
/// \brief Load Model from TiXMLDocument
|
||||
bool initXml(const urdf::ModelInterface &urdf_model, TiXmlDocument *xml);
|
||||
/// \brief Load Model given a filename
|
||||
bool initFile(const urdf::ModelInterface &urdf_model, const std::string& filename);
|
||||
/// \brief Load Model from a XML-string
|
||||
bool initString(const urdf::ModelInterface &urdf_model, const std::string& xmlstring);
|
||||
|
||||
/** \brief A group consists of a set of joints and the
|
||||
corresponding descendant links. There are multiple ways to
|
||||
specify a group. Directly specifying joints, links or
|
||||
chains, or referring to other defined groups. */
|
||||
struct Group
|
||||
{
|
||||
/// The name of the group
|
||||
std::string name_;
|
||||
/** \brief A group consists of a set of joints and the
|
||||
corresponding descendant links. There are multiple ways to
|
||||
specify a group. Directly specifying joints, links or
|
||||
chains, or referring to other defined groups. */
|
||||
struct Group
|
||||
{
|
||||
/// The name of the group
|
||||
std::string name_;
|
||||
|
||||
/// Directly specified joints to be included in the
|
||||
/// group. Descendent links should be implicitly
|
||||
/// considered to be part of the group, although this
|
||||
/// parsed does not add them to links_. The joints are
|
||||
/// checked to be in the corresponding URDF.
|
||||
std::vector<std::string> joints_;
|
||||
/// Directly specified joints to be included in the
|
||||
/// group. Descendent links should be implicitly
|
||||
/// considered to be part of the group, although this
|
||||
/// parsed does not add them to links_. The joints are
|
||||
/// checked to be in the corresponding URDF.
|
||||
std::vector<std::string> joints_;
|
||||
|
||||
/// Directly specified links to be included in the
|
||||
/// group. Parent joints should be implicitly considered
|
||||
/// to be part of the group. The links are checked to be
|
||||
/// in the corresponding URDF.
|
||||
std::vector<std::string> links_;
|
||||
/// Directly specified links to be included in the
|
||||
/// group. Parent joints should be implicitly considered
|
||||
/// to be part of the group. The links are checked to be
|
||||
/// in the corresponding URDF.
|
||||
std::vector<std::string> links_;
|
||||
|
||||
/// Specify a chain of links (and the implicit joints) to
|
||||
/// be added to the group. Each chain is specified as a
|
||||
/// pair of base link and tip link. It is checked that the
|
||||
/// chain is indeed a chain in the specified URDF.
|
||||
std::vector<std::pair<std::string, std::string> > chains_;
|
||||
/// Specify a chain of links (and the implicit joints) to
|
||||
/// be added to the group. Each chain is specified as a
|
||||
/// pair of base link and tip link. It is checked that the
|
||||
/// chain is indeed a chain in the specified URDF.
|
||||
std::vector<std::pair<std::string, std::string> > chains_;
|
||||
|
||||
/// It is sometimes convenient to refer to the content of
|
||||
/// another group. A group can include the content of the
|
||||
/// referenced groups
|
||||
std::vector<std::string> subgroups_;
|
||||
};
|
||||
/// It is sometimes convenient to refer to the content of
|
||||
/// another group. A group can include the content of the
|
||||
/// referenced groups
|
||||
std::vector<std::string> subgroups_;
|
||||
};
|
||||
|
||||
/// In addition to the joints specified in the URDF it is
|
||||
/// sometimes convenient to add special (virtual) joints. For
|
||||
/// example, to connect the robot to the environment in a
|
||||
/// meaningful way.
|
||||
struct VirtualJoint
|
||||
{
|
||||
/// The name of the new joint
|
||||
std::string name_;
|
||||
/// In addition to the joints specified in the URDF it is
|
||||
/// sometimes convenient to add special (virtual) joints. For
|
||||
/// example, to connect the robot to the environment in a
|
||||
/// meaningful way.
|
||||
struct VirtualJoint
|
||||
{
|
||||
/// The name of the new joint
|
||||
std::string name_;
|
||||
|
||||
/// The type of this new joint. This can be "fixed" (0 DOF), "planar" (3 DOF: x,y,yaw) or "floating" (6DOF)
|
||||
std::string type_;
|
||||
/// The type of this new joint. This can be "fixed" (0 DOF), "planar" (3 DOF: x,y,yaw) or "floating" (6DOF)
|
||||
std::string type_;
|
||||
|
||||
/// The transform applied by this joint to the robot model brings that model to a particular frame.
|
||||
std::string parent_frame_;
|
||||
/// The transform applied by this joint to the robot model brings that model to a particular frame.
|
||||
std::string parent_frame_;
|
||||
|
||||
/// The link this joint applies to
|
||||
std::string child_link_;
|
||||
};
|
||||
/// The link this joint applies to
|
||||
std::string child_link_;
|
||||
};
|
||||
|
||||
/// Representation of an end effector
|
||||
struct EndEffector
|
||||
{
|
||||
/// The name of the end effector
|
||||
std::string name_;
|
||||
/// Representation of an end effector
|
||||
struct EndEffector
|
||||
{
|
||||
/// The name of the end effector
|
||||
std::string name_;
|
||||
|
||||
/// The name of the link this end effector connects to
|
||||
std::string parent_link_;
|
||||
/// The name of the link this end effector connects to
|
||||
std::string parent_link_;
|
||||
|
||||
/// The name of the group that includes the joints & links this end effector consists of
|
||||
std::string component_group_;
|
||||
};
|
||||
/// The name of the group that includes the joints & links this end effector consists of
|
||||
std::string component_group_;
|
||||
};
|
||||
|
||||
/// A named state for a particular group
|
||||
struct GroupState
|
||||
{
|
||||
/// The name of the state
|
||||
std::string name_;
|
||||
/// A named state for a particular group
|
||||
struct GroupState
|
||||
{
|
||||
/// The name of the state
|
||||
std::string name_;
|
||||
|
||||
/// The name of the group this state is specified for
|
||||
std::string group_;
|
||||
/// The name of the group this state is specified for
|
||||
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
|
||||
std::map<std::string, std::vector<double> > joint_values_;
|
||||
};
|
||||
/// 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_;
|
||||
};
|
||||
|
||||
/// Get the name of this model
|
||||
const std::string& getName(void) const
|
||||
{
|
||||
return name_;
|
||||
}
|
||||
/// Get the name of this model
|
||||
const std::string& getName(void) const
|
||||
{
|
||||
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)
|
||||
const std::vector<std::pair<std::string, std::string> >& getDisabledCollisions(void) const
|
||||
{
|
||||
return disabled_collisions_;
|
||||
}
|
||||
/// 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
|
||||
{
|
||||
return disabled_collisions_;
|
||||
}
|
||||
|
||||
/// Get the list of groups defined for this model
|
||||
const std::vector<Group>& getGroups(void) const
|
||||
{
|
||||
return groups_;
|
||||
}
|
||||
/// Get the list of groups defined for this model
|
||||
const std::vector<Group>& getGroups(void) const
|
||||
{
|
||||
return groups_;
|
||||
}
|
||||
|
||||
/// Get the list of virtual joints defined for this model
|
||||
const std::vector<VirtualJoint>& getVirtualJoints(void) const
|
||||
{
|
||||
return virtual_joints_;
|
||||
}
|
||||
/// Get the list of virtual joints defined for this model
|
||||
const std::vector<VirtualJoint>& getVirtualJoints(void) const
|
||||
{
|
||||
return virtual_joints_;
|
||||
}
|
||||
|
||||
/// Get the list of end effectors defined for this model
|
||||
const std::vector<EndEffector>& getEndEffectors(void) const
|
||||
{
|
||||
return end_effectors_;
|
||||
}
|
||||
/// Get the list of end effectors defined for this model
|
||||
const std::vector<EndEffector>& getEndEffectors(void) const
|
||||
{
|
||||
return end_effectors_;
|
||||
}
|
||||
|
||||
/// Get the list of group states defined for this model
|
||||
const std::vector<GroupState>& getGroupStates(void) const
|
||||
{
|
||||
return group_states_;
|
||||
}
|
||||
/// Get the list of group states defined for this model
|
||||
const std::vector<GroupState>& getGroupStates(void) const
|
||||
{
|
||||
return group_states_;
|
||||
}
|
||||
|
||||
/// Clear the model
|
||||
void clear(void);
|
||||
/// 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);
|
||||
void loadGroupStates(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 loadVirtualJoints(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 loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
|
||||
void loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
|
||||
|
||||
std::string name_;
|
||||
std::vector<Group> groups_;
|
||||
std::vector<GroupState> group_states_;
|
||||
std::vector<VirtualJoint> virtual_joints_;
|
||||
std::vector<EndEffector> end_effectors_;
|
||||
std::vector<std::pair<std::string, std::string> > disabled_collisions_;
|
||||
};
|
||||
std::string name_;
|
||||
std::vector<Group> groups_;
|
||||
std::vector<GroupState> group_states_;
|
||||
std::vector<VirtualJoint> virtual_joints_;
|
||||
std::vector<EndEffector> end_effectors_;
|
||||
std::vector<std::pair<std::string, std::string> > disabled_collisions_;
|
||||
};
|
||||
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
@ -44,451 +45,451 @@
|
|||
|
||||
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");
|
||||
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)
|
||||
{
|
||||
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);
|
||||
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(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)
|
||||
{
|
||||
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");
|
||||
if (!gname)
|
||||
{
|
||||
ROS_ERROR("Group name not specified");
|
||||
continue;
|
||||
}
|
||||
Group g;
|
||||
g.name_ = std::string(gname);
|
||||
ROS_ERROR("Group name not specified");
|
||||
continue;
|
||||
}
|
||||
Group g;
|
||||
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"))
|
||||
{
|
||||
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);
|
||||
// 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 = 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);
|
||||
continue;
|
||||
}
|
||||
g.links_.push_back(lname_str);
|
||||
}
|
||||
|
||||
// check the subgroups
|
||||
std::set<std::string> known_groups;
|
||||
bool update = true;
|
||||
while (update)
|
||||
// get the joints in the groups
|
||||
for (TiXmlElement* joint_xml = group_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
|
||||
{
|
||||
update = false;
|
||||
for (std::size_t i = 0 ; i < groups_.size() ; ++i)
|
||||
const char *jname = joint_xml->Attribute("name");
|
||||
if (!jname)
|
||||
{
|
||||
ROS_ERROR("Joint name not specified");
|
||||
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)
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
// if there are erroneous groups, keep only the valid ones
|
||||
if (known_groups.size() != groups_.size())
|
||||
// get the chains in the groups
|
||||
for (TiXmlElement* chain_xml = group_xml->FirstChildElement("chain"); chain_xml; chain_xml = chain_xml->NextSiblingElement("chain"))
|
||||
{
|
||||
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);
|
||||
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)
|
||||
{
|
||||
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");
|
||||
const char *gname = gstate_xml->Attribute("group");
|
||||
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);
|
||||
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_ = 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)
|
||||
{
|
||||
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");
|
||||
const char *gname = eef_xml->Attribute("group");
|
||||
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);
|
||||
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); 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)
|
||||
{
|
||||
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");
|
||||
const char *link2 = c_xml->Attribute("link2");
|
||||
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));
|
||||
ROS_ERROR("A pair of links needs to be specified to disable collisions");
|
||||
continue;
|
||||
}
|
||||
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)
|
||||
{
|
||||
clear();
|
||||
if (!robot_xml || robot_xml->ValueStr() != "robot")
|
||||
{
|
||||
ROS_ERROR("Could not find the 'robot' element in the xml file");
|
||||
return false;
|
||||
}
|
||||
clear();
|
||||
if (!robot_xml || robot_xml->ValueStr() != "robot")
|
||||
{
|
||||
ROS_ERROR("Could not find the 'robot' element in the xml file");
|
||||
return false;
|
||||
}
|
||||
|
||||
// get the robot name
|
||||
const char *name = robot_xml->Attribute("name");
|
||||
if (!name)
|
||||
ROS_ERROR("No name given for the robot.");
|
||||
else
|
||||
{
|
||||
name_ = std::string(name);
|
||||
if (name_ != urdf_model.getName())
|
||||
ROS_ERROR("Semantic description is not specified for the same robot as the URDF");
|
||||
}
|
||||
// get the robot name
|
||||
const char *name = robot_xml->Attribute("name");
|
||||
if (!name)
|
||||
ROS_ERROR("No name given for the robot.");
|
||||
else
|
||||
{
|
||||
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");
|
||||
}
|
||||
|
||||
loadVirtualJoints(urdf_model, robot_xml);
|
||||
loadGroups(urdf_model, robot_xml);
|
||||
loadGroupStates(urdf_model, robot_xml);
|
||||
loadEndEffectors(urdf_model, robot_xml);
|
||||
loadDisabledCollisions(urdf_model, robot_xml);
|
||||
loadVirtualJoints(urdf_model, robot_xml);
|
||||
loadGroups(urdf_model, robot_xml);
|
||||
loadGroupStates(urdf_model, robot_xml);
|
||||
loadEndEffectors(urdf_model, robot_xml);
|
||||
loadDisabledCollisions(urdf_model, robot_xml);
|
||||
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlDocument *xml)
|
||||
{
|
||||
TiXmlElement *robot_xml = xml ? xml->FirstChildElement("robot") : NULL;
|
||||
if (!robot_xml)
|
||||
{
|
||||
ROS_ERROR("Could not find the 'robot' element in the xml file");
|
||||
return false;
|
||||
}
|
||||
return initXml(urdf_model, robot_xml);
|
||||
TiXmlElement *robot_xml = xml ? xml->FirstChildElement("robot") : NULL;
|
||||
if (!robot_xml)
|
||||
{
|
||||
ROS_ERROR("Could not find the 'robot' element in the xml file");
|
||||
return false;
|
||||
}
|
||||
return initXml(urdf_model, robot_xml);
|
||||
}
|
||||
|
||||
|
||||
bool srdf::Model::initFile(const urdf::ModelInterface &urdf_model, const std::string& filename)
|
||||
{
|
||||
// get the entire file
|
||||
std::string xml_string;
|
||||
std::fstream xml_file(filename.c_str(), std::fstream::in);
|
||||
if (xml_file.is_open())
|
||||
// get the entire file
|
||||
std::string xml_string;
|
||||
std::fstream xml_file(filename.c_str(), std::fstream::in);
|
||||
if (xml_file.is_open())
|
||||
{
|
||||
while (xml_file.good())
|
||||
{
|
||||
while (xml_file.good())
|
||||
{
|
||||
std::string line;
|
||||
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;
|
||||
std::string line;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
bool srdf::Model::initString(const urdf::ModelInterface &urdf_model, const std::string& xmlstring)
|
||||
{
|
||||
TiXmlDocument xml_doc;
|
||||
xml_doc.Parse(xmlstring.c_str());
|
||||
return initXml(urdf_model, &xml_doc);
|
||||
TiXmlDocument xml_doc;
|
||||
xml_doc.Parse(xmlstring.c_str());
|
||||
return initXml(urdf_model, &xml_doc);
|
||||
}
|
||||
|
||||
|
||||
void srdf::Model::clear(void)
|
||||
{
|
||||
name_ = "";
|
||||
groups_.clear();
|
||||
group_states_.clear();
|
||||
virtual_joints_.clear();
|
||||
end_effectors_.clear();
|
||||
disabled_collisions_.clear();
|
||||
name_ = "";
|
||||
groups_.clear();
|
||||
group_states_.clear();
|
||||
virtual_joints_.clear();
|
||||
end_effectors_.clear();
|
||||
disabled_collisions_.clear();
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue