[sdformat] 01/02: Update urdfdom patch with latest work done by upstream
Jose Luis Rivero
jrivero-guest at moszumanska.debian.org
Fri Nov 21 01:05:21 UTC 2014
This is an automated email from the git hooks/post-receive script.
jrivero-guest pushed a commit to branch master
in repository sdformat.
commit c42c1dd4f1e57f8f9409160e414244ff75e3a21d
Author: Jose Luis Rivero <jrivero at osrfoundation.org>
Date: Fri Nov 21 01:03:13 2014 +0000
Update urdfdom patch with latest work done by upstream
The changes are still in a pull request waiting for review
but should be good enough to improve the current situation.
---
debian/patches/0001-fix-external-urdfdom.patch | 840 ++++++++++++++-----------
1 file changed, 469 insertions(+), 371 deletions(-)
diff --git a/debian/patches/0001-fix-external-urdfdom.patch b/debian/patches/0001-fix-external-urdfdom.patch
index 9f9d434..e615119 100644
--- a/debian/patches/0001-fix-external-urdfdom.patch
+++ b/debian/patches/0001-fix-external-urdfdom.patch
@@ -1,35 +1,84 @@
From: Jose Luis Rivero <jrivero at osrfoundation.org>
-Date: Mon, 28 April 204 01:45 +0200
-Subject: Patch to use new version of urdfdom
+Date: Mon, 21 Nov 2014 01:45 +0200
+Subject: Patch to use new version of urdfdom 0.3.0
Bug: https://bitbucket.org/osrf/sdformat/issue/59
+Origin: https://bitbucket.org/osrf/sdformat/pull-request/122
-diff -r 049841be931a src/parser_urdf.cc
---- a/src/parser_urdf.cc Mon Apr 14 14:28:28 2014 -0700
-+++ b/src/parser_urdf.cc Mon Apr 28 01:41:56 2014 +0200
-@@ -297,85 +297,15 @@
- }
+Last commit in pull request: afad3d3e12e43f42552d41c0114053ce91282562
+
+diff -r b5ef37039cc7aaf7d3ba69cf30b5be54ec30b8f7 -r 944d3f38d9a4d895996acc45692f05988e58398a cmake/SearchForStuff.cmake
+--- a/cmake/SearchForStuff.cmake
++++ b/cmake/SearchForStuff.cmake
+@@ -79,6 +79,9 @@
+ set(URDF_INCLUDE_DIRS ${URDF_INCLUDEDIR})
+ set(URDF_LIBRARY_DIRS ${URDF_LIBDIR})
+ endif()
++else()
++ # the internal copy is currently 0.3.0
++ set (URDF_GE_0P3 TRUE)
+ endif()
+
+ #################################################
+diff -r b5ef37039cc7aaf7d3ba69cf30b5be54ec30b8f7 -r 944d3f38d9a4d895996acc45692f05988e58398a cmake/sdf_config.h.in
+--- a/cmake/sdf_config.h.in
++++ b/cmake/sdf_config.h.in
+@@ -26,7 +26,7 @@
+ #cmakedefine BUILD_TYPE_RELEASE 1
+ #cmakedefine HAVE_URDFDOM 1
+ #cmakedefine USE_EXTERNAL_URDF 1
+-#cmakedefine URDF_GT_0P3 1
++#cmakedefine URDF_GE_0P3 1
+
+ #define SDF_SHARE_PATH "${CMAKE_INSTALL_PREFIX}/share/"
+ #define SDF_VERSION_PATH "${CMAKE_INSTALL_PREFIX}/share/sdformat/${SDF_PKG_VERSION}"
+diff -r b5ef37039cc7aaf7d3ba69cf30b5be54ec30b8f7 -r 944d3f38d9a4d895996acc45692f05988e58398a src/parser_urdf.cc
+--- a/src/parser_urdf.cc
++++ b/src/parser_urdf.cc
+@@ -298,82 +298,103 @@
////////////////////////////////////////////////////////////////////////////////
--void ReduceCollisionToParent(UrdfLinkPtr _link,
+ void ReduceCollisionToParent(UrdfLinkPtr _link,
- const std::string &_groupName, UrdfCollisionPtr _collision)
-+void ReduceCollisionToParent(UrdfLinkPtr _link, UrdfCollisionPtr _collision)
++#ifndef URDF_GE_0P3
++ const std::string &_groupName,
++#else
++ const std::string &/*_groupName*/,
++#endif
++ UrdfCollisionPtr _collision)
{
- boost::shared_ptr<std::vector<UrdfCollisionPtr> > cols;
--#if USE_EXTERNAL_URDF
-- if (_link->collision)
-- {
+-#if USE_EXTERNAL_URDF && defined(URDF_GE_0P3)
++ typedef std::vector<UrdfCollisionPtr> UrdfCollisionPtrVector;
++
++ // Define cols as the shared pointer which default to a copy of
++ // collision_array vector
++ boost::shared_ptr<UrdfCollisionPtrVector> cols(
++ new UrdfCollisionPtrVector(_link->collision_array));
++
++#ifndef URDF_GE_0P3
++
+ if (_link->collision)
+ {
- cols.reset(new std::vector<UrdfCollisionPtr>);
-- cols->push_back(_link->collision);
-- }
++ cols.reset(new UrdfCollisionPtrVector());
+ cols->push_back(_link->collision);
+ }
- else
-- {
++
++ if (cols->empty())
+ {
- cols = boost::shared_ptr<std::vector<UrdfCollisionPtr> >(
- &_link->collision_array);
-- }
++ // new group name, create vector, add vector to map and
++ // add Collision to the vector
++ _link->collision_groups.insert(make_pair(_groupName, cols));
++ sdfdbg << "successfully added a new collision group name ["
++ << _groupName << "]\n";
+ }
-#else
- cols = _link->getCollisions(_groupName);
--#endif
--
+ #endif
+
- if (!cols)
- {
- // group does not exist, create one and add to map
@@ -41,30 +90,55 @@ diff -r 049841be931a src/parser_urdf.cc
- // group exists, add Collision to the vector in the map
- std::vector<UrdfCollisionPtr>::iterator colIt =
- find(cols->begin(), cols->end(), _collision);
-- if (colIt != cols->end())
-- sdfwarn << "attempted to add collision to link ["
-- << _link->name
-- << "], but it already exists under group ["
-- << _groupName << "]\n";
-- else
-- cols->push_back(_collision);
-+ _link->collision_array.push_back(_collision);
++ // group exists, add Collision to the vector in the map if it's not there
++ UrdfCollisionPtrVector::iterator colIt
++ = find(cols->begin(), cols->end(), _collision);
++
+ if (colIt != cols->end())
+ sdfwarn << "attempted to add collision to link ["
+ << _link->name
++#ifndef URDF_GE_0P3
+ << "], but it already exists under group ["
+ << _groupName << "]\n";
++#else
++ << "], but it already exists in link.\n";
++#endif
+ else
+ cols->push_back(_collision);
}
////////////////////////////////////////////////////////////////////////////////
--void ReduceVisualToParent(UrdfLinkPtr _link,
+ void ReduceVisualToParent(UrdfLinkPtr _link,
- const std::string &_groupName, UrdfVisualPtr _visual)
-+void ReduceVisualToParent(UrdfLinkPtr _link, UrdfVisualPtr _visual)
++#ifndef URDF_GE_0P3
++ const std::string &_groupName,
++#else
++ const std::string &/*_groupName*/,
++#endif
++ UrdfVisualPtr _visual)
{
- boost::shared_ptr<std::vector<UrdfVisualPtr> > viss;
--#if USE_EXTERNAL_URDF
-- if (_link->visual)
-- {
+-#if USE_EXTERNAL_URDF && defined(URDF_GE_0P3)
++
++ typedef std::vector<UrdfVisualPtr> UrdfVisualPtrVector;
++
++ // Define viss as the shared pointer which default to a copy of
++ // visual_array vector
++ boost::shared_ptr<UrdfVisualPtrVector> viss(
++ new UrdfVisualPtrVector(_link->visual_array));
++
++#ifndef URDF_GE_0P3
++
+ if (_link->visual)
+ {
- viss.reset(new std::vector<UrdfVisualPtr>);
-- viss->push_back(_link->visual);
-- }
++ viss.reset(new UrdfVisualPtrVector());
+ viss->push_back(_link->visual);
+ }
- else
-- {
++
++ if (viss->empty())
+ {
- viss = boost::shared_ptr<std::vector<UrdfVisualPtr> >(&_link->visual_array);
- }
-#else
@@ -75,380 +149,221 @@ diff -r 049841be931a src/parser_urdf.cc
- {
- // group does not exist, create one and add to map
- viss.reset(new std::vector<UrdfVisualPtr>);
-- // new group name, create vector, add vector to map and
+ // new group name, create vector, add vector to map and
- // add Visual to the vector
-- _link->visual_groups.insert(make_pair(_groupName, viss));
-- sdfdbg << "successfully added a new visual group name ["
-- << _groupName << "]\n";
-- }
--
-- // group exists, add Visual to the vector in the map if it's not there
++ // add Visual to the vector
+ _link->visual_groups.insert(make_pair(_groupName, viss));
+ sdfdbg << "successfully added a new visual group name ["
+ << _groupName << "]\n";
+ }
++#endif
+
+ // group exists, add Visual to the vector in the map if it's not there
- std::vector<UrdfVisualPtr>::iterator visIt
-- = find(viss->begin(), viss->end(), _visual);
-- if (visIt != viss->end())
-- sdfwarn << "attempted to add visual to link ["
-- << _link->name
-- << "], but it already exists under group ["
-- << _groupName << "]\n";
-- else
-- viss->push_back(_visual);
-+ _link->visual_array.push_back(_visual);
++ UrdfVisualPtrVector::iterator visIt
+ = find(viss->begin(), viss->end(), _visual);
++
+ if (visIt != viss->end())
+ sdfwarn << "attempted to add visual to link ["
+ << _link->name
++#ifndef URDF_GE_0P3
+ << "], but it already exists under group ["
+ << _groupName << "]\n";
++#else
++ << "], but it already exists in link.\n";
++#endif
+ else
+ viss->push_back(_visual);
}
-
- ////////////////////////////////////////////////////////////////////////////////
-@@ -839,54 +769,17 @@
- /// reduce fixed joints: lump visuals to parent link
- void ReduceVisualsToParent(UrdfLinkPtr _link)
- {
-- // lump visual to parent
-- // lump all visual to parent, assign group name
-- // "lump::"+group name+"::'+_link name
-- // lump but keep the _link name in(/as) the group name,
-- // so we can correlate visuals to visuals somehow.
-- for (std::map<std::string,
-- boost::shared_ptr<std::vector<UrdfVisualPtr> > >::iterator
-- visualsIt = _link->visual_groups.begin();
-- visualsIt != _link->visual_groups.end(); ++visualsIt)
+@@ -844,6 +865,7 @@
+ // "lump::"+group name+"::'+_link name
+ // lump but keep the _link name in(/as) the group name,
+ // so we can correlate visuals to visuals somehow.
++#ifndef URDF_GE_0P3
+ for (std::map<std::string,
+ boost::shared_ptr<std::vector<UrdfVisualPtr> > >::iterator
+ visualsIt = _link->visual_groups.begin();
+@@ -888,6 +910,21 @@
+ }
+ }
+ }
++#else
++ std::string lumpGroupName = std::string("lump::")+_link->name;
+ for (std::vector<UrdfVisualPtr>::iterator
+ visualIt = _link->visual_array.begin();
+ visualIt != _link->visual_array.end(); ++visualIt)
- {
-- if (visualsIt->first.find(std::string("lump::")) == 0)
-- {
-- // it's a previously lumped mesh, re-lump under same _groupName
-- std::string lumpGroupName = visualsIt->first;
-- sdfdbg << "re-lumping group name [" << lumpGroupName
-- << "] to link [" << _link->getParent()->name << "]\n";
-- for (std::vector<UrdfVisualPtr>::iterator
-- visualIt = visualsIt->second->begin();
-- visualIt != visualsIt->second->end(); ++visualIt)
-- {
-- // transform visual origin from _link frame to parent link
-- // frame before adding to parent
-- (*visualIt)->origin = TransformToParentFrame((*visualIt)->origin,
-- _link->parent_joint->parent_to_joint_origin_transform);
-- // add the modified visual to parent
-- ReduceVisualToParent(_link->getParent(), lumpGroupName,
-- *visualIt);
-- }
-- }
-- else
-- {
-- // default and any other groups meshes
-- std::string lumpGroupName = std::string("lump::")+_link->name;
-- sdfdbg << "adding modified lump group name [" << lumpGroupName
-- << "] to link [" << _link->getParent()->name << "].\n";
-- for (std::vector<UrdfVisualPtr>::iterator
-- visualIt = visualsIt->second->begin();
-- visualIt != visualsIt->second->end(); ++visualIt)
-- {
-- // transform visual origin from _link frame to
-- // parent link frame before adding to parent
-- (*visualIt)->origin = TransformToParentFrame((*visualIt)->origin,
-- _link->parent_joint->parent_to_joint_origin_transform);
-- // add the modified visual to parent
-- ReduceVisualToParent(_link->getParent(), lumpGroupName,
-- *visualIt);
-- }
-- }
-+ // transform visual origin from _link frame to parent link
-+ // frame before adding to parent
++ {
++ // transform visual origin from _link frame to
++ // parent link frame before adding to parent
+ (*visualIt)->origin = TransformToParentFrame((*visualIt)->origin,
+ _link->parent_joint->parent_to_joint_origin_transform);
+ // add the modified visual to parent
-+ // ReduceVisualToParent(_link->getParent(), *visualIt);
-+ _link->getParent()->visual_array.push_back(*visualIt);
- }
++ ReduceVisualToParent(_link->getParent(), lumpGroupName,
++ *visualIt);
++ }
++#endif
}
-@@ -894,63 +787,19 @@
- /// reduce fixed joints: lump collisions to parent link
- void ReduceCollisionsToParent(UrdfLinkPtr _link)
- {
-- // lump collision parent
-- // lump all collision to parent, assign group name
-- // "lump::"+group name+"::'+_link name
-- // lump but keep the _link name in(/as) the group name,
-- // so we can correlate visuals to collisions somehow.
-- for (std::map<std::string,
-- boost::shared_ptr<std::vector<UrdfCollisionPtr> > >::iterator
-- collisionsIt = _link->collision_groups.begin();
-- collisionsIt != _link->collision_groups.end(); ++collisionsIt)
+ /////////////////////////////////////////////////
+@@ -899,6 +936,7 @@
+ // "lump::"+group name+"::'+_link name
+ // lump but keep the _link name in(/as) the group name,
+ // so we can correlate visuals to collisions somehow.
++#ifndef URDF_GE_0P3
+ for (std::map<std::string,
+ boost::shared_ptr<std::vector<UrdfCollisionPtr> > >::iterator
+ collisionsIt = _link->collision_groups.begin();
+@@ -951,6 +989,23 @@
+ }
+ }
+ // this->PrintCollisionGroups(_link->getParent());
++#else
++ std::string lumpGroupName = std::string("lump::")+_link->name;
+ for (std::vector<UrdfCollisionPtr>::iterator
+ collisionIt = _link->collision_array.begin();
+ collisionIt != _link->collision_array.end(); ++collisionIt)
- {
-- if (collisionsIt->first.find(std::string("lump::")) == 0)
-- {
-- // if it's a previously lumped mesh, relump under same _groupName
-- std::string lumpGroupName = collisionsIt->first;
-- sdfdbg << "re-lumping collision [" << collisionsIt->first
-- << "] for link [" << _link->name
-- << "] to parent [" << _link->getParent()->name
-- << "] with group name [" << lumpGroupName << "]\n";
-- for (std::vector<UrdfCollisionPtr>::iterator
-- collisionIt = collisionsIt->second->begin();
-- collisionIt != collisionsIt->second->end(); ++collisionIt)
-- {
-- // transform collision origin from _link frame to
-- // parent link frame before adding to parent
-- (*collisionIt)->origin = TransformToParentFrame(
-- (*collisionIt)->origin,
-- _link->parent_joint->parent_to_joint_origin_transform);
-- // add the modified collision to parent
-- ReduceCollisionToParent(_link->getParent(), lumpGroupName,
-- *collisionIt);
-- }
-- }
-- else
-- {
-- // default and any other group meshes
-- std::string lumpGroupName = std::string("lump::")+_link->name;
-- sdfdbg << "lumping collision [" << collisionsIt->first
-- << "] for link [" << _link->name
-- << "] to parent [" << _link->getParent()->name
-- << "] with group name [" << lumpGroupName << "]\n";
-- for (std::vector<UrdfCollisionPtr>::iterator
-- collisionIt = collisionsIt->second->begin();
-- collisionIt != collisionsIt->second->end(); ++collisionIt)
-- {
-- // transform collision origin from _link frame to
-- // parent link frame before adding to parent
-- (*collisionIt)->origin = TransformToParentFrame(
-- (*collisionIt)->origin,
-- _link->parent_joint->parent_to_joint_origin_transform);
--
-- // add the modified collision to parent
-- ReduceCollisionToParent(_link->getParent(), lumpGroupName,
-- *collisionIt);
-- }
-- }
++ {
+ // transform collision origin from _link frame to
+ // parent link frame before adding to parent
+ (*collisionIt)->origin = TransformToParentFrame(
+ (*collisionIt)->origin,
+ _link->parent_joint->parent_to_joint_origin_transform);
++
+ // add the modified collision to parent
-+ // ReduceCollisionToParent(_link->getParent(), *collisionIt);
-+ _link->getParent()->collision_array.push_back(*collisionIt);
- }
-- // this->PrintCollisionGroups(_link->getParent());
++ ReduceCollisionToParent(_link->getParent(), lumpGroupName,
++ *collisionIt);
++ }
++#endif
}
/////////////////////////////////////////////////
-@@ -1827,17 +1676,8 @@
+@@ -1163,6 +1218,19 @@
+ }
+ else if (childElem->ValueStr() == "visual")
+ {
++ // anything inside of visual tags:
++ // <gazebo reference="link_name">
++ // <visual>
++ // <extention_stuff_here/>
++ // </visual>
++ // </gazebl>
++ // are treated as blobs that gets inserted
++ // into visuals for the link
++ // <visual name="link_name[anything here]">
++ // <stuff_from_urdf_link_visuals/>
++ // <extention_stuff_here/>
++ // </visual>
++
+ // a place to store converted doc
+ for (TiXmlElement* e = childElem->FirstChildElement(); e;
+ e = e->NextSiblingElement())
+@@ -1834,6 +1902,7 @@
+ ////////////////////////////////////////////////////////////////////////////////
void PrintCollisionGroups(UrdfLinkPtr _link)
{
++#ifndef URDF_GE_0P3
sdfdbg << "COLLISION LUMPING: link: [" << _link->name << "] contains ["
-- << static_cast<int>(_link->collision_groups.size())
-+ << static_cast<int>(_link->collision_array.size())
+ << static_cast<int>(_link->collision_groups.size())
<< "] collisions.\n";
-- for (std::map<std::string,
-- boost::shared_ptr<std::vector<UrdfCollisionPtr > > >::iterator
-- colsIt = _link->collision_groups.begin();
-- colsIt != _link->collision_groups.end(); ++colsIt)
-- {
-- sdfdbg << " collision_groups: [" << colsIt->first << "] has ["
-- << static_cast<int>(colsIt->second->size())
-- << "] Collision objects\n";
-- }
- }
-
- ////////////////////////////////////////////////////////////////////////////////
-@@ -2206,86 +2046,21 @@
+@@ -1846,6 +1915,11 @@
+ << static_cast<int>(colsIt->second->size())
+ << "] Collision objects\n";
+ }
++#else
++ sdfdbg << "COLLISION LUMPING: link: [" << _link->name << "] contains ["
++ << static_cast<int>(_link->collision_array.size())
++ << "] collisions.\n";
++#endif
}
////////////////////////////////////////////////////////////////////////////////
--void CreateCollisions(TiXmlElement* _elem,
-- ConstUrdfLinkPtr _link)
-+void CreateCollisions(TiXmlElement* _elem, ConstUrdfLinkPtr _link)
+@@ -2219,6 +2293,7 @@
{
-- // loop through all collision groups. as well as additional collision from
-- // lumped meshes (fixed joint reduction)
-- for (std::map<std::string,
-- boost::shared_ptr<std::vector<UrdfCollisionPtr> > >::const_iterator
-- collisionsIt = _link->collision_groups.begin();
-- collisionsIt != _link->collision_groups.end(); ++collisionsIt)
-+ // loop through collisions
+ // loop through all collision groups. as well as additional collision from
+ // lumped meshes (fixed joint reduction)
++#ifndef URDF_GE_0P3
+ for (std::map<std::string,
+ boost::shared_ptr<std::vector<UrdfCollisionPtr> > >::const_iterator
+ collisionsIt = _link->collision_groups.begin();
+@@ -2295,6 +2370,33 @@
+ }
+ }
+ }
++#else
++ unsigned int defaultMeshCount = 0;
+ for (std::vector<UrdfCollisionPtr>::const_iterator
-+ collision = _link->collision_array.begin();
++ collision = _link->collision_array.begin();
+ collision != _link->collision_array.end();
+ ++collision)
- {
-- unsigned int defaultMeshCount = 0;
-- unsigned int groupMeshCount = 0;
-- unsigned int lumpMeshCount = 0;
-- // loop through collisions in each group
-- for (std::vector<UrdfCollisionPtr>::iterator
-- collision = collisionsIt->second->begin();
-- collision != collisionsIt->second->end();
-- ++collision)
-- {
-- if (collisionsIt->first == "default")
-- {
-- sdfdbg << "creating default collision for link [" << _link->name
-- << "]";
++ {
+ sdfdbg << "creating default collision for link [" << _link->name
+ << "]";
-
-- std::string collisionPrefix = _link->name;
-+ std::string collisionPrefix = _link->name + (*collision)->name;
-
-- if (defaultMeshCount > 0)
-- {
-- // append _[meshCount] to link name for additional collisions
-- std::ostringstream collisionNameStream;
-- collisionNameStream << collisionPrefix << "_" << defaultMeshCount;
-- collisionPrefix = collisionNameStream.str();
-- }
--
-- /* make a <collision> block */
-- CreateCollision(_elem, _link, *collision, collisionPrefix);
--
-- // only 1 default mesh
-- ++defaultMeshCount;
-- }
-- else if (collisionsIt->first.find(std::string("lump::")) == 0)
-- {
-- // if collision name starts with "lump::", pass through
-- // original parent link name
-- sdfdbg << "creating lump collision [" << collisionsIt->first
-- << "] for link [" << _link->name << "].\n";
-- /// collisionPrefix is the original name before lumping
-- std::string collisionPrefix = collisionsIt->first.substr(6);
--
-- if (lumpMeshCount > 0)
-- {
-- // append _[meshCount] to link name for additional collisions
-- std::ostringstream collisionNameStream;
-- collisionNameStream << collisionPrefix << "_" << lumpMeshCount;
-- collisionPrefix = collisionNameStream.str();
-- }
--
-- CreateCollision(_elem, _link, *collision, collisionPrefix);
-- ++lumpMeshCount;
-- }
-- else
-- {
-- sdfdbg << "adding collisions from collision group ["
-- << collisionsIt->first << "]\n";
--
-- std::string collisionPrefix = _link->name + std::string("_") +
-- collisionsIt->first;
--
-- if (groupMeshCount > 0)
-- {
-- // append _[meshCount] to _link name for additional collisions
-- std::ostringstream collisionNameStream;
-- collisionNameStream << collisionPrefix << "_" << groupMeshCount;
-- collisionPrefix = collisionNameStream.str();
-- }
--
-- CreateCollision(_elem, _link, *collision, collisionPrefix);
-- ++groupMeshCount;
-- }
-- }
++
++ std::string collisionPrefix = _link->name;
++
++ if (defaultMeshCount > 0)
++ {
++ // append _[meshCount] to link name for additional collisions
++ std::ostringstream collisionNameStream;
++ collisionNameStream << collisionPrefix << "_" << defaultMeshCount;
++ collisionPrefix = collisionNameStream.str();
++ }
++
+ /* make a <collision> block */
+ CreateCollision(_elem, _link, *collision, collisionPrefix);
- }
++
++ // only 1 default mesh
++ ++defaultMeshCount;
++ }
++#endif
}
-@@ -2293,83 +2068,18 @@
- void CreateVisuals(TiXmlElement* _elem,
- ConstUrdfLinkPtr _link)
+ ////////////////////////////////////////////////////////////////////////////////
+@@ -2303,6 +2405,7 @@
{
-- // loop through all visual groups. as well as additional visuals from
-- // lumped meshes (fixed joint reduction)
-- for (std::map<std::string,
-- boost::shared_ptr<std::vector<UrdfVisualPtr> > >::const_iterator
-- visualsIt = _link->visual_groups.begin();
-- visualsIt != _link->visual_groups.end(); ++visualsIt)
-+ // loop through visuals
+ // loop through all visual groups. as well as additional visuals from
+ // lumped meshes (fixed joint reduction)
++#ifndef URDF_GE_0P3
+ for (std::map<std::string,
+ boost::shared_ptr<std::vector<UrdfVisualPtr> > >::const_iterator
+ visualsIt = _link->visual_groups.begin();
+@@ -2379,6 +2482,33 @@
+ }
+ }
+ }
++#else
++ unsigned int defaultMeshCount = 0;
+ for (std::vector<UrdfVisualPtr>::const_iterator
+ visual = _link->visual_array.begin();
-+ visual != _link->visual_array.end(); ++visual)
- {
-- unsigned int defaultMeshCount = 0;
-- unsigned int groupMeshCount = 0;
-- unsigned int lumpMeshCount = 0;
-- // loop through all visuals in this group
-- for (std::vector<UrdfVisualPtr>::iterator
-- visual = visualsIt->second->begin();
-- visual != visualsIt->second->end();
-- ++visual)
-- {
-- if (visualsIt->first == "default")
-- {
-- sdfdbg << "creating default visual for link [" << _link->name
-- << "]";
++ visual != _link->visual_array.end();
++ ++visual)
++ {
+ sdfdbg << "creating default visual for link [" << _link->name
+ << "]";
-
-- std::string visualPrefix = _link->name;
-+ std::string visualPrefix = _link->name + (*visual)->name;
-
-- if (defaultMeshCount > 0)
-- {
-- // append _[meshCount] to _link name for additional visuals
-- std::ostringstream visualNameStream;
-- visualNameStream << visualPrefix << "_" << defaultMeshCount;
-- visualPrefix = visualNameStream.str();
-- }
--
-- // create a <visual> block
-- CreateVisual(_elem, _link, *visual, visualPrefix);
--
-- // only 1 default mesh
-- ++defaultMeshCount;
-- }
-- else if (visualsIt->first.find(std::string("lump::")) == 0)
-- {
-- // if visual name starts with "lump::", pass through
-- // original parent link name
-- sdfdbg << "creating lump visual [" << visualsIt->first
-- << "] for link [" << _link->name << "].\n";
-- /// visualPrefix is the original name before lumping
-- std::string visualPrefix = visualsIt->first.substr(6);
--
-- if (lumpMeshCount > 0)
-- {
-- // append _[meshCount] to _link name for additional visuals
-- std::ostringstream visualNameStream;
-- visualNameStream << visualPrefix << "_" << lumpMeshCount;
-- visualPrefix = visualNameStream.str();
-- }
--
-- CreateVisual(_elem, _link, *visual, visualPrefix);
-- ++lumpMeshCount;
-- }
-- else
-- {
-- sdfdbg << "adding visuals from visual group ["
-- << visualsIt->first << "]\n";
--
-- std::string visualPrefix = _link->name + std::string("_") +
-- visualsIt->first;
--
-- if (groupMeshCount > 0)
-- {
-- // append _[meshCount] to _link name for additional visuals
-- std::ostringstream visualNameStream;
-- visualNameStream << visualPrefix << "_" << groupMeshCount;
-- visualPrefix = visualNameStream.str();
-- }
--
-- CreateVisual(_elem, _link, *visual, visualPrefix);
-- ++groupMeshCount;
-- }
-- }
++
++ std::string visualPrefix = _link->name;
++
++ if (defaultMeshCount > 0)
++ {
++ // append _[meshCount] to _link name for additional visuals
++ std::ostringstream visualNameStream;
++ visualNameStream << visualPrefix << "_" << defaultMeshCount;
++ visualPrefix = visualNameStream.str();
++ }
++
+ // create a <visual> block
+ CreateVisual(_elem, _link, *visual, visualPrefix);
- }
++
++ // only 1 default mesh
++ ++defaultMeshCount;
++ }
++#endif
}
-diff -r 049841be931a src/urdf/urdf_model/link.h
---- a/src/urdf/urdf_model/link.h Mon Apr 14 14:28:28 2014 -0700
-+++ b/src/urdf/urdf_model/link.h Mon Apr 28 01:41:56 2014 +0200
+ ////////////////////////////////////////////////////////////////////////////////
+diff -r b5ef37039cc7aaf7d3ba69cf30b5be54ec30b8f7 -r 944d3f38d9a4d895996acc45692f05988e58398a src/urdf/urdf_model/link.h
+--- a/src/urdf/urdf_model/link.h
++++ b/src/urdf/urdf_model/link.h
@@ -62,7 +62,7 @@
class SDFORMAT_HIDDEN Sphere : public Geometry
{
@@ -561,9 +476,9 @@ diff -r 049841be931a src/urdf/urdf_model/link.h
private:
boost::weak_ptr<Link> parent_link_;
-diff -r 049841be931a src/urdf/urdf_model/model.h
---- a/src/urdf/urdf_model/model.h Mon Apr 14 14:28:28 2014 -0700
-+++ b/src/urdf/urdf_model/model.h Mon Apr 28 01:41:56 2014 +0200
+diff -r b5ef37039cc7aaf7d3ba69cf30b5be54ec30b8f7 -r 944d3f38d9a4d895996acc45692f05988e58398a src/urdf/urdf_model/model.h
+--- a/src/urdf/urdf_model/model.h
++++ b/src/urdf/urdf_model/model.h
@@ -192,12 +192,10 @@
/// \brief complete list of Materials
std::map<std::string, boost::shared_ptr<Material> > materials_;
@@ -579,9 +494,9 @@ diff -r 049841be931a src/urdf/urdf_model/model.h
boost::shared_ptr<Link> root_link_;
-diff -r 049841be931a src/urdf/urdf_parser/link.cpp
---- a/src/urdf/urdf_parser/link.cpp Mon Apr 14 14:28:28 2014 -0700
-+++ b/src/urdf/urdf_parser/link.cpp Mon Apr 28 01:41:56 2014 +0200
+diff -r b5ef37039cc7aaf7d3ba69cf30b5be54ec30b8f7 -r 944d3f38d9a4d895996acc45692f05988e58398a src/urdf/urdf_parser/link.cpp
+--- a/src/urdf/urdf_parser/link.cpp
++++ b/src/urdf/urdf_parser/link.cpp
@@ -94,8 +94,8 @@
if (!has_rgb && !has_filename) {
if (!only_name_is_ok) // no need for an error if only name is ok
@@ -789,9 +704,9 @@ diff -r 049841be931a src/urdf/urdf_parser/link.cpp
xml->LinkEndChild(link_xml);
-diff -r 049841be931a src/urdf/urdf_parser/model.cpp
---- a/src/urdf/urdf_parser/model.cpp Mon Apr 14 14:28:28 2014 -0700
-+++ b/src/urdf/urdf_parser/model.cpp Mon Apr 28 01:41:56 2014 +0200
+diff -r b5ef37039cc7aaf7d3ba69cf30b5be54ec30b8f7 -r 944d3f38d9a4d895996acc45692f05988e58398a src/urdf/urdf_parser/model.cpp
+--- a/src/urdf/urdf_parser/model.cpp
++++ b/src/urdf/urdf_parser/model.cpp
@@ -38,6 +38,7 @@
#include <vector>
#include "urdf_parser/urdf_parser.h"
@@ -1021,9 +936,9 @@ diff -r 049841be931a src/urdf/urdf_parser/model.cpp
}
-diff -r 049841be931a src/urdf/urdf_world/world.h
---- a/src/urdf/urdf_world/world.h Mon Apr 14 14:28:28 2014 -0700
-+++ b/src/urdf/urdf_world/world.h Mon Apr 28 01:41:56 2014 +0200
+diff -r b5ef37039cc7aaf7d3ba69cf30b5be54ec30b8f7 -r 944d3f38d9a4d895996acc45692f05988e58398a src/urdf/urdf_world/world.h
+--- a/src/urdf/urdf_world/world.h
++++ b/src/urdf/urdf_world/world.h
@@ -35,7 +35,7 @@
/* Author: John Hsu */
@@ -1033,3 +948,186 @@ diff -r 049841be931a src/urdf/urdf_world/world.h
for details
*/
/* example world XML
+diff -r b5ef37039cc7aaf7d3ba69cf30b5be54ec30b8f7 -r 944d3f38d9a4d895996acc45692f05988e58398a test/integration/fixed_joint_reduction.cc
+--- a/test/integration/fixed_joint_reduction.cc
++++ b/test/integration/fixed_joint_reduction.cc
+@@ -27,6 +27,9 @@
+ const std::string SDF_TEST_FILE_SIMPLE =
+ std::string(PROJECT_SOURCE_PATH)
+ + "/test/integration/fixed_joint_reduction_simple.urdf";
++const std::string SDF_TEST_FILE_COLLISION =
++ std::string(PROJECT_SOURCE_PATH)
++ + "/test/integration/fixed_joint_reduction_collision.urdf";
+ const std::string SDF_TEST_FILE_VISUAL =
+ std::string(PROJECT_SOURCE_PATH)
+ + "/test/integration/fixed_joint_reduction_visual.urdf";
+@@ -51,6 +54,14 @@
+ }
+
+ /////////////////////////////////////////////////
++// This is a copy of the previous test, with visuals replaced
++// by collisions.
++TEST(SDFParser, FixedJointReductionCollisionTest)
++{
++ FixedJointReductionEquivalence(SDF_TEST_FILE_COLLISION);
++}
++
++/////////////////////////////////////////////////
+ void FixedJointReductionEquivalence(const std::string &_file)
+ {
+ char *pathCStr = getenv("SDF_PATH");
+diff -r b5ef37039cc7aaf7d3ba69cf30b5be54ec30b8f7 -r 944d3f38d9a4d895996acc45692f05988e58398a test/integration/fixed_joint_reduction_collision.urdf
+--- /dev/null
++++ b/test/integration/fixed_joint_reduction_collision.urdf
+@@ -0,0 +1,151 @@
++<?xml version="1.0" ?>
++<robot name="fixed_joint_reduction_test_odel">
++
++ <link name="world"/>
++
++ <joint name="jointw0" type="continuous">
++ <origin rpy="0 0 1.57079632679" xyz="0 0 1.0"/>
++ <axis xyz="1 0 0"/> <!-- in child (link1) frame -->
++ <parent link="world"/>
++ <child link="link0"/>
++ </joint>
++
++ <link name="link0">
++ <inertial>
++ <mass value="1000"/>
++ <origin rpy="0 0 0" xyz="0 0 -0.5"/>
++ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
++ </inertial>
++ <collision>
++ <geometry>
++ <box size="0.1 0.1 1.0"/>
++ </geometry>
++ <origin rpy="0 0 0" xyz="0 0 -0.5"/>
++ </collision>
++ </link>
++
++ <joint name="joint01" type="continuous">
++ <origin rpy="0 0 1.57079632679" xyz="0 0 -1"/>
++ <axis xyz="1 0 0"/> <!-- in child (link1) frame -->
++ <parent link="link0"/>
++ <child link="link1"/>
++ </joint>
++
++ <link name="link1">
++ <inertial>
++ <mass value="100"/>
++ <origin rpy="1 3 4" xyz="0 -1.5 0"/>
++ <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
++ </inertial>
++ <collision>
++ <geometry>
++ <box size="0.2 3.0 0.2"/>
++ </geometry>
++ <origin rpy="0 0 0" xyz="0 -1.5 0"/>
++ </collision>
++ </link>
++
++ <joint name="joint12" type="revolute">
++ <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 -3.0 0.0"/>
++ <axis xyz="0 1 0"/> <!-- in child (link1) frame -->
++ <limit lower="0" upper="0" velocity="1000" effort="10000"/>
++ <parent link="link1"/>
++ <child link="link2"/>
++ </joint>
++
++ <link name="link2">
++ <inertial>
++ <mass value="200"/>
++ <origin rpy="2 -3 -1.57079632679" xyz="0.2 0.4 1.0"/>
++ <inertia ixx="5" ixy="0" ixz="0" iyy="6" iyz="0" izz="7"/>
++ </inertial>
++ <collision>
++ <geometry>
++ <box size="0.3 0.3 2.0"/>
++ </geometry>
++ <origin rpy="0 0 0" xyz="0 0 1.0"/>
++ </collision>
++ </link>
++
++ <joint name="joint23" type="revolute">
++ <origin rpy="-1.57079632679 0 -1.57079632679" xyz="-0.5 0.5 2.5"/>
++ <axis xyz="0 1 0"/> <!-- in child (link1) frame -->
++ <limit lower="0" upper="0" velocity="1000" effort="10000"/>
++ <parent link="link2"/>
++ <child link="link3"/>
++ </joint>
++
++ <link name="link3">
++ <inertial>
++ <mass value="400"/>
++ <origin rpy="2 3 4" xyz="0.1 0.2 0.3"/>
++ <inertia ixx="8" ixy="0" ixz="0" iyy="9" iyz="0" izz="10"/>
++ </inertial>
++ <collision>
++ <geometry>
++ <box size="0.3 0.4 0.5"/>
++ </geometry>
++ <origin rpy="2 3 4" xyz="0.1 0.2 0.3"/>
++ </collision>
++ </link>
++
++ <joint name="joint01a" type="continuous">
++ <origin rpy="0 0 1.57079632679" xyz="0 0 -1"/>
++ <axis xyz="1 0 0"/> <!-- in child (link1) frame -->
++ <parent link="link0"/>
++ <child link="link1a"/>
++ </joint>
++
++ <link name="link1a">
++ <inertial>
++ <mass value="100"/>
++ <origin rpy="1 3 4" xyz="0 -1.5 0"/>
++ <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
++ </inertial>
++ </link>
++
++ <joint name="joint12a" type="fixed">
++ <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 -3.0 0.0"/>
++ <axis xyz="0 1 0"/> <!-- in child (link1) frame -->
++ <limit lower="0" upper="0" velocity="1000" effort="10000"/>
++ <parent link="link1a"/>
++ <child link="link2a"/>
++ </joint>
++
++ <link name="link2a">
++ <inertial>
++ <mass value="200"/>
++ <origin rpy="2 -3 -1.57079632679" xyz="0.2 0.4 1.0"/>
++ <inertia ixx="5" ixy="0" ixz="0" iyy="6" iyz="0" izz="7"/>
++ </inertial>
++ <collision>
++ <geometry>
++ <box size="0.3 0.3 2.0"/>
++ </geometry>
++ <origin rpy="0 0 0" xyz="0 0 1.0"/>
++ </collision>
++ </link>
++
++ <joint name="joint23a" type="fixed">
++ <origin rpy="-1.57079632679 0 -1.57079632679" xyz="-0.5 0.5 2.5"/>
++ <axis xyz="0 1 0"/> <!-- in child (link1) frame -->
++ <limit lower="0" upper="0" velocity="1000" effort="10000"/>
++ <parent link="link2a"/>
++ <child link="link3a"/>
++ </joint>
++
++ <link name="link3a">
++ <inertial>
++ <mass value="400"/>
++ <origin rpy="2 3 4" xyz="0.1 0.2 0.3"/>
++ <inertia ixx="8" ixy="0" ixz="0" iyy="9" iyz="0" izz="10"/>
++ </inertial>
++ <collision>
++ <geometry>
++ <box size="0.3 0.4 0.5"/>
++ </geometry>
++ <origin rpy="2 3 4" xyz="0.1 0.2 0.3"/>
++ </collision>
++ </link>
++
++</robot>
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/sdformat.git
More information about the debian-science-commits
mailing list