雷达开启这是一个回调函数收到雷达开启的请求后返回响应。首先打印日志雷达启动成功。self.get_logger().info(\033[1;32m%s\033[0m % lidar enter)调用reset_value()函数重置参数self.reset_value()设置通讯规则后续创建订阅的时候会用到这个规则----------后续出一期这方面的学习内容qos QoSProfile(depth1, reliabilityQoSReliabilityPolicy.BEST_EFFORT)开始订阅雷达数据订阅只能收不能发里面传参为数据类型、话题名称、收到数据后交给谁处理、使用上述的通讯规则self.lidar_sub self.create_subscription (LaserScan, /scan_raw, self.lidar_callback, qos) # 订阅雷达数据(subscribe to Lidar data)初始化舵机位姿利用元组set_servo_position(self.joints_pub, 1, ((10, 300), (5, 500), (4, 210), (3, 40), (2, 665), (1, 500)))最后因为我们是服务端我们要告诉客户端我们执行成功response.success True response.message enter return response雷达退出有了上面的雷达开启经验这里雷达退出我们就能很快读懂了唯一的区别上面是订阅现在是发布因为我们需要发布停机指令才会停止工作。def exit_srv_callback(self, request, response): self.get_logger().info(\033[1;32m%s\033[0m % lidar exit) self.reset_value() //发布一个空的 Twist 让小车立刻停止不动Twist() 是空的速度指令 self.mecanum_pub.publish(Twist()) response.success True response.message exit return response结论订阅只能收数据发布只能发数据。然后服务就是进入时触发订阅离开时触发发布。雷达模式选择这里给出4种模式选择0关闭1避障2跟随3警卫(0 close, 1 obstacle avoidance, 2 Lidar following, 3 Lidar guarding)。这里也就有两个默认逻辑第一我输入的数字超出0、1、2、3这4个数字执行器是不执行任何动作的同时我输入0-3数字要返回给客户端设置成功的回应同时我们在切换雷达工作模式时应该停止车辆避免发生危险类似于一键关机。逻辑捋通了我们现在就来看代码首先打印客户端发来的请求内容并把客户端传来的数字取出来同时打印当前的模式self.get_logger().info(str(request)) new_running_mode request.data self.get_logger().info(\033[1;32m%s\033[0m % (set_running str(new_running_mode)))其次及就是判断输出进来数是否满足0-3范围满足就要修好当前模式了if not 0 new_running_mode 3: response.success False response.message Invalid running mode {}.format(new_running_mode) else: response.success True response.message set_running # 线程安全地修改当前模式 with self.lock: self.running_mode new_running_mode无论切换什么模式都先让小车停止安全逻辑self.mecanum_pub.publish(Twist()) return response设置参数雷达扫描角度、避障距离阈值、速度从服务请求里获取拿到三个参数客户端发送的new_parameters request.data new_threshold, new_scan_angle, new_speed new_parameters self.get_logger().info(\033[1;32mn_t:{:2f}, n_a:{:2f}, n_s:{:2f}\033[0m.format(new_threshold, new_scan_angle, new_speed))检查三个变量的阈值如果满足就保存到节点内部变量if not 0.3 new_threshold 1.5: response.success False response.message New threshold ({:.2f}) is out of range (0.3 ~ 1.5).format(new_threshold) return response if not 0 new_scan_angle 90: response.success False response.message New scan angle ({:.2f}) is out of range (0 ~ 90) return response if not new_speed 0: response.success False response.message Invalid speed return response //只要修改共享变量必须加锁 with self.lock: self.threshold new_threshold self.scan_angle math.radians(new_scan_angle) self.speed new_speed self.speed self.speed return response雷达工作整体的逻辑框架def lidar_callback(self, lidar_data): 1. 初始化速度指令 twist 2. 解析雷达原始数据 → 分成【左边距离】【右边距离】 3. 加锁读取当前参数scan_angle, threshold, speed 4. 根据 running_mode 执行不同逻辑 - 模式1避障碰到障碍就转弯 - 模式2跟随跟着最近的物体走 - 模式3警戒发现东西就转向 5. 发布速度 twist → 让小车动然后我们一步一步捋一下雷达每发一帧数据就自动跑一次整体的框架逻辑所以我们第一步要创建一个空白的速度指令包防止上一次的速度残留保证控制安全。逻辑要发控制指令 先创建空消息 避免历史残留 安全稳定//Twist() 是 ROS2 里专门用来控制小车运动的标准消息类型。 twist Twist()之后我们就要处理雷达数据了。这里引入激光雷达避障经典的处理逻辑雷达视野太大 → 先切成左右 → 再切小只看中间关键区域。我们这里应用A1雷达//查看前方MAX_SCAN_ANGLE / 2.0度的范围里有多少个雷达数据点 if self.lidar_type ! G4: //最大扫描角度/2----转成弧度制-----÷每扫描一次增加的角度雷达每两个点之间的角度间隔 max_index int(math.radians(MAX_SCAN_ANGLE / 2.0) / lidar_data.angle_increment)取左边数据left_ranges lidar_data.ranges[:max_index] # 左半边数据 (the left data)取右边数据//[::-1] 把雷达数组倒过来倒过来再取前 N 个 拿到右边区域的数据 right_ranges lidar_data.ranges[::-1][:max_index]到这里我们就把雷达扫描的角度转化为雷达点并且分为左右两个范围了接下来我们就要细分我们从客户端获取到的角度了处理客户端发来的消息需要加线程锁。与上面一样的道理我们自己的扫描角度÷2只要每边取一半合起来就是完整的前方视野。angle self.scan_angle / 2 //获取雷达点数 angle_index int(angle / lidar_data.angle_increment 0.50) left_range, right_range np.array(left_ranges[:angle_index]), np.array(right_ranges[:angle_index])最后就是我们整个雷达避障 / 跟随 / 警戒的控制逻辑因为我们是麦轮底盘车辆所以我们先看麦轮车辆的雷达控制代码模式1避障//现在是【避障模式】并且【冷却时间已经结束】才能执行避障动作 //后续解释为什么只有避障才会加冷却时间等待 if self.running_mode 1 and self.timestamp time.time(): //把雷达数据里的错误值、无效值全部扔掉只留下真实可靠的障碍距离 //把距离 0 的点扔掉雷达有时候会返回 0这是无效数据不是真的距离为 0 left_nonzero left_range.nonzero() right_nonzero right_range.nonzero() //去掉无穷大、错误值inf /nan雷达测不到东西时会返回 nan 或 inf这些不能用来计算 left_nonan np.isfinite(left_range[left_nonzero]) right_nonan np.isfinite(right_range[right_nonzero]) //只留下不是 0 不是错误值的有效数据 min_dist_left_ left_range[left_nonzero][left_nonan] min_dist_right_ right_range[right_nonzero][right_nonan] # 取左右最近的距离(Take the nearest distance left and right) //如果左右两边的数据超过一个就选择最近的点。因为避障就是看最近的障碍 if len(min_dist_left_) 1 and len(min_dist_right_) 1: min_dist_left min_dist_left_.min() min_dist_right min_dist_right_.min() //左侧有障碍 if min_dist_left self.threshold and min_dist_right self.threshold: //转弯时向前速度尽量小 twist.linear.x self.speed / 6 //转90度(turn 90 degree) max_angle math.radians(90) //转弯速度尽量大 w self.speed * 6.0 //向右转弯;这里是ROS2固定规则angular.z 正数 → 左逆时针转 angular.z 负数 → 顺时针转 twist.angular.z -w //如果上一次动作不是直行、也不是 “左障右转” if self.last_act ! 0 and self.last_act ! 1: //反转状态 twist.angular.z w //记录现在的动作 self.last_act 1 //把速度指令发给小车 self.mecanum_pub.publish(twist) //设置冷却时间这段时间内不能再乱转 self.timestamp time.time() (max_angle / w / 2) //两侧都有障碍 elif min_dist_left self.threshold and min_dist_right self.threshold: //转弯时向前速度尽量小 twist.linear.x self.speed / 6 //转弯速度尽量大 w self.speed * 6.0 // 原地转圈掉头 twist.angular.z w //记录当前的状态 self.last_act 3 //把速度指令发给小车 self.mecanum_pub.publish(twist) //设置冷却时间 转180度 self.timestamp time.time() (math.radians(180) / w / 2) //右侧有障碍 elif min_dist_left self.threshold and min_dist_right self.threshold: twist.linear.x self.speed / 6 max_angle math.radians(90) w self.speed * 6.0 twist.angular.z w if self.last_act ! 0 and self.last_act ! 2: twist.angular.z -w self.last_act 2 self.mecanum_pub.publish(twist) self.timestamp time.time() (max_angle / w / 2) //没有障碍 else: self.last_act 0 twist.linear.x self.speed # 直行(go straight) self.mecanum_pub.publish(twist)模式2跟随模式//雷达找到最近的物体 → 小车自动跟着它走靠近了后退、远离了前进、偏左左转、偏右右转 elif self.running_mode 2: //拼合距离数据, 从右半侧逆时针到左半侧 ranges np.append(right_range[::-1], left_range) //打印雷达扫描到的所有距离数据 self.get_logger().info(str(ranges)) //去掉 0、错误值、无穷大只留真实距离和避障模式里的作用完全一样。 nonzero ranges.nonzero() nonan np.isfinite(ranges[nonzero]) dist_ ranges[nonzero][nonan] //有有效的雷达数据才继续执行 if len(dist_) 0: //找到最近的“物体”跟随模式就是跟着最近的东西走 dist dist_.min() //找到这个最近物体在雷达数据里是第几个点 min_index list(ranges).index(dist) //计算最小值对应的角度 angle -angle lidar_data.angle_increment * min_index # self.get_logger().info(str(dist_)) # self.get_logger().info(str([dist, angle])) //只有满足以下两个条件才开始跟随因为是自动跟随所以当角度出现偏差的时候就要控制左右了 //控制左右 if dist self.threshold and abs(math.degrees(angle)) 5: if self.lidar_type ! G4: //平滑转向这里用到了PID控制后续讲解 self.pid_yaw.update(-angle) twist.angular.z common.set_range(self.pid_yaw.output, -self.speed * 6.0, self.speed * 6.0) elif self.lidar_type G4: self.pid_yaw.update(angle) twist.angular.z -common.set_range(self.pid_yaw.output, -self.speed * 6.0, self.speed * 6.0) else: self.pid_yaw.clear() //控制前后距离近 偏差超过2厘米防抖操作因为雷达会有微小噪声 → 前进/后退 if dist self.threshold and abs(0.2 - dist) 0.02: # 控制前后(control the front and back) self.pid_dist.update(self.threshold / 2 - dist) twist.linear.x common.set_range(self.pid_dist.output, -self.speed, self.speed) else: self.pid_dist.clear() //防抖死区优化防止小车轻微抖动 if abs(twist.angular.z) 0.008: //转向角速度极小直接设为0 twist.angular.z 0.0 if abs(twist.linear.x) 0.05: //前后速度极小直接设为0 twist.linear.x 0.0 //发布小车控制指定 self.mecanum_pub.publish(twist)模式3警卫模式就是模式2的左右转向elif self.running_mode 3: //雷达数据拼接与模式2一样 ranges np.append(right_range[::-1], left_range) //数据选择 nonzero ranges.nonzero() nonan np.isfinite(ranges[nonzero]) dist_ ranges[nonzero][nonan] //当有效雷达点的数据大于1时动作防止单点噪声误触发 if len(dist_) 1: dist dist_.min() //找到最近障碍物的距离 位置索引 min_index list(ranges).index(dist) angle -angle lidar_data.angle_increment * min_index # 计算最小值对应的角度 //与模式2的逻辑一致 if dist self.threshold and abs(math.degrees(angle)) 5: # 控制左右 if self.lidar_type ! G4: self.pid_yaw.update(-angle) twist.angular.z common.set_range(self.pid_yaw.output, -self.speed * 6.0, self.speed * 6.0) elif self.lidar_type G4: self.pid_yaw.update(angle) twist.angular.z -common.set_range(self.pid_yaw.output, -self.speed * 6.0, self.speed * 6.0) else: self.pid_yaw.clear() if abs(twist.angular.z) 0.008: twist.angular.z 0.0 self.mecanum_pub.publish(twist)