ROS中调用第三方库tinyxml2解析XML文件（以解析launch文件为例）

1.TinyXML2的简介

第三方库TinyXML2是简单、小型、高效、开源的C++ XML文件解析库，可以很方便的应用到现有的项目之中，非常适合解析XML文件，存储简单数据，配置文件以及对象序列化等数据量不是很大的操作。
TinyXML2详细介绍与详见：TinyXML2官网。

2.TinyXML2的获取与安装

TinyXML2可以通过其Github主页获取源代码，具体来说可以采用git clone命令或直接下载其ZIP压缩包，git clone命令的调用情况如下：

git clone https://github.com/leethomason/tinyxml2

在命令行进入tinyxml2目录下，使用下列命令编译并安装TinyXML2，命令执行情况如下(其中的$符号为shell下的命令提示符)：

$ sudo make install
mkdir -p /usr/local
mkdir -p /usr/local/bin
mkdir -p /usr/local/lib
mkdir -p /usr/local/include
install xmltest /usr/local/bin/xmltest
install -m 644 tinyxml2.h /usr/local/include/tinyxml2.h
install -m 644 libtinyxml2.a /usr/local/lib/libtinyxml2.a

由此，便将tinyxml2库安装到了本地环境中，在tinyxml2目录下执行如下命令，可以运行TinyXML2的测试代码：

$ xmltest

该例程执行时用到的xml文件均位于tinyxml2目录中的resources目录下，该程序执行结束后的终端提示如下图所示：

由此说明tinyxml2库已经成功安装完成。

2.TinyXML2的简单使用示例

将TinyXML2安装到本地环境之后，下面演示TinyXML2的简单使用：
将TinyXML2源码文件夹中的文件xmltest.cpp和resources/dream.xml复制到目标文件夹用于示例演示。

这里，在xmltest.c文件中引用了头文件tinyxml2.h：

#include "tinyxml2.h"

同时声明了TinyXML2的命名空间：

using namespace tinyxml2;

另外，在main函数中定义：若程序的参数大于1，则创建XMLDocument对象并加载程序参数指定的文件，最后若成功加载文件，则输出相关处理时间，该部分代码如下所示。

	if ( argc > 1 ) {
		XMLDocument* doc = new XMLDocument();
		clock_t startTime = clock();
		doc->LoadFile( argv[1] );
 		clock_t loadTime = clock();
		int errorID = doc->ErrorID();
		delete doc; doc = 0;
 		clock_t deleteTime = clock();

		printf( "Test file '%s' loaded. ErrorID=%d\n", argv[1], errorID );
		if ( !errorID ) {
			printf( "Load time=%u\n",   (unsigned)(loadTime - startTime) );
			printf( "Delete time=%u\n", (unsigned)(deleteTime - loadTime) );
			printf( "Total time=%u\n",  (unsigned)(deleteTime - startTime) );
		}
		exit(0);
	}

使用如下命令编译程序：

$ g++ -o xmltest xmltest.cpp -ltinyxml2

若出现找不到头文件或找不到静态库的错误提示，还可以手动指定头文件目录（-I）和库文件目录（-L）：

$ g++ -o xmltest xmltest.cpp -I /usr/local/include -L /usr/local/lib -ltinyxml2 

输入如下命令，执行示例程序：

$ xmltest dream.xml

可以看到程序输出如下，即程序成功加载了XML文件dream.xml。

3.TinyXML2在ROS中的使用

下面通过在ROS工作空间创建一个ROS功能包tinyxml_test使用TinyXML2解析ROS中的Launch文件，以此演示TnyXML2在ROS中的使用。
下面是该功能包的package.xml文件：

<package format="2">
  <name>tinyxml_testname>
  <version>0.0.0version>
  <license>TODOlicense>
  <maintainer email="[email protected]">jackymaintainer>
  <description>The tinyxml_test packagedescription>
  <buildtool_depend>catkinbuildtool_depend>
  <build_depend>roscppbuild_depend>
  <build_depend>tfbuild_depend>
  <build_export_depend>roscppbuild_export_depend>
  <build_export_depend>tfbuild_export_depend>
  <exec_depend>roscppexec_depend>
  <exec_depend>tfexec_depend>
package>

功能包中的Node源文件tinyxml_test_node.cpp代码如下：

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace tinyxml2;//使用tinyxml2命名空间
using namespace std;

