ros库解读(二)

KDL Tree

C++创建

1.为了使用KDL parser需要在package.xml中添加相关依赖项：

<package>
    ...
    <build_depend package="kdl_parser" />
    ...
    <run_depend package="kdl_parser" />
    ...
  </package>

2.另外还需要在C++程序中加入相关的头文件：

#include <kdl_parser/kdl_parser.hpp>

从urdf创建KDL tree的方式：

From a file

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

From the parameter server

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;
   }

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;
   }

From a 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;
   }