## 1、雷达基础使用
#### 1、程序功能说明
程序运行后，驱动思岚的激光雷达，开启雷达扫描数据，在rviz中可视化激光雷达扫描的数据。
#### 2、程序代码参考路径
**我们的车是思岚C1雷达**
```
#C1雷达程序路径
~/yahboomcar_ros2_ws/software/library_ws/src/sllidar_ros2/launch/sllidar_c1_launch.py

#C1雷达可视化路径
~/yahboomcar_ros2_ws/software/library_ws/src/sllidar_ros2/launch/view_sllidar_c1_launch.py
```
#### 3、程序启动
打开终端输入
#启动c1雷达
`ros2 launch sllidar_ros2 sllidar_c1_launch.py`
#启动c1雷达+rviz可视化数据
`ros2 launch sllidar_ros2 view_sllidar_c1_launch.py`
可以通过以下命令，打印雷达扫描的数据
`ros2 topic echo /scan`

![](/media/202511/c5f6713e28994b05a94fe25bbc3a290e4528.png)

## 2、雷达避障
#### 1、程序功能说明
程序启动后，小车会往前走，当检测范围内出现障碍物的时候，会调整姿态，避开障碍物，随后继续往前走。
#### 2、程序代码参考路径
该功能源码的位置位于
`~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_laser/yahboomcar_laser/laser_Avoidance_M1.py`
#### 3、程序启动
**3.1、启动命令**
需要修改成对应的雷达型号

```
#启动小车底盘
ros2 run yahboomcar_bringup Mcnamu_driver_M1
#启动c1雷达
ros2 launch sllidar_ros2 sllidar_c1_launch.py
#启动雷达避障程序
ros2 run yahboomcar_laser laser_Avoidance_M1
```
底盘终端

![](/media/202511/770dca196a5e4589a68df2737551886a3856.png)
启动雷达驱动终端

![](/media/202511/8d10cfe5c62e4adb9809a8a73f48098c9856.png)
启动避障程序终端，log信息可以看到正前方没有障碍物，往前走

![](/media/202511/bfb52ee02fda4b94ad24b38af101ba749524.png)

**3.2、动态参数调节器**
还可以通过动态参数调节器来设置参数的大小，终端输入
`ros2 run rqt_reconfigure rqt_reconfigure`

![](/media/202511/65a362f375f544c6ba43dda16d1588e86574.png)
☑️ 修改完参数，点击GUI空白处写入参数值。注意只会生效当次启动，永久生效需要将参数修改到源码当中。

参数解析：

【linear】：避障过程中小车运动的线速度

【angular】：避障过程中小车运动的角速度

【LaserAngle】：雷达检测角度

【ResponseDist】：障碍物检测距离

【Switch】：玩法开关，可以通过这个案件开启和停止避障

以上参数都可调节，除了Switc以外，其他四个需要设置时候需要是小数，修改完，点击空白处才可以写入。

## 3、核心代码
### 3.1、laser_Avoidance_M1.py
这个程序主要有以下几个功能：

订阅激光雷达话题，雷达周围数据；

雷达数据区域划分区分避障场景；

根据相对于避障策略，发布小车前进速度距离、转弯角度。

部分核心代码如下，

```
#雷达数据接收与转换 #Radar data reception and conversion
ranges = np.array(scan_data.ranges)  # 将激光数据转为NumPy数组 Convert laser data to NumPy array
angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG  # 计算每个点的角度 Calculate the angle of each point
self.Right_warning = 0  # 右侧障碍计数器 Right obstacle counter
self.Left_warning = 0   # 左侧障碍计数器 Left obstacle counter
self.front_warning = 0  # 前方障碍计数器 Front obstacle counter
#右前有障碍 There is an obstacle in the right front
elif front>10 and left<=10 and right>10:
    print ('2, There is an obstacle to the middle right, turn left')
    angular.z = self.angular 
    # 后续检测是否左侧也出现障碍 #Follow up testing to see if there are any obstacles on the left side as well
    if left>10 and right<=10:
        angular.z = -self.angular 
#左前有障碍 There is an obstacle in the left front
elif front>10 and left>10 and right<=10:
    print ('4. There is an obstacle in the middle left, turn right')
    angular.z = -self.angular 
    if left<=10 and right>10:
        angular.z = self.angular 
#控制执行与延时 #Control execution and delay
self.pub_vel.publish(twist)  # 发布速度指令 #Issue speed command
sleep(0.2~0.5)  # 维持动作时间 #Maintain action time
```

