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 UrdfCollisionPtr; typedef boost::shared_ptr UrdfVisualPtr; typedef boost::shared_ptr 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 > cols; #if USE_EXTERNAL_URDF @@ -312,9 +319,6 @@ void ReduceCollisionToParent(UrdfLinkPtr cols = boost::shared_ptr >( &_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 >( + &_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::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 > viss; #if USE_EXTERNAL_URDF @@ -351,9 +370,6 @@ void ReduceVisualToParent(UrdfLinkPtr _l { viss = boost::shared_ptr >(&_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 >(&_link->visual_array); +#endif // 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 +#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 > >::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::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 > >::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::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: + // + // + // + // + // + // are treated as blobs that gets inserted + // into visuals for the link + // + // + // + // + // 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(_link->collision_groups.size()) << "] collisions.\n"; @@ -1838,6 +1910,11 @@ void PrintCollisionGroups(UrdfLinkPtr _l << static_cast(colsIt->second->size()) << "] Collision objects\n"; } +#else + sdfdbg << "COLLISION LUMPING: link: [" << _link->name << "] contains [" + << static_cast(_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 > >::const_iterator collisionsIt = _link->collision_groups.begin(); @@ -2287,6 +2365,33 @@ void CreateCollisions(TiXmlElement* _ele } } } +#else + unsigned int defaultMeshCount = 0; + for (std::vector::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 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 > >::const_iterator visualsIt = _link->visual_groups.begin(); @@ -2371,6 +2477,33 @@ void CreateVisuals(TiXmlElement* _elem, } } } +#else + unsigned int defaultMeshCount = 0; + for (std::vector::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 block + CreateVisual(_elem, _link, *visual, visualPrefix); + + // only 1 default mesh + ++defaultMeshCount; + } +#endif } ////////////////////////////////////////////////////////////////////////////////