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

using rosconsole_bridge

This commit is contained in:
Ioan Sucan 2012-07-02 12:13:27 -07:00
parent a2d667f269
commit 7e8e862802
15 changed files with 55 additions and 4249 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
collada_parser/CMakeLists.txt
View File

@ -13,6 +13,9 @@ rosbuild_init()
rosbuild_add_boost_directories()
find_package(urdfdom_headers REQUIRED)
include_directories(${urdfdom_headers_INCLUDE_DIRS})
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake-extensions/ )
find_package(PkgConfig)
find_package(COLLADA_DOM 2.3 COMPONENTS 1.5)

4
collada_parser/manifest.xml
View File

@ -12,12 +12,12 @@
<url>http://ros.org/wiki/collada_parser</url>
<depend package="roscpp"/>
<depend package="urdf_interface"/>
<rosdep name="urdfdom_headers"/>
<rosdep name="colladadom"/>
<export>
<cpp cflags="-I${prefix}/include `pkg-config --silence-errors --cflags collada15dom || echo` `pkg-config --silence-errors --cflags collada-dom-150 || echo`"
<cpp cflags="-I${prefix}/include `pkg-config --cflags urdfdom_headers` `pkg-config --silence-errors --cflags collada15dom || echo` `pkg-config --silence-errors --cflags collada-dom-150 || echo`"
lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lcollada_parser `pkg-config --silence-errors --libs collada15dom || echo` `pkg-config --silence-errors --libs collada-dom-150 || echo`"/>
</export>

30
srdf/CMakeLists.txt
View File

@ -1,30 +0,0 @@
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()
rosbuild_add_boost_directories()
#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)
find_package(PkgConfig REQUIRED)
pkg_check_modules(TXML REQUIRED tinyxml)
include_directories(${TXML_INCLUDE_DIRS})
link_directories(${TXML_LIBRARY_DIRS})
#common commands for building c++ executables and libraries
rosbuild_add_library(${PROJECT_NAME} src/model.cpp)
target_link_libraries(${PROJECT_NAME} ${TXML_LIBRARIES})
rosbuild_add_gtest(test_parser test/test_parser.cpp)
target_link_libraries(test_parser ${PROJECT_NAME})

1
srdf/Makefile
View File

@ -1 +0,0 @@
include $(shell rospack find mk)/cmake.mk

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

