444 lines
17 KiB
Diff
444 lines
17 KiB
Diff
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);
|
|
}
|
|
}
|
|
|