如何通过dynamic reconfigure动态调整lidar的可视检测角度范围

搭建仿真机器人过程中，需要实现搭载的LiDAR仅检测前方指定角度范围内的物体，计划通过dynamic reconfigure动态控制LiDAR目标检测角度窗口，目前已完成dynamic reconfigure的基础配置，配置界面如下：

当前实现方式为在LiDAR回调函数中直接修改接收到的LaserScan数据的angle_max、angle_min参数，核心代码如下：

# Initialize dynamic reconfigure
self.enable = 0
self.speed = 0.0
self.window=60
self.yaw_rate=0

# Define the lidar subscriber
self.sub_lidar = rospy.Subscriber('scan', LaserScan, 
                                  self.lidar_callback, queue_size=1)
################################
# Dynamic Reconfigure callback
################################
def dyn_reconfig_callback(self, config, level):
    self.enable = config['enable']
    self.speed = config['speed']
    self.window= config['window']
    self.dyn_config = config
    return config

#########################
# Lidar callback
#########################
def lidar_callback(self, data):
    data.angle_max=(self.window*np.pi/180)/2
    data.angle_min=(self.window*np.pi/180)/-2
    min_idx=None
    min_dist=data.range_max
    for idx in range(len(data.ranges)):
        if(data.ranges[idx]>data.range_min and data.ranges[idx]<min_dist):
            min_idx=idx
            min_dist=data.ranges[idx]

    if(min_idx is not None):
        rospy.loginfo('closest object is at %.2f deg and dist = %.3f '%(min_idx*data.angle_increment*180/np.pi, min_dist))
    
        msg=Twist()
        msg.angular.z=min_idx*data.angle_increment
        self.pub_twist.publish(msg)
        if(min_dist<.30):
            self.enable=False

该方案无法达到预期的角度窗口过滤效果。

LaserScan消息中的angle_min、angle_max、angle_increment是雷达驱动发布数据时自带的标定元数据，和ranges数组的长度、每个数组索引对应的实际物理角度是严格绑定的。直接修改这两个参数不会自动过滤ranges数组中窗口外的点，后续遍历全量ranges数组计算最近点时，依然会把窗口外的点纳入计算，同时修改元数据后，索引和角度的映射关系会错乱，最终计算出的目标角度、距离结果都会出错。

• 保留原始LaserScan消息的所有元数据，不要直接修改angle_min、angle_max、angle_increment字段，这些值是计算每个点真实角度的唯一基准。
• 根据dynamic reconfigure传入的窗口大小，换算成弧度制的角度上下限，正前方为0度时，检测窗口范围为[-window/2 * π/180, window/2 * π/180]。
• 遍历ranges数组时，先通过原始元数据计算当前索引点对应的真实角度，仅保留角度落在检测窗口内的点参与最近点距离计算。
• 计算转向控制量时，使用原始角度增量换算目标点的真实角度，避免映射错误。

修正后的LiDAR回调代码如下：

def lidar_callback(self, data):
    # 计算当前配置的检测窗口半角（弧度）
    half_window_rad = (self.window * np.pi / 180) / 2
    angle_low = -half_window_rad
    angle_high = half_window_rad

    min_idx = None
    min_dist = data.range_max

    for idx in range(len(data.ranges)):
        # 计算当前雷达点对应的真实物理角度
        current_angle = data.angle_min + idx * data.angle_increment
        # 仅处理检测窗口内、距离合法的点
        if angle_low <= current_angle <= angle_high:
            if data.range_min < data.ranges[idx] < min_dist:
                min_idx = idx
                min_dist = data.ranges[idx]

    if min_idx is not None:
        target_angle = data.angle_min + min_idx * data.angle_increment
        rospy.loginfo('closest object is at %.2f deg and dist = %.3f '%(target_angle * 180/np.pi, min_dist))

        msg = Twist()
        msg.angular.z = target_angle
        self.pub_twist.publish(msg)
        if min_dist < 0.3:
            self.enable = False

补充说明：如果雷达存在安装角度偏移，0度不对齐正前方，计算current_angle时叠加对应的偏移量即可。如果需要将裁剪后的LaserScan数据发布给其他节点使用，除了过滤ranges数组外，还需要同步更新angle_min、angle_max字段，保证修改后的角度范围和裁剪后的数组长度匹配，避免其他节点解析数据时出现坐标计算错误。

内容的提问来源于stack exchange，提问作者Ben Freeman