上一节介绍和测试了cartographer的官方demo。

本节会编写ros系统中，最常用的激光雷达LaserScan传感数据的订阅和发布，方便在cartographer中加入自己的数据进行建图与定位。(作者使用的是SICK-NAV350)

官方文档：

http://wiki.ros.org/navigation/Tutorials/RobotSetup/Sensors

---

目录

1：sensor_msgs/LaserScan消息类型

2：发布LaserScan消息

3：订阅LaserScan消息

---

1：sensor_msgs/LaserScan消息类型

在终端查看消息数据结构：

rosmsg show sensor_msgs/LaserScan

LaserScan消息类型数据结构如下：

std_msgs/Header header          
　　uint32 seq
　　time stamp           
　　string frame_id 　　  # in frame frame_id, angles are measured around  the positive Z axis (counterclockwise, if Z is up)
                         # with zero angle being forward along the x axis           
                         
float32 angle_min        # start angle of the scan [rad]
float32 angle_max        # end angle of the scan [rad]
float32 angle_increment  # angular distance between measurements [rad]

float32 time_increment   # time between measurements [seconds] - if your scanner
                         # is moving, this will be used in interpolating position
                         # of 3d points
float32 scan_time        # time between scans [seconds]

float32 range_min        # minimum range value [m]
float32 range_max        # maximum range value [m]

float32[] ranges &nbsp; &nbsp; &nbsp; &nbsp; # range data [m] (Note: values < range_min or > range_max should be discarded)
float32[] intensities &nbsp; &nbsp;# intensity data [device-specific units]. &nbsp;If your device does not provide intensities, please leave the array empty.

得到了消息数据结构之后，就可以根据雷达数据填充消息内容。

其中angle_min angle_max angle_increment time_increment range_min range_max等基本上是雷达设置常量。

而ranges表示对应角度的测量距离，intensities表示对应角度的反射强度，这些是动态变化的，需要根据雷达频率更新。

---

2：发布LaserScan消息

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

int main(int argc, char** argv)
{
&nbsp;&nbsp; &nbsp;//ros节点初始化
&nbsp; &nbsp; ros::init(argc, argv, "laser_scan_publisher");
&nbsp; &nbsp; ros::NodeHandle n;

&nbsp;&nbsp; &nbsp;//创建LaserScan消息发布Publisher
&nbsp; &nbsp; ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);

&nbsp;&nbsp; &nbsp;//激光雷达参数设置
&nbsp; &nbsp; unsigned int num_readings = 100;
&nbsp; &nbsp; double laser_frequency = 40;
&nbsp; &nbsp; double ranges[num_readings];
&nbsp; &nbsp; double intensities[num_readings];
&nbsp; &nbsp; int count = 0;

&nbsp; &nbsp; ros::Rate r(1.0);

&nbsp; &nbsp; while(n.ok())
&nbsp; &nbsp; {
&nbsp; &nbsp; &nbsp; &nbsp; //为激光扫描生成一些假数据(可替换成自己激光雷达真实数据)
&nbsp; &nbsp; &nbsp; &nbsp; for(unsigned int i = 0; i < num_readings; ++i)
&nbsp; &nbsp; &nbsp; &nbsp; {
&nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp; ranges[i] = count;
&nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp; intensities[i] = 100 + count;
&nbsp; &nbsp; &nbsp; &nbsp; }
&nbsp; &nbsp; &nbsp; &nbsp; ros::Time scan_time = ros::Time::now();
&nbsp;&nbsp; &nbsp;
&nbsp; &nbsp; &nbsp; &nbsp; //填充 LaserScan 消息
&nbsp; &nbsp; &nbsp; &nbsp; sensor_msgs::LaserScan scan;
&nbsp; &nbsp; &nbsp; &nbsp; scan.header.stamp = scan_time;
&nbsp; &nbsp; &nbsp; &nbsp; scan.header.frame_id = "base_link";
&nbsp; &nbsp; &nbsp; &nbsp; scan.angle_min = -1.57;
&nbsp; &nbsp; &nbsp; &nbsp; scan.angle_max = 1.57;
&nbsp; &nbsp; &nbsp; &nbsp; scan.angle_increment = 3.14 / num_readings;
&nbsp; &nbsp; &nbsp; &nbsp; scan.time_increment = (1 / laser_frequency) / (num_readings);
&nbsp; &nbsp; &nbsp; &nbsp; scan.range_min = 0.0;
&nbsp; &nbsp; &nbsp; &nbsp; scan.range_max = 100.0;
&nbsp; &nbsp; &nbsp; &nbsp; scan.ranges.resize(num_readings);
&nbsp; &nbsp; &nbsp; &nbsp; scan.intensities.resize(num_readings);
&nbsp; &nbsp; &nbsp; &nbsp; for(unsigned int i = 0; i < num_readings; ++i)
&nbsp; &nbsp; &nbsp; &nbsp; {
&nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp; scan.ranges[i] = ranges[i];
&nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp; scan.intensities[i] = intensities[i];
&nbsp; &nbsp; &nbsp; &nbsp; }

&nbsp;&nbsp; &nbsp;&nbsp;&nbsp; &nbsp;//通过 ROS 发布消息
&nbsp; &nbsp; &nbsp; &nbsp; scan_pub.publish(scan);

&nbsp; &nbsp; &nbsp; &nbsp; ++count;
&nbsp; &nbsp; &nbsp; &nbsp; r.sleep();
&nbsp; &nbsp; }
}

---

3：订阅LaserScan消息

通常来说，发布自己激光雷达的信息到ros系统中，同时cartographer节点能够自动订阅接收LaserScan消息，就可以实现将雷达数据传给cartographer算法的目的。

但特定情况下可能需要订阅查看激光雷达的信息。

(1) 通过rosbag订阅

rostopic echo /scan

(2) 通过rviz查看

打开rviz

rosrun rviz rviz

Fixed Frame修改为base_link，添加LaserScan并将Topic设为/scan

(3) 编写程序打印

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"

void ScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg)
{
    size_t size = msg->ranges.size();
    std::string ranges ="";
&nbsp; &nbsp; for(size_t i =0; i< size;i++){
&nbsp; &nbsp; &nbsp; &nbsp; ranges += std::to_string(msg->ranges[i]) + ", ";
&nbsp; &nbsp; }
&nbsp; &nbsp; ROS_INFO("Scan: [%s]", ranges);
}

int main(int argc, char **argv)
{
&nbsp; &nbsp; ros::init(argc, argv, "listener");
&nbsp; &nbsp; ros::NodeHandle node;
&nbsp; &nbsp; ros::Subscriber subScan = node.subscribe("scan", 1000, ScanCallback);
&nbsp; &nbsp; ros::spin();
&nbsp; &nbsp; return 0;
}

【完】

---

下一节会介绍里程计Odom数据的发布和订阅。