ROS探索总结（十九）~（二十四）（转）

（十九）——如何配置机器人的导航功能

1、概述

        ROS的二维导航功能包，简单来说，就是根据输入的里程计等传感器的信息流和机器人的全局位置，通过导航算法，计算得出安全可靠的机器人速度控制指令。但是，如何在特定的机器人上实现导航功能包的功能，却是一件较为复杂的工程。作为导航功能包使用的必要先决条件，机器人必须运行ROS，发布tf变换树，并发布使用ROS消息类型的传感器数据。同时，为了让机器人更好的完成导航任务，开发者还要根据机器人的外形尺寸和性能，配置导航功能包的一些参数。

 

2、硬件要求

        尽管导航功能包设计得尽可能通用，但是仍然对机器人的硬件有以下三个要求：

       （1）导航功能包仅对差分等轮式机器人有效，并且假设机器人可直接使用速度指令进行控制，速度指令的格式为：x方向速度、y方向速度、速度向量角度。

       （2）导航功能包要求机器人必须安装有激光雷达等二维平面测距设备。

       （3）导航功能包以正方型的机器人为模型进行开发，所以对于正方形或者圆形外形的机器人支持度较好，而对于其他外形的机器人来讲，虽然仍然可以正常使用，但是表现则很有可能不佳。

 

3、机器人配置

        导航功能包的结构如上图所示，在自己的机器人平台上实现自主导航，简单来说，就是按照上图将需要的功能按照需求完成即可。其中白色的部分是ROS功能包已经完成的部分，不需要我们去实现，灰色的是可选的部分，也由ROS完成，在使用中根据需求使用，需要关注的重点部分是蓝色部分，这些需要我们根据输入输出的要求完成相应的功能。

3.1、ROS

        首先，请确保你的机器人安装了ROS框架。

3.2、tf变换(sensortransforms)

        导航功能包要求机器人以tf树的形式发布各个相关参考系的变换关系。

3.3、传感器信息(sensor sources)

        导航功能包需要采集机器人的传感器信息，以达到实时避障的效果。这些传感器要求能够通过ROS发布

sensor_msgs/LaserScan或者sensor_msgs/PointCloud 格式的消息，也就是二维雷达信息或者三维点云数据。ROS社区已经支持大部分激光雷达、Kinect等设备的驱动，可以直接使用社区提供的驱动功能包发布满足要求的传感器信息。如果你使用的传感器没有ROS支持，或者你想使用自己的驱动，也可以自己将传感器信息封装成要求的格式。

3.4、里程计信息(odometrysource)

      导航功能包要求机器人发布nav_msgs/Odometry格式的里程计信息，同时在也要发布相应的tf变换。

3.5、机器人控制器（base_controller）

      导航功能包最终的输出是针对机器人geometry_msgs/Twist格式的控制指令，这就要求机器人控制节点具备解析控制指令中速度、角度的能力，并且最终通过这些指令控制机器人完成相应的运动目标。

3.6、地图（map_server）

       地图并不是导航功能所必需的。

 

4、导航功能包集的配置

        在满足以上条件的前提下，我们来针对导航功能进行一些配置。

4.1、创建一个功能包

       首先，我们需要创建一个功能包，用来存储导航需要用到的所有的配置文件和launch启动文件。在创建功能包的时候，我们需要添加相关的所有依赖，包括机器人配置中使用到的功能包，当然不要忘记了move_base功能包，因为该包有很多我们后面需要用到的接口。找到合适的位置，输入以下命令来创建包：

catkin_create_pkg my_robot_name_2dnav move_base my_tf_configuration_depmy_odom_configuration_dep my_sensor_configuration_dep

4.2、创建机器人启动文件

        现在，我们已经有了一个存储各种文件的工作空间，下一步，我们来创建一个机器人启动文件，用来启动机器人配置中所提到的所有硬件，并发布相应的消息和变换关系。

        打开编辑器，输入以下格式的内容，并保存为my_robot_configuration.launch命名的文件：

<launch>
  <nodepkg="sensor_node_pkg" type="sensor_node_type"name="sensor_node_name" output="screen">
    <paramname="sensor_param" value="param_value" />
 </node>
 
  <nodepkg="odom_node_pkg" type="odom_node_type"name="odom_node" output="screen">
    <paramname="odom_param" value="param_value" />
 </node>
 
  <nodepkg="transform_configuration_pkg"type="transform_configuration_type"name="transform_configuration_name" output="screen">
    <paramname="transform_configuration_param" value="param_value"/>
 </node>
</launch>

   让我们来详细的解读以上内容的含义：

<launch>
  <nodepkg="sensor_node_pkg" type="sensor_node_type"name="sensor_node_name" output="screen">
    <paramname="sensor_param" value="param_value" />

      这部分代码用来启动机器人的传感器，根据以上格式，修改你所使用到的传感器驱动包名称、类型、命名等信息，并且添加驱动包节点需要使用到的参数。当然，如果你需要使用多个传感器，可以使用相同的方法，启动多个传感器的驱动节点。

<node pkg="odom_node_pkg"type="odom_node_type" name="odom_node"output="screen">
    <paramname="odom_param" value="param_value" />
 </node>

      这部分代码用来启动机器人上的里程计，根据需要修改功能包名、类型、节点名、参数。

 <nodepkg="transform_configuration_pkg"type="transform_configuration_type"name="transform_configuration_name" output="screen">
    <paramname="transform_configuration_param" value="param_value"/>
 </node>

      这部分代码需要启动机器人相关的坐标变换。

4.3、代价地图的配置 (local_costmap)& (global_costmap)

       导航功能包使用两种代价地图存储周围环境中的障碍信息，一种用于全局路径规划，一种用于本地路径规划和实时避障。两种代价地图需要使用一些共同和独立的配置文件：通用配置文件，全局规划配置文件，本地规划配置文件。以下将详细讲解这三种配置文件：

（1）通用配置文件（Common Configuration (local_costmap) &(global_costmap)）

      代价地图用来存储周围环境的障碍信息，其中需要注明地图关注的机器人传感器消息，以便于地图信息进行更行。针对两种代价地图通用的配置选项，创建名为costmap_common_params.yaml的配置文件：

obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[x0, y0], [x1, y1], ... [xn, yn]]
#robot_radius: ir_of_robot
inflation_radius: 0.55
 
observation_sources: laser_scan_sensor point_cloud_sensor
 
laser_scan_sensor: {sensor_frame: frame_name, data_type: LaserScan, topic: topic_name, marking: true, clearing: true}
 
point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true}

      详细解析以上配置文件的内容：

obstacle_range: 2.5
raytrace_range: 3.0

       这两个参数用来设置代价地图中障碍物的相关阈值。obstacle_range参数用来设置机器人检测障碍物的最大范围，设置为2.5意为在2.5米范围内检测到的障碍信息，才会在地图中进行更新。raytrace_range参数用来设置机器人检测自由空间的最大范围，设置为3.0意为在3米范围内，机器人将根据传感器的信息，清除范围内的自由空间。