## 4、雷达跟随
#### 4.1、程序功能说明
运行程序后，小车上的雷达扫描设定范围内的最近的一个物体，并且根据设定的跟踪距离，调试自身的速度，与该物体保持一定的距离。通过动态参数调节器可以调整雷达检测的范围和避障检测的距离等参数。
#### 4.2、程序代码参考路径
该功能源码的位置位于，
`~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_laser/yahboomcar_laser/laser_Tracker_M1.py`
#### 4.3、程序启动
**4.3.1、启动命令**
#启动小车底盘

```
ros2 run yahboomcar_bringup Mcnamu_driver_M1
```

```
#c1
ros2 launch sllidar_ros2 sllidar_c1_launch.py
#启动雷达跟随程序
ros2 run yahboomcar_laser laser_Tracker_M1
```
程序启动后，会寻找雷达扫描范围内最近的物体，并且与它保持设定的距离。缓慢移动被跟踪的物体，小车会跟踪物体移动。

![](/media/202511/9d328fdef327476aa448e861bf6d701e9271.png)
#### 4.4、核心代码
**4.4.1、laser_Tracker_M1.py**
这个程序主要有以下几个功能：

订阅激光雷达话题，雷达周围数据；

获取周围最近雷达数据预处理；

根据相跟随策略，发布小车前进速度距离、转弯角度。

部分核心代码如下：

```
#数据预处理 #Data preprocessing
if not isinstance(scan_data, LaserScan): return
ranges = np.array(scan_data.ranges)  # 转换为数组 #Convert to array
offset = 0.5  # 安全距离偏移量 #Safe distance offset
frontDistList = []  # 优先区域距离列表 #Priority Area Distance List
frontDistIDList = []  # 优先区域角度列表 Priority Area Angle List
minDistList = []  # 次要区域距离列表 Secondary Area Distance List
minDistIDList = []  # 次要区域角度列表 List of secondary area angles

#优先跟随（前方区域） #Priority Follow (Forward Area)
if abs(angle) > (180 - self.priorityAngle):
    if ranges[i] < (self.ResponseDist + offset) and ranges[i]!=0:
        frontDistList.append(ranges[i])
        frontDistIDList.append(angle)

#跟随决策逻辑 #Follow decision logic
if len(frontDistIDList) != 0:
    minDist = min(frontDistList)  # 取最小距离 #Take the minimum distance
    minDistID = frontDistIDList[frontDistList.index(minDist)]  # 对应角度 #Corresponding angle
else: 
    minDist = min(minDistList)  # 取次要区域最小距离 #Take the minimum distance from the secondary area
    minDistID = minDistIDList[minDistList.index(minDist)]
    
# 距离微调（防抖动） #Distance adjustment (anti shake)
if abs(minDist - self.ResponseDist) < 0.1:
    minDist = self.ResponseDist
# 线速度PID控制器 #Linear velocity PID controller
velocity.linear.x = -self.lin_pid.pid_compute(self.ResponseDist, minDist)
# 角速度误差计算 #Calculation of angular velocity error
error = (180 - abs(minDistID)) / 72  # 角度归一化 #Angle normalization
ang_pid_compute = self.ang_pid.pid_compute(error, 0)  # 目标角度=0（正前方） Target angle=0 (straight ahead)

# 方向决策 #Direction decision
if minDistID > 0:  # 障碍物在右侧 #The obstacle is on the right side
    velocity.angular.z = -ang_pid_compute  # 左转 #Turn left
else: 
    velocity.angular.z = ang_pid_compute  # 右转 #Turn right
```
## 5、雷达巡逻
#### 1、程序功能说明
运行程序后，在动态参数调节器设定需要巡逻的路线，然后点击开始。小车会根据设定的巡逻路线进行运动，同时小车上的雷达会扫描设定的雷达角度和设定障碍检测距离范围内是否有障碍物，有障碍则会停下且蜂鸣器会响，没有障碍物则继续巡逻。

