ROS学习笔记39（Start using the KDL parser）

1 编译KDL parser

 $ rosdep install kdl_parser

This will install all the external dependencies for the kdl_parser. 编译功能包，如下:

 $ rosmake kdl_parser

2 在你的程序中使用

首先，在你的package.xml文件中添加the KDL parser作为一个依赖项:

  
    ...
    
    ...
    
    ...
  

在您的C++程序中开始使用KDL parser，include the following file:

 #include 

现在有不同的方法进行出来。你可以从urdf文件构建KDL树以不同的形式:

2.1 From a file

KDL::Tree my_tree;
   if (!kdl_parser::treeFromFile("filename", my_tree)){
      ROS_ERROR("Failed to construct kdl tree");
      return false;
   }

To create the PR2 URDF file, run the following command:

$ rosrun xacro xacro.py `rospack find pr2_description`/robots/pr2.urdf.xacro > pr2.urdf

2.2 从参数服务器

KDL::Tree my_tree;
   ros::NodeHandle node;
   std::string robot_desc_string;
   node.param("robot_description", robot_desc_string, std::string());
   if (!kdl_parser::treeFromString(robot_desc_string, my_tree)){
      ROS_ERROR("Failed to construct kdl tree");
      return false;
   }

2.3 From an xml element

 KDL::Tree my_tree;
   TiXmlDocument xml_doc;
   xml_doc.Parse(xml_string.c_str());
   xml_root = xml_doc.FirstChildElement("robot");
   if (!xml_root){
      ROS_ERROR("Failed to get robot from xml document");
      return false;
   }
   if (!kdl_parser::treeFromXml(xml_root, my_tree)){
      ROS_ERROR("Failed to construct kdl tree");
      return false;
   }

2.4 从 URDF model

 KDL::Tree my_tree;
   urdf::Model my_model;
   if (!my_model.initXml(....)){
      ROS_ERROR("Failed to parse urdf robot model");
      return false;
   }
   if (!kdl_parser::treeFromUrdfModel(my_model, my_tree)){
      ROS_ERROR("Failed to construct kdl tree");
      return false;
   }