【ROS教程 009】导航功能包集入门

本节我们将一起探索ROS系统最强大的特性之一，让你的机器人自主导航和运动。很振奋人心吧~Let Go.首先要感谢开源社区和共享代码，ROS拥有大量用于导航的算法。

（1）ROS导航功能包集

为了理解导航功能包集，我们可以将它看做一套使用机器人传感器和测距功能的算法，这个功能包集能够为任何机器人所用，但也需要调整一些配置文件并且功能包集写一些节点。当然我们的功能包集是需要满足一些条件的。ROS机器人导航功能包集配置方式为：

（2）创建转换

导航功能包集需要知道传感器、轮子和关节的位置。我们使用TF（Transform Frame）软件库来管理坐标转换树。TF软件库允许我们向机器人添加更多的传感器和组件并为我们处理这些设备之间的关系。如果激光雷达向后移动10cm或者向前移动20cm，我们都需要添加一个带有这些偏移的新坐标系到坐标变化树。
step1：创建广播机构
在chapter7_tutorials/src文件夹下以tf_broadcaster.cpp为名称创建一个新的文件，并将以下代码放入文件中：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_publisher");
  ros::NodeHandle n;

  ros::Rate r(100);

  tf::TransformBroadcaster broadcaster;

  while(n.ok()){
  broadcaster.sendTransform(
  tf::StampedTransform(
  tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.0, 0.0, 0.0)),
  ros::Time::now(),"base_link", "base_laser"));
  r.sleep();
  }
}

记住将下面这行加入到你的CMakeLists.txt文件中并创建新的可执行文件：

$ rosbuild_add_executable(tf_broadcaster src/tf_broadcaster.cpp)

step2：创建侦听器
在src文件夹下创建tf_listener.cpp

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>

void transformPoint(const tf::TransformListener& listener){
  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame

  geometry_msgs::PointStamped laser_point;
  laser_point.header.frame_id = "base_laser";

  //we'll just use the most recent transform available for our simple example
  laser_point.header.stamp = ros::Time();

  //just an arbitrary point in space
  laser_point.point.x = 1.0;
  laser_point.point.y = 2.0;
  laser_point.point.z = 0.0;

  geometry_msgs::PointStamped base_point;

  //listener.waitForTransform(

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

// listener.lookupTransform("base_link", "wheel_2", ros::Time(0), ros::Duration(10.0));
  ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
  laser_point.point.x, laser_point.point.y, laser_point.point.z,
  base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());

}

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_listener");
  ros::NodeHandle n;

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

  //we'll transform a point once every second
  ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

  ros::spin();
}

同样在CMakeLists.txt中添加编译链接。编译这个功能包并运行这两个节点。

$ rosmake chapter7_tutorials
$ rosrun chapter7_tutorials tf_broadcaster
$ rosrun chapter7_tutorials tf_listener

然后可以看到如下消息

（3）发布传感器信息

机器人可以有很多传感器，我们可以编写很多个节点来处理这些数据，但是导航功能包集仅支持使用平面激光雷达传感器，所以这些传感器必须使用sensor_msgs/LaserScan或sensor_msgs/PointCloud发布数据。我们将要在Gazebo中使用位于机器人前部的激光雷达进行导航。它在base_scan/scan坐标系上发布数据。如果使用真实的激光雷达，ROS很可能已经包含了它的驱动，但有时还有可能会用到ROS没有提供官方驱动的激光雷达，这时需要自己编写一个节点来发布传感器数据,
首先看一下sensor_msgs/LaserScan的消息类型有哪些：

接下来我们创建一个激光雷达节点，在src文件夹下以laser.cpp为名创建一个新的文件代码如下：

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