**2、程序代码参考路径**
该功能源码的位置位于：
```
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_bringup/yahboomcar_bringup/patrol_M1.py
```
终端输入
#启动小车底盘
`ros2 run yahboomcar_bringup Mcnamu_driver_M1`
`ros2 run yahboomcar_base_node base_node_M1`
#启动c1雷达
`ros2 launch sllidar_ros2 sllidar_c1_launch.py`
#启动雷达巡逻程序
`ros2 run yahboomcar_bringup patrol_M1`
其中底盘终端

![](/media/202511/caec125307be4ac0bdd097b3dbd553788916.png)
启动odomtf转换，终端显示

![](/media/202511/305f7540f577479bbb90c73060e7ef1a8790.png)
启动雷达驱动终端

![](/media/202511/d6c96b09144f4474ba64ceee6df610c33637.png)
程序启动后，会一直打印开关现在是关闭状态等待使用参数调节器，给定巡逻路线。

![](/media/202511/fb4596338fba4c49bd1ee830e8a879309828.png)
#### 3、动态参数调节器
还可以通过动态参数调节器来设置参数的大小，终端输入
`ros2 run rqt_reconfigure rqt_reconfigure`
注：刚开始打开的时候可能没有以上节点，点击Refresh刷新后可以看到全部节点。显示的YahboomCarPatrol就是巡逻的节点。

![](/media/202511/70819ee510a6481eb5e68e949388ff764264.png)
☑️ 修改完参数，点击GUI空白处写入参数值。注意只会生效当次启动，永久生效需要将参数修改到源码当中。

参数解析：

odom_frame：里程计的坐标系名称
base_frame：基坐标系的名称
circle_adjust：巡逻路线为圆形时，该值可以作为调整圆大小的系数，详细见代码
Switch：玩法开关
Command：巡逻路线，有以下几种路线：【LengthTest】-直线巡逻、【Circle】-圆圈巡逻、【Square】-正方形巡逻。
Set_loop：重新巡逻的开发，设置后将会更具设定的路线继续循环巡逻下去
ResponseDist：障碍物检测的距离
LaserAngle：雷达检测的角度
Linear：线速度大小
Angular：角速度大小
Length：直线运动的距离
RotationTolerance：旋转误差允许的容忍值
RotationScaling：旋转的比例系数
修改完以上的参数，需要点击空白处，才能把参数传入程序中。
#### 4、开始巡逻
在rqt界面中，找到YahboomCarPatrol节点，里边的【Command】就是设定的巡逻路线这里以正方形巡逻路线为例，下边会说明有哪几种巡逻路线。在【Command】设定了路线后，点击Switch即可开始巡逻。

![](/media/202511/4bcc2dadcfec4c908ec82372ef0fd2711370.png)

这里选择了【Square】正方形为例，巡逻状态可以自行修改，先走直线后转弯90度，再走直线，再转弯90度，以此类推，直到走完的路线为一个正方形。

![](/media/202511/06ce0f1d911a49c8bd7f3b16e030ac644965.png)
如果在走的过程中，遇到障碍物，则会停下蜂鸣器响，终端打印

![](/media/202511/ff067cbf124344ec8bcc09e551a1cd575856.png)
#### 5、核心代码
**5.1、patrol_M1.py**
这个程序主要有以下几个功能：

订阅激光雷达话题，雷达周围数据；
获取周围最近雷达数据预处理；
根据相跟随策略，发布小车前进速度距离、转弯角度。
部分核心代码如下，

创建雷达、遥控数据订阅者

```
self.sub_scan = self.create_subscription(LaserScan,"/scan",self.LaserScanCallback,1)
self.sub_joy = self.create_subscription(Bool,"/JoyState",self.JoyStateCallback,1)
```
创建速度、蜂鸣器数据发布者

