层级 | 系统 | 硬件 | 功能 |
---|---|---|---|
边缘层 | linux系统 ROS 16.04 kinetic | 树莓派3B+ | 接收传感器数据(2D激光雷达),传输控制命令到执行层 |
执行层 | keil5编译环境 | stm32F103RCT8 | 负责接收控制命令,驱动级的控制车轮 |
软件上主要是介绍雷达在树莓派3B+上的使用 ,键盘输入发布速度控制信息到串口,树莓派与stm32之间的串口连接,stm32的串口接收速度控制信息,本地ROS与树莓派ROS之间的通信,cartographer的使用等内容。
首先,这部分内容要实现的是用树莓派驱动激光雷达的旋转,获取雷达的数据,是在 2D激光雷达 的基础上实现的,其接口是USB线。
$ mkdir /slam2D_ws/src
$ cd slam2D_ws
https://github.com/robopeak/rplidar_ros
$ catkin_make
然后,在basherc文件中添加路径
$ sudo gedit ~/.bashrc
最后一行,添加:({机器名}自行修改,不要忘记了)
source /home/{机器名}/slam2D_ws/devel/setup.bash
最后,source一下。或者关闭终端重新打开终端。
$ source ~/.bashrc
$ ls -l / dev | grep ttyUSB
将显示一行…ttyUSB0(一般是)的结果,为串口添加写权限:
$ sudo chmod 666 /dev/ttyUSB0
$ roslaunch rplidar_ros view_rplidar.launch
但是树莓派运行rviz可能会占用很多内存,故还有下一种运行方式(常用):
$ roslaunch rplidar_ros rplidar.launch
可以利用简单的显示功能rplidarNodeClient接收/scan节点的数据,并显示出来,运行命令如下。
$ rosrun rplidar_ros rplidarNodeClient
当然,你也可以用其他的SLAM建图算法处理/scan节点的雷达数据,本文用百度开源的cartographer建图算法,在后文列出。
刚安装上系统的树莓派,需要重启一下网络就能连接wifi。
sudo service network-manager restart
http://ukonline2000.com/?p=880
但是本教程使用的是Ubuntu mate 16.04系统,需要安装下面的方法:
使用命令看一下树莓派3b+支持的GPIO串口
$ ls -la /dev/
这是最后需要配置完成的界面(即serial0->ttyS0),刚查看时可不是这个界面,大家注意。
$ sudo raspi-config
打开系统配置界面如下图,选择Inerfacing Options
然后选择 serial
先选择No,在选择Yes,最后选择保存,Esc退出即可。
$ 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
然后,再次重启即可。
$ 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”
然后,重启。以后就不用了每次都给权限,直接用就可以了。
#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()
$ hcitool scan
此处使用pygame来进行键盘按键的输入操作,同时对数据进行打包,完成数据传输。pygame的好处是不会像标准键盘输入input()需等待,pygame不用一直等待键盘输入,让其他程序可以一直运行,不影响其他程序。键盘有输入时pygame才作用一次,然后修改一些值等,继续让其他程序跑下去。
$ 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
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()
本地采用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;
}
本部分内容是在同一个局域网同时运行上位机的本地ROS(一台较高算力的电脑)和树莓派ROS,在本地ROS运行roscore。树莓派ROS搭载传感器获取数据(从机),而本地ROS(即主机)处理传感器数据,进行SLAM建图。
设置主机为PC,运行roscore,从机为树莓派。
PC和树莓派在同一局域网下;也就是都连同一个WIFI(路由器生成的那种,或者手机热点,校园网不行),或者PC连了WIFI、用根网线把树莓派和PC连起来也可以。用ifconfig可以查看IP,用hostname可以查看主机名】假设Master在PC端运行。
$ sudo gedit /etc/hosts
在其中添加两行,一行主机(nuvo-7000)的IP和名字,一行从机(car2-dek)的IP和名字。
192.168.31.112 nuvo-7000
192.168.31.227 car2-dek
PC和树莓派都需要修改。
ping nuvo-7000
ping car2-dek
可以直接ping 网络地址。
成功的图示如下,出现64 bytes from …等字样。
$ sudo gedit ~/.bashrc
在PC端和树莓派的.bashrc都文件中都加1行:
export ROS_MASTER_URI=‘http://192.168.31.113:11311’
IP地址是主机的,即PC的
然后,保存退出,
$ source ~/.bashrc
首先是cartographer的官方安装教程,但是无法google上网,就不能下载。
https://google-cartographer-ros.readthedocs.io/en/latest/
以下是github安装方式:
$ sudo apt-get install -y python-wstool python-rosdep ninja-build
$ 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
~$ '/home/yhexie/catkin_ws/src/cartographer/scripts/install_proto3.sh'
$ 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
~/catkin_ws$ catkin_make_isolated --install --use-ninja
$ sudo gedit ~/.bashrc
$ source ~/.bashrc
最后一行添加,注意是install_isolated
source ~/catkin_ws/install_isolated/setup.bash
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
$ roslaunch cartographer_ros offline_backpack_3d.launch bag_filenames:='/media/carto/Data/Datasets/b3-2016-03-01-13-39-41.bag'
跑出来的需要较长时间,需要花大概十几分钟。
-- 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",
<!--
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用的。
~$ cd ~/catkin_ws
~/catkin_ws$ catkin_make_isolated --install --use-ninja
$ roslaunch rplidar_ros rplidar.launch
$ ~/catkin_ws$ roslaunch cartographer_ros demo_revo_lds_rplidar.launch
可以看到从rplidar获取数据传输给/scan类型,给cartographer的算法,进行slam建图。另外打开一个终端,查看节点图
$ rosrun rqt_graph rqt_graph