diff -r d79259b26096 src/parser_urdf.cc
--- a/src/parser_urdf.cc Mon Aug 04 13:07:16 2014 -0700
+++ b/src/parser_urdf.cc Sat Aug 23 14:30:18 2014 -0400
@@ -297,85 +297,15 @@
}
////////////////////////////////////////////////////////////////////////////////
-void ReduceCollisionToParent(UrdfLinkPtr _link,
- const std::string &_groupName, UrdfCollisionPtr _collision)
+void ReduceCollisionToParent(UrdfLinkPtr _link, UrdfCollisionPtr _collision)
{
- boost::shared_ptr<std::vector<UrdfCollisionPtr> > cols;
-#if USE_EXTERNAL_URDF && defined(URDF_GE_0P3)
- if (_link->collision)
- {
- cols.reset(new std::vector<UrdfCollisionPtr>);
- cols->push_back(_link->collision);
- }
- else
- {
- cols = boost::shared_ptr<std::vector<UrdfCollisionPtr> >(
- &_link->collision_array);
- }
-#else
- cols = _link->getCollisions(_groupName);
-#endif
-
- if (!cols)
- {
- // group does not exist, create one and add to map
- cols.reset(new std::vector<UrdfCollisionPtr>);
- // new group name, create add vector to map and add Collision to the vector
- _link->collision_groups.insert(make_pair(_groupName, cols));
- }
-
- // 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);
}
////////////////////////////////////////////////////////////////////////////////
-void ReduceVisualToParent(UrdfLinkPtr _link,
- const std::string &_groupName, UrdfVisualPtr _visual)
+void ReduceVisualToParent(UrdfLinkPtr _link, UrdfVisualPtr _visual)
{
- boost::shared_ptr<std::vector<UrdfVisualPtr> > viss;
-#if USE_EXTERNAL_URDF && defined(URDF_GE_0P3)
- if (_link->visual)
- {
- viss.reset(new std::vector<UrdfVisualPtr>);
- viss->push_back(_link->visual);
- }
- else
- {
- viss = boost::shared_ptr<std::vector<UrdfVisualPtr> >(&_link->visual_array);
- }
-#else
- viss = _link->getVisuals(_groupName);
-#endif
-
- if (!viss)
- {
- // 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
- // 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
- 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);
}
////////////////////////////////////////////////////////////////////////////////
@@ -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)
+ 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
+ (*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);
}
}
@@ -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)
+ 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());
}
/////////////////////////////////////////////////
@@ -1827,17 +1676,8 @@
void PrintCollisionGroups(UrdfLinkPtr _link)
{
sdfdbg << "COLLISION LUMPING: link: [" << _link->name << "] contains ["
- << static_cast<int>(_link->collision_groups.size())
+ << static_cast<int>(_link->collision_array.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 @@
}
////////////////////////////////////////////////////////////////////////////////
-void CreateCollisions(TiXmlElement* _elem,
- ConstUrdfLinkPtr _link)
+void CreateCollisions(TiXmlElement* _elem, ConstUrdfLinkPtr _link)
{
- // 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
+ for (std::vector<UrdfCollisionPtr>::const_iterator
+ 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;
- }
- }
+ /* make a <collision> block */
+ CreateCollision(_elem, _link, *collision, collisionPrefix);
}
}
@@ -2293,83 +2068,18 @@
void CreateVisuals(TiXmlElement* _elem,
ConstUrdfLinkPtr _link)
{
- // 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
+ 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
- << "]";
+ 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;
- }
- }
+ // create a <visual> block
+ CreateVisual(_elem, _link, *visual, visualPrefix);
}
}