/* 功能函数 */
float String2Float(std::string numStr){
  std::stringstream ss(numStr);
  float num;
  ss>>num;
  return num;
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "tinyxml_test_node");
  ros::NodeHandle nh("~"), nh_param("~");

  std::string tf_launch_file;
  //注："test.launch"文件位于catkin_ws/src/tinyxml_test/launch/目录下
  nh_param.param<std::string>("tf_launch_file", tf_launch_file,"test.launch");

  /*Load launch file to get the tf args.*/
  XMLDocument* doc = new XMLDocument();
  std::map<std::string,tf::StampedTransform> stampedTransformMap;//用于存储多个不同坐标系之间的转换关系

  std::string filePath = ros::package::getPath("tinyxml_test");//获取tinyxml_test功能包的绝对路径
  filePath+="/launch/";
  filePath+=tf_launch_file;
  doc->LoadFile( filePath.c_str() );
  int errorID = doc->ErrorID();
  if(errorID){
      ROS_FATAL("[radar_freespace] Failed to load launch file %s.", filePath.c_str());
      return -1;
  }
  ROS_INFO( "[radar_freespace] Launch file '%s' loaded.", filePath.c_str());

  XMLElement* rootElement = doc->RootElement();//获得根元素，即launch
  std::cout<<"Root Element:"<<rootElement->Value()<<std::endl;
  for(const XMLElement* element =rootElement->FirstChildElement();element;element=element->NextSiblingElement()){
    const XMLAttribute* argsAttr = element->FindAttribute("args");
    
    std::string argsAttrStr=std::string(argsAttr->Value());
    std::cout<<"Args Attribute:"<<argsAttrStr<<std::endl;
    std::stringstream ss(argsAttrStr);
    std::vector<std::string> argStrVec;
    std::string tmpArgStr = "";
    while (std::getline(ss, tmpArgStr, ' ')) {
      argStrVec.push_back(tmpArgStr);
    }
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(String2Float(argStrVec.at(0)),String2Float(argStrVec.at(1)),String2Float(argStrVec.at(2))));
    tf::Quaternion quaternion;
    quaternion.setRPY(String2Float(argStrVec.at(5)),String2Float(argStrVec.at(4)),String2Float(argStrVec.at(3)));
    transform.setRotation(quaternion);
    stampedTransformMap[argStrVec.at(6)]=tf::StampedTransform(transform,ros::Time::now(),argStrVec.at(6),argStrVec.at(7));
	
	//输出launch文件中记录的坐标系转换关系
    std::cout<<stampedTransformMap[argStrVec.at(6)].getOrigin().x()<<","<<stampedTransformMap[argStrVec.at(6)].getOrigin().y()<<","<<stampedTransformMap[argStrVec.at(6)].getOrigin().z()<<",";
    std::cout<<stampedTransformMap[argStrVec.at(6)].getRotation().getW()<<","<<stampedTransformMap[argStrVec.at(6)].getRotation().getX()<<","<<stampedTransformMap[argStrVec.at(6)].getRotation().getY()<<","<<stampedTransformMap[argStrVec.at(6)].getRotation().getZ()<<std::endl;
  }
  
  delete doc; //删除对象
  doc = NULL; //避免野指针

  ros::spin();
  return 0;
}

该Node能够利用TinyXML2解析本功能包中的Launch文件（XML格式），该文件为位于catkin_ws/src/tinyxml_test/launch/目录下的文件test.launch，并且最后可以根据读取的launch文件输出其中保存的tf坐标转换信息。test.launch文件的内容如下：

<launch>
  <node pkg="tf" type="static_transform_publisher" name="sensor_frame_to_world" args="2 2 2 0 0 0 world sensor_frame 100" />
  <node pkg="tf" type="static_transform_publisher" name="imu_to_sensor_frame" args="0 0 0 0 0 0 sensor_frame imu 100" />
  <node pkg="tf" type="static_transform_publisher" name="lidar_to_sensor_frame" args="1 1 1 0 0 0 sensor_frame lidar 100" />
launch>

要编译该功能包，还需要修改其CMakeLists.txt文件，添加必要的功能包、包含目录、源代码和链接库等。该文件的完整内容如下：

cmake_minimum_required(VERSION 2.8.3)
project(tinyxml_test)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  roslib
  tf
)

###################################
## catkin specific configuration ##
###################################

catkin_package(
  LIBRARIES 
    ${PROJECT_NAME}
  CATKIN_DEPENDS
   roscpp tf
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_node src/tinyxml_test_node.cpp)

## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_node
  -ltinyxml2
  ${catkin_LIBRARIES}
)

#############
## Install ##
#############

## Mark executables and/or libraries for installation
install(
  TARGETS 
    ${PROJECT_NAME}_node
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

需要注意的是：由于程序需要调用第三方库TinyXML2，因此需要在CMakeLists.txt文件中的find_package选项中添加roslib功能包，并在target_link_libraries选项中指定链接该静态库（-ltinyxml2）。

最后，执行命令catkin_make将功能包编译成功之后，使用如下命令运行节点（需提前运行roscore节点）：

$ rosrun tinyxml_test tinyxml_test_node 

程序能够输出launch文件中保存的tf坐标转换信息：

大功告成！