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 <origin xyz="-2 5 0" rpy="0 0 1.57" />
如果对所有元素重复此操作，我们的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"/>
<orgin xyz="5 3 0" rpy="0 0 0" />
</joint>
<joint name="joint2" type="continuous">
<parent link="link1"/>
<child link="link3"/>
<orgin xyz="-2 5 0" rpy="0 0 1.57" />
</joint>
<joint name="joint3" type="continuous">
<parent link="link3"/>
<child link="link4"/>
<orgin xyz="5 0 0" rpy="0 0 -1.57" />
</joint>
</robot>
更新文件my_robot.urdf并通过解析器运行它:
1.3 完成运动学模型
我们尚未指定的是关节围绕哪个轴旋转。 添加完之后，我们实际上就拥有了该机器人的完整运动学模型！ 我们要做的就是将元素添加到每个关节。 该轴指定局部框架中的旋转轴。
因此，如果看一下joint2，就会看到它绕着Y轴正方向旋转。 因此，只需将以下xml添加到joint元素中：
切换行号显示
1 <axis xyz="0 1 0" />
同样，关节1围绕以下轴旋转：
切换行号显示
1 <axis xyz="-0.707 0.707 0" />
请注意，对轴进行归一化是个好主意。
<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"/>
<origin xyz="5 3 0" rpy="0 0 0" />
<axis xyz="-0.9 0.15 0" />
</joint>
<joint name="joint2" type="continuous">
<parent link="link1"/>
<child link="link3"/>
<origin xyz="-2 5 0" rpy="0 0 1.57" />
<axis xyz="-0.707 0.707 0" />
</joint>
<joint name="joint3" type="continuous">
<parent link="link3"/>
<child link="link4"/>
<origin xyz="5 0 0" rpy="0 0 -1.57" />
<axis xyz="0.707 -0.707 0" />
</joint>
</robot>
如果将其添加到机器人的所有关节，则我们的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 <urdf/model.h>
#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
$ .<path>/parser <path>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机械手描述和关节位置信息源之后，只需创建一个启动文件，如下所示：
<launch>
<!-- Load the urdf into the parameter server. -->
<param name="my_robot_description" textfile="$(find mypackage)/urdf/robotmodel.xml"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" >
<remap from="robot_description" to="my_robot_description" />
<remap from="joint_states" to="different_joint_states" />
</node>
</launch>
3.2 作为library运行
高级用户还可以从自己的C ++代码中以库的形式运行机械手状态发布器。 包含标题后：
#include <robot_state_publisher/robot_state_publisher.h>
您只需要一个包含KDL树的构造函数:
RobotStatePublisher(const KDL::Tree& tree);
现在，每次您要发布机器人状态时，都可以调用publishTransforms函数：
// publish moving joints
void publishTransforms(const std::map<std::string, double>& 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）文件中：
<package>
...
<build_depend package="kdl_parser" />
...
<run_depend package="kdl_parser" />
...
</package>
要开始在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”程序包中。
切换行号显示
<launch>
<param name="robot_description" command="cat $(find r2d2)/model.xml" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="state_publisher" pkg="r2d2" type="state_publisher" />
</launch>
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显示.
