📋 はじめに

LiDAR点群データから作成した3次元のPoint Cloud Map(.pcd / .ply)を、ROS2の経路生成で使用する2次元のOccupancy Grid Map（占有格子地図 .pgm）に変換しする一連の方法です。
この方法はClaude Sonnet 4 を使用し、トライアンドエラーで確立できた方法を元に作成しています。

🎯 達成される成果

高解像度地図: 5cm/pixel精度の詳細な占有格子地図
大規模対応: 177m × 215m の広域カバー
ROS2完全対応: Navigation2で即座に使用可能
高品質変換: 建物、道路、植栽を精密に識別

🛠️ 環境要件

システム要件

OS: Ubuntu 22.04 LTS
ROS2: Humble Hawksbill
Python: 3.10+
メモリ: 8GB以上推奨（大規模点群処理のため）

サンプルデータ

【GLIM】Getting startedのサンプルデータで作成した.plyファイル(ならびに.plyファイルを変換した.pcdファイル)で動作確認済みです。

必要なパッケージ

# ROS2 Humble
sudo apt install ros-humble-desktop-full

# 依存パッケージ
sudo apt install python3-rosdep2 python3-pip

📦 インストール手順

1. ROS2ワークスペースの準備

bash

# メインワークスペース作成
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

# pointcloud_to_gridパッケージのクローン
git clone https://github.com/jkk-research/pointcloud_to_grid -b ros2

cd ~/ros2_ws

2. grid_map依存関係の解決

bash

# grid_mapワークスペース作成
mkdir -p ~/gridmap_ws/src
cd ~/gridmap_ws/src

# grid_mapをクローン
git clone https://github.com/ANYbotics/grid_map.git --branch humble

cd ~/gridmap_ws

# rosdepの初期化
rosdep update
rosdep install -y --ignore-src --from-paths src

# grid_mapをビルド
colcon build --symlink-install --packages-select grid_map_cmake_helpers
colcon build --symlink-install --packages-up-to grid_map_msgs grid_map_core

# 環境設定
source ~/gridmap_ws/install/setup.bash
echo "source ~/gridmap_ws/install/setup.bash" >> ~/.bashrc

3. pointcloud_to_gridのビルド

bash

cd ~/ros2_ws
colcon build --packages-select pointcloud_to_grid --symlink-install
source ~/ros2_ws/install/setup.bash

4. Pythonライブラリのインストール

bash

# NumPyの互換性問題を解決
pip install "numpy<2.0" --user --force-reinstall

# 必要なライブラリをインストール
pip install open3d numpy pillow pyyaml

🔄 変換手順

1. 点群変換スクリプトの準備

pcd_to_occupancy_grid.pyを作成：

pcd_to_occupancy_grid.py

#!/usr/bin/env python3
"""
Point Cloud (.pcd/.ply) to Occupancy Grid Map (.pgm) Converter
"""

import open3d as o3d
import numpy as np
from PIL import Image
import argparse
import os
import yaml

