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 > cols; -#if USE_EXTERNAL_URDF && defined(URDF_GE_0P3) - if (_link->collision) - { - cols.reset(new std::vector); - cols->push_back(_link->collision); - } - else - { - cols = boost::shared_ptr >( - &_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); - // 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::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 > viss; -#if USE_EXTERNAL_URDF && defined(URDF_GE_0P3) - if (_link->visual) - { - viss.reset(new std::vector); - viss->push_back(_link->visual); - } - else - { - viss = boost::shared_ptr >(&_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); - // 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::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 > >::iterator - visualsIt = _link->visual_groups.begin(); - visualsIt != _link->visual_groups.end(); ++visualsIt) + for (std::vector::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::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::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 > >::iterator - collisionsIt = _link->collision_groups.begin(); - collisionsIt != _link->collision_groups.end(); ++collisionsIt) + for (std::vector::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::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::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(_link->collision_groups.size()) + << static_cast(_link->collision_array.size()) << "] collisions.\n"; - for (std::map > >::iterator - colsIt = _link->collision_groups.begin(); - colsIt != _link->collision_groups.end(); ++colsIt) - { - sdfdbg << " collision_groups: [" << colsIt->first << "] has [" - << static_cast(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 > >::const_iterator - collisionsIt = _link->collision_groups.begin(); - collisionsIt != _link->collision_groups.end(); ++collisionsIt) + // loop through collisions + for (std::vector::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::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 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 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 > >::const_iterator - visualsIt = _link->visual_groups.begin(); - visualsIt != _link->visual_groups.end(); ++visualsIt) + // loop through visuals + for (std::vector::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::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 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 block + CreateVisual(_elem, _link, *visual, visualPrefix); } }