URDF解析器
在前两篇文章1, 2中， 我们简单的介绍了如何使用URDF来描述一个机器人的渲染模型、碰撞模型、物理特性。我们在进行机器人软件的开发的时候会频繁的查询机器人模型，来完成预期的功能。 在本文中我们介绍如何在程序中访问URDF文件描述的模型。
我们先根据前两篇文章，创建了一个URDF文件，描述了一个如右图所示的机器人，图中白色的部分都是可以转动的关节，蓝色的部分则是四肢、头和躯干。 相应的URDF文件可以从这里下载。这是一个有23个自由度的双足机器人，叫做GRobot。
1. 一个简单例程
首先我们在工作空间下创建一个"learning_urdf"的package:
$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_urdf rospy roscpp urdf
$ cd learning_urdf
在新建的package的根目录下，创建一个urdf的子目录用于保存教程中需要用到的urdf文件。我们可以把GRobot从这里下载下来，保存到这个子目录中。
$ mkdir urdf
$ cd urdf
$ wget http://gaoyichao.com/Xiaotu//ros/src/GRobot.urdf
$ cd ..
然后创建一个src子目录用于保存用到的CPP源代码，把下面的示例代码保存到这个子目录中。这段代码从指定的urdf文件中解析出了机器人的模型，并在标准输出中打印了一些关于机器人的信息。
我们会在本文的后续内容中详细介绍urdf相关的API。
#include <urdf/model.h>
#include <ros/ros.h>
#include <iostream>
int main(int argc, char *argv[]) {
ros::init(argc, argv, "parser");
if (2 != argc) {
ROS_ERROR("Need a urdf file as argument");
return -1;
}
std::string urdf_file_name = argv[1];
urdf::Model model;
if (!model.initFile(urdf_file_name)) {
ROS_ERROR("Failed to parse urdf file");
return -1;
}
std::cout << "Successfully parsed urdf file" << std::endl;
std::cout << "机器人名称: " << model.getName() << std::endl;
std::cout << "根节点: " << model.getRoot()->name << std::endl;
std::cout << "根节点下有 " << model.getRoot()->child_joints.size() << "个关节" << std::endl;
std::cout << "分别是:" << std::endl;
for (int i = 0; i < model.getRoot()->child_joints.size(); i++)
std::cout << model.getRoot()->child_joints[i]->name << std::endl;
return 0;
}
接下来修改CMakeLists.txt文件，添加对例程的编译规则:
add_executable(parser src/parser.cpp)
target_link_libraries(parser ${catkin_LIBRARIES})
切换到工作空间的根目录下，编译运行就可以看到如下的结果，说明我们已经可以通过程序访问机器人模型了。
$ cd ~/catkin_ws
$ catkin_make
$ source ./devel/sertup.bash
$ roscd learning_urdf
$ rosrun learning_urdf parser urdf/GRobot.urdf
Successfully parsed urdf file
机器人名称: GRobot
根节点: base_link
根节点下有 5个关节
分别是:
base_to_left_hip_z
base_to_left_shoulder_y
base_to_neck_z
base_to_right_hip_z
base_to_right_shoulder_y
2. urdf接口的简单介绍
在上一节的例程当中，为了访问urdf的接口，我们包含了头文件"urdf/model.h"，并在main函数中构建了一个urdf::Model的对象。 在Github上找到这个头文件，可以看到urdf::Model这个类是从ModelInterface中派生出来的。 而urdf::Model中只是实现了各种初始化对象的函数:
namespace urdf{
class Model: public ModelInterface {
public:
/// \brief Load Model from TiXMLElement
bool initXml(TiXmlElement *xml);
/// \brief Load Model from TiXMLDocument
bool initXml(TiXmlDocument *xml);
/// \brief Load Model given a filename
bool initFile(const std::string& filename);
/// \brief Load Model given the name of a parameter on the parameter server
bool initParam(const std::string& param);
/// \brief Load Model given the name of a parameter on the parameter server using provided nodehandle
bool initParamWithNodeHandle(const std::string& param, const ros::NodeHandle& nh = ros::NodeHandle());
/// \brief Load Model from a XML-string
bool initString(const std::string& xmlstring);
};
// shared_ptr declarations moved to urdf/urdfdom_compatibility.h to allow for std::shared_ptrs in latest version
}
阅读其所对应的源文件，可以发现这些初始化的函数最终都会归结到函数initString()。
在我们的例程中是通过initFile()进行初始化的，这个函数首先从指定的文件中把urdf文件内容读到一个字符串std::String对象中，然后调用initString()完成机器人模型的创建。其它的函数都是类似的操作。
而ModelInterface是在另外一个叫做urdfdom_headers的package中定义和实现的。它是一个与ROS环境无关的package， 我们可以在其它非ROS的软件中使用这个package。
在头文件中"urdf_model/model.h"定义了ModelInterface。 它提供了getRoot()函数可以获得机器人模型的根节点。
LinkConstSharedPtr getRoot(void) const;
我们也可以通过名称访问机器人的特定Link或者Joint。
LinkConstSharedPtr getLink(const std::string& name) const;
JointConstSharedPtr getJoint(const std::string& name) const;
还可以获知URDF文件中定义的材料(Material):
MaterialSharedPtr getMaterial(const std::string& name) const;
URDF文件中也就是描述了Link,Joint和Material这三种对象。它们在ModelInterface中都是由public的std::map容器保存的，我们也可以直接访问这些容器：
std::map<std::string, LinkSharedPtr> links_;
std::map<std::string, JointSharedPtr> joints_;
std::map<std::string, MaterialSharedPtr> materials_;
其中LinkSharedPtr, JointSharedPtr, MaterialSharedPtr分别是指向Link, Joint和Material对象的智能指针。 智能指针是一个模板类，它对C++中原生的指针进行了一些封装，使得我们在使用的时候不必考虑过多的指针释放的问题。
下面是从官方Wiki上盗下的的图，其中描述了URDF相关的各个package的关系。其中urdf, urdf_parser_plugin, collada_parser都是robot_model的一部分， 现在他们已经从robot_model中独立出来了。urdf, urdf_parser_plugin一起托管在urdf下， collada_parser则托管在collada_urdf下。它们都是运行在ROS环境中的package。
 |
$ ls /usr/include | grep urdf
urdf_exception
urdf_model
urdf_model_state
urdf_parser
urdf_sensor
urdf_world
3. 总结
在本文中，我们介绍了如何在C++程序中访问URDF模型，简单的介绍了URDF相关的API以及链接。具体的API使用方法还需要查看源码，官方给的文档和示例并不是很清楚。
基本上，包含了头文件"urdf/model.h"并创建了urdf::Model对象，就可以通过该对象获得关于机器人模型的所有数据。urdf::Model只是定义了一些初始化对象的函数，它派生自ModelInterface。 ModelInterface则提供了访问机器人模型的各种接口，我们可以通过get函数来获取指定的Link, Joint和Material也可以直接通过pulic成员links_, joints_, materials_来访问。