footprint: [[x0, y0], [x1, y1], ... [xn, yn]]
#robot_radius: ir_of_robot
inflation_radius: 0.55

       这些参数用来设置机器人在二维地图上的占用面积，如果机器人外形是圆形，则需要设置机器人的外形半径。所有参数以机器人的中心作为坐标（0，0）点。inflation_radius参数是设置障碍物的膨胀参数，也就是机器人应该与障碍物保持的最小安全距离，这里设置为0.55意为为机器人规划的路径应该与机器人保持0.55米以上的安全距离。

observation_sources: laser_scan_sensorpoint_cloud_sensor

     observation_sources参数列出了代价地图需要关注的所有传感器信息，每一个传感器信息都将在后边列出详细信息。

laser_scan_sensor: {sensor_frame: frame_name, data_type:LaserScan, topic: topic_name, marking: true, clearing: true}

       以激光雷达为例，sensor_frame标识传感器的参考系名称，data_type表示激光数据或者点云数据使用的消息类型，topic_name表示传感器发布的话题名称，而marking和clearing参数用来表示是否需要使用传感器的实时信息来添加或清楚代价地图中的障碍物信息。

（2）全局规划配置文件（Global Configuration (global_costmap)）

        全局规划配置文件用来存储用于全局代价地图的配置参数，我们使用global_costmap_params.yaml来命名，内容如下：

global_costmap:
 global_frame: /map
 robot_base_frame: base_link
 update_frequency: 5.0
  static_map:true

     global_frame参数用来表示全局代价地图需要在那个参考系下运行，这里我们选择了map这个参考系。robot_base_frame参数表示代价地图可以参考的机器人本体的参考系。update_frequency参数绝地全局地图信息更新的频率，单位是Hz。static_map参数决定代价地图是否需要根据map_server提供的地图信息进行初始化，如果你不需要使用已有的地图或者map_server，最好将该参数设置为false。

（3）本地规划配置文件（Local Configuration (local_costmap)）

       本地规划配置文件用来存储用于本地代价地图的配置参数，命名为local_costmap_params.yaml，内容如下：

local_costmap:
 global_frame: odom
 robot_base_frame: base_link
 update_frequency: 5.0
 publish_frequency: 2.0
  static_map:false
 rolling_window: true
  width: 6.0
  height: 6.0
  resolution:0.05

      "global_frame", "robot_base_frame","update_frequency", 和 "static_map"参数的意义与全局规划配置文件中的参数相同。publish_frequency设置代价地图发布可视化信息的频率，单位是Hz。rolling_window参数是用来设置在机器人移动过程中是否需要滚动窗口，以保持机器人处于中心位置。"width," "height," 和"resolution" 设置设置代价地图长（米）、高（米）和分辨率（米/格）。分辨率可以设置的与静态地图不同，但是一般情况下两者是相同的。

4.4 本地规划器配置

       本地规划器base_local_planner的主要作用是根据规划的全局路径，计算发布给机器人的速度指令。该规划器需要我们根据机器人的规格，配置一些相应的参数。我们创建名为base_local_planner_params.yaml的配置文件：

TrajectoryPlannerROS:
  max_vel_x: 0.45
  min_vel_x: 0.1
  max_vel_theta: 1.0
  min_in_place_vel_theta: 0.4
 
  acc_lim_theta: 3.2
  acc_lim_x: 2.5
  acc_lim_y: 2.5
 
  holonomic_robot: true

      该配置文件声明了机器人的本地规划采用Trajectory Rollout算法。第一段设置了机器人的速度阈值，第二段设置了机器人的加速度阈值。

4.5 为导航功能包创建一个启动文件

      到此为止，我们已经创建完毕所有需要用到的配置文件，接下来我们需要创建一个启动文件，来启动所有需要的功能。创建move_base.launch的文件：

<launch>
  <masterauto="start"/>
 
  <!-- Runthe map server -->
  <nodename="map_server" pkg="map_server"type="map_server" args="$(find my_map_package)/my_map.pgm my_map_resolution"/>
 
  <!---Run AMCL -->
  <includefile="$(find amcl)/examples/amcl_omni.launch" />
 
  <nodepkg="move_base" type="move_base" respawn="false"name="move_base" output="screen">
   <rosparam file="$(find my_robot_name_2dnav)/costmap_common_params.yaml"command="load" ns="global_costmap" />
   <rosparam file="$(findmy_robot_name_2dnav)/costmap_common_params.yaml" command="load"ns="local_costmap" />
   <rosparam file="$(findmy_robot_name_2dnav)/local_costmap_params.yaml" command="load" />
   <rosparam file="$(findmy_robot_name_2dnav)/global_costmap_params.yaml" command="load"/>
   <rosparam file="$(findmy_robot_name_2dnav)/base_local_planner_params.yaml"command="load" />
 </node>
</launch>

      在该配置文件中，你需要修改的只有map-server输入的地图，以及如果使用差分驱动的机器人，需要修改"amcl_omni.launch"成"amcl_diff.launch" 。

4.6 AMCL功能包的设置

      AMCL有许多的参数设置，会影响机器人的定位效果，具体参考amcldocumentation。

5、运行导航功能包

       现在，我们已经完成了所有需要的工作，最后一步，运行启动文件，开始导航之旅：

roslaunch my_robot_configuration.launch
roslaunch move_base.launch

        现在导航功能包应该已经可以顺利运行了，但这绝对不是结束，因为你只能从终端里看到一端乱蹦的代码，如何使用更友好的方式进行机器人导航呢？如果你想使用UI界面，请参考rviz and navigationtutorial，如果你想使用代码，请参考Sending SimpleNavigation Goals 。

（二十）——发布导航需要的传感器信息

在导航过程中，传感器的信息至关重要，这些传感器可以是激光雷达、摄像机、声纳、红外线、碰撞开关，但是归根结底，导航功能包要求机器人必须发布sensor_msgs/LaserScan或sensor_msgs/PointCloud格式的传感器信息，本篇将详细介绍如何使用代码发布所需要的消息。

1、ROS的消息头信息

    无论是 sensor_msgs/LaserScan，还是sensor_msgs/PointCloud ，都和ROS中tf帧信息等时间相关的消息一样，带标准格式的头信息。

1. #Standard metadata for higher-level flow data types

2. #sequence ID: consecutively increasing ID 

3. uint32 seq

5. #Two-integer timestamp that is expressed as:

6. # * stamp.secs: seconds (stamp_secs) since epoch

7. # * stamp.nsecs: nanoseconds since stamp_secs

8. # time-handling sugar is provided by the client library

9. time stamp

11. #Frame this data is associated with

12. # 0: no frame

13. # 1: global frame

14. string frame_id

       以上是标准头信息的主要部分。seq是消息的顺序标识，不需要手动设置，发布节点在发布消息时，会自动累加。stamp 是消息中与数据相关联的时间戳，例如激光数据中，时间戳对应激光数据的采集时间点。frame_id 是消息中与数据相关联的参考系id，例如在在激光数据中，frame_id对应激光数据采集的参考系。

