亚博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: 1737861564nanosec: 600000000frame_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  订阅激光雷达数据的主题 /scanself.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_valueself.declare_parameter("angular",1.0)#角速度self.angular = self.get_parameter('angular').get_parameter_value().double_valueself.declare_parameter("LaserAngle",45.0)#激光雷达角度self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_valueself.declare_parameter("ResponseDist",0.55)#响应距离self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_valueself.declare_parameter("Switch",False)#开关self.Switch = self.get_parameter('Switch').get_parameter_value().bool_valueself.Right_warning = 0#存储各方向障碍物检测结果self.Left_warning = 0self.front_warning = 0self.Joy_active = False #手柄控制是否激活self.Moving = Falseself.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_valueself.angular = self.get_parameter('angular').get_parameter_value().double_valueself.linear = self.get_parameter('linear').get_parameter_value().double_valueself.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_valueself.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value#手柄回调函数def JoyStateCallback(self, msg):if not isinstance(msg, Bool): returnself.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 +cmd2os.system(cmd)#激光雷达回调函数 ：处理订阅到的雷达数据   def registerScan(self, scan_data):if not isinstance(scan_data, LaserScan): returnranges = np.array(scan_data.ranges)#数据转换self.Right_warning = 0self.Left_warning = 0self.front_warning = 0for i in range(len(ranges)):angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEGif 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 += 1if -self.LaserAngle < angle < -20:if ranges[i] < self.ResponseDist*1.5:self.Right_warning += 1if abs(angle) <= 20:if ranges[i] <= self.ResponseDist*1.5: self.front_warning += 1if self.Joy_active or self.Switch == True: #手柄或者开关打开，则停止移动if self.Moving == True:self.pub_vel.publish(Twist())self.Moving = not self.Movingreturnself.Moving = Truetwist = 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.lineartwist.angular.z = -self.angularself.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.lineartwist.angular.z = self.angularself.pub_vel.publish(twist)sleep(0.2)if self.Left_warning > 10 and self.Right_warning <= 10:twist.linear.x = self.lineartwist.angular.z = -self.angularself.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.lineartwist.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.lineartwist.angular.z = self.angularself.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.lineartwist.angular.z = self.angularself.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.lineartwist.angular.z = -self.angularself.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.lineartwist.angular.z = -self.angularself.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.lineartwist.angular.z = self.angularself.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.lineartwist.angular.z = 0.0self.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:passfinally: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 控制小车运动避障。

至此。