ROS学习笔记( 二十一)： URDF 进阶

1.创建自己的urdf机器人描述文件。

1.1 创建树结构

图像中的机器人是树形结构。 让我们开始非常简单，创建树结构的描述，而不必担心尺寸等。启动您喜欢的文本编辑器，并创建一个名为my_robot.urdf的文件：

<robot name="test_robot">
  <link name="link1" />
  <link name="link2" />
  <link name="link3" />
  <link name="link4" />


  <joint name="joint1" type="continuous">
    <parent link="link1"/>
    <child link="link2" />
  </joint>

  <joint name="joint2" type="continuous">
    <parent link="link1" />
    <child link="link3" />
  </joint>

  <joint name="joint3" type="continuous">
    <parent link="link3"/>
    <child link ="link4"/>

</robot>

因此，仅需创建结构就非常简单！ 现在，让我们看看是否可以解析此urdf文件。 有一个简单的命令行工具可以为您解析urdf文件，并告诉您语法是否正确：
您可能需要将urdfdom安装为独立于ROS的上游软件包：

sudo apt-get install liburdfdom-tools

现在在刚刚创建的urdf文件相应文件夹中打开终端，运行检查命令：

check_urdf	*.urdf

1.2 添加维度

现在我们有了基本的树结构，让我们添加适当的尺寸。 如您在机器人图像中所注意到的，每个链接的参考框架（绿色）位于链接的底部，并且与关节的参考框架相同。 因此，要为树添加尺寸，我们只需指定从链接到其子节点的偏移量即可。 为此，我们将字段添加到每个关节。

让我们看第二个关节。 关节2在Y方向上相对于链接1偏移，在负X方向上相对于链接1偏移，并且它绕Z轴旋转了90度。 因此，我们需要添加以下元素：

切换行号显示

   1   

如果对所有元素重复此操作，我们的URDF将如下所示：

更新文件my_robot.urdf并通过解析器运行它:

1.3 完成运动学模型

我们尚未指定的是关节围绕哪个轴旋转。 添加完之后，我们实际上就拥有了该机器人的完整运动学模型！ 我们要做的就是将元素添加到每个关节。 该轴指定局部框架中的旋转轴。

因此，如果看一下joint2，就会看到它绕着Y轴正方向旋转。 因此，只需将以下xml添加到joint元素中：

切换行号显示

   1   

同样，关节1围绕以下轴旋转：

切换行号显示

   1   

请注意，对轴进行归一化是个好主意。

如果将其添加到机器人的所有关节，则我们的URDF如下所示：

check_urdf my_robot.urdf

就是这样，您创建了第一个URDF机器人说明！ 现在，您可以尝试使用graphiz可视化URDF：

urdf_to_graphiz my_robot.urdf

并使用您喜欢的pdf查看器打开生成的文件：

evince test_robot.pdf

2.解析urdf文件

2.1 读取URDF文件

让我们首先创建一个依赖于urdf解析器的包：

 cd ~/catkin_ws/src
 catkin_create_pkg testbot_description urdf
  cd testbot_description

现在创建一个/ urdf文件夹来存储我们刚刚创建的urdf文件：

mkdir urdf
cd urdf

这遵循始终将机器人的URDF文件存储在名为MYROBOT_description的ROS包中以及位于名为/ urdf的子文件夹中的约定。 机器人描述包中的其他标准子文件夹包括/ meshes，/ media和/ cad，如下所示：

#include 
#include "ros/ros.h"

int main(int argc, char** argv){
	ros::init(argc, argv, "my_parser");
	if (argc !=2){
	ROS_ERROR("Need a urdf file as argument" );
	return -1;
	}
	std::string urdf_file = argv[1];

	urdf::Model model;
	if (!model.initFile(urdf_file)){
	ROS_ERROR("Failed to parse urdf file" );
	return -1;
	}

	ROS_INFO("Successfully parsed urdf file");
	return 0;
}

实际动作发生在第12-13行。 在这里，我们创建一个解析器对象，并通过提供文件名从文件中对其进行初始化。 如果已成功解析URDF文件，则initFile方法将返回true。

现在，让我们尝试运行此代码。 首先将以下行添加到您的CMakeList.txt文件中：
构建您的程序包并运行它。

$ cd ~/catkin_ws   
$ catkin_make
$ ./parser my_robot.urdf
# Example: ./devel/lib/testbot_description/parser ./src/testbot_description/urdf/my_robot.urdf

接下来就会出现：

3.在自己的机器人上使用机器人状态发布器