class PointCloudToOccupancyGrid:
    def __init__(self, resolution=0.05, height_threshold=0.3, min_height=-2.0, max_height=2.0):
        self.resolution = resolution
        self.height_threshold = height_threshold
        self.min_height = min_height
        self.max_height = max_height
        
    def load_point_cloud(self, file_path):
        """点群ファイルを読み込み"""
        print(f"点群ファイルを読み込み中: {file_path}")
        
        if not os.path.exists(file_path):
            raise FileNotFoundError(f"ファイルが見つかりません: {file_path}")
        
        pcd = o3d.io.read_point_cloud(file_path)
        
        if len(pcd.points) == 0:
            raise ValueError("点群データが空です")
        
        points = np.asarray(pcd.points)
        print(f"読み込み完了: {len(points)} 点")
        print(f"X範囲: [{points[:, 0].min():.2f}, {points[:, 0].max():.2f}]")
        print(f"Y範囲: [{points[:, 1].min():.2f}, {points[:, 1].max():.2f}]")
        print(f"Z範囲: [{points[:, 2].min():.2f}, {points[:, 2].max():.2f}]")
        
        return points
    
    def filter_by_height(self, points):
        """高さによる点群フィルタリング"""
        z_mask = (points[:, 2] >= self.min_height) & (points[:, 2] <= self.max_height)
        filtered_points = points[z_mask]
        print(f"高さフィルタリング後: {len(filtered_points)} 点")
        return filtered_points
    
    def points_to_grid(self, points):
        """点群を2Dグリッドに変換"""
        xy_points = points[:, :2]
        
        min_x, min_y = xy_points.min(axis=0)
        max_x, max_y = xy_points.max(axis=0)
        
        width = int(np.ceil((max_x - min_x) / self.resolution))
        height = int(np.ceil((max_y - min_y) / self.resolution))
        
        print(f"グリッドサイズ: {width} x {height}")
        print(f"グリッド範囲: X[{min_x:.2f}, {max_x:.2f}], Y[{min_y:.2f}, {max_y:.2f}]")
        
        grid_x = ((xy_points[:, 0] - min_x) / self.resolution).astype(int)
        grid_y = ((xy_points[:, 1] - min_y) / self.resolution).astype(int)
        
        grid_x = np.clip(grid_x, 0, width - 1)
        grid_y = np.clip(grid_y, 0, height - 1)
        
        occupancy_grid = np.full((height, width), -1, dtype=np.int8)
        
        for i in range(len(points)):
            gx, gy = grid_x[i], grid_y[i]
            z = points[i, 2]
            
            if z > self.height_threshold:
                occupancy_grid[height - 1 - gy, gx] = 100
            else:
                if occupancy_grid[height - 1 - gy, gx] != 100:
                    occupancy_grid[height - 1 - gy, gx] = 0
        
        metadata = {
            'resolution': self.resolution,
            'origin': [min_x, min_y, 0.0],
            'width': width,
            'height': height,
            'negate': 0,
            'occupied_thresh': 0.65,
            'free_thresh': 0.196
        }
        
        return occupancy_grid, metadata
    
    def save_pgm(self, occupancy_grid, metadata, pgm_path):
        """PGMファイルとYAMLファイルを保存"""
        pgm_data = np.zeros_like(occupancy_grid, dtype=np.uint8)
        
        pgm_data[occupancy_grid == -1] = 205  # 未知 -> グレー
        pgm_data[occupancy_grid == 0] = 254   # 自由 -> 白
        pgm_data[occupancy_grid == 100] = 0   # 占有 -> 黒
        
        print(f"PGMファイルを保存中: {pgm_path}")
        img = Image.fromarray(pgm_data, mode='L')
        img.save(pgm_path)
        
        yaml_path = pgm_path.replace('.pgm', '.yaml')
        print(f"YAMLファイルを保存中: {yaml_path}")
        
        yaml_data = {
            'image': os.path.basename(pgm_path),
            'resolution': metadata['resolution'],
            'origin': metadata['origin'],
            'negate': metadata['negate'],
            'occupied_thresh': metadata['occupied_thresh'],
            'free_thresh': metadata['free_thresh']
        }
        
        with open(yaml_path, 'w') as f:
            yaml.dump(yaml_data, f, default_flow_style=False)
        
        print(f"変換完了!")
        print(f"  - PGMファイル: {pgm_path}")
        print(f"  - YAMLファイル: {yaml_path}")
        
        total_cells = occupancy_grid.size
        occupied_cells = np.sum(occupancy_grid == 100)
        free_cells = np.sum(occupancy_grid == 0)
        unknown_cells = np.sum(occupancy_grid == -1)
        
        print(f"\n統計情報:")
        print(f"  - 総セル数: {total_cells}")
        print(f"  - 占有セル: {occupied_cells} ({occupied_cells/total_cells*100:.1f}%)")
        print(f"  - 自由セル: {free_cells} ({free_cells/total_cells*100:.1f}%)")
        print(f"  - 未知セル: {unknown_cells} ({unknown_cells/total_cells*100:.1f}%)")
    
    def convert(self, input_path, output_path):
        """メイン変換処理"""
        try:
            points = self.load_point_cloud(input_path)
            filtered_points = self.filter_by_height(points)
            occupancy_grid, metadata = self.points_to_grid(filtered_points)
            self.save_pgm(occupancy_grid, metadata, output_path)
        except Exception as e:
            print(f"エラーが発生しました: {e}")
            return False
        
        return True

