激光雷达(LiDAR)通过发射激光束并测量反射时间来计算距离。2D激光雷达在一个平面上旋转扫描，生成一圈距离数据，即LaserScan消息。
# sensor_msgs/msg/LaserScan
std_msgs/Header header
float32 angle_min # 起始角度 rad (-π)
float32 angle_max # 终止角度 rad (π)
float32 angle_increment # 角度增量 rad
float32 time_increment # 时间增量 s
float32 scan_time # 扫描周期 s
float32 range_min # 最小有效距离 m
float32 range_max # 最大有效距离 m
float32[] ranges # 距离数据数组
float32[] intensities # 反射强度数组#!/usr/bin/env python3
# laser_sector_analyzer.py - 激光雷达扇区分析
# ✅ Docker验证通过
import math, rclpy, numpy as np
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from std_msgs.msg import String
class LaserSectorAnalyzer(Node):
def __init__(self):
super().__init__('laser_sector_analyzer')
self.declare_parameter('num_sectors', 8) # 扇区数量
self.declare_parameter('safe_distance', 1.0) # 安全距离 m
self.declare_parameter('stop_distance', 0.5) # 停止距离 m
self.declare_parameter('front_sector_angle', 60) # 前方扇区角度 °
n_sec = self.get_parameter('num_sectors').value
self.safe_dist = self.get_parameter('safe_distance').value
self.stop_dist = self.get_parameter('stop_distance').value
self.front_angle = math.radians(self.get_parameter('front_sector_angle').value)
# 扇区标签
self.sector_names = ['前','右前','右','右后','后','左后','左','左前'][:n_sec]
# QoS
scan_qos = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST, depth=5)
self.scan_sub = self.create_subscription(
LaserScan, '/scan', self._scan_cb, scan_qos)
self.alert_pub = self.create_publisher(String, '/laser_alert', 10)
self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)
self.timer = self.create_timer(0.5, self._report_status)
self.sector_mins = [float('inf')] * n_sec
self.scan_count = 0
self.get_logger().info(f'📡 激光扇区分析器启动 - {n_sec}个扇区')
def _scan_cb(self, msg: LaserScan):
self.scan_count += 1
n_sec = len(self.sector_mins)
sector_size = len(msg.ranges) // n_sec
for i in range(n_sec):
start = i * sector_size
end = start + sector_size if i < n_sec-1 else len(msg.ranges)
sector_ranges = [r for r in msg.ranges[start:end]
if msg.range_min < r < msg.range_max]
self.sector_mins[i] = min(sector_ranges) if sector_ranges else float('inf')
# 前方障碍物检测
front_idx = 0 # 前方扇区
if self.sector_mins[front_idx] < self.stop_dist:
alert = String()
alert.data = f'🛑 前方障碍! {self.sector_mins[front_idx]:.2f}m'
self.alert_pub.publish(alert)
self.get_logger().warn(alert.data)
def _report_status(self):
if self.scan_count == 0: return
status_parts = []
for i, (name, dist) in enumerate(zip(self.sector_names, self.sector_mins)):
d = dist if dist != float('inf') else 999
emoji = '🔴' if d < self.stop_dist else '🟡' if d < self.safe_dist else '🟢'
status_parts.append(f'{emoji}{name}:{d:.1f}m')
self.get_logger().info(' '.join(status_parts))
def main(args=None):
rclpy.init(args=args); rclpy.spin(LaserSectorAnalyzer()); rclpy.shutdown()#!/usr/bin/env python3
# scan_filter_node.py - 激光扫描数据过滤器
import math, rclpy, numpy as np
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy
from sensor_msgs.msg import LaserScan
class ScanFilterNode(Node):
def __init__(self):
super().__init__('scan_filter')
self.declare_parameter('min_range', 0.3)
self.declare_parameter('max_range', 8.0)
self.declare_parameter('filter_angle_min', -2.09) # -120°
self.declare_parameter('filter_angle_max', 2.09) # +120°
self.declare_parameter('median_window', 3)
self.min_r = self.get_parameter('min_range').value
self.max_r = self.get_parameter('max_range').value
self.filt_min = self.get_parameter('filter_angle_min').value
self.filt_max = self.get_parameter('filter_angle_max').value
self.med_w = self.get_parameter('median_window').value
qos = QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT, depth=5)
self.sub = self.create_subscription(LaserScan, '/scan', self._cb, qos)
self.pub = self.create_publisher(LaserScan, '/scan_filtered', qos)
self.get_logger().info('🔍 扫描过滤器已启动')
def _cb(self, msg: LaserScan):
filtered = LaserScan()
filtered.header = msg.header
filtered.angle_min = msg.angle_min
filtered.angle_max = msg.angle_max
filtered.angle_increment = msg.angle_increment
filtered.time_increment = msg.time_increment
filtered.scan_time = msg.scan_time
filtered.range_min = msg.range_min
filtered.range_max = msg.range_max
filtered.ranges = list(msg.ranges)
filtered.intensities = list(msg.intensities) if msg.intensities else []
# 距离过滤
for i in range(len(filtered.ranges)):
r = filtered.ranges[i]
if r < self.min_r or r > self.max_r:
filtered.ranges[i] = float('inf')
# 中值滤波去噪
if self.med_w >= 3:
orig = list(filtered.ranges)
half = self.med_w // 2
for i in range(half, len(orig) - half):
window = [v for v in orig[i-half:i+half+1] if math.isfinite(v)]
if window:
filtered.ranges[i] = sorted(window)[len(window)//2]
# 角度范围过滤
for i in range(len(filtered.ranges)):
angle = msg.angle_min + i * msg.angle_increment
if angle < self.filt_min or angle > self.filt_max:
filtered.ranges[i] = float('inf')
self.pub.publish(filtered)
def main(args=None):
rclpy.init(args=args); rclpy.spin(ScanFilterNode()); rclpy.shutdown()#!/usr/bin/env python3
# obstacle_monitor.py - 基于激光的障碍物距离监测
import math, rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from std_msgs.msg import Bool
class ObstacleMonitor(Node):
def __init__(self):
super().__init__('obstacle_monitor')
self.declare_parameter('safe_distance', 0.8)
self.declare_parameter('front_angle_range', 60) # degrees
self.declare_parameter('auto_stop', True)
self.safe_d = self.get_parameter('safe_distance').value
self.front_range = math.radians(self.get_parameter('front_angle_range').value)
self.auto_stop = self.get_parameter('auto_stop').value
from rclpy.qos import QoSProfile, ReliabilityPolicy
qos = QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT, depth=5)
self.scan_sub = self.create_subscription(LaserScan, '/scan', self._scan_cb, qos)
self.cmd_sub = self.create_subscription(Twist, '/cmd_vel_raw', self._cmd_cb, 10)
self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)
self.safety_pub = self.create_publisher(Bool, '/safety_stop', 10)
self.is_safe = True
self.get_logger().info('🛡️ 障碍物监测已启动')
def _scan_cb(self, msg):
front_ranges = []
for i, r in enumerate(msg.ranges):
angle = msg.angle_min + i * msg.angle_increment
if abs(angle) <= self.front_range/2 and msg.range_min < r < msg.range_max:
front_ranges.append(r)
min_dist = min(front_ranges) if front_ranges else float('inf')
was_safe = self.is_safe
self.is_safe = min_dist > self.safe_d
if was_safe and not self.is_safe:
self.get_logger().warn(f'⚠️ 障碍物接近! {min_dist:.2f}m')
elif not was_safe and self.is_safe:
self.get_logger().info('✅ 障碍物已清除')
safety = Bool(); safety.data = not self.is_safe
self.safety_pub.publish(safety)
def _cmd_cb(self, msg):
if self.auto_stop and not self.is_safe and msg.linear.x > 0:
# 前方有障碍，阻止前进
safe_cmd = Twist()
safe_cmd.linear.x = 0.0
safe_cmd.angular.z = msg.angular.z
self.cmd_pub.publish(safe_cmd)
else:
self.cmd_pub.publish(msg)
def main(args=None):
rclpy.init(args=args); rclpy.spin(ObstacleMonitor()); rclpy.shutdown()// scan_processor.cpp - C++激光扫描处理
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include <vector><algorithm><cmath>
class ScanProcessor : public rclcpp::Node {
public:
ScanProcessor() : Node("scan_processor") {
declare_parameter("min_range", 0.3);
declare_parameter("max_range", 8.0);
declare_parameter("num_sectors", 8);
sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
"/scan", rclcpp::SensorDataQoS(),
[this](sensor_msgs::msg::LaserScan::SharedPtr msg) {
process_scan(*msg);
});
RCLCPP_INFO(get_logger(), "C++扫描处理器已启动");
}
private:
void process_scan(const sensor_msgs::msg::LaserScan& msg) {
int n_sec = get_parameter("num_sectors").as_int();
double min_r = get_parameter("min_range").as_double();
double max_r = get_parameter("max_range").as_double();
int sector_sz = static_cast<int>(msg.ranges.size()) / n_sec;
for (int s = 0; s < n_sec; s++) {
double min_dist = std::numeric_limits<double>::infinity();
for (int i = s*sector_sz; i < (s+1)*sector_sz && i < static_cast<int>(msg.ranges.size()); i++) {
double r = msg.ranges[i];
if (r > min_r && r < max_r && r < min_dist)
min_dist = r;
}
if (s == 0 && min_dist < 0.5) // 前方扇区
RCLCPP_WARN(get_logger(), "前方障碍: %.2fm", min_dist);
}
}
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr sub_;
};运行LaserSectorAnalyzer，在不同位置观察8个扇区的距离分布。
使用ObstacleMonitor实现自动停车，测试不同safe_distance的效果。
实现一个只保留左半边扫描数据的过滤器，用于左侧墙壁跟踪。
经验值：+250 XP