using rosconsole_bridge
This commit is contained in:
Ioan Sucan
parent
a2d667f269
commit
7e8e862802
|
|
@ -13,6 +13,9 @@ rosbuild_init()
|
||||||
|
|
||||||
rosbuild_add_boost_directories()
|
rosbuild_add_boost_directories()
|
||||||
|
|
||||||
|
find_package(urdfdom_headers REQUIRED)
|
||||||
|
include_directories(${urdfdom_headers_INCLUDE_DIRS})
|
||||||
|
|
||||||
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake-extensions/ )
|
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake-extensions/ )
|
||||||
find_package(PkgConfig)
|
find_package(PkgConfig)
|
||||||
find_package(COLLADA_DOM 2.3 COMPONENTS 1.5)
|
find_package(COLLADA_DOM 2.3 COMPONENTS 1.5)
|
||||||
|
|
|
||||||
|
|
@ -12,12 +12,12 @@
|
||||||
<url>http://ros.org/wiki/collada_parser</url>
|
<url>http://ros.org/wiki/collada_parser</url>
|
||||||
|
|
||||||
<depend package="roscpp"/>
|
<depend package="roscpp"/>
|
||||||
<depend package="urdf_interface"/>
|
|
||||||
|
|
||||||
|
<rosdep name="urdfdom_headers"/>
|
||||||
<rosdep name="colladadom"/>
|
<rosdep name="colladadom"/>
|
||||||
|
|
||||||
<export>
|
<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`"/>
|
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>
|
</export>
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -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 +0,0 @@
|
||||||
include $(shell rospack find mk)/cmake.mk
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -7,23 +7,14 @@
|
||||||
</description>
|
</description>
|
||||||
<author>Ioan Sucan</author>
|
<author>Ioan Sucan</author>
|
||||||
<license>BSD</license>
|
<license>BSD</license>
|
||||||
<review status="unreviewed" notes=""/>
|
<review status="deprecated" notes=""/>
|
||||||
<url>http://ros.org/wiki/srdf</url>
|
<url>http://ros.org/wiki/srdf</url>
|
||||||
|
|
||||||
<rosdep name="tinyxml" />
|
<rosdep name="tinyxml" />
|
||||||
|
<rosdep name="srdfdom" />
|
||||||
<depend package="urdf" />
|
|
||||||
<depend package="rosconsole" />
|
|
||||||
|
|
||||||
<export>
|
<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>
|
</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>
|
</package>
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -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;
|
|
||||||
}
|
|
||||||
|
|
@ -1,3 +0,0 @@
|
||||||
<?xml version="1.0" ?>
|
|
||||||
<robot name="pr2">
|
|
||||||
</robot>
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -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>
|
|
||||||
File diff suppressed because it is too large
Load Diff
|
|
@ -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();
|
|
||||||
}
|
|
||||||
|
|
@ -21,13 +21,14 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
|
||||||
|
|
||||||
rosbuild_gensrv()
|
rosbuild_gensrv()
|
||||||
|
|
||||||
find_package(PkgConfig REQUIRED)
|
find_package(urdfdom REQUIRED)
|
||||||
pkg_check_modules(urdfdom REQUIRED urdfdom)
|
|
||||||
include_directories(${urdfdom_INCLUDE_DIRS})
|
include_directories(${urdfdom_INCLUDE_DIRS})
|
||||||
link_directories(${urdfdom_LIBRARY_DIRS})
|
|
||||||
|
|
||||||
rosbuild_add_library(${PROJECT_NAME} src/model.cpp)
|
find_package(rosconsole_bridge REQUIRED)
|
||||||
target_link_libraries(${PROJECT_NAME} ${urdfdom_LIBRARIES})
|
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)
|
if(APPLE)
|
||||||
set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup")
|
set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup")
|
||||||
|
|
|
||||||
|
|
@ -11,9 +11,11 @@
|
||||||
<url>http://ros.org/wiki/urdf</url>
|
<url>http://ros.org/wiki/urdf</url>
|
||||||
|
|
||||||
<depend package="roscpp" />
|
<depend package="roscpp" />
|
||||||
<depend package="urdf_parser" />
|
|
||||||
<depend package="collada_parser" />
|
<depend package="collada_parser" />
|
||||||
|
|
||||||
|
<rosdep name="rosconsole_bridge" />
|
||||||
|
<rosdep name="urdfdom" />
|
||||||
|
|
||||||
<export>
|
<export>
|
||||||
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lurdf"/>
|
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lurdf"/>
|
||||||
</export>
|
</export>
|
||||||
|
|
|
||||||
|
|
@ -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;
|
||||||
|
|
||||||
Loading…
Reference in New Issue