def main():
    parser = argparse.ArgumentParser(description="Point Cloud to Occupancy Grid Map Converter")
    parser.add_argument("input", help="入力点群ファイル (.pcd または .ply)")
    parser.add_argument("output", help="出力PGMファイル")
    parser.add_argument("--resolution", type=float, default=0.05, 
                       help="グリッド解像度 [m/pixel] (デフォルト: 0.05)")
    parser.add_argument("--height_threshold", type=float, default=1.0,
                       help="占有判定の高さ閾値 [m] (デフォルト: 1.0)")
    parser.add_argument("--min_height", type=float, default=-1.0,
                       help="処理する最小高さ [m] (デフォルト: -1.0)")
    parser.add_argument("--max_height", type=float, default=3.0,
                       help="処理する最大高さ [m] (デフォルト: 3.0)")
    
    args = parser.parse_args()
    
    if not args.input.lower().endswith(('.pcd', '.ply')):
        print("エラー: 入力ファイルは .pcd または .ply である必要があります")
        return
    
    if not args.output.lower().endswith('.pgm'):
        print("エラー: 出力ファイルは .pgm である必要があります")
        return
    
    converter = PointCloudToOccupancyGrid(
        resolution=args.resolution,
        height_threshold=args.height_threshold,
        min_height=args.min_height,
        max_height=args.max_height
    )
    
    success = converter.convert(args.input, args.output)
    
    if success:
        print("\n変換が正常に完了しました!")
    else:
        print("\n変換に失敗しました。")

if __name__ == "__main__":
    main()

2. 点群の変換実行

bash

# 実行権限を付与
chmod +x pcd_to_occupancy_grid.py

# PCDファイルを変換
python3 pcd_to_occupancy_grid.py input.pcd output_map.pgm --resolution 0.05 --height_threshold 1.0

# PLYファイルを変換
python3 pcd_to_occupancy_grid.py input.ply output_map.pgm --resolution 0.05 --height_threshold 1.0

⚙️ パラメータ調整

解像度の調整

# 高解像度（2cm/pixel）
--resolution 0.02

# 標準解像度（5cm/pixel）
--resolution 0.05

# 低解像度（10cm/pixel）
--resolution 0.10

高さ閾値の調整

# 地面レベル重視
--height_threshold 0.3 --min_height -0.2 --max_height 2.0

# 一般的な屋内環境
--height_threshold 1.0 --min_height -1.0 --max_height 3.0

# 屋外大規模環境
--height_threshold 1.5 --min_height -2.0 --max_height 5.0

3. マップファイルの可視化