2、如何发布激光扫描消息

2.1、激光消息的结构

      针对激光雷达，ROS在sensor_msgs 包中定义了专用了数据结构来存储激光消息的相关信息，成为LaserScan。LaserScan消息的格式化定义，为虚拟的激光雷达数据采集提供了方便，在我们讨论如何使用他之前，来看看该消息的结构是什么样的：

1. #

2. # Laser scans angles are measured counter clockwise, with 0 facing forward

3. # (along the x-axis) of the device frame

4. #

6. Header header

7. float32 angle_min        # start angle of the scan [rad]

8. float32 angle_max        # end angle of the scan [rad]

9. float32 angle_increment  # angular distance between measurements [rad]

10. float32 time_increment   # time between measurements [seconds]

11. float32 scan_time        # time between scans [seconds]

12. float32 range_min        # minimum range value [m]

13. float32 range_max        # maximum range value [m]

14. float32[] ranges         # range data [m] (Note: values < range_min or > range_max should be discarded)

15. float32[] intensities    # intensity data [device-specific units]

      备注中已经详细介绍了每个参数的意义。

2.2、通过代码发布LaserScan消息

      使用ROS发布LaserScan格式的激光消息非常简洁，下边是一个简单的例程：

1. #include <ros/ros.h>

2. #include <sensor_msgs/LaserScan.h>

4. int main(int argc, char** argv){

5.   ros::init(argc, argv, "laser_scan_publisher");

7.   ros::NodeHandle n;

8.   ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);

10.   unsigned int num_readings = 100;

11.   double laser_frequency = 40;

12.   double ranges[num_readings];

13.   double intensities[num_readings];

15.   int count = 0;

16.   ros::Rate r(1.0);

17.   while(n.ok()){

18.     //generate some fake data for our laser scan

19.     for(unsigned int i = 0; i < num_readings; ++i){

20.       ranges[i] = count;

21.       intensities[i] = 100 + count;

22.     }

23.     ros::Time scan_time = ros::Time::now();

25.     //populate the LaserScan message

26.     sensor_msgs::LaserScan scan;

27.     scan.header.stamp = scan_time;

28.     scan.header.frame_id = "laser_frame";

29.     scan.angle_min = -1.57;

30.     scan.angle_max = 1.57;

31.     scan.angle_increment = 3.14 / num_readings;

32.     scan.time_increment = (1 / laser_frequency) / (num_readings);

33.     scan.range_min = 0.0;

34.     scan.range_max = 100.0;

36.     scan.ranges.resize(num_readings);

37.     scan.intensities.resize(num_readings);

38.     for(unsigned int i = 0; i < num_readings; ++i){

39.       scan.ranges[i] = ranges[i];

40.       scan.intensities[i] = intensities[i];

41.     }

43.     scan_pub.publish(scan);

44.     ++count;

45.     r.sleep();

46.   }

47. }

      我们将代码分解以便于分析：

1. #include <sensor_msgs/LaserScan.h>

      首先我们需要先包含Laserscan的数据结构。

1. ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);

      创建一个发布者，以便于后边发布针对scan主题的Laserscan消息。

1.   unsigned int num_readings = 100;

2.   double laser_frequency = 40;

3.   double ranges[num_readings];

4.   double intensities[num_readings];

6.   int count = 0;

7.   ros::Rate r(1.0);

8.   while(n.ok()){

9.     //generate some fake data for our laser scan

10.     for(unsigned int i = 0; i < num_readings; ++i){

11.       ranges[i] = count;

12.       intensities[i] = 100 + count;

13.     }

14.     ros::Time scan_time = ros::Time::now();

      这里的例程中我们虚拟一些激光雷达的数据，如果使用真是的激光雷达，这部分数据需要从驱动中获取。

1.     //populate the LaserScan message

2.     sensor_msgs::LaserScan scan;

3.     scan.header.stamp = scan_time;

4.     scan.header.frame_id = "laser_frame";

5.     scan.angle_min = -1.57;

6.     scan.angle_max = 1.57;

7.     scan.angle_increment = 3.14 / num_readings;

8.     scan.time_increment = (1 / laser_frequency) / (num_readings);

9.     scan.range_min = 0.0;

10.     scan.range_max = 100.0;

12.     scan.ranges.resize(num_readings);

13.     scan.intensities.resize(num_readings);

14.     for(unsigned int i = 0; i < num_readings; ++i){

15.       scan.ranges[i] = ranges[i];

16.       scan.intensities[i] = intensities[i];

17.     }

    创建scan_msgs::LaserScan数据类型的变量scan，把我们之前伪造的数据填入格式化的消息结构中。

1.    scan_pub.publish(scan);

    数据填充完毕后，通过前边定义的发布者，将数据发布。

3、如何发布点云数据

3.1、点云消息的结构

      为存储和发布点云消息，ROS定义了sensor_msgs/PointCloud消息结构：

1. #This message holds a collection of 3d points, plus optional additional information about each point.

3. #Each Point32 should be interpreted as a 3d point in the frame given in the header

7. Header header

9. geometry_msgs/Point32[] points  #Array of 3d points

11. ChannelFloat32[] channels       #Each channel should have the same number of elements as points array, and the data in each channel should correspond 1:1 with each point

      如上所示，点云消息的结构支持存储三维环境的点阵列，而且channels参数中，可以设置这些点云相关的数据，例如可以设置一个强度通道，存储每个点的数据强度，还可以设置一个系数通道，存储每个点的反射系数，等等。

3.2、通过代码发布点云数据

     ROS发布点云数据同样简洁：

1. #include <ros/ros.h>

2. #include <sensor_msgs/PointCloud.h>

4. int main(int argc, char** argv){

5.   ros::init(argc, argv, "point_cloud_publisher");

7.   ros::NodeHandle n;

8.   ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);

10.   unsigned int num_points = 100;

12.   int count = 0;

13.   ros::Rate r(1.0);

14.   while(n.ok()){

15.     sensor_msgs::PointCloud cloud;

16.     cloud.header.stamp = ros::Time::now();

17.     cloud.header.frame_id = "sensor_frame";

19.     cloud.points.resize(num_points);

21.     //we'll also add an intensity channel to the cloud

22.     cloud.channels.resize(1);

23.     cloud.channels[0].name = "intensities";

24.     cloud.channels[0].values.resize(num_points);

26.     //generate some fake data for our point cloud

27.     for(unsigned int i = 0; i < num_points; ++i){

28.       cloud.points[i].x = 1 + count;

29.       cloud.points[i].y = 2 + count;

30.       cloud.points[i].z = 3 + count;

31.       cloud.channels[0].values[i] = 100 + count;

32.     }

34.     cloud_pub.publish(cloud);

35.     ++count;

36.     r.sleep();

37.   }

38. }

     分解代码来分析：

1. #include <sensor_msgs/PointCloud.h>

     首先也是要包含sensor_msgs/PointCloud消息结构。

1.     ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);

     定义一个发布点云消息的发布者。

1.     sensor_msgs::PointCloud cloud;

2.     cloud.header.stamp = ros::Time::now();