int main(int argc, char** argv){
 ros::init(argc, argv, "laser_scan_publisher");
 ros::NodeHandle n;
 ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);
 unsigned int num_readings = 100;
 double laser_frequency = 40;
 double ranges[num_readings];
 double intensities[num_readings];
 int count = 0;
 ros::Rate r(1.0);
 while(n.ok()){
  //generate some fake data for our laser scan
  for(unsigned int i = 0; i < num_readings; ++i){
  ranges[i] = count;
  intensities[i] = 100 + count;
  }
  ros::Time scan_time = ros::Time::now();
  //populate the LaserScan message
  sensor_msgs::LaserScan scan;
  scan.header.stamp = scan_time;
  scan.header.frame_id = "base_link";
  scan.angle_min = -1.57;
  scan.angle_max = 1.57;
  scan.angle_increment = 3.14 / num_readings;
  scan.time_increment = (1 / laser_frequency) / (num_readings);
  scan.range_min = 0.0;
  scan.range_max = 100.0;
  scan.ranges.resize(num_readings);
  scan.intensities.resize(num_readings);
  for(unsigned int i = 0; i < num_readings; ++i){
  scan.ranges[i] = ranges[i];
  scan.intensities[i] = intensities[i];
  }
  scan_pub.publish(scan);
  ++count;
  r.sleep();
 }
}

通过这个示例模板你可以使用激光雷达而无需考虑ROS是否已经有了它的驱动。另外这个模板还能用于将其他传感器通过数据格式转换伪装成一个激光雷达。例如能够将双目视觉测距系统或声呐传感器模拟成一个激光雷达。

（4）发布里程数据

里程信息是指机器人相对于某一点的距离。在我们的示例中应该是从base_link坐标系原点到odom坐标系原点的距离。导航功能包集使用的消息类型是nav_msgs/Odometry可以使用以下命令查看：

nav_msgs/Odometry提供了从机器人frame_id坐标系到child_frame_id坐标系的相对位置。它还通过 geometry_msgs/Pose消息提供了位姿信息。通过geometry_msgs/Twist消息提供了速度信息。
num1：Gazebo如何获取里程数据
在Gazebo示例中，机器人在仿真环境中的移动和真实世界中的机器人是一样的。机器人使用的驱动插件是diffdrive_plugin。这个插件会发布机器人在仿真环境中的里程信息，所以我们不需要为Gazebo再去编写任何代码。在命令窗口中输入以下命令在Gazebo中执行我们的示例机器人，并查看里程数据

$ roslaunch chapter7_tutorials gazebo_xacro.launch model:="./urdf/robot1_base_04.xacro"
$ rosrun erratic_teleop erratic_keyboard_teleop

Gazebo会不间断地发布里程数据。可以点击主题来查看具体发布的数据，也可以在命令行窗口中输入以下命令：

$ rostopic echo /odom/pose/pose

正如我们看到的Gazebo在机器人移动时会发布里程数据，仔细看一下这个插件的源代码。插件的源文件保存在erratic_gazebo_plugins功能包中，具体的文件名是
diffdrive_plugin.cpp。

$ rosed erratic_gazebo_plugins diffdrive_plugin.cpp

打开文件后可以看到其源代码。
num2：创建自定义里程数据
在src文件夹下以odometry.cpp为名创建新的文件代码如下：

#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

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

ros::init(argc, argv, "state_publisher");
ros::NodeHandle n;
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 10);

// initial position
double x = 0.0;
double y = 0.0;
double th = 0;

// velocity
double vx = 0.4;
double vy = 0.0;
double vth = 0.4;

ros::Time current_time;
ros::Time last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();

tf::TransformBroadcaster broadcaster;
ros::Rate loop_rate(20);

const double degree = M_PI/180;

// message declarations
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_footprint";

while (ros::ok()) {
current_time = ros::Time::now();

double dt = (current_time - last_time).toSec();
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
double delta_th = vth * dt;

x += delta_x;
y += delta_y;
th += delta_th;

geometry_msgs::Quaternion odom_quat;
odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,th);

// update transform
odom_trans.header.stamp = current_time;
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(th);

//filling the odometry
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_footprint";

// position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;

//velocity
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.linear.z = 0.0;
odom.twist.twist.angular.x = 0.0;
odom.twist.twist.angular.y = 0.0;
odom.twist.twist.angular.z = vth;

last_time = current_time;

// publishing the odometry and the new tf
broadcaster.sendTransform(odom_trans);
odom_pub.publish(odom);

loop_rate.sleep();
}
return 0;
}

该代码实现了前后运动和转向。记得在CMakeLists.txt中添加编译链接语句然后编译，调用命令启动机器人并使用rviz来查看模型和机器人的运动。

$ roslaunch chapter7_tutorials display_xacro.launch model:="./urdf/robot1_base_04.xacro"
$ rosrun chapter7_tutorials odometry

