diff -up ./src/parser_urdf.cc.urdf3 ./src/parser_urdf.cc
--- ./src/parser_urdf.cc.urdf3 2014-04-11 14:42:48.000000000 -0400
+++ ./src/parser_urdf.cc 2014-05-26 14:31:34.251525903 -0400
@@ -28,6 +28,8 @@
using namespace sdf;
+#undef USE_EXTERNAL_URDF
+
typedef boost::shared_ptr<urdf::Collision> UrdfCollisionPtr;
typedef boost::shared_ptr<urdf::Visual> UrdfVisualPtr;
typedef boost::shared_ptr<urdf::Link> UrdfLinkPtr;
@@ -298,7 +300,12 @@ std::string Vector32Str(const urdf::Vect
////////////////////////////////////////////////////////////////////////////////
void ReduceCollisionToParent(UrdfLinkPtr _link,
- const std::string &_groupName, UrdfCollisionPtr _collision)
+#if USE_EXTERNAL_URDF
+ const std::string &_groupName,
+#else
+ const std::string &/*_groupName*/,
+#endif
+ UrdfCollisionPtr _collision)
{
boost::shared_ptr<std::vector<UrdfCollisionPtr> > cols;
#if USE_EXTERNAL_URDF
@@ -312,9 +319,6 @@ void ReduceCollisionToParent(UrdfLinkPtr
cols = boost::shared_ptr<std::vector<UrdfCollisionPtr> >(
&_link->collision_array);
}
-#else
- cols = _link->getCollisions(_groupName);
-#endif
if (!cols)
{
@@ -323,22 +327,37 @@ void ReduceCollisionToParent(UrdfLinkPtr
// new group name, create add vector to map and add Collision to the vector
_link->collision_groups.insert(make_pair(_groupName, cols));
}
+#else
+ cols = boost::shared_ptr<std::vector<UrdfCollisionPtr> >(
+ &_link->collision_array);
+#endif
- // group exists, add Collision to the vector in the map
+ // add Collision to the vector of collisions in link
std::vector<UrdfCollisionPtr>::iterator colIt =
find(cols->begin(), cols->end(), _collision);
+
if (colIt != cols->end())
sdfwarn << "attempted to add collision to link ["
<< _link->name
+#if USE_EXTERNAL_URDF
<< "], 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,
- const std::string &_groupName, UrdfVisualPtr _visual)
+#if USE_EXTERNAL_URDF
+ const std::string &_groupName,
+#else
+ const std::string &/*_groupName*/,
+#endif
+ UrdfVisualPtr _visual)
{
boost::shared_ptr<std::vector<UrdfVisualPtr> > viss;
#if USE_EXTERNAL_URDF
@@ -351,9 +370,6 @@ void ReduceVisualToParent(UrdfLinkPtr _l
{
viss = boost::shared_ptr<std::vector<UrdfVisualPtr> >(&_link->visual_array);
}
-#else
- viss = _link->getVisuals(_groupName);
-#endif
if (!viss)
{
@@ -365,15 +381,23 @@ void ReduceVisualToParent(UrdfLinkPtr _l
sdfdbg << "successfully added a new visual group name ["
<< _groupName << "]\n";
}
+#else
+ viss = boost::shared_ptr<std::vector<UrdfVisualPtr> >(&_link->visual_array);
+#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
+#if USE_EXTERNAL_URDF
<< "], but it already exists under group ["
<< _groupName << "]\n";
+#else
+ << "], but it already exists in link.\n";
+#endif
else
viss->push_back(_visual);
}
@@ -844,6 +868,7 @@ void ReduceVisualsToParent(UrdfLinkPtr _
// "lump::"+group name+"::'+_link name
// lump but keep the _link name in(/as) the group name,
// so we can correlate visuals to visuals somehow.
+#if USE_EXTERNAL_URDF
for (std::map<std::string,
boost::shared_ptr<std::vector<UrdfVisualPtr> > >::iterator
visualsIt = _link->visual_groups.begin();
@@ -888,6 +913,21 @@ void ReduceVisualsToParent(UrdfLinkPtr _
}
}
}
+#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)
+ {
+ // 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);
+ }
+#endif
}
/////////////////////////////////////////////////
@@ -899,6 +939,7 @@ void ReduceCollisionsToParent(UrdfLinkPt
// "lump::"+group name+"::'+_link name
// lump but keep the _link name in(/as) the group name,
// so we can correlate visuals to collisions somehow.
+#if USE_EXTERNAL_URDF
for (std::map<std::string,
boost::shared_ptr<std::vector<UrdfCollisionPtr> > >::iterator
collisionsIt = _link->collision_groups.begin();
@@ -951,6 +992,23 @@ void ReduceCollisionsToParent(UrdfLinkPt
}
}
// 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)
+ {
+ // 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);
+ }
+#endif
}
/////////////////////////////////////////////////
@@ -1161,8 +1219,21 @@ void URDF2SDF::ParseSDFExtension(TiXmlDo
{
sdf->material = GetKeyValueAsString(childElem);
}
- else if (childElem->ValueStr() == "visual")
+ else if (0 && 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())
@@ -1826,6 +1897,7 @@ std::string GetGeometryBoundingBox(
////////////////////////////////////////////////////////////////////////////////
void PrintCollisionGroups(UrdfLinkPtr _link)
{
+#if USE_EXTERNAL_URDF
sdfdbg << "COLLISION LUMPING: link: [" << _link->name << "] contains ["
<< static_cast<int>(_link->collision_groups.size())
<< "] collisions.\n";
@@ -1838,6 +1910,11 @@ void PrintCollisionGroups(UrdfLinkPtr _l
<< 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
}
////////////////////////////////////////////////////////////////////////////////
@@ -2211,6 +2288,7 @@ void CreateCollisions(TiXmlElement* _ele
{
// loop through all collision groups. as well as additional collision from
// lumped meshes (fixed joint reduction)
+#if USE_EXTERNAL_URDF
for (std::map<std::string,
boost::shared_ptr<std::vector<UrdfCollisionPtr> > >::const_iterator
collisionsIt = _link->collision_groups.begin();
@@ -2287,6 +2365,33 @@ void CreateCollisions(TiXmlElement* _ele
}
}
}
+#else
+ unsigned int defaultMeshCount = 0;
+ for (std::vector<UrdfCollisionPtr>::const_iterator
+ collision = _link->collision_array.begin();
+ collision != _link->collision_array.end();
+ ++collision)
+ {
+ sdfdbg << "creating default collision for link [" << _link->name
+ << "]";
+
+ 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
}
////////////////////////////////////////////////////////////////////////////////
@@ -2295,6 +2400,7 @@ void CreateVisuals(TiXmlElement* _elem,
{
// loop through all visual groups. as well as additional visuals from
// lumped meshes (fixed joint reduction)
+#if USE_EXTERNAL_URDF
for (std::map<std::string,
boost::shared_ptr<std::vector<UrdfVisualPtr> > >::const_iterator
visualsIt = _link->visual_groups.begin();
@@ -2371,6 +2477,33 @@ void CreateVisuals(TiXmlElement* _elem,
}
}
}
+#else
+ unsigned int defaultMeshCount = 0;
+ for (std::vector<UrdfVisualPtr>::const_iterator
+ visual = _link->visual_array.begin();
+ visual != _link->visual_array.end();
+ ++visual)
+ {
+ sdfdbg << "creating default visual for link [" << _link->name
+ << "]";
+
+ 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
}
////////////////////////////////////////////////////////////////////////////////