3.     cloud.header.frame_id = "sensor_frame";

     为点云消息填充头信息，包括时间戳和相关的参考系id。

1.     cloud.points.resize(num_points);

        设置存储点云数据的空间大小。

1.     //we'll also add an intensity channel to the cloud

2.     cloud.channels.resize(1);

3.     cloud.channels[0].name = "intensities";

4.     cloud.channels[0].values.resize(num_points);

        设置一个名为“intensity“的强度通道，并且设置存储每个点强度信息的空间大小。

1.     //generate some fake data for our point cloud

2.     for(unsigned int i = 0; i < num_points; ++i){

3.       cloud.points[i].x = 1 + count;

4.       cloud.points[i].y = 2 + count;

5.       cloud.points[i].z = 3 + count;

6.       cloud.channels[0].values[i] = 100 + count;

7.     }

        将我们伪造的数据填充到点云消息结构当中。

1.    cloud_pub.publish(cloud);

       最后，发布点云数据。

 

参考连接： http://wiki.ros.org/navigation/Tutorials/RobotSetup/Sensors

（二十一）——如何发布里程计消息

ROS使用tf来决定机器人的位置和静态地图中的传感器数据，但是tf中没有机器人的速度信息，所以导航功能包要求机器人能够通过里程计信息源发布包含速度信息的里程计nav_msgs/Odometry 消息。 本篇将介绍nav_msgs/Odometry消息，并且通过代码实现消息的发布，以及tf树的变换。

1、nav_msgs/Odometry消息格式

       nav_msgs/Odometry消息包含有机器人在自由空间中的位置和速度估算值。

1. # This represents an estimate of a position and velocity in free space.

2. # The pose in this message should be specified in the coordinate frame given by header.frame_id.

3. # The twist in this message should be specified in the coordinate frame given by the child_frame_id

4. Header header

5. string child_frame_id

6. geometry_msgs/PoseWithCovariance pose

7. geometry_msgs/TwistWithCovariance twist

       pose参数包含机器人在里程计参考系下的位置估算值，同时带有可选的估算协方差。twist参数包含机器人在子参考系（一般是机器人基础参考系）下的速度，同时带有可选的速度估算协方差。

2、使用tf发布里程计变换信息

        参见 Transform Configuration教程。

3、代码例程

        这里使用一个简单的例程，实现 nav_msgs/Odometry消息的发布和tf变换，通过伪造的数据，实现机器人圆周运动。

        首先需要将一些以来添加到manifest.xml中：

1. <depend package="tf"/>

2. <depend package="nav_msgs"/>

        实现代码如下：

1. #include <ros/ros.h>

2. #include <tf/transform_broadcaster.h>

3. #include <nav_msgs/Odometry.h>

5. int main(int argc, char** argv){

6.   ros::init(argc, argv, "odometry_publisher");

8.   ros::NodeHandle n;

9.   ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);

10.   tf::TransformBroadcaster odom_broadcaster;

12.   double x = 0.0;

13.   double y = 0.0;

14.   double th = 0.0;

16.   double vx = 0.1;

17.   double vy = -0.1;

18.   double vth = 0.1;

20.   ros::Time current_time, last_time;

21.   current_time = ros::Time::now();

22.   last_time = ros::Time::now();

24.   ros::Rate r(1.0);

25.   while(n.ok()){

27.     ros::spinOnce();               // check for incoming messages

28.     current_time = ros::Time::now();

30.     //compute odometry in a typical way given the velocities of the robot

31.     double dt = (current_time - last_time).toSec();

32.     double delta_x = (vx * cos(th) - vy * sin(th)) * dt;

33.     double delta_y = (vx * sin(th) + vy * cos(th)) * dt;

34.     double delta_th = vth * dt;

36.     x += delta_x;

37.     y += delta_y;

38.     th += delta_th;

40.     //since all odometry is 6DOF we'll need a quaternion created from yaw

41.     geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

43.     //first, we'll publish the transform over tf

44.     geometry_msgs::TransformStamped odom_trans;

45.     odom_trans.header.stamp = current_time;

46.     odom_trans.header.frame_id = "odom";

47.     odom_trans.child_frame_id = "base_link";

49.     odom_trans.transform.translation.x = x;

50.     odom_trans.transform.translation.y = y;

51.     odom_trans.transform.translation.z = 0.0;

52.     odom_trans.transform.rotation = odom_quat;

54.     //send the transform

55.     odom_broadcaster.sendTransform(odom_trans);

57.     //next, we'll publish the odometry message over ROS

58.     nav_msgs::Odometry odom;

59.     odom.header.stamp = current_time;

60.     odom.header.frame_id = "odom";

62.     //set the position

63.     odom.pose.pose.position.x = x;

64.     odom.pose.pose.position.y = y;

65.     odom.pose.pose.position.z = 0.0;

66.     odom.pose.pose.orientation = odom_quat;

68.     //set the velocity

69.     odom.child_frame_id = "base_link";

70.     odom.twist.twist.linear.x = vx;

71.     odom.twist.twist.linear.y = vy;

72.     odom.twist.twist.angular.z = vth;

74.     //publish the message

75.     odom_pub.publish(odom);

77.     last_time = current_time;

78.     r.sleep();

79.   }

80. }

       下面来剖析代码进行分析：

1. #include <tf/transform_broadcaster.h>

2. #include <nav_msgs/Odometry.h>

       我们需要实现“odom”参考系到“base_link”参考系的变换，以及nav_msgs/Odometry消息的发布，所以首先需要包含相关的头文件。

1.   ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);  

2.   tf::TransformBroadcaster odom_broadcaster;

       定义一个消息发布者来发布“odom”消息，在定义一个tf广播，来发布tf变换信息。

1.   double x = 0.0;

2.   double y = 0.0;

3.   double th = 0.0;

       默认机器人的起始位置是odom参考系下的0点。

1.   double vx = 0.1;

2.   double vy = -0.1;

3.   double vth = 0.1;

       我们设置机器人的默认前进速度，让机器人的base_link参考系在odom参考系下以x轴方向0.1m/s，Y轴速度-0.1m/s，角速度0.1rad/s的状态移动，这种状态下，可以让机器人保持圆周运动。

1.   ros::Rate r(1.0);

        使用1Hz的频率发布odom消息，当然，在实际系统中，往往需要更快的速度进行发布。

1.     //compute odometry in a typical way given the velocities of the robot

2.     double dt = (current_time - last_time).toSec();

3.     double delta_x = (vx * cos(th) - vy * sin(th)) * dt;

4.     double delta_y = (vx * sin(th) + vy * cos(th)) * dt;

5.     double delta_th = vth * dt;

7.     x += delta_x;

8.     y += delta_y;

9.     th += delta_th;

        使用我们设置的速度信息，来计算并更新里程计的信息，包括单位时间内机器人在x轴、y轴的坐标变化和角度的变化。在实际系统中，需要更具里程计的实际信息进行更新。

1.    //since all odometry is 6DOF we'll need a quaternion created from yaw

