亚博microros小车-原生ubuntu支持系列：13 激光雷达避障
一 背景知识
小车发了了数据包含激光雷达数据，类型是sensor_msgs/msg/LaserScan
bohu@bohu-TM1701:~$ ros2 node info /YB_Car_Node
/YB_Car_Node
Subscribers:
/beep: std_msgs/msg/UInt16
/cmd_vel: geometry_msgs/msg/Twist
/servo_s1: std_msgs/msg/Int32
/servo_s2: std_msgs/msg/Int32
Publishers:
/battery: std_msgs/msg/UInt16
/imu: sensor_msgs/msg/Imu
/odom_raw: nav_msgs/msg/Odometry
/scan: sensor_msgs/msg/LaserScan
Service Servers:
Service Clients:
Action Servers:
Action Clients
激光雷达数据
sensor_msgs/msg/LaserScan 格式
scan_data 是从激光雷达（LiDAR）获取的激光扫描数据，通常使用 sensor_msgs/LaserScan 消息类型表示。该消息包含以下字段：
header:
std_msgs/Header 类型，包含消息的元数据，如时间戳和坐标系 ID。
angle_min:
float32 类型。表示扫描的最小角度，通常以弧度为单位。
angle_max:
float32 类型。表示扫描的最大角度，通常以弧度为单位。
angle_increment:
float32 类型。每个测量点之间的角度增量，即相邻点之间的角度差，通常以弧度为单位。
time_increment:
float32 类型。相邻测量点之间的时间增量，单位是秒。
scan_time:
float32 类型。完成一次扫描的总时间，单位是秒。
range_min:
float32 类型。激光雷达可检测到的最小距离，通常以米为单位。该值用于过滤无效数据。
range_max:
float32 类型。激光雷达可检测到的最大距离，通常以米为单位。该值用于过滤无效数据。
ranges:
float32[] 数组。存储每个扫描点的距离值，通常以米为单位。数组中的每个元素代表从激光雷达到障碍物的距离。
intensities:
float32[] 数组。存储每个扫描点的强度值，表示反射回来的信号强度。这个字段并不是所有激光雷达都支持或使用的。
小车的贴一下实际获取数据如下
header:
stamp:
sec: 1737861564
nanosec: 600000000
frame_id: laser_frame
angle_min: -3.1415927410125732
angle_max: 3.1415927410125732
angle_increment: 0.01745329238474369
time_increment: 0.0
scan_time: 0.0
range_min: 0.11999999731779099
range_max: 8.0
ranges:
- 1.0069999694824219
- 1.0269999504089355
- 1.0880000591278076
- 1.1549999713897705
- 1.2380000352859497
- 1.3289999961853027
- 1.50600004196167
- 0.9179999828338623
- 0.9210000038146973
- 1.4880000352859497
- 1.4830000400543213
- 1.4769999980926514
- 1.468999981880188
- 1.4670000076293945
- 1.3170000314712524
- 1.4570000171661377
- 1.4509999752044678
- 1.4459999799728394
- 1.440999984741211
- 1.4390000104904175
- 1.437000036239624
- 1.434000015258789
- 1.4329999685287476
- 1.4329999685287476
- 1.4329999685287476
- 1.4329999685287476
- 1.1959999799728394
- 1.1779999732971191
- 1.1759999990463257
- 1.1720000505447388
- 0.12200000137090683
- 0.13300000131130219
- 0.13300000131130219
- 0.0
- 0.03099999949336052
- 1.4450000524520874
- 1.4479999542236328
- 1.4520000219345093
- 0.847000002861023
- 0.8130000233650208
- 0.7940000295639038
- 0.7979999780654907
- 0.8029999732971191
- 0.8050000071525574
- 0.8050000071525574
- 0.8090000152587891
- 0.8240000009536743
- 1.5520000457763672
- 1.562999963760376
- 1.5729999542236328
- 1.593999981880188
- 1.6030000448226929
- 1.6130000352859497
- 1.6349999904632568
- 1.6349999904632568
- 1.6579999923706055
- 1.6790000200271606
- 1.690999984741211
- 1.7170000076293945
- 0.6190000176429749
- 0.6159999966621399
- 0.6330000162124634
- 0.6779999732971191
- 0.9139999747276306
- 0.890999972820282
- 0.8830000162124634
- 0.8709999918937683
- 0.8410000205039978
- 0.6729999780654907
- 0.6650000214576721
- 0.6510000228881836
- 0.6460000276565552
- 0.6389999985694885
- 0.6190000176429749
- 0.6119999885559082
- 0.6039999723434448
- 0.5929999947547913
- 0.5870000123977661
- 0.5809999704360962
- 0.5699999928474426
- 0.5649999976158142
- 0.5600000023841858
- 0.550000011920929
- 0.5460000038146973
- 0.5410000085830688
- 0.5239999890327454
- 0.5230000019073486
- '...'
intensities:
- 154.0
- 154.0
- 155.0
- 87.0
- 78.0
- 78.0
- 124.0
- 177.0
- 166.0
- 170.0
- 171.0
- 175.0
- 175.0
- 170.0
二 激光雷达避障
小车连接上代理，运行程序，小车上的雷达扫描设定范围内是否有障碍物，有障碍物则会根据障碍物的位置，自动调整速度，使其自身避开障碍物。通过动态参数调节器可以调整雷达检测的范围和避障检测的距离等参数。
src/yahboomcar_laser/yahboomcar_laser/目录下新建文件laser_Avoidance.py
代码如下：
#ros lib
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan #激光雷达消息类型
import os
import sys
#commom lib
import math
import numpy as np
import time
from time import sleep
from yahboomcar_laser.common import *
print ("improt done")
RAD2DEG = 180 / math.pi #弧度转角度
class laserAvoid(Node):
def __init__(self,name):
super().__init__(name)
#create a sub 订阅激光雷达数据的主题 /scan
self.sub_laser = self.create_subscription(LaserScan,"/scan",self.registerScan,1)
self.sub_JoyState = self.create_subscription(Bool,'/JoyState', self.JoyStateCallback,1)#订阅手柄控制状态的主题
#create a pub 发布机器人控制指令的主题
self.pub_vel = self.create_publisher(Twist,'/cmd_vel',1)
#declareparam 声明参数
self.declare_parameter("linear",0.3)#线速度
self.linear = self.get_parameter('linear').get_parameter_value().double_value
self.declare_parameter("angular",1.0)#角速度
self.angular = self.get_parameter('angular').get_parameter_value().double_value
self.declare_parameter("LaserAngle",45.0)#激光雷达角度
self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
self.declare_parameter("ResponseDist",0.55)#响应距离
self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
self.declare_parameter("Switch",False)#开关
self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
self.Right_warning = 0#存储各方向障碍物检测结果
self.Left_warning = 0
self.front_warning = 0
self.Joy_active = False #手柄控制是否激活
self.Moving = False
self.ros_ctrl = SinglePID()#前面common的PID控制器
self.timer = self.create_timer(0.01,self.on_timer)#定时器，每0.01秒调用on_timer（）
def on_timer(self):
self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
self.angular = self.get_parameter('angular').get_parameter_value().double_value
self.linear = self.get_parameter('linear').get_parameter_value().double_value
self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
#手柄回调函数
def JoyStateCallback(self, msg):
if not isinstance(msg, Bool): return
self.Joy_active = msg.data
#退出时发的指令
def exit_pro(self):
cmd1 = "ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist "
cmd2 = '''"{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"'''
cmd = cmd1 +cmd2
os.system(cmd)
#激光雷达回调函数 ：处理订阅到的雷达数据
def registerScan(self, scan_data):
if not isinstance(scan_data, LaserScan): return
ranges = np.array(scan_data.ranges)#数据转换
self.Right_warning = 0
self.Left_warning = 0
self.front_warning = 0
for i in range(len(ranges)):
angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG
if angle > 180: angle = angle - 360
#60 means that the range of radar detection is set to 120 degrees
#根据设定的雷达检测的角度和障碍物检测距离判断前、左、右是否有障碍物存在
if 20 < angle < self.LaserAngle:
if ranges[i] < self.ResponseDist*1.5:
self.Left_warning += 1
if -self.LaserAngle < angle < -20:
if ranges[i] < self.ResponseDist*1.5:
self.Right_warning += 1
if abs(angle) <= 20:
if ranges[i] <= self.ResponseDist*1.5:
self.front_warning += 1
if self.Joy_active or self.Switch == True: #手柄或者开关打开，则停止移动
if self.Moving == True:
self.pub_vel.publish(Twist())
self.Moving = not self.Moving
return
self.Moving = True
twist = Twist()
#根据检测到障碍物，发布小车的指令让小车避开障碍物
if self.front_warning > 10 and self.Left_warning > 10 and self.Right_warning > 10:
print ('1, there are obstacles in the left and right, turn right')
twist.linear.x = self.linear
twist.angular.z = -self.angular
self.pub_vel.publish(twist)
sleep(0.2)
elif self.front_warning > 10 and self.Left_warning <= 10 and self.Right_warning > 10:
print ('2, there is an obstacle in the middle right, turn left')
twist.linear.x = self.linear
twist.angular.z = self.angular
self.pub_vel.publish(twist)
sleep(0.2)
if self.Left_warning > 10 and self.Right_warning <= 10:
twist.linear.x = self.linear
twist.angular.z = -self.angular
self.pub_vel.publish(twist)
sleep(0.5)
elif self.front_warning > 10 and self.Left_warning > 10 and self.Right_warning <= 10:
print ('4. There is an obstacle in the middle left, turn right')
twist.linear.x = self.linear
twist.angular.z = -self.angular
#self.pub_vel.publish(twist)
sleep(0.2)
if self.Left_warning <= 10 and self.Right_warning > 10:
twist.linear.x = self.linear
twist.angular.z = self.angular
self.pub_vel.publish(twist)
sleep(0.5)
elif self.front_warning > 10 and self.Left_warning < 10 and self.Right_warning < 10:
print ('6, there is an obstacle in the middle, turn left')
twist.linear.x = self.linear
twist.angular.z = self.angular
self.pub_vel.publish(twist)
sleep(0.2)
elif self.front_warning < 10 and self.Left_warning > 10 and self.Right_warning > 10:
print ('7. There are obstacles on the left and right, turn right')
twist.linear.x = self.linear
twist.angular.z = -self.angular
self.pub_vel.publish(twist)
sleep(0.4)
elif self.front_warning < 10 and self.Left_warning > 10 and self.Right_warning <= 10:
print ('8, there is an obstacle on the left, turn right')
twist.linear.x = self.linear
twist.angular.z = -self.angular
self.pub_vel.publish(twist)
sleep(0.2)
elif self.front_warning < 10 and self.Left_warning <= 10 and self.Right_warning > 10:
print ('9, there is an obstacle on the right, turn left')
twist.linear.x = self.linear
twist.angular.z = self.angular
self.pub_vel.publish(twist)
sleep(0.2)
elif self.front_warning <= 10 and self.Left_warning <= 10 and self.Right_warning <= 10:
print ('10, no obstacles, go forward')
twist.linear.x = self.linear
twist.angular.z = 0.0
self.pub_vel.publish(twist)
#self.pub_vel.publish(twist)
def main():
rclpy.init()
laser_avoid = laserAvoid("laser_Avoidance")
print ("start it")
try:
rclpy.spin(laser_avoid)
except KeyboardInterrupt:
pass
finally:
laser_avoid.exit_pro()
laser_avoid.destroy_node()
rclpy.shutdown()
我加了注释，主要逻辑是收到激光雷达数据后回调处理registerScan，通过发布/cmd_vel 来控制小车避障。
小车运行：
ros2 run yahboomcar_laser laser_Avoidance
6, there is an obstacle in the middle, turn left
6, there is an obstacle in the middle, turn left
6, there is an obstacle in the middle, turn left
6, there is an obstacle in the middle, turn left
6, there is an obstacle in the middle, turn left
2, there is an obstacle in the middle right, turn left
9, there is an obstacle on the right, turn left
10, no obstacles, go forward
10, no obstacles, go forward
10, no obstacles, go forward
10, no obstacles, go forward
10, no obstacles, go forward
控制参数调整
ros2 run rqt_reconfigure rqt_reconfigure
以上的参数说明如下：
-
linera：线速度大小
-
angular：角速度大小
-
LaserAngle：雷达检测角度
-
ResponseDist：障碍物检测距离，当检测的物体在该范围内，则认为是障碍物
-
Switch：玩法开关
我测试了下，大一些障碍物能识别到，垃圾桶、鞋盒子这种。椅子腿细一些的不太行会撞上去。
太矮的也不行。能从房间自主识别到门口出去。这算是小车第一次不是根据键盘控制自主运行，虽然没一会就回遇到障碍物困住了。避障的逻辑处理的相对简单些。
从节点图来看，小车的激光雷达发布的数据被程序节点laser_Avoidance接受，通过/cmd_vel 控制小车运动避障。
至此。