作成した.pgm`のマップを確認

bash

# 画像ビューアーで確認
eog ~/localization/output_map_pcd.pgm

# または、別の画像ビューアーで確認
display ~/localization/output_map_pcd.pgm  # ImageMagickが必要

4. YAMLファイルの修正

以下のfix_yaml.pyでNumPyオブジェクトをROSと互換性のある形式に変換：

fix_yaml.py

#!/usr/bin/env python3
"""
YAML ファイル修正スクリプト
"""

import yaml
import numpy as np

def fix_yaml_file(yaml_path):
    """YAMLファイルを修正"""
    print(f"YAMLファイルを修正中: {yaml_path}")
    
    with open(yaml_path, 'r') as f:
        data = yaml.unsafe_load(f)
    
    if 'origin' in data:
        origin = data['origin']
        if isinstance(origin, list):
            new_origin = []
            for item in origin:
                if isinstance(item, np.ndarray) or hasattr(item, 'item'):
                    try:
                        new_origin.append(float(item))
                    except:
                        new_origin.append(float(item.item()) if hasattr(item, 'item') else item)
                else:
                    new_origin.append(item)
            data['origin'] = new_origin
    
    backup_path = yaml_path.replace('.yaml', '_backup.yaml')
    with open(backup_path, 'w') as f:
        f.write(open(yaml_path, 'r').read())
    
    with open(yaml_path, 'w') as f:
        yaml.dump(data, f, default_flow_style=False)
    
    print("YAMLファイルの修正完了!")
    return True

if __name__ == "__main__":
    import sys
    if len(sys.argv) > 1:
        fix_yaml_file(sys.argv[1])

実行：

bash

python3 fix_yaml.py output_map.yaml

🎯 ROS2での可視化

1. シンプルなマップパブリッシャーの作成

以下のsimple_map_publisher.pyを作成します

simple_map_publisher.py

#!/usr/bin/env python3
"""
シンプルなマップパブリッシャー
PGMファイルとYAMLファイルからROSマップメッセージを作成してパブリッシュ

使用方法:
python3 simple_map_publisher.py output_map_pcd.yaml
"""

import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import Pose
import yaml
import numpy as np
from PIL import Image
import sys
import os

class SimpleMapPublisher(Node):
    def __init__(self, yaml_file):
        super().__init__('simple_map_publisher')
        
        # YAMLファイルを読み込み
        self.load_map_data(yaml_file)
        
        # パブリッシャーを作成
        self.map_publisher = self.create_publisher(OccupancyGrid, '/map', 1)
        
        # タイマーを作成（1Hzでマップをパブリッシュ）
        self.timer = self.create_timer(1.0, self.publish_map)
        
        self.get_logger().info(f'マップパブリッシャーを開始: {yaml_file}')
    
    def load_map_data(self, yaml_file):
        """YAMLファイルとPGMファイルを読み込み"""
        yaml_dir = os.path.dirname(os.path.abspath(yaml_file))
        
        # YAMLファイルを読み込み
        with open(yaml_file, 'r') as f:
            map_metadata = yaml.safe_load(f)
        
        # PGMファイルのパスを取得
        pgm_file = os.path.join(yaml_dir, map_metadata['image'])
        
        # PGMファイルを読み込み
        img = Image.open(pgm_file).convert('L')
        img_array = np.array(img)
        
        # OccupancyGridメッセージを作成
        self.occupancy_grid = OccupancyGrid()
        
        # ヘッダー設定
        self.occupancy_grid.header.frame_id = "map"
        
        # メタデータ設定
        self.occupancy_grid.info.resolution = float(map_metadata['resolution'])
        self.occupancy_grid.info.width = img_array.shape[1]
        self.occupancy_grid.info.height = img_array.shape[0]
        
        # 原点設定
        pose = Pose()
        pose.position.x = float(map_metadata['origin'][0])
        pose.position.y = float(map_metadata['origin'][1])
        pose.position.z = float(map_metadata['origin'][2])
        pose.orientation.w = 1.0
        self.occupancy_grid.info.origin = pose
        
        # 画像データをOccupancyGridデータに変換
        # PGM: 0=黒(占有), 255=白(自由), 205=グレー(未知)
        # OccupancyGrid: 0-100 (0=自由, 100=占有, -1=未知)
        occupancy_data = []
        
        # Y軸を反転して処理（画像座標系からマップ座標系へ）
        for y in range(img_array.shape[0]-1, -1, -1):
            for x in range(img_array.shape[1]):
                pixel_value = img_array[y, x]
                
                if pixel_value == 0:  # 黒 -> 占有
                    occupancy_data.append(100)
                elif pixel_value == 254:  # 白 -> 自由
                    occupancy_data.append(0)
                else:  # グレーなど -> 未知
                    occupancy_data.append(-1)
        
        self.occupancy_grid.data = occupancy_data
        
        self.get_logger().info(f'マップ読み込み完了:')
        self.get_logger().info(f'  - サイズ: {self.occupancy_grid.info.width} x {self.occupancy_grid.info.height}')
        self.get_logger().info(f'  - 解像度: {self.occupancy_grid.info.resolution} m/pixel')
        self.get_logger().info(f'  - 原点: [{pose.position.x:.2f}, {pose.position.y:.2f}]')
    
    def publish_map(self):
        """マップをパブリッシュ"""
        self.occupancy_grid.header.stamp = self.get_clock().now().to_msg()
        self.map_publisher.publish(self.occupancy_grid)

def main(args=None):
    rclpy.init(args=args)
    
    if len(sys.argv) < 2:
        print("使用方法: python3 simple_map_publisher.py <yaml_file>")
        print("例: python3 simple_map_publisher.py output_map_pcd.yaml")
        return
    
    yaml_file = sys.argv[1]
    
    if not os.path.exists(yaml_file):
        print(f"エラー: ファイルが見つかりません: {yaml_file}")
        return
    
    try:
        node = SimpleMapPublisher(yaml_file)
        print(f"マップをパブリッシュ中: {yaml_file}")
        print("RVizで /map トピックを購読して確認してください")
        print("停止するには Ctrl+C を押してください")
        
        rclpy.spin(node)
    except KeyboardInterrupt:
        print("\nマップパブリッシャーを停止中...")
    except Exception as e:
        print(f"エラーが発生しました: {e}")
    finally:
        if 'node' in locals():
            node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

2. マップの表示

bash

# ターミナル1: RViz起動
ros2 run rviz2 rviz2

# ターミナル2: 座標変換パブリッシュ
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 map base_link

# ターミナル3: マップパブリッシュ
python3 simple_map_publisher.py output_map.yaml

3. RVizでの設定

Fixed Frame: map に設定
Add Display → Map を選択
Topic: /map を選択
Color Scheme: map を選択

📊 結果例

サンプルファイルでresolution 0.05 height_threshold 1.0で処理した場合の結果です。

変換統計

点群データ: 3,711,017点 → 1,329,640点（フィルタリング後）
マップサイズ: 3552 × 4298ピクセル
カバー範囲: 177m × 215m
ファイルサイズ: 15MByte（PGM）+ 477Byte（YAML）

品質指標

占有セル: 1.1%（建物・障害物）
自由セル: 1.6%（移動可能領域）
未知セル: 97.3%（未探査領域）

今回は動作確認が目的なので品質にはこだわっていませんが、未知セルがあまりにも多いので、実際に使用するマップではパラメータを調整して占有セル+自由セル=100%に近づくようにすべきです。

🚀 Navigation2での使用

# Navigation2スタックのインストール
sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup

# 自律ナビゲーションの起動
ros2 launch nav2_bringup navigation_launch.py map:=$PWD/output_map.yaml

# ローカライゼーションのみ
ros2 launch nav2_bringup localization_launch.py map:=$PWD/output_map.yaml

🔧 トラブルシューティング

NumPy互換性エラー

pip install "numpy<2.0" --user --force-reinstall

grid_mapビルドエラー

# 依存関係を再インストール
rosdep install -y --ignore-src --from-paths src --reinstall

メモリ不足エラー

より粗い解像度を使用（--resolution 0.10）
高さ範囲を制限（--min_height/-max_height調整）
ダウンサンプリング実行

📝 ファイル構成

project/
├── pcd_to_occupancy_grid.py    # メイン変換スクリプト
├── fix_yaml.py                 # YAML修正スクリプト
├── simple_map_publisher.py     # ROS2マップパブリッシャー
├── output_map.pgm              # 占有格子地図
└── output_map.yaml             # メタデータファイル