2.     geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

        为了兼容二维和三维的功能包，让消息结构更加通用，里程计的偏航角需要转换成四元数才能发布，辛运的是，ROS为我们提供了偏航角与四元数相互转换的功能。

1.     //first, we'll publish the transform over tf

2.     geometry_msgs::TransformStamped odom_trans;

3.     odom_trans.header.stamp = current_time;

4.     odom_trans.header.frame_id = "odom";

5.     odom_trans.child_frame_id = "base_link";

        创建一个tf发布需要使用的TransformStamped类型消息，然后根据消息结构填充当前的时间戳、参考系id、子参考系id，注意两个参考系的id必须要是“odom”和“base_link”。

1.     odom_trans.transform.translation.x = x;

2.     odom_trans.transform.translation.y = y;

3.     odom_trans.transform.translation.z = 0.0;

4.     odom_trans.transform.rotation = odom_quat;

        填充里程计信息，然后发布tf变换的消息。

1.     //next, we'll publish the odometry message over ROS

2.     nav_msgs::Odometry odom;

3.     odom.header.stamp = current_time;

        别忘了，我们还要发布nav_msgs/Odometry消息，让导航包获取机器人的速度。创建消息变量，然后填充时间戳。

1.     //set the position

2.     odom.pose.pose.position.x = x;

3.     odom.pose.pose.position.y = y;

4.     odom.pose.pose.position.z = 0.0;

5.     odom.pose.pose.orientation = odom_quat;

7.     //set the velocity

8.     odom.child_frame_id = "base_link";

9.     odom.twist.twist.linear.x = vx;

10.     odom.twist.twist.linear.y = vy;

11.     odom.twist.twist.angular.z = vth;

        填充机器人的位置、速度，然后发布消息。注意，我们发布的是机器人本体的信息，所以参考系需要填"base_link"。

 

        参考链接：http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom

（二十二）——设置机器人的tf变换

1、什么是tf变换

        ROS中的很多软件包都需要机器人发布tf变换树，那么什么是tf变换树呢？抽象的来讲，一棵tf变换树定义了不同坐标系之间的平移与旋转变换关系。具体来说，我们假设有一个机器人，包括一个机器人移动平台和一个安装在平台之上的激光雷达，以这个机器人为例，定义两个坐标系，一个坐标系以机器人移动平台的中心为原点，称为base_link参考系，另一个坐标系以激光雷达的中心为原点，称为base_laser参考系。

        假设在机器人运行过程中，激光雷达可以采集到距离前方障碍物的数据，这些数据当然是以激光雷达为原点的测量值，换句话说，也就是base_laser参考系下的测量值。现在，如果我们想使用这些数据帮助机器人完成避障功能，当然，由于激光雷达在机器人之上，直接使用这些数据不会产生太大的问题，但是激光雷达并不在机器人的中心之上，在极度要求较高的系统中，会始终存在一个雷达与机器人中心的偏差值。这个时候，如果我们采用一种坐标变换，将及激光数据从base_laser参考系变换到base_link参考下，问题不就解决了么。这里我们就需要定义这两个坐标系之间的变换关系。

         为了定义这个变换关系，假设我们已知激光雷达安装的位置在机器人的中心点上方20cm，前方10cm处。这就根据这些数据，就足以定义这两个参考系之间的变换关系：当我们获取激光数据后，采用(x: 0.1m, y: 0.0m, z: 0.2m)的坐标变换，就可以将数据从base_laser参考系变换到base_link参考系了。当然，如果要方向变化，采用(x: -0.1m, y: 0.0m, z: -0.20m)的变换即可。

        从上边的示例看来，参考系之间的坐标变换好像并不复杂，但是在复杂的系统中，存在的参考系可能远远大于两个，如果我们都使用这种手动的方式进行变换，估计很快你就会被繁杂的坐标关系搞蒙了。ROS提供的tf变换就是为解决这个问题而生的，tf功能包提供了存储、计算不同数据在不同参考系之间变换的功能，我们只需要告诉tf树这些参考系之间的变换公式即可，这颗tf树就可以树的数据结构，管理我们所需要的参考系变换。

       还是以上边的示例为基础，为了定义和存储base_link和base_laser两个参考系之间的关系，我们需要将他们添加到tf树中。从树的概念上来讲，tf树中的每个节点都对应一个参考系，而节点之间的边对应于参考系之间的变换关系。tf就是使用这样的树结构，保证每两个参考系之间只有一种遍历方式，而且所有变换关系，都是母节点到子节点的变换。

        为了定义上边示例中的参考系，我们需要定义两个节点，一个对应于base_link参考系，一个对应于base_laser参考系。为了创建两个节点之间的边，我们首先需要决定哪一个节点作为母节点，哪一个节点作为子节点，这一点在tf树中是非常重要的。这里我们选择base_link作为母节点，这样会方便后边为机器人添加更多的传感器作为子节点。所以，根据之前的分析，从base_link节点到base_laser节点的变换关系为(x: 0.1m, y: 0.0m, z: 0.2m)。设置完毕后，我们就可以通过调用tf库，直接完成base_laser参考系到base_link参考系的数据坐标变换了。

2、代码流程

       通过上边的分析，应该可以从理论上帮助你理解什么是tf，以及tf的功能了。在实际应用中，我们需要使用代码来完成这些理论，下边我们就来看看如何使用代码来调用tf的变换。

       代码的总体思路分为两个部分：

       （1）编写一个节点，广播两个参考系之间的tf变换关系

       （2）编写另外一个节点，订阅tf树，然后从tf树中遍历到两个参考系之间的变换公式，然后通过公式计算数据的变换。

        我们先来完成第一步。首先我们来创建一个功能包，用于后边程序的放置，这里需要依赖roscpp、tf、geometry_msgs。

1. $ cd %TOP_DIR_YOUR_CATKIN_WS%/src

2. $ catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs

3、编写广播节点      

       进入功能包，创建src/tf_broadcaster.cpp文件，来完成广播节点的代码：

1. #include <ros/ros.h>

2. #include <tf/transform_broadcaster.h>

4. int main(int argc, char** argv){

5.   ros::init(argc, argv, "robot_tf_publisher");

6.   ros::NodeHandle n;

8.   ros::Rate r(100);

10.   tf::TransformBroadcaster broadcaster;

12.   while(n.ok()){

13.     broadcaster.sendTransform(

14.       tf::StampedTransform(

15.         tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),

16.         ros::Time::now(),"base_link", "base_laser"));

17.     r.sleep();

18.   }

19. }

   逐行分析如上代码：

1. #include <tf/transform_broadcaster.h>

        因为后边会使用到tf::TransformBroadcaster类的实例，来完成tf树的广播，所以需要先包含相关的头文件。

1. tf::TransformBroadcaster broadcaster;

     创建一个tf::TransformBroadcaster类的实例，用来广播 base_link → base_laser的变换关系。