（5）创建基础控制器

一个基础控制器能够直接和你的机器人的电子设备通信。机器人是通过geometry_msgs/Twist类型的消息所控制的。这个类型正是我们之前看到的Odometry消息所使用的。基础控制器必须订阅以cmd_vel为名称的主题，必须生成正确的线速度和角速度来驱动机器人。输入以下命令复习一下消息的结构。

线速度向量linear包含了x、y和z轴的线速度。角速度向量angular包含了各个轴向的角速度。对于我们的机器人只需要使用线速度x和角速度z。这是因为我们的机器人是基于差分驱动平台的，驱动它的两个电机只能够让机器人前进、后退和转向。先在Gazebo中运行我们的机器人，来理解基础控制器的作用。我们在新的命令行中运行以下命令：

$ roslaunch chapter7_tutorials gazebo_xacro.launch model:="./urdf/robot1_base_04.xacro"
$ rosrun erratic_teleop erratic_keyboard_teleop

在所有节点都启动和运行之后，我们使用节点状态图命令rxgraph来查看各个节点之间的关系:

$ rxgraph

这里可以看到Gazebo自动订阅了由远程操作节点生成的cmd_vel主题。仿真程序插件通过cmd_vel主题获取控制命令数据，同时仿真程序插件在Gazebo环境中完成机器人的驱动并生成里程信息。
为了解使用Gazebo创建里程数据的工作原理，可以仔细查看一下仿真程序插件diffdrive_plugin.cpp文件中的代码。
创建基础控制器需要做的工作和上面类似，也就是说上面的很多代码可以直接用在带有两个驱动轮、电机和编码器的实际机器人身上。在src文件夹下创建
base_controller.cpp代码如下：

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <iostream>

using namespace std;

double width_robot = 0.1;
double vl = 0.0;
double vr = 0.0;
ros::Time last_time;
double right_enc = 0.0;
double left_enc = 0.0;
double right_enc_old = 0.0;
double left_enc_old = 0.0;
double distance_left = 0.0;
double distance_right = 0.0;
double ticks_per_meter = 100;
double x = 0.0;
double y = 0.0;
double th = 0.0;
geometry_msgs::Quaternion odom_quat;

void cmd_velCallback(const geometry_msgs::Twist &twist_aux)
{
geometry_msgs::Twist twist = twist_aux;
double vel_x = twist_aux.linear.x;
double vel_th = twist_aux.angular.z;
double right_vel = 0.0;
double left_vel = 0.0;

if(vel_x == 0){ // turning
right_vel = vel_th * width_robot / 2.0;
left_vel = (-1) * right_vel;
}else if(vel_th == 0){ // fordward / backward
left_vel = right_vel = vel_x;
}else{ // moving doing arcs
left_vel = vel_x - vel_th * width_robot / 2.0;
right_vel = vel_x + vel_th * width_robot / 2.0;
}
vl = left_vel;
vr = right_vel;
}

int main(int argc, char** argv){
ros::init(argc, argv, "base_controller");
ros::NodeHandle n;
ros::Subscriber cmd_vel_sub = n.subscribe("cmd_vel", 10, cmd_velCallback);
ros::Rate loop_rate(10);

while(ros::ok())
{

double dxy = 0.0;
double dth = 0.0;
ros::Time current_time = ros::Time::now();
double dt;
double velxy = dxy / dt;
double velth = dth / dt;

ros::spinOnce();
dt = (current_time - last_time).toSec();;
last_time = current_time;

// calculate odomety
if(right_enc == 0.0){
distance_left = 0.0;
distance_right = 0.0;
}else{
distance_left = (left_enc - left_enc_old) / ticks_per_meter;
distance_right = (right_enc - right_enc_old) / ticks_per_meter;
}

left_enc_old = left_enc;
right_enc_old = right_enc;

dxy = (distance_left + distance_right) / 2.0;
dth = (distance_right - distance_left) / width_robot;

if(dxy != 0){
x += dxy * cosf(dth);
y += dxy * sinf(dth);
}

if(dth != 0){
th += dth;
}
odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,th);
loop_rate.sleep();
}
}