```
self.pub_cmdVel = self.create_publisher(Twist,"cmd_vel",5)
self.pub_Buzzer = self.create_publisher(Bool,'/Buzzer',1)
```
监听odom和base_footprint的TF变换，计算当前的XY坐标以及旋转的角度

```
def get_position(self):
    try:
        now = rclpy.time.Time()
        trans = self.tf_buffer.lookup_transform(self.odom_frame,self.base_frame,now)
        return trans
    except (LookupException, ConnectivityException, ExtrapolationException):
        self.get_logger().info('transform not ready')
        raise
        return
self.position.x = self.get_position().transform.translation.x
self.position.y = self.get_position().transform.translation.y
    
def get_odom_angle(self):
    try:
        now = rclpy.time.Time()
        rot = self.tf_buffer.lookup_transform(self.odom_frame,self.base_frame,now)   
        #print("oring_rot: ",rot.transform.rotation) 
        cacl_rot = PyKDL.Rotation.Quaternion(rot.transform.rotation.x, rot.transform.rotation.y, rot.transform.rotation.z, rot.transform.rotation.w)
        #print("cacl_rot: ",cacl_rot)
        angle_rot = cacl_rot.GetRPY()[2]
        return angle_rot
    except (LookupException, ConnectivityException, ExtrapolationException):
        self.get_logger().info('transform not ready')
        raise
        return
self.odom_angle = self.get_odom_angle()
两个重要的函数，直线advancing和旋转Spin，所有的巡逻路线不过与是直线与旋转的组合动作，

def advancing(self,target_distance):
    self.position.x = self.get_position().transform.translation.x
    self.position.y = self.get_position().transform.translation.y
    move_cmd = Twist()
    self.distance = sqrt(pow((self.position.x - self.x_start), 2) +
                         pow((self.position.y - self.y_start), 2))
    self.distance *= self.LineScaling
    print("distance: ",self.distance)
    self.error = self.distance - target_distance
    move_cmd.linear.x = self.Linear
    if abs(self.error) < self.LineTolerance : 
        print("stop")
        self.distance = 0.0
        self.pub_cmdVel.publish(Twist())
        self.x_start = self.position.x;
        self.y_start = self.position.y;
        self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
        all_new_parameters = [self.Switch]
        self.set_parameters(all_new_parameters)
        return True
.....
spin

def Spin(self,angle):
    self.target_angle = radians(angle)
    self.odom_angle = self.get_odom_angle()
    self.delta_angle = self.RotationScaling * self.normalize_angle(self.odom_angle -        self.last_angle)
    self.turn_angle += self.delta_angle
    print("turn_angle: ",self.turn_angle)
    self.error = self.target_angle - self.turn_angle
    print("error: ",self.error)
    self.last_angle = self.odom_angle
    move_cmd = Twist()
    if abs(self.error) < self.RotationTolerance or self.Switch==False :
        self.pub_cmdVel.publish(Twist())
        self.turn_angle = 0.0
        return True
    if self.Joy_active or self.warning > 10:
        if self.moving == True:
            self.pub_cmdVel.publish(Twist())
            self.moving = False
            b = UInt16()
            b.data = 1
            self.pub_Buzzer.publish(b)
            print("obstacles")
.....
有了走直线和旋转的函数了，那么则可以根据巡逻路线来排列组合，以正方形为例，

def Square(self):
    if self.index == 0:
    print("Length")
    #首先直线行走1米
    step1 = self.advancing(self.Length)
    #sleep(0.5)
    if step1 == True:
        #self.distance = 0.0
        self.index = self.index + 1; 
        self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
        all_new_parameters = [self.Switch]
        self.set_parameters(all_new_parameters)
    elif self.index == 1:
        print("Spin")
        #旋转90度
        step2 = self.Spin(90)
        #sleep(0.5)
        if step2 == True:
            self.index = self.index + 1;
            self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
            all_new_parameters = [self.Switch]
            self.set_parameters(all_new_parameters)
.....        
```

8、雷达基础使用