1.    broadcaster.sendTransform(

2.       tf::StampedTransform(

3.         tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),

4.         ros::Time::now(),"base_link", "base_laser"));

    这部分是代码的关键所在。通过TransformBroadcaster 类来发布变换关系的接口，需要五个参数。首先是两个参考系之间的旋转变换，通过btQuaternion四元数来存储旋转变换的参数，因为我们用到的两个参考系没有发生旋转变换，所以倾斜角、滚动角、偏航角都是0。第二个参数是坐标的位移变换，我们用到的两个参考系在X轴和Z轴发生了位置，根据位移值填入到btVector3 向量中。第三个参数是时间戳，直接太难过ROS的API完成。第四个参数是母节点存储的参考系，即base_link，最后一个参数是子节点存储的参考系，即base_laser。

4、编写订阅节点

        上一节讲解了如何使用代码编写一个广播tf变换的节点，这一节，我们编写一个订阅tf广播的节点，并且使用tf树中base_link到base_laser的变换关系，完成数据的坐标变换。在robot_setup_tf功能包中创建src/tf_listener.cpp文件，代码如下：

1. #include <ros/ros.h>

2. #include <geometry_msgs/PointStamped.h>

3. #include <tf/transform_listener.h>

5. void transformPoint(const tf::TransformListener& listener){

6.   //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame

7.   geometry_msgs::PointStamped laser_point;

8.   laser_point.header.frame_id = "base_laser";

10.   //we'll just use the most recent transform available for our simple example

11.   laser_point.header.stamp = ros::Time();

13.   //just an arbitrary point in space

14.   laser_point.point.x = 1.0;

15.   laser_point.point.y = 0.2;

16.   laser_point.point.z = 0.0;

18.   try{

19.     geometry_msgs::PointStamped base_point;

20.     listener.transformPoint("base_link", laser_point, base_point);

22.     ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",

23.         laser_point.point.x, laser_point.point.y, laser_point.point.z,

24.         base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());

25.   }

26.   catch(tf::TransformException& ex){

27.     ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());

28.   }

29. }

31. int main(int argc, char** argv){

32.   ros::init(argc, argv, "robot_tf_listener");

33.   ros::NodeHandle n;

35.   tf::TransformListener listener(ros::Duration(10));

37.   //we'll transform a point once every second

38.   ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

40.   ros::spin();

42. }

   进行详细的分析：

1. #include <tf/transform_listener.h>

   在后边的代码中，我们会使用到tf::TransformListener对象，该对象会自动订阅ROS中的tf消息，并且管理所有的变换关系数据。所以需要先包含相关的头文件。

1. void transformPoint(const tf::TransformListener& listener){

   我们创建一个回调函数，每次收到tf消息时，都会自动调用该函数，上一节我们设置了发布tf消息的频率是1Hz，所以回调函数执行的频率也是1Hz。在回调函数中，我们需要完成数据从base_laser到base_link参考系的坐标变换。

1. //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame

2.   geometry_msgs::PointStamped laser_point;

3.   laser_point.header.frame_id = "base_laser";

5.   //we'll just use the most recent transform available for our simple example

6.   laser_point.header.stamp = ros::Time();

8.   //just an arbitrary point in space

9.   laser_point.point.x = 1.0;

10.   laser_point.point.y = 0.2;

11.   laser_point.point.z = 0.0;

   我们创建了一个geometry_msgs::PointStamped类型的虚拟点，该点的坐标为（1.0，0.2，0.0）。该类型包含标准的header消息结构，这样，我们可以就可以在消息中加入发布数据的时间戳和参考系的id。

1. try{

2.     geometry_msgs::PointStamped base_point;

3.     listener.transformPoint("base_link", laser_point, base_point);

5.     ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",

6.         laser_point.point.x, laser_point.point.y, laser_point.point.z,

7.         base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());

8.   }

   这里是代码的关键位置。我们已经在base_laser参考系下虚拟了一个数据点，那么怎样将该点的数据转换到base_base参考系下呢？使用TransformListener 对象中的transformPoint（）函数即可，该函数包含三个参数：第一个参数是需要转换到的参考系id，当然是base_link了；第二个参数是需要转换的原始数据；第三个参数用来存储转换完成的数据。该函数执行完毕后，base_point就是我们转换完成的点坐标了！

1. catch(tf::TransformException& ex){

2.     ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());

3.   }

   为了保证代码的稳定性，我们也需要应对出错处理，例如当tf并没有发布需要的变换关系时，在执行transformPoint时就会出现错误。

5、编译代码

     到此为止，我们已经完成了代码编写的全部流程，接下来就是编译代码了！打开功能包的CMakeLists.txt文件，加入相应的编译选项：

1. add_executable(tf_broadcaster src/tf_broadcaster.cpp)

2. add_executable(tf_listener src/tf_listener.cpp)

3. target_link_libraries(tf_broadcaster ${catkin_LIBRARIES})

4. target_link_libraries(tf_listener ${catkin_LIBRARIES})

     然后保存文件，经行编译：

1. $ cd %TOP_DIR_YOUR_CATKIN_WS%

2. $ catkin_make

6、运行代码

     终于来到最激动人心的一步，见证奇迹的时刻就要来临！

     打开一个终端，运行roscore：

1. roscore

     再打开一个终端，运行tf广播tf_broadcaster：

1. rosrun robot_setup_tf tf_broadcaster

     还要再打开一个终端，运行tf监听tf_listener，将我们虚拟的点坐标，从base_laser转换到base_link参考系下：

1. rosrun robot_setup_tf tf_listener

     如果一切正常，你应该已经可以在第三个终端中看到如下的数据了：

[ INFO] 1248138528.200823000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138528.19

[ INFO] 1248138529.200820000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138529.19

[ INFO] 1248138530.200821000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138530.19

[ INFO] 1248138531.200835000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138531.19

[ INFO] 1248138532.200849000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138532.19

     从上边的信息中，我们可以清晰的看到，我们虚拟的坐标点，成功转换到了base_link参考系下，坐标位置变换成了（1.10, 0.20, 0.20）。

     到此为止，我们已经完成了例程的所有讲解，在你需要完整的真实系统中，请根据需求将PointStamped数据类型修改成你所使用传感器发布的实际消息对应类型。

 

参考链接：http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF

（二十三）——解读URDF

Unified Robot Description Format，统一机器人描述格式，简称为URDF。ROS中的urdf功能包包含一个URDF的C++解析器，URDF文件使用XML格式描述机器人模型。

1      URDF语法规范

参见：XML specifications

2      组件

      URDF由一些不同的功能包和组件组成，下图描述了这些组件之间的联系。

3      URDF教程

如下图所示，本节将创建一个如下图所示结构的机器人模型。

3.1       基础模型

上图所示是一个树形机器人模型，我们先从机器人的整体结构出发，不考虑过多的细节，可以将机器人通过如下的URDF表示：

1. <robot name="test_robot">

2.   <link name="link1" />

3.   <link name="link2" />

4.   <link name="link3" />

5.   <link name="link4" />

7.   <joint name="joint1" type="continuous">

8.     <parent link="link1"/>

9.     <child link="link2"/>

10.   </joint>

12.   <joint name="joint2" type="continuous">

13.     <parent link="link1"/>

