add initial version of srdf (no tests yet)
This commit is contained in:
Ioan Sucan
parent
5ddcc7da5e
commit
9fac541542
|
|
@ -0,0 +1,26 @@
|
||||||
|
cmake_minimum_required(VERSION 2.4.6)
|
||||||
|
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
|
||||||
|
|
||||||
|
# Set the build type. Options are:
|
||||||
|
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
|
||||||
|
# Debug : w/ debug symbols, w/o optimization
|
||||||
|
# Release : w/o debug symbols, w/ optimization
|
||||||
|
# RelWithDebInfo : w/ debug symbols, w/ optimization
|
||||||
|
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
|
||||||
|
set(ROS_BUILD_TYPE Debug)
|
||||||
|
|
||||||
|
rosbuild_init()
|
||||||
|
|
||||||
|
#set the default path for built executables to the "bin" directory
|
||||||
|
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
|
||||||
|
#set the default path for built libraries to the "lib" directory
|
||||||
|
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
|
||||||
|
|
||||||
|
#uncomment if you have defined messages
|
||||||
|
#rosbuild_genmsg()
|
||||||
|
#uncomment if you have defined services
|
||||||
|
#rosbuild_gensrv()
|
||||||
|
|
||||||
|
#common commands for building c++ executables and libraries
|
||||||
|
rosbuild_add_library(${PROJECT_NAME} src/model.cpp)
|
||||||
|
target_link_libraries(${PROJECT_NAME} tinyxml)
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
include $(shell rospack find mk)/cmake.mk
|
||||||
|
|
@ -0,0 +1,208 @@
|
||||||
|
/*********************************************************************
|
||||||
|
* Software License Agreement (BSD License)
|
||||||
|
*
|
||||||
|
* Copyright (c) 2011, Willow Garage, Inc.
|
||||||
|
* All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* * Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* * Redistributions in binary form must reproduce the above
|
||||||
|
* copyright notice, this list of conditions and the following
|
||||||
|
* disclaimer in the documentation and/or other materials provided
|
||||||
|
* with the distribution.
|
||||||
|
* * Neither the name of the Willow Garage nor the names of its
|
||||||
|
* contributors may be used to endorse or promote products derived
|
||||||
|
* from this software without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||||
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*********************************************************************/
|
||||||
|
|
||||||
|
/* Author Ioan Sucan */
|
||||||
|
|
||||||
|
#ifndef SRDF_MODEL_
|
||||||
|
#define SRDF_MODEL_
|
||||||
|
|
||||||
|
#include <map>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
#include <utility>
|
||||||
|
#include <urdf_interface/model.h>
|
||||||
|
#include <tinyxml.h>
|
||||||
|
|
||||||
|
/// Main namespace
|
||||||
|
namespace srdf
|
||||||
|
{
|
||||||
|
|
||||||
|
/** \brief Representation of semantic information about the robot */
|
||||||
|
class Model
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
|
||||||
|
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 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 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_;
|
||||||
|
|
||||||
|
/// 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_;
|
||||||
|
|
||||||
|
/// 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 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_;
|
||||||
|
|
||||||
|
/// 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_;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// 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 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 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 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 group states defined for this model
|
||||||
|
const std::vector<GroupState>& getGroupStates(void) const
|
||||||
|
{
|
||||||
|
return group_states_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Clear the model
|
||||||
|
void clear(void);
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
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
|
||||||
|
|
@ -0,0 +1,13 @@
|
||||||
|
/**
|
||||||
|
\mainpage
|
||||||
|
\htmlinclude manifest.html
|
||||||
|
|
||||||
|
\b srdf is a parser for SRDF (Semantic Robot Description Format) documents.
|
||||||
|
Please see http://www.ros.org/wiki/srdf for more details.
|
||||||
|
|
||||||
|
<!--
|
||||||
|
Provide an overview of your package.
|
||||||
|
-->
|
||||||
|
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
@ -0,0 +1,29 @@
|
||||||
|
<package>
|
||||||
|
<description brief="srdf">
|
||||||
|
|
||||||
|
SRDF (Semantic Robot Description Format) is a representation of
|
||||||
|
semantic information about robots.
|
||||||
|
|
||||||
|
</description>
|
||||||
|
<author>Ioan Sucan</author>
|
||||||
|
<license>BSD</license>
|
||||||
|
<review status="unreviewed" notes=""/>
|
||||||
|
<url>http://ros.org/wiki/srdf</url>
|
||||||
|
|
||||||
|
<rosdep name="tinyxml" />
|
||||||
|
|
||||||
|
<depend package="urdf_interface" />
|
||||||
|
<depend package="rosconsole" />
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lsrdf"/>
|
||||||
|
</export>
|
||||||
|
|
||||||
|
<platform os="ubuntu" version="10.04"/>
|
||||||
|
<platform os="ubuntu" version="10.10"/>
|
||||||
|
<platform os="ubuntu" version="11.04"/>
|
||||||
|
<platform os="ubuntu" version="11.10"/>
|
||||||
|
|
||||||
|
</package>
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,476 @@
|
||||||
|
/*********************************************************************
|
||||||
|
* Software License Agreement (BSD License)
|
||||||
|
*
|
||||||
|
* Copyright (c) 2011, Willow Garage, Inc.
|
||||||
|
* All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* * Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* * Redistributions in binary form must reproduce the above
|
||||||
|
* copyright notice, this list of conditions and the following
|
||||||
|
* disclaimer in the documentation and/or other materials provided
|
||||||
|
* with the distribution.
|
||||||
|
* * Neither the name of the Willow Garage nor the names of its
|
||||||
|
* contributors may be used to endorse or promote products derived
|
||||||
|
* from this software without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||||
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*********************************************************************/
|
||||||
|
|
||||||
|
/* Author Ioan Sucan */
|
||||||
|
|
||||||
|
#include "srdf/model.h"
|
||||||
|
#include <ros/console.h>
|
||||||
|
#include <fstream>
|
||||||
|
#include <sstream>
|
||||||
|
#include <set>
|
||||||
|
|
||||||
|
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"))
|
||||||
|
{
|
||||||
|
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.name_ = std::string(jname);
|
||||||
|
vj.child_link_ = std::string(child);
|
||||||
|
vj.parent_frame_ = std::string(parent);
|
||||||
|
vj.type_ = std::string(type);
|
||||||
|
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"))
|
||||||
|
{
|
||||||
|
const char *gname = group_xml->Attribute("name");
|
||||||
|
if (!gname)
|
||||||
|
{
|
||||||
|
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));
|
||||||
|
}
|
||||||
|
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"))
|
||||||
|
{
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
std::string jval_str = std::string(jval);
|
||||||
|
std::stringstream ss(jval_str);
|
||||||
|
while (!ss.eof())
|
||||||
|
{
|
||||||
|
double val; ss >> val;
|
||||||
|
gs.joint_values_[jname_str].push_back(val);
|
||||||
|
}
|
||||||
|
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"))
|
||||||
|
{
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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"))
|
||||||
|
{
|
||||||
|
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));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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");
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
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())
|
||||||
|
{
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void srdf::Model::clear(void)
|
||||||
|
{
|
||||||
|
name_ = "";
|
||||||
|
groups_.clear();
|
||||||
|
group_states_.clear();
|
||||||
|
virtual_joints_.clear();
|
||||||
|
end_effectors_.clear();
|
||||||
|
disabled_collisions_.clear();
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue