ROS的树莓派与stm32的地面移动机器人构建问题

硬件基础

层级	系统	硬件	功能
边缘层	linux系统 ROS 16.04 kinetic	树莓派3B+	接收传感器数据（2D激光雷达），传输控制命令到执行层
执行层	keil5编译环境	stm32F103RCT8	负责接收控制命令，驱动级的控制车轮

软件内容

软件上主要是介绍雷达在树莓派3B+上的使用 ，键盘输入发布速度控制信息到串口，树莓派与stm32之间的串口连接，stm32的串口接收速度控制信息，本地ROS与树莓派ROS之间的通信，cartographer的使用等内容。

First 雷达在树莓派3B+上的使用

首先，这部分内容要实现的是用树莓派驱动激光雷达的旋转，获取雷达的数据，是在 2D激光雷达 的基础上实现的，其接口是USB线。

1. 在树莓派上创建一个catkin工作空间：slam2D_ws（可修改工作空间名）

$ mkdir /slam2D_ws/src
$ cd slam2D_ws

2. 通过下面的地址获取激光雷达的官方驱动，放置在src文件夹下，将程序包的名字改为rplidar_ros（因为github下载下来的程序包都带master后缀，删掉即可），然后catkin_make编译一下整个工作空间。

https://github.com/robopeak/rplidar_ros

$ catkin_make

然后，在basherc文件中添加路径

$ sudo gedit ~/.bashrc

最后一行，添加：（{机器名}自行修改，不要忘记了）

source /home/{机器名}/slam2D_ws/devel/setup.bash

最后，source一下。或者关闭终端重新打开终端。

$ source ~/.bashrc

3. 驱动中总共有两个可执行程序，即rplidarNode和rplidarNodeClient，

• rplidarNode：负责驱动激光雷达的旋转和雷达数据获取，获取的数据发布在/scan上
• rplidarNodeClient：官方给的一个从/scan节点接收雷达数据并显示在终端中的程序

4. 运行程序前， 需要检查rplidar的串行端口的权限：

$ ls -l / dev | grep ttyUSB

将显示一行…ttyUSB0（一般是）的结果，为串口添加写权限：

$ sudo chmod 666 /dev/ttyUSB0

5. 有两种方法来运行rplidar_ros包

• 在rviz运行rplidar节点和rviz雷达的视图，运用 roslaunch 命令。此处要注意树莓派需要修改rviz的一个配置，修改雷达的数据类型为points，才可正常显示雷达的数据。

$ roslaunch rplidar_ros view_rplidar.launch

但是树莓派运行rviz可能会占用很多内存，故还有下一种运行方式（常用）：

• 首先运行rplidar.launch，该运行方式和上述方式的唯一区别是，没有运行rviz，方便使用/scan数据的打开rviz查看（例如SLAM建图算法直接显示地图，不显示雷达数据）。

$ roslaunch rplidar_ros rplidar.launch

可以利用简单的显示功能rplidarNodeClient接收/scan节点的数据，并显示出来，运行命令如下。

$ rosrun rplidar_ros rplidarNodeClient

当然，你也可以用其他的SLAM建图算法处理/scan节点的雷达数据，本文用百度开源的cartographer建图算法，在后文列出。

Second 树莓派与stm32之间的串口连接

1. 树莓派的系统安装：
   虽然很简单，但是还是要提一下。安装树莓派建议选择 Ubuntu Mate 系统，我选择的就是 Ubuntu Mate 16.04 版本。在安装一些工具包时，这个系统兼容性比较好。
   将树莓派上的 SD 卡插入读卡器，然后读卡器插入电脑串口，在 Etcher 软件（如下图）
   
   选择已经下载好的 Ubuntu Mate 系统镜像，然后单击 Flash！ 就可以了。

刚安装上系统的树莓派，需要重启一下网络就能连接wifi。

sudo service network-manager restart

2. 树莓派的串口配置，弃用蓝牙，将蓝牙的串口映射给GPIO使用。
   树莓派上有两个串口，一个是高性能串口，树莓派3B+默认将该串口分配给蓝牙使用，另一个是miniuart，即树莓派的两排引脚的GPIO13/14，网上均说该串口性能不好，会随着主频的跳动而跳动，会影响该串口的波特率，传输数据不稳定。故该教程是将蓝牙的高性能串口映射给GPIO使用。
   网上方法很多，但是树莓派安装的系统各有不同，就有不同的设置方法。下面是一个比较全的教程。

http://ukonline2000.com/?p=880

但是本教程使用的是Ubuntu mate 16.04系统，需要安装下面的方法：

使用命令看一下树莓派3b+支持的GPIO串口

$ ls -la /dev/

这是最后需要配置完成的界面（即serial0->ttyS0），刚查看时可不是这个界面，大家注意。

• 启用串口ttyS0

$ sudo raspi-config

打开系统配置界面如下图，选择Inerfacing Options

然后选择 serial

先选择No，在选择Yes，最后选择保存，Esc退出即可。

• 关闭Console

$ sudo systemctl stop serial-getty@ttyS0.service
$ sudo systemctl disable serial-getty@ttyS0.service

然后配置cmdline.txt，本教程使用的是gedit，也可以使用其他编辑器。

$ sudo vim /boot/cmdline.txt

打开以后会看的如下内容：

dwc_otg.lpm_enable=0 console=serial0,115200 console=tty1 root=/dev/mmcblk0p2 rootfstype=ext4 elevator=deadline fsck.repair=yes root wait

删掉里面的console=serial0,115200，注意，其他的不要删掉，不管后面是什么，都只删掉中间的这一句话。保存，然后重启树莓派后继续编辑cmdline.txt。

$ sudo reboot
$ sudo vim /boot/cmdline.txt

在里面输入console=serial0(or ttyAMA0),115200，后面的不要动，在中间插入就行，没错，括号里的也要写上去不要乱改。

dwc_otg.lpm_enable=0 console=tty1 console=serial0(or ttyAMA0),115200 root=/dev/mmcblk0p2 rootfstype=ext4 elevator=deadline fsck.repair=yes rootwait

然后，再次重启即可。

• 硬件连接，RX->TX，TX->RX，GND->GND。
  
• 添加串口权限
  运行代码之前，需要先提升串口/dev/ttyS0的读写权限

$ sudo chmod 666 /dev/ttyS0

• 永久添加串口权限

$ sudo systemctl mask serial-getty@ttyS0.service
$ sudo vim /etc/udev/rules.d/90-local.rules

在里面添加入如下内容：

KERNEL==“ttyS0*”, OWNER=“root”, GROUP=“tty”, MODE=“0666”

然后，重启。以后就不用了每次都给权限，直接用就可以了。

3. 在python3.5 IDE环境下，进行读写串口操作（python）
   由于直接用python读写串口，其中pack的方式传输浮点型、整形，转换成字节传输到stm32后即可，下面是测试程序。

#coding=utf-8
import os
import sys
import serial
from time import sleep

def serial_send():
    speed_X = 0
    speed_Y = 0
    speed_limit = 20
    ser = serial.Serial('/dev/ttyS0',115200)
    data =''
    
    while True:
        # ch = sys.stdin.readline(1)
   
        #data pack
        speed_X_data=struct.pack('i',speed_X)
        speed_Y_data=struct.pack('i',speed_Y)
        data = speed_X_data + speed_Y_data
        print(data)
        
        #data send
        if(1):
            ser.write(data)
            #print("Send success! the car is walking.\n")
            sleep(0.2)
            
if __name__=="__main__":
    serial_send()
    

4. （附）树莓派的蓝牙配置使用
   树莓派与HC06的蓝牙模块的连接：

• 首先，点开树莓派的蓝牙，配对HC06的蓝牙，一般密码是1234或者0000。
• 然后，要知道HC06的设备号：

$ hcitool scan

• 配对不一定等于连接，连接有很多方法，我只试验出一种可行：
  需要修改连接模式，从slave到master，以下是教程：
  
  保持这个窗口，然后你会看到树莓派的蓝牙按钮里，hc-06是绿色打钩的状态（之前是红色打叉）。

Third 键盘输入发布速度控制信息到串口

此处使用pygame来进行键盘按键的输入操作，同时对数据进行打包，完成数据传输。pygame的好处是不会像标准键盘输入input（）需等待，pygame不用一直等待键盘输入，让其他程序可以一直运行，不影响其他程序。键盘有输入时pygame才作用一次，然后修改一些值等，继续让其他程序跑下去。

1. pygame的安装
   pygame只适用于python3以上，
   首先安装依赖：

$ sudo apt install python3.5-dev mercurial libsdl-image1.2-dev libsdl2-dev libsdl-ttf2.0-dev
$ sudo apt install libsdl-mixer1.2-dev libportmidi-dev libswscale-dev libsmpeg-dev libavformat-dev libavcodec-dev
$ sudo apt install python-numpy

依赖安装完毕就可以安装pygame了：

$ python3 -m pip install -U pygame --user

以下命令是一个小例程，没试过。

$ python3 -m pygame.examples.aliens

2. 完整的程序（pygame检测键盘输入和数据打包）
   运行程序以后发现一个小黑框（可修改大小），python命令行里在打印发送到数据。键盘检测是在小黑框里的，分别设置里前后左右以及空格，分别对应前进的速度增加，前进的速度减小，横向速度增加，横向速度减小，以及置零。
   此处是用来初始化pygame的窗口的，否则后面再多的代码都没用。

pygame.init()
SCREEN_SIZE = (128,96)
screen = pygame.display.set_mode(SCREEN_SIZE,0,32)

数据的打包方式（和stm32的解包相对应）：

#BB BB 0b 03 ff 07 xx xx xx xx xx xx xx xx check

其中0xBB 0xBB是数据开始，0x0b是数据长度，0x03是地面移动机器人的ID，07是消息类型（速度信息/位置信息），后面四位是vx，再后面四位是vy，check是数据校验位。

完整代码（python3.5）：

#coding=utf-8
import os
import sys
import serial
import struct
from time import sleep
import pygame
from pygame.locals import *

#BB BB 0b 03 ff 07  xx xx xx xx xx xx xx xx check

def serial_send():
    start = b'\xBB\xBB'
    speed_X = 0
    speed_Y = 0
    speed_limit = 20
    ser = serial.Serial('/dev/ttyS0',115200)
    data =''
    pygame.init()
    SCREEN_SIZE = (128,96)
    screen = pygame.display.set_mode(SCREEN_SIZE,0,32)
    
    while True:
        # ch = sys.stdin.readline(1)

        for event in pygame.event.get():
            if event.type == QUIT:
                pygame.quit()
                exit()
            if event.type == KEYDOWN:
                print("you pressed!")
                key_pressed = pygame.key.get_pressed()
                if event.key == K_RIGHT or event.key == K_d :
                    print("RIGHT, X:+2")
                    speed_X += 2
                if event.key == K_LEFT or event.key == K_a:
                    print(" LEFT, X: -2")
                    speed_X -= 2
                if event.key == K_UP or event.key == K_w:
                    print("  UP , Y: +2")
                    speed_Y += 2
                if event.key == K_DOWN or event.key == K_s:
                    print("DOWN, Y: -2")
                    speed_Y -= 2
                if event.key == K_SPACE:
                    speed_X = 0
                    speed_Y = 0
        #limit speed
        if(speed_X>=speed_limit):
            speed_X = speed_limit
        if(speed_Y>=speed_limit):
            speed_Y = speed_limit
        print('[speed]X-axis is:{0}, Y-axis is:{1}'.format(speed_X,speed_Y))
        if(speed_X<speed_limit/5 and speed_X>-speed_limit/5):
            speed_X = 0
        if(speed_Y<speed_limit/5 and speed_Y>-speed_limit/5):
            speed_Y = 0
        
        #data pack
        speed_X_data=struct.pack('i',speed_X)
        speed_Y_data=struct.pack('i',speed_Y)
        data =  b'\x03\xff\x07' +speed_X_data+speed_Y_data
        datacheck=struct.pack('i',uchar_checksum(data,byteorder='little'))[0:1]#产生校验位
        data = start + b'\x0b' + data + datacheck 
        print(data)
        
        #data send
        if(1):
            ser.write(data)
            #print("Send success! the car is walking.\n")
            sleep(1)
            
#定义求校验位的函数
def uchar_checksum(data,byteorder='little'):
    length=len(data)
    checksum=0
    for i in range(0,length):
        checksum+=int.from_bytes(data[i:i+1],byteorder,signed=False)
        checksum&=0xff
    return checksum

if __name__=="__main__":
    serial_send()
    

Fourth stm32的串口接收速度控制信息

本地采用STM32F103RCT8系列芯片，用串口1进行通信（与树莓派连接）。

贴一下数据类型定义：

#define BufferLength 32
typedef union DATA
{
	char c[4];
	float f;
	int i;
}DATA;
void Blue_DataUnpack(void);
typedef struct Blue_car
{
	DATA X;
	DATA Y;
}Blue_car;
extern char Blue_RX_Buffer[BufferLength];
extern char Blue_TX_Buffer[BufferLength];

贴一下串口配置：

void uart_init(u32 bound){
  //GPIO端口配置
  	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;
	

	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1|RCC_APB2Periph_GPIOA|RCC_APB2Periph_AFIO, ENABLE);	//使能USART1，GPIOA时钟
	USART_DeInit(USART1);
	//USART1_TX   GPIOA.9
  	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; //PA.9
  	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;	//复用推挽输出
  	GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIOA.9
   
  //USART1_RX	  GPIOA.10
  	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;//PA10
  	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//¸¡¿ÕÊäÈë
 	 GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIOA.10  
	//UsartNVIC配置
	
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
  	NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority= 0 ;//抢占优先级
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;		//子优先级
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;			//IRQ通道使能
	NVIC_Init(&NVIC_InitStructure);	//
	//USART初始化设置
	USART_InitStructure.USART_BaudRate = bound;//串口波特率
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
	USART_InitStructure.USART_StopBits = USART_StopBits_1;//停止位
	USART_InitStructure.USART_Parity = USART_Parity_No;//没有奇偶校验
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无数据硬件流控制
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//收发模式
  	USART_Init(USART1, &USART_InitStructure); //初始化串口1
  

  	USART_Cmd(USART1, ENABLE);                    //使能串口1 
	USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启串口接收中断

}

贴一下接收中断：

//Blue
int Blue_counter=0;
int Blue_state=0;
u8 d;
Blue_car car3[3];

int USART1_IRQHandler(void)
{	
	u8 Blue_rev;
	static int Blue_data_length=0;
	static char Blue_data_check=0;
	char Blue_start = 0xBB;
	if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //接收到数据
	{
		Blue_rev = USART_ReceiveData(USART1);
		
		 if(Blue_state == 0)
      {
				  	if(Blue_rev == Blue_start)    
						{Blue_state = 1;}
		        else
        	       Blue_state = 0;  
      }
			else if(Blue_state == 1)
      {
				  	if(Blue_rev == Blue_start)    
        	       Blue_state = 2;
		        else
        	       Blue_state = 0;  
      }
			else if(Blue_state==2)
			{
				 Blue_data_length = Blue_rev;
				 Blue_state = 3;
			}
			else if(Blue_state==3)
			{
				if((Blue_counter<Blue_data_length)&&(Blue_data_length<BufferLength))
				{
				 Blue_RX_Buffer[Blue_counter] = Blue_rev;
				 Blue_data_check+=Blue_RX_Buffer[Blue_counter];
					//USART_printf(USART2," (%d-%d)",Blue_counter,Blue_rev);
					Blue_counter++;
				}
				else
				{
					
					Blue_counter = 0;
					Blue_state=0;
		//			data_check=0xff&data_check;
					if(Blue_rev==Blue_data_check)
					{				
						//USART_printf(USART2,"YES");
						Blue_DataUnpack();
						
				  }	
					//USART_printf(USART2,"--(check:%d) \n",Blue_data_check);
					Blue_data_check=0;
					Blue_data_length = 0;
			   }			
			 }
		
	}//数据处理结束
	
return 0;	
}

贴一下解包程序：

void Blue_DataUnpack(void)
{
	  int i;
			for(i = 0; i < 4; i++)
			{
				car3[2].X.c[i] = Blue_RX_Buffer[i+3];
			}
			Blue_X = car3[2].X.i;
			//USART_printf(USART2,"%d",Blue_X);
			for(i = 0; i < 4; i++)
			{
				car3[2].Y.c[i] = Blue_RX_Buffer[i+7];
			}
			Blue_Y = car3[2].Y.i;
}

Fifth 本地ROS与树莓派ROS之间的通信

本部分内容是在同一个局域网同时运行上位机的本地ROS（一台较高算力的电脑）和树莓派ROS，在本地ROS运行roscore。树莓派ROS搭载传感器获取数据（从机），而本地ROS（即主机）处理传感器数据，进行SLAM建图。

实现主机-从机在同一个局域网下的连接

设置主机为PC，运行roscore，从机为树莓派。
PC和树莓派在同一局域网下；也就是都连同一个WIFI（路由器生成的那种，或者手机热点，校园网不行），或者PC连了WIFI、用根网线把树莓派和PC连起来也可以。用ifconfig可以查看IP，用hostname可以查看主机名】假设Master在PC端运行。

1. 修改hosts文件

$ sudo gedit /etc/hosts

在其中添加两行，一行主机（nuvo-7000）的IP和名字，一行从机（car2-dek）的IP和名字。

192.168.31.112 nuvo-7000
192.168.31.227 car2-dek

PC和树莓派都需要修改。

2. 主机和从机相互识别，ping命令：

• 在树莓派上运行：

ping nuvo-7000

• 在PC上运行：

ping car2-dek

可以直接ping 网络地址。
成功的图示如下，出现64 bytes from …等字样。

3. 设置ROS-MASTER-URI

$ sudo gedit ~/.bashrc

在PC端和树莓派的.bashrc都文件中都加1行：

export ROS_MASTER_URI=‘http://192.168.31.113:11311’

IP地址是主机的，即PC的
然后，保存退出，

$ source ~/.bashrc

4. PC运行roscore即可，双方运行的节点就都在一个节点图里了。

Sixth cartographer的使用

A 在本地ROS下载安装百度开源SLAM算法cartographer

首先是cartographer的官方安装教程，但是无法google上网，就不能下载。

https://google-cartographer-ros.readthedocs.io/en/latest/

以下是github安装方式：

1. 先装好wstool、rosdep、ninja：

$ sudo apt-get install -y python-wstool python-rosdep ninja-build

2. 建立工作空间：

$ mkdir catkin_ws
$ cd catkin_ws
~/catkin_ws$ wstool init src

3.从github上下载：
github的源码网址：

https://github.com/ceres-solver/ceres-solver
https://github.com/googlecartographer/cartographer
https://github.com/googlecartographer/cartographer_ros

以下是安装过程：

~/catkin_ws$ cd ./src
~/catkin_ws/src$ git clone https://github.com/googlecartographer/cartographer
~/catkin_ws/src$ git clone https://github.com/googlecartographer/cartographer_ros
~/catkin_ws/src$ git clone https://github.com/ceres-solver/ceres-solver

4. 安装proto3：

~$ '/home/yhexie/catkin_ws/src/cartographer/scripts/install_proto3.sh'

5. 安装依赖库

$ sudo apt-get update
$ sudo apt-get install -y \
    cmake \
    g++ \
    git \
    google-mock \
    libboost-all-dev \
    libcairo2-dev \
    libeigen3-dev \
    libgflags-dev \
    libgoogle-glog-dev \
    liblua5.2-dev \
    libsuitesparse-dev \
    ninja-build \
    python-sphinx

6. 编译（较久）：

~/catkin_ws$ catkin_make_isolated --install --use-ninja

7. 添加环境变量：

$ sudo gedit ~/.bashrc
$ source ~/.bashrc

最后一行添加，注意是install_isolated

source ~/catkin_ws/install_isolated/setup.bash

8. 测试官方demo：

• 2D测试：

wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag

运行命令：

$ roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag

• 3D测试
  运行命令：

$ roslaunch cartographer_ros offline_backpack_3d.launch bag_filenames:='/media/carto/Data/Datasets/b3-2016-03-01-13-39-41.bag'

跑出来的需要较长时间，需要花大概十几分钟。

B 对cartographer和rplidar的适配

1. 位于/catkin_ws/src/cartographer_ros/cartographer_ros/configuration_files文件夹下，定义一个名为revo_lds_rplidar.lua的文件，在revo_lds_.lua基础上修改：

-- Copyright 2016 The Cartographer Authors
--
-- Licensed under the Apache License, Version 2.0 (the "License");
-- you may not use this file except in compliance with the License.
-- You may obtain a copy of the License at
--
--      http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS,
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-- See the License for the specific language governing permissions and
-- limitations under the License.
 
include "map_builder.lua"
include "trajectory_builder.lua"
 
options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "laser",
  published_frame = "laser",
  odom_frame = "odom",
  provide_odom_frame = true,
  use_odometry = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
}
 
MAP_BUILDER.use_trajectory_builder_2d = true
 
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
 
POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65
 
return options

注意这两行：

  tracking_frame = "laser",
  published_frame = "laser",

2. 位于~/catkin_ws/src/cartographer_ros/cartographer_ros/launch文件夹下，编辑一个launch文件demo_revo_lds_rplidar.launch，在demo_revo_lds.launch基础上修改，注意23和25行。

<!--
  Copyright 2016 The Cartographer Authors
 
  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at
 
       http://www.apache.org/licenses/LICENSE-2.0
 
  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
-->
 
<launch>
  <param name="/use_sim_time" value="true" />
 
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename revo_lds_rplidar.lua"
      output="screen">
    <remap from="scan" to="scan" />
  </node>
 
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
 
  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
 
</launch>

注意这两行

	-configuration_basename revo_lds_rplidar.lua"
    <remap from="scan" to="scan" />

最后一行的bag包需要删掉，因为是测试数据集demo用的。

3. 编译一下;

~$ cd ~/catkin_ws
~/catkin_ws$ catkin_make_isolated --install --use-ninja

4. 用两个终端，分别运行两个节点：

$ roslaunch rplidar_ros rplidar.launch
$ ~/catkin_ws$ roslaunch cartographer_ros demo_revo_lds_rplidar.launch

可以看到从rplidar获取数据传输给/scan类型，给cartographer的算法，进行slam建图。另外打开一个终端，查看节点图

$ rosrun rqt_graph rqt_graph