14.     <child link="link3"/>

15.   </joint>

17.   <joint name="joint3" type="continuous">

18.     <parent link="link3"/>

19.     <child link="link4"/>

20.   </joint>

21. </robot>

上边的URDF模型定义了机器人的4个环节（link），然后定义了三个关节（joint）来描述环节之间的关联。

ROS为用户提供了一个检查URDF语法的工具：

$ sudo apt-get install liburdfdom-tools

安装完毕后，执行检查：

check_urdf my_robot.urdf

如果一切正常，将会有如下显示：

robot name is: test_robot

---------- Successfully Parsed XML ---------------

root Link: link1 has 2 child(ren)

    child(1):  link2

    child(2):  link3

        child(1):  link4

3.2       添加机器人尺寸

在基础模型之上，我们为机器人添加尺寸大小。由于每个环节的参考系都位于该环节的底部，关节也是如此，所以在表示尺寸大小时，只需要描述其相对于连接的关节的相对位置关系即可。URDF中的<origin>域就是用来表示这种相对关系。

例如，joint2相对于连接的link1在x轴和y轴都有相对位移，而且在x轴上还有90度的旋转变换，所以表示成<origin>域的参数就如下所示：

1. <origin xyz="-2 5 0" rpy="0 0 1.57" />

为所有关节应用尺寸：

1. <robot name="test_robot">

2.   <link name="link1" />

3.   <link name="link2" />

4.   <link name="link3" />

5.   <link name="link4" />

7.   <joint name="joint1" type="continuous">

8.     <parent link="link1"/>

9.     <child link="link2"/>

10.     <origin xyz="5 3 0" rpy="0 0 0" />

11.   </joint>

13.   <joint name="joint2" type="continuous">

14.     <parent link="link1"/>

15.     <child link="link3"/>

16.     <origin xyz="-2 5 0" rpy="0 0 1.57" />

17.   </joint>

19.   <joint name="joint3" type="continuous">

20.     <parent link="link3"/>

21.     <child link="link4"/>

22.     <origin xyz="5 0 0" rpy="0 0 -1.57" />

23.   </joint>

24. </robot>

再次使用check_urdf检查通过后继续下一步。

3.3       添加运动学参数

如果我们为机器人的关节添加旋转轴参数，那么该机器人模型就可以具备基本的运动学参数。

例如，joint2围绕正y轴旋转，可以表示成：

1. <axis xyz="0 1 0" />

同理，joint1的旋转轴是：

1. <axis xyz="-0.707 0.707 0" />

应用到我们的URDF中：

1. <robot name="test_robot">

2.   <link name="link1" />

3.   <link name="link2" />

4.   <link name="link3" />

5.   <link name="link4" />

7.   <joint name="joint1" type="continuous">

8.     <parent link="link1"/>

9.     <child link="link2"/>

10.     <origin xyz="5 3 0" rpy="0 0 0" />

11.     <axis xyz="-0.9 0.15 0" />

12.   </joint>

14.   <joint name="joint2" type="continuous">

15.     <parent link="link1"/>

16.     <child link="link3"/>

17.     <origin xyz="-2 5 0" rpy="0 0 1.57" />

18.     <axis xyz="-0.707 0.707 0" />

19.   </joint>

21.   <joint name="joint3" type="continuous">

22.     <parent link="link3"/>

23.     <child link="link4"/>

24.     <origin xyz="5 0 0" rpy="0 0 -1.57" />

25.     <axis xyz="0.707 -0.707 0" />

26.   </joint>

27. </robot>

       便忘记使用check_urdf检查语法错误。

3.4       图形化显示URDF模型

我们现在已经完成了一个简单的URDF模型创建，ROS提供了相应的工具可以让URDF图像化显示出来。

  $ urdf_to_graphiz my_robot.urdf

然后打开生成的pdf文件，即可看到图形化的URDF：

4      示例

参见：Example

5      调试工具

5.1       验证工具

上文已经提到了URDF语法检查工具check_urdf，在indigo版本的ROS中，执行如下命令即可：

rosrun urdfdom check_urdf /tmp/pr2.urdf

检查结果如下：

5.2       可视化工具

上文同样提到的URDF可视化工具urdf_to_graphiz，在indigo版本额ROS中属于liburdfdom-tools包中的一个工具，可以使用如下命令安装：

sudo apt-get install liburdfdom-tools

可视化工具的使用方法：

urdf_to_graphiz pr2.urdf

结果如下所示：

 

参考链接

1、urdf功能包：http://wiki.ros.org/urdf

2、urdf语法规范：http://wiki.ros.org/urdf/XML

3、urdf教程：http://wiki.ros.org/urdf/Tutorials

4、urdf示例：http://wiki.ros.org/urdf/Examples

（二十四）——使用gazebo中的插件

URDF文件完成后，可以在rviz中显示机器人的模型，如果要在gazebo中进行物理环境仿真，还需要为URDF文件加入一些gazebo相关的标签。既然是仿真，那么机器人应该像真在真实环境中一样，可以通过传感器感知周围环境，还可以根据指令进行运动。

在gazebo中可以通过插入一些插件，来仿真机器人的传感器、执行器的特性，这些插件通过<gazebo>元素中的<plugin>标签描述，使用下边的格式加入到URDF文件中：

1. <gazebo>

2.   <plugin name="differential_drive_controller" filename="libdiffdrive_plugin.so">

3.     ... plugin parameters ...

4.   </plugin>

5. </gazebo>

当然，这些插件常常需要附着于机器人的某个部分，比如要加入一个激光传感器的插件，那么我们需要将这个插件与机器人模型中的激光传感器link绑定：

1. <gazebo reference="your_link_name">

2.   <plugin name="your_link_laser_controller" filename="libgazebo_ros_laser.so">

3.     ... plugin parameters ...

4.   </plugin>

5. </gazebo>

gazebo默认支持不少常用的设备，可以找到ros安装目录中的gazebo_plugins功能包，里边就是所有支持的插件。当然，你也可以自己编写需要的gazebo插件。

下边列出一些常用插件的使用方法:

1．差速驱动插件

很多机器人本体都采用差速驱动的方式，gazebo提供差速机器人的仿真插件，可以直接将下边的代码放到URDF文件中，修改相应的参数，指定运动控制需要订阅的主题，让机器人在gazebo中动起来。

1. <gazebo>

2.   <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">

3.     <alwaysOn>true</alwaysOn>

4.     <updateRate>${update_rate}</updateRate>

5.     <leftJoint>base_link_right_wheel_joint</leftJoint>

6.     <rightJoint>base_link_left_wheel_joint</rightJoint>

7.     <wheelSeparation>0.5380</wheelSeparation>

8.     <wheelDiameter>0.2410</wheelDiameter>

9.     <torque>20</torque>

10.     <commandTopic>cmd_vel</commandTopic>

11.     <odometryTopic>odom</odometryTopic>

12.     <odometryFrame>odom</odometryFrame>

13.     <robotBaseFrame>base_footprint</robotBaseFrame>

14.   </plugin>

15. </gazebo>

