diff --git a/collada_parser/CMakeLists.txt b/collada_parser/CMakeLists.txt index d4df99a..cb50e6c 100644 --- a/collada_parser/CMakeLists.txt +++ b/collada_parser/CMakeLists.txt @@ -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) diff --git a/collada_parser/manifest.xml b/collada_parser/manifest.xml index 7eff689..7c3c8a2 100644 --- a/collada_parser/manifest.xml +++ b/collada_parser/manifest.xml @@ -12,12 +12,12 @@ http://ros.org/wiki/collada_parser - + - diff --git a/srdf/CMakeLists.txt b/srdf/CMakeLists.txt deleted file mode 100644 index 2390f7c..0000000 --- a/srdf/CMakeLists.txt +++ /dev/null @@ -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}) diff --git a/srdf/Makefile b/srdf/Makefile deleted file mode 100644 index b75b928..0000000 --- a/srdf/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/srdf/include/srdf/model.h b/srdf/include/srdf/model.h deleted file mode 100644 index a81e1c0..0000000 --- a/srdf/include/srdf/model.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include - -/// 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 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 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 > 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 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 > 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& getDisabledCollisionPairs(void) const - { - return disabled_collisions_; - } - - /// \deprecated{ Use the version returning DisabledCollision } - __attribute__ ((deprecated)) - std::vector > getDisabledCollisions(void) const; - - /// Get the list of groups defined for this model - const std::vector& getGroups(void) const - { - return groups_; - } - - /// Get the list of virtual joints defined for this model - const std::vector& getVirtualJoints(void) const - { - return virtual_joints_; - } - - /// Get the list of end effectors defined for this model - const std::vector& getEndEffectors(void) const - { - return end_effectors_; - } - - /// Get the list of group states defined for this model - const std::vector& 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 groups_; - std::vector group_states_; - std::vector virtual_joints_; - std::vector end_effectors_; - std::vector disabled_collisions_; -}; - -} -#endif diff --git a/srdf/manifest.xml b/srdf/manifest.xml index d5180b3..7bec38b 100644 --- a/srdf/manifest.xml +++ b/srdf/manifest.xml @@ -7,23 +7,14 @@ Ioan Sucan BSD - + http://ros.org/wiki/srdf - - - + - + - - - - - - - diff --git a/srdf/src/model.cpp b/srdf/src/model.cpp deleted file mode 100644 index 1cec218..0000000 --- a/srdf/src/model.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include - -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 l = urdf_model.getLink(tip_str); - std::set 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 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 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(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 > srdf::Model::getDisabledCollisions(void) const -{ - std::vector > 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; -} diff --git a/srdf/test/res/pr2_desc.1.srdf b/srdf/test/res/pr2_desc.1.srdf deleted file mode 100644 index 5f11ab9..0000000 --- a/srdf/test/res/pr2_desc.1.srdf +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/srdf/test/res/pr2_desc.2.srdf b/srdf/test/res/pr2_desc.2.srdf deleted file mode 100644 index 06c3fd7..0000000 --- a/srdf/test/res/pr2_desc.2.srdf +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/srdf/test/res/pr2_desc.3.srdf b/srdf/test/res/pr2_desc.3.srdf deleted file mode 100644 index d567cc0..0000000 --- a/srdf/test/res/pr2_desc.3.srdf +++ /dev/null @@ -1,72 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/srdf/test/res/pr2_desc.urdf b/srdf/test/res/pr2_desc.urdf deleted file mode 100644 index cbc5c77..0000000 --- a/srdf/test/res/pr2_desc.urdf +++ /dev/null @@ -1,3238 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - true - 1000.0 - - - - true - 1.0 - 5 - - power_state - 10.0 - 87.78 - -474 - 525 - 15.52 - 16.41 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 640 - 640 - 1 - 0.0 0.0 0.0 - false - -129.998394137 - 129.998394137 - 0.08 - 10.0 - 0.01 - 20 - - 0.005 - true - 20 - base_scan - base_laser_link - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - true - - base_link_geom - 100.0 - - true - 100.0 - base_bumper - - - - - - - true - 100.0 - base_link - base_pose_ground_truth - 0.01 - map - 25.7 25.7 0 - - 0 0 0 - - - base_footprint - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - torso_lift_link_geom - 100.0 - - true - 100.0 - torso_lift_bumper - - - - - - - - -52143.33 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - 100.0 - imu_link - torso_lift_imu/data - 2.89e-08 - 0 0 0 - 0 0 0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 6.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 6.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - R8G8B8 - 2448 2050 - 45 - 0.1 - 100 - 20.0 - - true - 20.0 - /prosilica/image_raw - /prosilica/camera_info - /prosilica/request_image - high_def_frame - 1224.5 - 1224.5 - 1025.5 - 2955 - - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 640 480 - BAYER_BGGR8 - 90 - 0.1 - 100 - 25.0 - - true - 25.0 - wide_stereo/left/image_raw - wide_stereo/left/camera_info - wide_stereo_optical_frame - 0 - 320.5 - 320.5 - 240.5 - - - 320 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - - - - true - PR2/Blue - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 640 480 - BAYER_BGGR8 - 90 - 0.1 - 100 - 25.0 - - true - 25.0 - wide_stereo/right/image_raw - wide_stereo/right/camera_info - wide_stereo_optical_frame - 0.09 - 320.5 - 320.5 - 240.5 - - - 320 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - - - - true - PR2/Blue - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 640 480 - L8 - 45 - 0.1 - 100 - 25.0 - - true - 25.0 - narrow_stereo/left/image_raw - narrow_stereo/left/camera_info - narrow_stereo_optical_frame - 0 - 320.5 - 320.5 - 240.5 - - - 772.55 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - - - - true - PR2/Blue - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 640 480 - L8 - 45 - 0.1 - 100 - 25.0 - - true - 25.0 - narrow_stereo/right/image_raw - narrow_stereo/right/camera_info - narrow_stereo_optical_frame - 0.09 - 320.5 - 320.5 - 240.5 - - - 772.55 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - - - - true - PR2/Blue - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - 15.0 - stereo_projection_pattern_high_res_red.png - projector_wg6802418_child_frame - stereo_projection_pattern_filter.png - projector_wg6802418_controller/image - projector_wg6802418_controller/projector - 0.785398163397 - 0.4 - 10 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 640 - 640 - 1 - 0.0 0.0 0.0 - false - -79.9999999086 - 79.9999999086 - 0.08 - 10.0 - 0.01 - 40 - - 0.005 - true - 40 - tilt_scan - laser_tilt_link - - - - - - - - - - -6.05 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - - - - - - - - 32.6525111499 - - - true - - - - - - - true - - - - - - - - - 63.1552452977 - - - - - 61.8948225713 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - - - - - - -90.5142857143 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -1.0 - - - true - - - - - - - - - -36.167452007 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - true - - - - - - - true - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - r_gripper_l_finger_link_geom - 100.0 - - true - 100.0 - r_gripper_l_finger_link - r_gripper_l_finger_bumper - - - - - - - - - - - - - - - - - true - - r_gripper_r_finger_link_geom - 100.0 - - true - r_gripper_r_finger_link - 100.0 - r_gripper_r_finger_bumper - - - - - - - - - - - - - - - - true - false - - r_gripper_l_finger_tip_link_geom - 100.0 - - true - r_gripper_l_finger_tip_link - 100.0 - r_gripper_l_finger_tip_bumper - - - - - - - - - - - - - - - - true - false - - r_gripper_r_finger_tip_link_geom - 100.0 - - true - r_gripper_r_finger_tip_link - 100.0 - r_gripper_r_finger_tip_bumper - - - - - - - - - - - true - 100.0 - r_gripper_l_finger_link - r_gripper_l_finger_pose_ground_truth - 0.0 - base_link - - - - true - 100.0 - r_gripper_l_finger_link - r_gripper_l_finger_force_ground_truth - r_gripper_l_finger_link - - - - - - - - - - - - true - 0.17126 - 7.7562e-05 - 1.49095e-06 - -9.83385e-06 - 0.000197083 - -3.06125e-06 - 0.000181054 - 0.03598 - 0.0173 - -0.00164 - 0.82991 -0.157 0.790675 - 0 -0 0 - true - false - - - true - 0.17389 - 7.73841e-05 - -2.09309e-06 - -8.36228e-06 - 0.000198474 - 2.4611e-06 - 0.00018107 - 0.03576 - -0.01736 - -0.00095 - 0.82991 -0.219 0.790675 - 0 -0 0 - true - false - - - - - r_gripper_r_parallel_link - r_gripper_palm_link - r_gripper_palm_link - 0 0 -1 - 0.2 - 0.05891 -0.031 0 - - - r_gripper_l_parallel_link - r_gripper_palm_link - r_gripper_palm_link - 0 0 1 - 0.2 - 0.05891 0.031 0 - - - r_gripper_r_parallel_link - r_gripper_r_finger_tip_link - r_gripper_r_finger_tip_link - 0 0 1 - -0.018 -0.021 0 - - - r_gripper_l_parallel_link - r_gripper_l_finger_tip_link - r_gripper_l_finger_tip_link - 0 0 1 - -0.018 0.021 0 - - - r_gripper_l_finger_tip_link - r_gripper_r_finger_tip_link - r_gripper_r_finger_tip_link - 0 1 0 - - - - true - - - - true - - - - - - - - - - - - - true - - r_gripper_palm_link_geom - 100.0 - - true - 100.0 - r_gripper_palm_link - r_gripper_palm_bumper - - - - - - - true - 100.0 - r_gripper_palm_link - r_gripper_palm_pose_ground_truth - 0 0 0 - 0 0 0 - 0.0 - map - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - - - - - - - - 32.6525111499 - - - true - - - - - - - true - - - - - - - - - 63.1552452977 - - - - - 61.8948225713 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - - - - - - -90.5142857143 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -1.0 - - - true - - - - - - - - - -36.167452007 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - true - - - - - - - true - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - l_gripper_l_finger_link_geom - 100.0 - - true - 100.0 - l_gripper_l_finger_link - l_gripper_l_finger_bumper - - - - - - - - - - - - - - - - - true - - l_gripper_r_finger_link_geom - 100.0 - - true - l_gripper_r_finger_link - 100.0 - l_gripper_r_finger_bumper - - - - - - - - - - - - - - - - true - false - - l_gripper_l_finger_tip_link_geom - 100.0 - - true - l_gripper_l_finger_tip_link - 100.0 - l_gripper_l_finger_tip_bumper - - - - - - - - - - - - - - - - true - false - - l_gripper_r_finger_tip_link_geom - 100.0 - - true - l_gripper_r_finger_tip_link - 100.0 - l_gripper_r_finger_tip_bumper - - - - - - - - - - - true - 100.0 - l_gripper_l_finger_link - l_gripper_l_finger_pose_ground_truth - 0.0 - base_link - - - - true - 100.0 - l_gripper_l_finger_link - l_gripper_l_finger_force_ground_truth - l_gripper_l_finger_link - - - - - - - - - - - - true - 0.17126 - 7.7562e-05 - 1.49095e-06 - -9.83385e-06 - 0.000197083 - -3.06125e-06 - 0.000181054 - 0.03598 - 0.0173 - -0.00164 - 0.82991 0.219 0.790675 - 0 -0 0 - true - false - - - true - 0.17389 - 7.73841e-05 - -2.09309e-06 - -8.36228e-06 - 0.000198474 - 2.4611e-06 - 0.00018107 - 0.03576 - -0.01736 - -0.00095 - 0.82991 0.157 0.790675 - 0 -0 0 - true - false - - - - - l_gripper_r_parallel_link - l_gripper_palm_link - l_gripper_palm_link - 0 0 -1 - 0.2 - 0.05891 -0.031 0 - - - l_gripper_l_parallel_link - l_gripper_palm_link - l_gripper_palm_link - 0 0 1 - 0.2 - 0.05891 0.031 0 - - - l_gripper_r_parallel_link - l_gripper_r_finger_tip_link - l_gripper_r_finger_tip_link - 0 0 1 - -0.018 -0.021 0 - - - l_gripper_l_parallel_link - l_gripper_l_finger_tip_link - l_gripper_l_finger_tip_link - 0 0 1 - -0.018 0.021 0 - - - l_gripper_l_finger_tip_link - l_gripper_r_finger_tip_link - l_gripper_r_finger_tip_link - 0 1 0 - - - - true - - - - true - - - - - - - - - - - - - true - - l_gripper_palm_link_geom - 100.0 - - true - 100.0 - l_gripper_palm_link - l_gripper_palm_bumper - - - - - - - true - 100.0 - l_gripper_palm_link - l_gripper_palm_pose_ground_truth - 0 0 0 - 0 0 0 - 0.0 - map - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 640 480 - L8 - 90 - 0.1 - 100 - 25.0 - - true - 25.0 - l_forearm_cam/image_raw - l_forearm_cam/camera_info - l_forearm_cam_optical_frame - 0 - 320.5 - 320.5 - 240.5 - - - 320 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - - - - true - PR2/Blue - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 640 480 - L8 - 90 - 0.1 - 100 - 25.0 - - true - 25.0 - r_forearm_cam/image_raw - r_forearm_cam/camera_info - r_forearm_cam_optical_frame - 0 - 320.5 - 320.5 - 240.5 - - - 320 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - - - - true - PR2/Blue - - diff --git a/srdf/test/test_parser.cpp b/srdf/test/test_parser.cpp deleted file mode 100644 index baa13d7..0000000 --- a/srdf/test/test_parser.cpp +++ /dev/null @@ -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 -#include -#include - -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 &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 &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(); -} diff --git a/urdf/CMakeLists.txt b/urdf/CMakeLists.txt index 14eede2..5e84004 100644 --- a/urdf/CMakeLists.txt +++ b/urdf/CMakeLists.txt @@ -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") diff --git a/urdf/manifest.xml b/urdf/manifest.xml index 8dbb57b..099de84 100644 --- a/urdf/manifest.xml +++ b/urdf/manifest.xml @@ -11,9 +11,11 @@ http://ros.org/wiki/urdf - + + + diff --git a/urdf/src/rosconsole_bridge.cpp b/urdf/src/rosconsole_bridge.cpp new file mode 100644 index 0000000..b1cd394 --- /dev/null +++ b/urdf/src/rosconsole_bridge.cpp @@ -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 + +REGISTER_ROSCONSOLE_BRIDGE; +