这段代码只是最简单最通用的示例。如果要将这段代码移植到特定机器人上，你可能还需要添加相当多的代码才能让它正常工作。

（6）使用ROS创建地图

ROS中有一个map_server地图服务器，它能够帮助你使用激光雷达和机器人的里程信息创建地图。在本例中我们将会学习如何使用我们在Gazebo中创建的机器人来创建地图、保存地图和加载地图。在launch文件夹下以gazebo_mapping_robot.launch为名创建文件内容如下：

<?xml version="1.0"?>
<launch>
  <!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/pr2/erratic_defs/robots for full erratic -->

  <param name="/use_sim_time" value="true" />

  <!-- start up wg world -->
  <include file="$(find gazebo_worlds)/launch/wg_collada_world.launch"/>

  <arg name="model" />
  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
  <!-- start robot state publisher -->
  <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" output="screen" >
  <param name="publish_frequency" type="double" value="50.0" />
  </node>

  <node name="spawn_robot" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -z 0.1 -model robot_model" respawn="false" output="screen" />

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find chapter7_tutorials)/launch/mapping.vcg"/>
  <node name="slam_gmapping" pkg="gmapping" type="slam_gmapping">
  <remap from="scan" to="base_scan/scan"/>
  </node>

</launch>

有了这个.launch文件你就能够启动我们所需的Gazebo仿真环境。在这个仿真环境中，你不仅能够使用3D模型，还能通过rviz配置文件和slam_mapping来实时构建地图。在命令行中运行这个启动文件，在另一个命令行窗口中运行远程操作节点来移动机器人。

$ roslaunch chapter7_tutorials gazebo_mapping_robot.launch model:="./urdf/robot1_base_04.xacro"
$ rosrun erratic_teleop erratic_keyboard_teleop

当你使用键盘移动机器人的时候，你会在rviz屏幕上看到很多空白和未知的空间，也有一部分空间已经被地图所覆盖。这部分已知的地图就被称为覆盖网格地图(Occupancy Grid Map,OGM)。每当机器人移动时，salam_mapping节点都会更新地图状态。或者更详细一点说，是机器人移动后，它会更新激光雷达和里程数据，并对机器人的危机估计和地图计算上获得更优的计算结果时，将OGM发布到屏幕上显示。

num1：使用map_server保存地图
一旦完成了整个地图或者完成了你想要完成的部分，就能够通过导航功能包集将地保存起来并在下次使用时调用它。使用以下命令来保存地图：

这个命令会创建两个文件，map.pgm和map.yaml。
num2：使用map_server加载地图
当想要自己机器人所创建的地图时，必须将其加载到map_server功能包中，使用以下命令。

$ rosrun map_server map_server map.yaml

为了简化工作可以在launch文件夹下创建gazebo_map_robot.launch并添加以下代码：

<?xml version="1.0"?>
<launch>
  <!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/pr2/erratic_defs/robots for full erratic -->

  <param name="/use_sim_time" value="true" />

  <!-- start up wg world -->
  <include file="$(find gazebo_worlds)/launch/wg_collada_world.launch"/>

  <arg name="model" />
  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
  <!-- start robot state publisher -->
  <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" output="screen" >
  <param name="publish_frequency" type="double" value="50.0" />
  </node>

  <node name="spawn_robot" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -z 0.1 -model robot_model" respawn="false" output="screen" />

  <node name="map_server" pkg="map_server" type="map_server" args=" $(find chapter7_tutorials)/maps/map.yaml" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find chapter7_tutorials)/launch/mapping.vcg" />

</launch>

使用以下命令来启动文件，记住还要指定你所要使用的机器人的模型：

$ roslaunch chapter7_tutorials gazebo_map_robot.launch model:="./urdf/robot1_base_04.xacro"

然后你会在rviz中看到机器人和地图。为了能够获得机器人的位置，导航功能包集会使用地图服务器发布的地图数据和激光雷达的读数。这主要是通过使用扫描匹配算法来实现的，即使用自适应蒙特卡罗定位（AMCL）节点来估计机器人的位置。
References:
[1]. Aaron Martinez Enrique Fern andez, ROS机器人程序设计[B], P160-183, 2014.
http://wiki.ros.org/cn/navigation/Tutorials/RobotSetup
http://wiki.ros.org/slam_gmapping