当您使用具有许多相关框架的机器人时，将它们全部发布到tf成为一项艰巨的任务。 机器人状态发布者是一款可以为您完成此工作的工具。
机器人状态发布者可帮助您将机器人的状态广播到tf转换库。 机器人状态发布者在内部具有机器人的运动学模型。 因此，根据机器人的关节位置，机器人状态发布者可以计算和广播机器人中每个链接的3D姿态。

您可以将机械手状态发布器用作独立的ROS节点或库：

3.1 作为ROS节点运行

3.1.1 robot_state_publisher

运行机器人状态发布者的最简单方法是作为节点。 对于普通用户，这是建议的用法。 您需要执行以下两项操作来运行机械手状态发布器：

• 在Parameter Server上加载的urdf xml机械手描述。
• 将关节位置发布为sensor_msgs / JointState的源。

请阅读以下章节，了解如何为robot_state_publisher配置参数和主题。

3.1.1.1 Subscribed topics

joint_states (sensor_msgs/JointState)

关节位置信息

3.1.1.2 Parameters

robot_description (urdf map)

urdf xml机械手说明， 通过访问   `urdf_model::initParam` 

tf_prefix (string)

  设置tf前缀，以进行名称空间感知的转换发布。 有关更多详细信息，请参见tf_prefix。

publish_frequency (double)

状态发布者的发布频率, default: 50Hz. 

3.1.2 启动文件示例

设置XML机械手描述和关节位置信息源之后，只需创建一个启动文件，如下所示：

3.2 作为library运行

高级用户还可以从自己的C ++代码中以库的形式运行机械手状态发布器。 包含标题后：

  #include 

您只需要一个包含KDL树的构造函数:

RobotStatePublisher(const KDL::Tree& tree);

现在，每次您要发布机器人状态时，都可以调用publishTransforms函数：

  // publish moving joints
  void publishTransforms(const std::map& joint_positions,
                         const ros::Time& time);

  // publish fixed joints
  void publishFixedTransforms();

第一个参数是具有关节名称和关节位置的地图，第二个参数是记录关节位置的时间。 如果地图不包含所有关节名称，也可以。 如果地图包含的运动学模型不包含某些关节名称，也可以。 但是请注意，如果您不将运动模型中的某些关节告诉联合状态发布者，那么您的tf树将是不完整的。

4.开始使用KDL解析器

4.1 构建KDL解析器

rosdep install kdl_parser

这将为kdl_parser安装所有外部依赖项。 要生成软件包，请运行：

rosmake kdl_parser

4.2 在代码中使用

首先，将KDL解析器作为依赖项添加到package.xml（在ROS fuerte或更早版本中为manifest.xml）文件中：

    ...
    
    ...
    
    ...
  

要开始在C ++代码中使用KDL解析器，请包含以下文件：

切换行号显示

   1   #include <kdl_parser/kdl_parser.hpp>
   2 

现在有不同的方式进行。 您可以通过urdf以各种形式构造KDL树：

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

要创建PR2 URDF文件，请运行以下命令：

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

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

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

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

5. 将urdf与robot_state_publisher一起使用

5.1 创建URDF文件

这是7链接模型的URDF文件，大致近似于R2-D2。 将以下链接保存到您的计算机：model.xml

5.2 发布状态

现在，我们需要一种方法来指定机器人所处的状态。为此，我们必须指定所有三个关节和整个里程表。 首先创建一个包：

cd %TOP_DIR_YOUR_CATKIN_WS%/src
catkin_create_pkg r2d2 roscpp rospy tf sensor_msgs std_msgs

然后启动您喜欢的编辑器，并将以下代码粘贴到src / state_publisher.cpp文件中：

5.3 启动文件

此启动文件假定您正在使用程序包名称“ r2d2”和节点名称“ state_publisher”，并且已将此urdf保存到“ r2d2”程序包中。

切换行号显示

 
         
         
         
 

5.4 查看结果

首先，我们必须在保存上述源代码的程序包中编辑CMakeLists.txt。 确保除了其他依赖项之外还添加tf依赖项：

find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs tf)

注意，roscpp用于解析我们编写的代码并生成state_publisher节点。 我们还必须在CMakelists.txt的末尾添加以下内容，以生成state_publisher节点：

include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(state_publisher src/state_publisher.cpp)
target_link_libraries(state_publisher ${catkin_LIBRARIES})

现在，我们应该转到工作空间的目录并使用以下命令进行构建：

 catkin_make

现在启动该软件包（假设我们的启动文件名为display.launch）：

roslaunch r2d2 display.launch

使用以下命令在新终端中运行rviz：

rosrun rviz rviz

选择odom作为您的固定框架（在“全局选项”下）。 然后选择“添加显示”并添加机器人模型显示和TF显示.