https://a1026302.hatenablog.com/entry/2025/05/10/095453

センサーから、前後左右の障害物の距離を表示する方法をここにメモします。

センサーから出力される情報を参照する

デバック方法

以下の方法で range_min, range_max, angle_min, angle_increment を含めたメッセージ全体が確認できます。

ros2 topic echo /ldlidar_node/scan --no-arr

サンプル

以下が、LaserScan メッセージのメタ情報です。

root@c6c867d3de36:/ros2_ws# ros2 topic echo /ldlidar_node/scan --no-arr
header:
  stamp:
    sec: 1746804905
    nanosec: 683225316
  frame_id: ldlidar_link
angle_min: 0.0
angle_max: 6.2831854820251465
angle_increment: 0.013839615508913994
time_increment: 0.00036608963273465633
scan_time: 0.16620469093322754
range_min: 0.029999999329447746
range_max: 15.0
ranges: '<sequence type: float, length: 455>'
intensities: '<sequence type: float, length: 455>'

データ構造

LiDAR のデータは以下のような構成になっています。

angle_min: 0.0
angle_max: 約 2π（6.283...）
angle_increment: 約 0.01384 [rad]

1 スキャンあたり 360度（全周囲）のデータを 455 点取得

計算

前方のインデックスを計算

0ラジアン（angle_min）が前方（X軸正方向）だと仮定した場合、前方 = index 0 です。
しかし、多くのロボット用センサ設定では、前方はスキャンの中央に来るようになっています。

中央（前方）の角度を求める

総スキャン数: 455
中央 index: 455 // 2 = 227
該当角度: angle_min + index * angle_increment [≒ 0 + 227 * 0.01384 ≒ 3.14 rad（≒ 180°）]

このセンサの前方はスキャンの中央（index 227）に該当しています。

サンプルコード

前方距離を取得

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
import math


class ObstacleDetector(Node):
    def __init__(self):
        super().__init__('obstacle_detector')
        self.subscription = self.create_subscription(
            LaserScan,
            '/ldlidar_node/scan',
            self.scan_callback,
            10
        )
        self.get_logger().info("障害物検出ノードを起動しました。")

    def scan_callback(self, msg: LaserScan):
        ranges = msg.ranges
        center_index = len(ranges) // 2
        window = 5  # 前方 ±5点 を対象にする

        near_ranges = ranges[center_index - window:center_index + window + 1]
        valid_ranges = [
            r for r in near_ranges if not math.isnan(r) and r > 0.0
        ]

        if not valid_ranges:
            self.get_logger().info("前方測定不可")
            return

        min_distance = min(valid_ranges)

        if min_distance < 0.5:
            self.get_logger().warn(f"前方に障害物があります！（最短: {min_distance:.2f}m）")
        else:
            self.get_logger().info(f"前方クリア（{min_distance:.2f}m）")


def main(args=None):
    rclpy.init(args=args)
    node = ObstacleDetector()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

前後左右を測定

LiDARのスキャンデータは、通常、範囲情報（ranges）が一定の角度間隔で並んでいます。前後左右の測定値を表示するためには、ranges配列の中で対応する角度に対応する値を抽出する必要があります。
例えば、LiDARのスキャン範囲が0から2π（360度）までの範囲だと仮定して、前方、後方、左右を抽出する方法は以下の通りです。

前方: おおよそ0度（0ラジアン）付近に対応する値
後方: おおよそπ（3.14159ラジアン）付近に対応する値
左側: おおよそπ/2（1.5708ラジアン）付近に対応する値
右側: おおよそ3π/2（4.7124ラジアン）付近に対応する値

前提条件

angle_min = 0.0
angle_max = 2π (6.28319)
angle_increment = 0.0138
ranges の長さ = 455 (例えば、スキャンデータが360度を455個のデータ点に分割されている場合)

角度からインデックスへの変換

index = (angle / (angle_max - angle_min)) * len(ranges)

サンプルコード

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
import math


class ObstacleLoggerNode(Node):
    def __init__(self):
        super().__init__('obstacle_logger_node')

        # /ldlidar_node/scan トピックのサブスクライブ
        self.subscription = self.create_subscription(
            LaserScan,
            '/ldlidar_node/scan',  # LiDARのスキャンデータ
            self.scan_callback,
            10
        )
        self.get_logger().info('Obstacle Logger Node has been started.')

    def scan_callback(self, msg: LaserScan):
        # LiDARスキャンデータから必要なパラメータを取得
        angle_min = msg.angle_min
        angle_max = msg.angle_max
        angle_increment = msg.angle_increment
        ranges = msg.ranges

        # スキャンデータの総サンプル数
        num_samples = len(ranges)

        # 角度に対応するインデックスを計算
        front_index = int((0.0 - angle_min) / angle_increment)  # 0度（前方）
        back_index = int((math.pi - angle_min) / angle_increment)  # 180度（後方）
        left_index = int((math.pi / 2 - angle_min) / angle_increment)  # 90度（左）
        right_index = int((3 * math.pi / 2 - angle_min) / angle_increment)  # 270度（右）

        # 各方向の距離を取得（インデックスが範囲外でないことを確認）
        front_distance = ranges[front_index] if 0 <= front_index < num_samples else None
        back_distance = ranges[back_index] if 0 <= back_index < num_samples else None
        left_distance = ranges[left_index] if 0 <= left_index < num_samples else None
        right_distance = ranges[right_index] if 0 <= right_index < num_samples else None

        # 障害物が前方にある場合にメッセージを表示
        if front_distance is not None and front_distance < 1.0:  # 1.0m以内で障害物を検出
            self.get_logger().info(f"障害物検出！前方距離: {front_distance:.2f}m")

        # 他の方向の障害物距離も表示
        self.get_logger().info(f"前方距離: {front_distance:.2f}m")
        self.get_logger().info(f"後方距離: {back_distance:.2f}m")
        self.get_logger().info(f"左側距離: {left_distance:.2f}m")
        self.get_logger().info(f"右側距離: {right_distance:.2f}m")


def main(args=None):
    rclpy.init(args=args)
    node = ObstacleDetector()
    try:
        rclpy.spin(node)
    except:
        pass
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

scan_callbackメソッド:

LiDARのスキャンデータ (LaserScanメッセージ) を受信し、ranges配列を使用して前後左右の距離を計算します。
front_index、back_index、left_index、right_index を計算し、それに対応する距離データを取得します。