bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

improve detection of valid chains

This commit is contained in:
Ioan Sucan 2012-04-12 23:51:49 +03:00
parent e29657e464
commit 91ab5c7cdb
1 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
srdf/src/model.cpp
View File

@ -179,13 +179,26 @@ void srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElemen
} }
bool found = false; bool found = false;
boost::shared_ptr<const urdf::Link> l = urdf_model.getLink(tip_str); boost::shared_ptr<const urdf::Link> l = urdf_model.getLink(tip_str);
std::set<std::string> seen;
while (!found && l) while (!found && l)
{ {
seen.insert(l->name);
if (l->name == base_str) if (l->name == base_str)
found = true; found = true;
else else
l = l->getParent(); l = l->getParent();
} }
if (!found)
{
l = urdf_model.getLink(base_str);
while (!found && l)
{
if (seen.find(l->name) != seen.end())
found = true;
else
l = l->getParent();
}
}
if (found) if (found)
g.chains_.push_back(std::make_pair(base_str, tip_str)); g.chains_.push_back(std::make_pair(base_str, tip_str));
else else