ROS 导航——用Gmapping构建一张地图

Gmapping功能包介绍

参见wiki官网 Gmapping功能包.其中包括了Gmapping功能包订阅发布的话题，参数等。

使用Gmapping功能包的一些准备

根据官网介绍，Gmapping订阅了两个话题：/tf 和 /scan

/tf

tf指的是坐标变换，就是将机器人各组件的相对位置关系表示出来，对于Gmapping而言，它需要三个tf：雷达，机器人基坐标和里程计

雷达和机器人基座标

对于雷达和机器人基座标，我们可以有两种发布的方法。
第一种：通过urdf建立机器人模型，然后写一个launch文件将模型文件载入并发布机器人的状态信息给tf。urdf是一种机器人建模的一种方法，具体参见urdf wiki官网
launch文件示例如下：

<?xml version="1.0"?>
>
    <!--param name="/use_sim_time" value="true"/ -->
    
    
    

    <!-- 设置GUI参数，显示关节控制插件 -->
    

    <!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
    
    
    <!-- 运行robot_state_publisher节点，发布tf  -->
    

    <!-- 运行rviz可视化界面 -->
    
>

第二种：调用tf的API，编写代码发布tf。具体参见 tf API wiki官网。

里程计

里程计发布的是base_link → odom坐标变换，其中base_link是上面所说的机器人基坐标，odom是世界坐标系中固定的一个坐标。里程计反映的是机器人行驶的轨迹，这个轨迹是由机器人的速度与时间计算出来的，所以里程计的发布实际上是不断发布机器人的速度信息与时间信息。
时间信息可以由ROS帮我们计算，而速度信息需要结合机器人的实际情况，通常由电机编码器将速度反馈给电机驱动器的控制板，然后由控制板发布给ROS。
以Arduino Uno作为驱动板为例，发布里程计的示例代码如下：

/* 
 * rosserial Planar Odometry Example
 */

#include 
#include 
#include 
#include 

ros::NodeHandle  nh;

geometry_msgs::TransformStamped t;
tf::TransformBroadcaster broadcaster;

double x = 1.0;
double y = 0.0;
double theta = 1.57;

char base_link[] = "/base_link";
char odom[] = "/odom";

void setup()
{
  nh.initNode();
  broadcaster.init(nh);
}

void loop()
{  
  // drive in a circle
  double dx = 0.2;
  double dtheta = 0.18;
  x += cos(theta)*dx*0.1;
  y += sin(theta)*dx*0.1;
  theta += dtheta*0.1;
  if(theta > 3.14)
    theta=-3.14;
    
  // tf odom->base_link
  t.header.frame_id = odom;
  t.child_frame_id = base_link;
  
  t.transform.translation.x = x;
  t.transform.translation.y = y;
  
  t.transform.rotation = tf::createQuaternionFromYaw(theta);
  t.header.stamp = nh.now();
  
  broadcaster.sendTransform(t);
  nh.spinOnce();
  
  delay(10);
}

/scan

/scan中发布的是激光雷达传感器的信息，这些信息用于描述周围障碍物。
该话题的发布需要结合激光雷达本身的数据来发布，消息格式参见wiki官网
发布话题的代码示例如下

#include 
#include 
 
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);
 
  //A real application would pull the following data from their laser driver
  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，
    //Populate the dummy laser data with values that increase by one every second.
    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，
    //Create a scan_msgs::LaserScan message and fill it with the data
    //that we've generated in preparation to send it over the wire.
    sensor_msgs::LaserScan scan;
    scan.header.stamp = scan_time;
    scan.header.frame_id = "laser_frame";
    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];
    }
 
    //Publish the message over ROS
    scan_pub.publish(scan);
    ++count;
    r.sleep();
  }
}

使用Gmmaping功能包

当 /tf 和 /scan话题都准备好后，我们就可以用一个launch文件来使用Gmapping功能包了。
launch文件示例如下：

<?xml version="1.0"?>
>
	<!--Run robot model -- >
	
	
	<!--Publish odom-->
	
	
	<!--Run lidar-->
	
	
	<!--Run Gmapping-->
	
    	>
	>
>

得到的地图

运行Gmapping后，进入rviz可以看到见图的实时进程。等地图建好后，可以用map_server功能包来保存地图，使用map_server的命令如下：

rosrun map_server map_saver map:=/<Map Topic> -f PATH_TO_YOUR_FILE/mymap

保存完成后得到如下所示的一张栅格图