From f641312f0a212258b8d40da9cbf9ada250ee21ab Mon Sep 17 00:00:00 2001 From: wim Date: Tue, 24 Nov 2009 21:04:39 +0000 Subject: [PATCH] update to latests design with optional root link --- urdf/include/urdf/model.h | 5 ++ urdf/src/model.cpp | 86 +++++++++++++++++---------- urdf/test/test_robot_model_parser.cpp | 2 +- 3 files changed, 61 insertions(+), 32 deletions(-) diff --git a/urdf/include/urdf/model.h b/urdf/include/urdf/model.h index cf7160b..c54ba52 100644 --- a/urdf/include/urdf/model.h +++ b/urdf/include/urdf/model.h @@ -99,6 +99,11 @@ private: /// it's time to build a tree bool initTree(std::map &parent_link_tree); + /// in initXml(), onece tree is built, + /// it's time to find the root Link + bool initRoot(std::map &parent_link_tree); + + /// Model is restricted to a tree for now, which means there exists one root link /// typically, root link is the world(inertial). Where world is a special link /// or is the root_link_ the link attached to the world by PLANAR/FLOATING joint? diff --git a/urdf/src/model.cpp b/urdf/src/model.cpp index d22305b..648c33c 100644 --- a/urdf/src/model.cpp +++ b/urdf/src/model.cpp @@ -238,63 +238,62 @@ bool Model::initXml(TiXmlElement *robot_xml) return false; } + // find the root link + if (!this->initRoot(parent_link_tree)) + { + ROS_ERROR("failed to find root link"); + return false; + } + return true; } bool Model::initTree(std::map &parent_link_tree) { - this->root_link_.reset(); - // loop through all joints, for every link, assign children links and children joints for (std::map >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) { std::string parent_link_name = joint->second->parent_link_name; std::string child_link_name = joint->second->child_link_name; - ROS_DEBUG(" Build tree: joint: '%s' has parent link '%s' and child link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str()); + ROS_DEBUG("build tree: joint: '%s' has parent link '%s' and child link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str()); - // case where joint is not connected to any link if (parent_link_name.empty() && !child_link_name.empty()) { - ROS_ERROR(" Joint %s specifies only parent link but not child link",(joint->second)->name.c_str()); + ROS_ERROR(" Joint %s specifies child link but not parent link.",(joint->second)->name.c_str()); return false; } - else if (!parent_link_name.empty() && child_link_name.empty()) + else if (!parent_link_name.empty() && child_link_name.empty()) { - ROS_ERROR(" Joint %s specifies only child link but not parent link",(joint->second)->name.c_str()); + ROS_ERROR(" Joint %s specifies parent link but not child link.",(joint->second)->name.c_str()); return false; } else if (parent_link_name.empty() && child_link_name.empty()) { - ROS_DEBUG(" Joint %s has not parent or child link, it is an abstract joint.",(joint->second)->name.c_str()); + ROS_DEBUG(" Joint %s specifies no parent link and no child link. This is an abstract joint.",(joint->second)->name.c_str()); } - - // normal joint case else { - // find child and parent link of joint + // find child and parent links boost::shared_ptr child_link, parent_link; this->getLink(child_link_name, child_link); - this->getLink(parent_link_name, parent_link); if (!child_link) { - ROS_ERROR(" Child link '%s' of joint: %s not found",child_link_name.c_str(),joint->first.c_str()); + ROS_ERROR(" child link '%s' of joint '%s' not found", child_link_name.c_str(), joint->first.c_str() ); return false; } + this->getLink(parent_link_name, parent_link); if (!parent_link) { - if (root_link_) - { - ROS_ERROR("This tree contains two root links"); - return false; - } - ROS_DEBUG(" Parent link '%s' of joint '%s' not found. This should be the root link.", parent_link_name.c_str(), joint->first.c_str() ); + ROS_DEBUG(" parent link '%s' of joint '%s' not found. Automatically adding it. This must be the root link", + parent_link_name.c_str(), joint->first.c_str() ); + parent_link.reset(new Link); parent_link->name = parent_link_name; - root_link_ = parent_link; - this->links_.insert(make_pair(parent_link_name, parent_link)); + this->links_.insert(make_pair(parent_link->name, parent_link)); + ROS_DEBUG(" successfully added new link '%s'", parent_link->name.c_str()); } - + //set parent link for child link child_link->setParent(parent_link); @@ -303,28 +302,53 @@ bool Model::initTree(std::map &parent_link_tree) //set child joint for parent link parent_link->addChildJoint(joint->second); - + //set child link for parent link parent_link->addChild(child_link); - + // fill in child/parent string map parent_link_tree[child_link->name] = parent_link_name; - + ROS_DEBUG(" now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size()); } } - // we should have found root - if (!root_link_){ - ROS_ERROR("The tree does not contain a root link"); - return false; - } - return true; } +bool Model::initRoot(std::map &parent_link_tree) +{ + + this->root_link_.reset(); + + for (std::map::iterator p=parent_link_tree.begin(); p!=parent_link_tree.end(); p++) + { + if (parent_link_tree.find(p->second) == parent_link_tree.end()) + { + if (this->root_link_) + { + ROS_DEBUG("child '%s', parent '%s', root '%s'", p->first.c_str(), p->second.c_str(), this->root_link_->name.c_str()); + if (this->root_link_->name != p->second) + { + ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), p->second.c_str()); + return false; + } + } + else + getLink(p->second,this->root_link_); + } + } + if (!this->root_link_) + { + ROS_ERROR("No root link found. The robot xml is not a valid tree."); + return false; + } + ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str()); + + return true; +} boost::shared_ptr Model::getMaterial(const std::string& name) const { diff --git a/urdf/test/test_robot_model_parser.cpp b/urdf/test/test_robot_model_parser.cpp index efef0fb..ef46eb1 100644 --- a/urdf/test/test_robot_model_parser.cpp +++ b/urdf/test/test_robot_model_parser.cpp @@ -76,7 +76,7 @@ TEST_F(TestParser, test) robot_model_xml.LoadFile(g_argv[i]); TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot"); ASSERT_TRUE(robot_xml != NULL); - if (i == g_argc-1) + if (i >= g_argc-2) ASSERT_TRUE(robot.initXml(robot_xml)); else ASSERT_FALSE(robot.initXml(robot_xml));