2．摄像头

   在gazebo中也可以让机器人的摄像头看到虚拟的环境，将下边的代码放到URDF文件中，“reference”修改成机器人的摄像头link，还有一些分辨率、刷新率、发布的图像主题等等参数。把机器人模型在gazebo中运行起来后，用image_view或者rviz就可以看到机器人在虚拟环境中的视野了。

1. <!-- camera -->

2.   <gazebo reference="camera_link">

3.     <sensor type="camera" name="camera1">

4.       <update_rate>30.0</update_rate>

5.       <camera name="head">

6.         <horizontal_fov>1.3962634</horizontal_fov>

7.         <image>

8.           <width>800</width>

9.           <height>800</height>

10.           <format>R8G8B8</format>

11.         </image>

12.         <clip>

13.           <near>0.02</near>

14.           <far>300</far>

15.         </clip>

16.         <noise>

17.           <type>gaussian</type>

18.           <!-- Noise is sampled independently per pixel on each frame.

19.                That pixel's noise value is added to each of its color

20.                channels, which at that point lie in the range [0,1]. -->

21.           <mean>0.0</mean>

22.           <stddev>0.007</stddev>

23.         </noise>

24.       </camera>

25.       <plugin name="camera_controller" filename="libgazebo_ros_camera.so">

26.         <alwaysOn>true</alwaysOn>

27.         <updateRate>0.0</updateRate>

28.         <cameraName>rrbot/camera1</cameraName>

29.         <imageTopicName>image_raw</imageTopicName>

30.         <cameraInfoTopicName>camera_info</cameraInfoTopicName>

31.         <frameName>camera_link</frameName>

32.         <hackBaseline>0.07</hackBaseline>

33.         <distortionK1>0.0</distortionK1>

34.         <distortionK2>0.0</distortionK2>

35.         <distortionK3>0.0</distortionK3>

36.         <distortionT1>0.0</distortionT1>

37.         <distortionT2>0.0</distortionT2>

38.       </plugin>

39.     </sensor>

40.   </gazebo>

     我们来解释一下这个插件中的参数含义：

1.   <gazebo reference="camera_link">

插件都是用来描述link、joint的，是一种虚无的属性描述，需要关联相应的实体，所以首先我们需要通过reference参数来定义关联的link或者joint。这里我们关联的是camera_link。

1. <sensor type="camera" name="camera1">

       然后声明插件的类型，并为该插件取一个唯一的名称。

1. <update_rate>30.0</update_rate>

       设置摄像头数据更新的最大频率。

1.         <horizontal_fov>1.3962634</horizontal_fov>

2.         <image>

3.           <width>800</width>

4.           <height>800</height>

5.           <format>R8G8B8</format>

6.         </image>

7.         <clip>

8.           <near>0.02</near>

9.           <far>300</far>

10.         </clip>

再来设置一些摄像头相关的参数，尽量让这些仿真的参数与实际使用的硬件相同。

1. <plugin name="camera_controller" filename="libgazebo_ros_camera.so">

关联摄像头插件，该插件已经在gazebo中实现，所以直接关联即可。

1.         <cameraName>rrbot/camera1</cameraName>

2.         <imageTopicName>image_raw</imageTopicName>

3.         <cameraInfoTopicName>camera_info</cameraInfoTopicName>

4.         <frameName>camera_link</frameName>

声明摄像头发布的消息话题，以及数据所在的参考系。

3．Kinect

Kinect在机器人导航中用的也很多，为机器人的kinect link绑定这个kinect插件，我们就可以在gazebo中进行SLAM、导航了，对于一些初学者，就算没有实体机器人，也可以在仿真器中玩转机器人。

1. <gazebo>

2.   <plugin name="${link_name}_controller" filename="libgazebo_ros_openni_kinect.so">

3.     <baseline>0.2</baseline>

4.     <alwaysOn>true</alwaysOn>

5.     <updateRate>1.0</updateRate>

6.     <cameraName>${camera_name}_ir</cameraName>

7.     <imageTopicName>/${camera_name}/depth/image_raw</imageTopicName>

8.     <cameraInfoTopicName>/${camera_name}/depth/camera_info</cameraInfoTopicName>

9.     <depthImageTopicName>/${camera_name}/depth/image_raw</depthImageTopicName>

10.     <depthImageInfoTopicName>/${camera_name}/depth/camera_info</depthImageInfoTopicName>

11.     <pointCloudTopicName>/${camera_name}/depth/points</pointCloudTopicName>

12.     <frameName>${frame_name}</frameName>

13.     <pointCloudCutoff>0.5</pointCloudCutoff>

14.     <distortionK1>0.00000001</distortionK1>

15.     <distortionK2>0.00000001</distortionK2>

16.     <distortionK3>0.00000001</distortionK3>

17.     <distortionT1>0.00000001</distortionT1>

18.     <distortionT2>0.00000001</distortionT2>

19.     <CxPrime>0</CxPrime>

20.     <Cx>0</Cx>

21.     <Cy>0</Cy>

22.     <focalLength>0</focalLength>

23.     <hackBaseline>0</hackBaseline>

24.   </plugin>

25. </gazebo>

4．激光

有没有感叹过激光传感器太贵了！现在不要9999，不要999，只要把激光传感器的插件放到URDF模型中，上万的激光传感器在gazebo里随便用，妈妈再也不用担心玩机器人败家了。

1. <gazebo reference="hokuyo_link">

2.     <sensor type="ray" name="head_hokuyo_sensor">

3.       <pose>0 0 0 0 0 0</pose>

4.       <visualize>false</visualize>

5.       <update_rate>40</update_rate>

6.       <ray>

7.         <scan>

8.           <horizontal>

9.             <samples>720</samples>

10.             <resolution>1</resolution>

11.             <min_angle>-1.570796</min_angle>

12.             <max_angle>1.570796</max_angle>

13.           </horizontal>

14.         </scan>

15.         <range>

16.           <min>0.10</min>

17.           <max>30.0</max>

18.           <resolution>0.01</resolution>

19.         </range>

20.         <noise>

21.           <type>gaussian</type>

22.           <!-- Noise parameters based on published spec for Hokuyo laser

23.                achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and

24.                stddev of 0.01m will put 99.7% of samples within 0.03m of the true

25.                reading. -->

26.           <mean>0.0</mean>

27.           <stddev>0.01</stddev>

28.         </noise>

29.       </ray>

30.       <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">

31.         <topicName>/rrbot/laser/scan</topicName>

32.         <frameName>hokuyo_link</frameName>

33.       </plugin>

34.     </sensor>

35.   </gazebo>

当然，实践是第一要义，这些插件用起来效果到底怎么样，还得你上手来试试。ROS中的很多机器人也提供了gazebo中仿真的模型和实验的案例，如果你还不太明白，直接运行这些已有的模型，看看别人写的代码，理解会更快。这里，推荐一个husky机器人的案例，可以在gazebo中SLAM和自主导航。

    husky机器人的主页是http://wiki.ros.org/Robots/Husky，仿真案例在Demo Applications那里。