@ -1,225 +0,0 @@
/*********************************************************************
* 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_;
};
/// The definition of a disabled collision between two links
struct DisabledCollision
{
/// The name of the first link (as in URDF) of the disabled collision
std::string link1_;
/// The name of the second link (as in URDF) of the disabled collision
std::string link2_;
/// The reason why the collision check was disabled
std::string reason_;
};
/// 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<DisabledCollision>& getDisabledCollisionPairs(void) const
{
return disabled_collisions_;
}
/// \deprecated{ Use the version returning DisabledCollision }
__attribute__ ((deprecated))
std::vector<std::pair<std::string, std::string> > getDisabledCollisions(void) const;
/// 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<DisabledCollision> disabled_collisions_;
};
}
#endif

15
srdf/manifest.xml
View File

@ -7,23 +7,14 @@
</description>
<author>Ioan Sucan</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<review status="deprecated" notes=""/>
<url>http://ros.org/wiki/srdf</url>
<rosdep name="tinyxml" />
<depend package="urdf" />
<depend package="rosconsole" />
<rosdep name="srdfdom" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lsrdf"/>
<cpp cflags="`pkg-config --cflags srdfdom`" lflags="`pkg-config --libs srdfdom`"/>
</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>

520
srdf/src/model.cpp
View File

@ -1,520 +0,0 @@
/*********************************************************************
* 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 <boost/lexical_cast.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <algorithm>
#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(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"))
{
const char *gname = group_xml->Attribute("name");
if (!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 = 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);
}
// 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 = 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);
std::set<std::string> seen;
while (!found && l)
{
seen.insert(l->name);
if (l->name == base_str)
found = true;
else
l = l->getParent();
}
if (!found)
{
l = urdf_model.getLink(base_str);
while (!found && l)
{
if (seen.find(l->name) != seen.end())
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"))
{
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_ = 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"))
{
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); 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"))
{
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;
}
DisabledCollision dc;
dc.link1_ = boost::trim_copy(std::string(link1));
dc.link2_ = boost::trim_copy(std::string(link2));
if (!urdf_model.getLink(dc.link1_))
{
ROS_ERROR("Link '%s' is not known to URDF. Cannot disable collisons.", link1);
continue;
}
if (!urdf_model.getLink(dc.link2_))
{
ROS_ERROR("Link '%s' is not known to URDF. Cannot disable collisons.", link2);
continue;
}
const char *reason = c_xml->Attribute("reason");
if (reason)
dc.reason_ = std::string(reason);
disabled_collisions_.push_back(dc);
}
}
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); 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);
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();
}
std::vector<std::pair<std::string, std::string> > srdf::Model::getDisabledCollisions(void) const
{
std::vector<std::pair<std::string, std::string> > result;
for (std::size_t i = 0 ; i < disabled_collisions_.size() ; ++i)
result.push_back(std::make_pair(disabled_collisions_[i].link1_, disabled_collisions_[i].link2_));
return result;
}

3
srdf/test/res/pr2_desc.1.srdf
View File

@ -1,3 +0,0 @@
<?xml version="1.0" ?>
<robot name="pr2">
</robot>

7
srdf/test/res/pr2_desc.2.srdf
View File

@ -1,7 +0,0 @@
<?xml version="1.0" ?>
<robot name="pr2">
<virtual_joint name="world_joint" type="planar" parent_frame="odom" child_link="base_footprint"/>
<group name="g1">
<joint name="torso_lift_joint"/>
</group>
</robot>

72
srdf/test/res/pr2_desc.3.srdf
View File

@ -1,72 +0,0 @@
<?xml version="1.0"?>
<robot name="pr2">
<virtual_joint name="world_joint" type="planar" parent_frame="odom" child_link="base_footprint"/>
<group name="right_arm">
<chain base_link="torso_lift_link" tip_link="r_wrist_roll_link"/>
</group>
<group name="left_arm">
<chain base_link="torso_lift_link" tip_link="l_wrist_roll_link"/>
</group>
<group name="arms">
<group name="left_arm"/>
<group name="right_arm"/>
</group>
<group_state name="tuck_arms" group="arms">
<joint name="l_shoulder_pan_joint" value="0.2" />
<!-- ... the rest of the joint values... -->
</group_state>
<group_state name="home" group="base">
<joint name="world_joint" value=" .4 0 -1 " />
<!-- ... the rest of the joint values... -->
</group_state>
<group name="base">
<joint name="world_joint"/>
</group>
<group name="whole_body">
<group name="arms"/>
<group name="base"/>
<joint name="torso_lift_joint"/>
</group>
<group name="l_end_effector">
<link name="l_gripper_palm_link" />
<joint name="l_gripper_palm_joint" />
<joint name="l_gripper_l_finger_joint" />
<joint name="l_gripper_l_finger_tip_joint" />
<joint name="l_gripper_led_joint" />
<joint name="l_gripper_motor_accelerometer_joint" />
<joint name="l_gripper_r_finger_joint" />
<joint name="l_gripper_r_finger_tip_joint" />
<joint name="l_gripper_joint" />
<joint name="l_gripper_tool_joint" />
</group>
<group name="r_end_effector">
<link name="r_gripper_palm_link" />
<joint name="r_gripper_palm_joint" />
<joint name="r_gripper_l_finger_joint" />
<joint name="r_gripper_l_finger_tip_joint" />
<joint name="r_gripper_led_joint" />
<joint name="r_gripper_motor_accelerometer_joint" />
<joint name="r_gripper_r_finger_joint" />
<joint name="r_gripper_r_finger_tip_joint" />
<joint name="r_gripper_joint" />
<joint name="r_gripper_tool_joint" />
</group>
<end_effector name="r_end_effector" parent_link="r_wrist_roll_link" group="r_end_effector"/>
<end_effector name="l_end_effector" parent_link="l_wrist_roll_link" group="l_end_effector"/>
<disable_collisions link1="r_shoulder_pan_link" link2="r_shoulder_lift_link" reason="adjacent" />
<disable_collisions link1="r_shoulder_pan_link" link2="l_gripper_palm_link" />
<!-- and many more disable_collisions tags -->
</robot>

3238
srdf/test/res/pr2_desc.urdf
View File

File diff suppressed because it is too large Load Diff

133
srdf/test/test_parser.cpp
View File

@ -1,133 +0,0 @@
/*********************************************************************
* 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 <urdf/model.h>
#include <gtest/gtest.h>
TEST(SRDF, Simple)
{
urdf::Model u;
srdf::Model s;
EXPECT_TRUE(u.initFile("test/res/pr2_desc.urdf"));
EXPECT_TRUE(s.initFile(u, "test/res/pr2_desc.1.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 0);
EXPECT_TRUE(s.getGroups().size() == 0);
EXPECT_TRUE(s.getGroupStates().size() == 0);
EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
EXPECT_TRUE(s.getEndEffectors().size() == 0);
EXPECT_TRUE(s.initFile(u, "test/res/pr2_desc.2.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 1);
EXPECT_TRUE(s.getGroups().size() == 1);
EXPECT_TRUE(s.getGroupStates().size() == 0);
EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
EXPECT_TRUE(s.getEndEffectors().size() == 0);
EXPECT_TRUE(s.initFile(u, "test/res/pr2_desc.1.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 0);
EXPECT_TRUE(s.getGroups().size() == 0);
EXPECT_TRUE(s.getGroupStates().size() == 0);
EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
EXPECT_TRUE(s.getEndEffectors().size() == 0);
}
TEST(SRDF, Complex)
{
urdf::Model u;
srdf::Model s;
EXPECT_TRUE(u.initFile("test/res/pr2_desc.urdf"));
EXPECT_TRUE(s.initFile(u, "test/res/pr2_desc.3.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 1);
EXPECT_TRUE(s.getGroups().size() == 7);
EXPECT_TRUE(s.getGroupStates().size() == 2);
EXPECT_TRUE(s.getDisabledCollisionPairs().size() == 2);
EXPECT_TRUE(s.getDisabledCollisionPairs()[0].reason_ == "adjacent");
EXPECT_TRUE(s.getEndEffectors().size() == 2);
EXPECT_EQ(s.getVirtualJoints()[0].name_, "world_joint");
EXPECT_EQ(s.getVirtualJoints()[0].type_, "planar");
for (std::size_t i = 0 ; i < s.getGroups().size() ; ++i)
{
if (s.getGroups()[i].name_ == "left_arm" || s.getGroups()[i].name_ == "right_arm")
EXPECT_TRUE(s.getGroups()[i].chains_.size() == 1);
if (s.getGroups()[i].name_ == "arms")
EXPECT_TRUE(s.getGroups()[i].subgroups_.size() == 2);
if (s.getGroups()[i].name_ == "base")
EXPECT_TRUE(s.getGroups()[i].joints_.size() == 1);
if (s.getGroups()[i].name_ == "l_end_effector" || s.getGroups()[i].name_ == "r_end_effector")
{
EXPECT_TRUE(s.getGroups()[i].links_.size() == 1);
EXPECT_TRUE(s.getGroups()[i].joints_.size() == 9);
}
if (s.getGroups()[i].name_ == "whole_body")
{
EXPECT_TRUE(s.getGroups()[i].joints_.size() == 1);
EXPECT_TRUE(s.getGroups()[i].subgroups_.size() == 2);
}
}
int index = 0;
if (s.getGroupStates()[0].group_ != "arms")
index = 1;
EXPECT_EQ(s.getGroupStates()[index].group_, "arms");
EXPECT_EQ(s.getGroupStates()[index].name_, "tuck_arms");
EXPECT_EQ(s.getGroupStates()[1-index].group_, "base");
EXPECT_EQ(s.getGroupStates()[1-index].name_, "home");
const std::vector<double> &v = s.getGroupStates()[index].joint_values_.find("l_shoulder_pan_joint")->second;
EXPECT_EQ(v.size(), 1);
EXPECT_EQ(v[0], 0.2);
const std::vector<double> &w = s.getGroupStates()[1-index].joint_values_.find("world_joint")->second;
EXPECT_EQ(w.size(), 3);
EXPECT_EQ(w[0], 0.4);
EXPECT_EQ(w[1], 0);
EXPECT_EQ(w[2], -1);
index = (s.getEndEffectors()[0].name_[0] == 'r') ? 0 : 1;
EXPECT_EQ(s.getEndEffectors()[index].name_, "r_end_effector");
EXPECT_EQ(s.getEndEffectors()[index].component_group_, "r_end_effector");
EXPECT_EQ(s.getEndEffectors()[index].parent_link_, "r_wrist_roll_link");
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

11
urdf/CMakeLists.txt
View File

@ -21,13 +21,14 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
rosbuild_gensrv()
find_package(PkgConfig REQUIRED)
pkg_check_modules(urdfdom REQUIRED urdfdom)
find_package(urdfdom REQUIRED)
include_directories(${urdfdom_INCLUDE_DIRS})
link_directories(${urdfdom_LIBRARY_DIRS})
rosbuild_add_library(${PROJECT_NAME} src/model.cpp)
target_link_libraries(${PROJECT_NAME} ${urdfdom_LIBRARIES})
find_package(rosconsole_bridge REQUIRED)
include_directories(${rosconsole_bridge_INCLUDE_DIRS})
rosbuild_add_library(${PROJECT_NAME} src/model.cpp src/rosconsole_bridge.cpp)
target_link_libraries(${PROJECT_NAME} ${urdfdom_LIBRARIES} ${rosconsole_bridge_LIBRARIES})
if(APPLE)
set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup")

4
urdf/manifest.xml
View File

@ -11,9 +11,11 @@
<url>http://ros.org/wiki/urdf</url>
<depend package="roscpp" />
<depend package="urdf_parser" />
<depend package="collada_parser" />
<rosdep name="rosconsole_bridge" />
<rosdep name="urdfdom" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lurdf"/>
</export>

38
urdf/src/rosconsole_bridge.cpp Normal file
View File

@ -0,0 +1,38 @@
/*********************************************************************
* 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.
*********************************************************************/
#include <rosconsole_bridge/bridge.h>
REGISTER_ROSCONSOLE_BRIDGE;