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

make joint to root mandatory (as in documentation), add visualization tool, and add regression test

This commit is contained in:
wim 2009-11-24 17:41:11 +00:00
parent efb5225fc3
commit e72682fe0b
7 changed files with 3626 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
urdf/CMakeLists.txt
View File

@ -27,8 +27,11 @@ rosbuild_add_library(${PROJECT_NAME} src/link.cpp src/joint.cpp src/model.cpp)
rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
rosbuild_add_executable(urdf_check src/urdf_check.cpp)
target_link_libraries(urdf_check ${PROJECT_NAME})
rosbuild_add_executable(check_urdf src/check_urdf.cpp)
target_link_libraries(check_urdf ${PROJECT_NAME})
rosbuild_add_executable(urdf_to_graphiz src/urdf_to_graphiz.cpp)
target_link_libraries(urdf_to_graphiz ${PROJECT_NAME})
rosbuild_add_executable(mem_test test/memtest.cpp)
target_link_libraries(mem_test ${PROJECT_NAME})

5
urdf/include/urdf/model.h
View File

@ -99,11 +99,6 @@ private:
/// it's time to build a tree
bool initTree(std::map<std::string, std::string> &parent_link_tree);
/// in initXml(), onece tree is built,
/// it's time to find the root Link
bool initRoot(std::map<std::string, std::string> &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?

0
urdf/src/urdf_check.cpp → urdf/src/check_urdf.cpp
View File

110
urdf/src/model.cpp
View File

@ -238,68 +238,63 @@ 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<std::string, std::string> &parent_link_tree)
{
this->root_link_.reset();
// loop through all joints, for every link, assign children links and children joints
for (std::map<std::string,boost::shared_ptr<Joint> >::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());
/// deal with deprecated case
if (parent_link_name == "world" && !this->getLink(parent_link_name)){
ROS_WARN("Joint %s specifies the world link as its parent, but there in no link called world. This used to be allowed, but this behavior has been deprecated. You need to explicitly specify each link in your tree.", joint->first.c_str());
boost::shared_ptr<Link> link;
link.reset(new Link);
link->name = "world";
this->links_.insert(make_pair(link->name,link));
ROS_DEBUG(" successfully added new link '%s'", link->name.c_str());
}
if (parent_link_name.empty())
// case where joint is not connected to any link
if (parent_link_name.empty() && !child_link_name.empty())
{
ROS_DEBUG(" Joint %s: does not have parent link name specified. Joint is an abstract joint.",(joint->second)->name.c_str());
}
else if (child_link_name.empty())
{
ROS_DEBUG(" Joint %s: does not have child link name specified. Joint is an abstract joint.",(joint->second)->name.c_str());
}
else
{
// find parent link
boost::shared_ptr<Link> parent_link;
this->getLink(parent_link_name, parent_link);
if (!parent_link)
{
ROS_ERROR(" parent link '%s' of joint '%s' not found", parent_link_name.c_str(), joint->first.c_str() );
ROS_ERROR(" Joint %s specifies only parent link but not child link",(joint->second)->name.c_str());
return false;
}
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());
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());
}
// normal joint case
else
{
// find child link
boost::shared_ptr<Link> child_link;
// find child and parent link of joint
boost::shared_ptr<Link> 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;
}
else
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() );
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));
}
//set parent link for child link
child_link->setParent(parent_link);
@ -318,7 +313,11 @@ bool Model::initTree(std::map<std::string, std::string> &parent_link_tree)
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;
@ -326,37 +325,6 @@ bool Model::initTree(std::map<std::string, std::string> &parent_link_tree)
bool Model::initRoot(std::map<std::string, std::string> &parent_link_tree)
{
this->root_link_.reset();
for (std::map<std::string, std::string>::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<Material> Model::getMaterial(const std::string& name) const
{

117
urdf/src/urdf_to_graphiz.cpp Normal file
View File

@ -0,0 +1,117 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redstributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include "urdf/model.h"
#include <iostream>
#include <fstream>
using namespace urdf;
using namespace std;
void addChildLinkNames(boost::shared_ptr<const Link> link, ofstream& os)
{
os << "\"" << link->name << "\" [label=\"" << link->name << "\"];" << endl;
for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
addChildLinkNames(*child, os);
}
void addChildJointNames(boost::shared_ptr<const Link> link, ofstream& os)
{
double r, p, y;
for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){
(*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y);
os << "\"" << link->name << "\" -> \"" << (*child)->parent_joint->name
<< "\" [label=\"xyz: "
<< (*child)->parent_joint->parent_to_joint_origin_transform.position.x << " "
<< (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " "
<< (*child)->parent_joint->parent_to_joint_origin_transform.position.z << " "
<< "\\nrpy: " << r << " " << p << " " << y << "\"]" << endl;
os << "\"" << (*child)->parent_joint->name << "\" -> \"" << (*child)->name << "\"" << endl;
addChildJointNames(*child, os);
}
}
void printTree(boost::shared_ptr<const Link> link, string file)
{
std::ofstream os;
os.open(file.c_str());
os << "digraph G {" << endl;
os << "node [shape=box];" << endl;
addChildLinkNames(link, os);
os << "node [shape=ellipse, color=blue, fontcolor=blue];" << endl;
addChildJointNames(link, os);
os << "}" << endl;
os.close();
}
int main(int argc, char** argv)
{
if (argc != 2){
std::cerr << "Usage: urdf_to_graphiz input.xml" << std::endl;
return -1;
}
TiXmlDocument robot_model_xml;
robot_model_xml.LoadFile(argv[1]);
TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
if (!robot_xml){
std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl;
return -1;
}
Model robot;
if (!robot.initXml(robot_xml)){
std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
return -1;
}
string output = robot.getName();
// print entire tree to file
printTree(robot.getRoot(), output+".gv");
cout << "Created file " << output << ".gv" << endl;
string command = "dot -Tpdf "+output+".gv -o "+output+".pdf";
system(command.c_str());
cout << "Created file " << output << ".pdf" << endl;
return 0;
}

3448
urdf/test/pr2_desc_explicit_world.xml Normal file
View File

File diff suppressed because it is too large Load Diff

2
urdf/test/test_robot_model_parser.launch
View File

@ -1,3 +1,3 @@
<launch>
<test test-name="robot_model_parser_test" pkg="urdf" type="test_parser" args="$(find urdf)/test/pr2_desc_double_joint.xml $(find urdf)/test/pr2_desc_loop.xml $(find urdf)/test/pr2_desc_no_joint.xml $(find urdf)/test/pr2_desc_two_trees.xml $(find urdf)/test/pr2_desc_bracket.xml $(find urdf)/test/pr2_desc_double.xml $(find urdf)/test/pr2_desc_no_joint2.xml $(find urdf)/test/pr2_desc_parent_itself.xml $(find urdf)/test/pr2_desc_no_filename_in_mesh.xml $(find urdf)/test/pr2_desc.xml" />
<test test-name="robot_model_parser_test" pkg="urdf" type="test_parser" args="$(find urdf)/test/pr2_desc_double_joint.xml $(find urdf)/test/pr2_desc_loop.xml $(find urdf)/test/pr2_desc_no_joint.xml $(find urdf)/test/pr2_desc_two_trees.xml $(find urdf)/test/pr2_desc_bracket.xml $(find urdf)/test/pr2_desc_double.xml $(find urdf)/test/pr2_desc_no_joint2.xml $(find urdf)/test/pr2_desc_parent_itself.xml $(find urdf)/test/pr2_desc_no_filename_in_mesh.xml $(find urdf)/test/pr2_desc_explicit_world.xml $(find urdf)/test/pr2_desc.xml" />
</launch>