🐥

CARLA0.10.0 で LIO-SAM してみる

2025/03/07に公開

はじめに

今回は、CARLA0.10.0を使用してLIO-SAMをしてみました。
備忘録がてら行ったことを書こうと思います。

CARLAとは

CARLAはオープンソースの自動運転研究用のシミュレータになります。
天候や時間帯などを自由に選択できたり、車両や歩行者などを自由に配置できるため、手軽に様々な環境を再現できます。

さて、そんなCARLAですが、 2024年12月に新しいバージョンである0.10.0がリリースされました。
この更新によってUnrial Engineのバージョンが4.6から5.5に上昇し、グラフィックが細かくなり、より現実に近いものになりました。
また、CARLAサーバー自体が直接ROS2と通信できるようになり、従来必要だった中間ブリッジが不要になりました。

そこで今回は、綺麗になったグラフィックと、ネイティブROS2通信を利用してLIO-SAMしてみました。

環境

今回、サーバとクライアントは同一端末で動かしています。
OS: ubuntu22.04
メモリ: 94GB
GPU: NVIDIA GeForce RTX 4090
CPU: 12th Gen Intel(R) Core(TM) i7-12700
CALRAバージョン: 0.10.0

環境構築

CARLAをインストールし、仮想環境上でPythonAPIを使用できるようにします。

詳しくは別記事で書いたのでそちらを見てください。

また、Rviz2という可視化ツールをDocker経由で起動するため、Dockerをインストールします。

Dockerのインストールの方法はこちら
sudo apt-get update
sudo apt-get install ca-certificates curl gnupg
sudo install -m 0755 -d /etc/apt/keyrings
curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo gpg --dearmor -o /etc/apt/keyrings/docker.gpg
sudo chmod a+r /etc/apt/keyrings/docker.gpg
echo \
  "deb [arch="$(dpkg --print-architecture)" signed-by=/etc/apt/keyrings/docker.gpg] https://download.docker.com/linux/ubuntu \
  "$(. /etc/os-release && echo "$VERSION_CODENAME")" stable" | \
  sudo tee /etc/apt/sources.list.d/docker.list > /dev/null

sudo apt-get update
sudo apt-get install docker-ce docker-ce-cli containerd.io docker-buildx-plugin docker-compose-plugin
sudo usermod -aG docker $USER
sudo reboot
docker run hello-world

ROS2ネイティブインターフェースを使ってみる

最初に、CARLAに標準実装されているコードを実行して、
ROS2のブリッジを介さずに、直接センサーデータの送信や制御コマンドの受信を行ってみます。

  1. ROS2統合機能を有効にしてCARLAを起動します。

    ./CarlaUnreal.sh --ros2 -RenderOffScreen
    

    このオプションにより、シミュレーターはROS2のネイティブインターフェースを有効化し、トピックの通信が可能になります。

  2. 別のターミナルを開き、JSONファイルからセンサー情報を読み込み、車両にセンサーを追加してスポーンします。

    conda activate carla
    cd PythonAPI/examples/ros2
    python3 ros2_native.py -f stack.json
    

    ros2_native.pyファイル内のenable_for_ros()でROS2データ送信ができるよう設定しています。

    (condaの環境作成についてはここで書いています)

  3. 更に別のターミナルを開き、センサーデータの可視化を行います。

    ./run_rviz.sh
    

ブリッジを介さなくても簡単にトピックを取得できました!!
すごく便利ですね。
次にこのコードを少し変更して、LIO-SAMをしてみます。

IMUの追加と速度変更

まずは、Carla-0.10.0-Linux-Shippingフォルダ内にあるサンプルコードを変更します。

stack.jsonの変更

LIO-SAMをする上で必要なIMUの情報をJSONファイルに追加します。

stack.json
stack.json
{
    "type": "vehicle.lincoln.mkz",
    "id": "hero",
    "sensors": [
        {
            "type": "sensor.camera.rgb",
            "id": "rgb",
            "spawn_point": {"x": -4.5, "y": 0.0, "z": 2.5, "roll": 0.0, "pitch": 20.0, "yaw": 0.0},
            "attributes": {
                "image_size_x": 400,
                "image_size_y": 200,
                "fov": 90.0
            }
        },
        {
            "type": "sensor.lidar.ray_cast",
            "id": "lidar",
            "spawn_point": {"x": 0.0, "y": 0.0, "z": 2.4, "roll": 0.0, "pitch": 0.0, "yaw": 0.0},
            "attributes": {
                "range": 85,
                "channels": 64,
                "points_per_second": 600000,
                "rotation_frequency": 20,
                "upper_fov": 10,
                "lower_fov": -30,
                "atmosphere_attenuation_rate": 0.004,
                "dropoff_general_rate": 0.45,
                "dropoff_intensity_limit": 0.8,
                "dropoff_zero_intensity": 0.4
            }
        },
        {
            "type": "sensor.other.imu",
            "id": "imu",
            "spawn_point": {"x": 0.0, "y": 0.0, "z": 2.5, "roll": 0.0, "pitch": 0.0, "yaw": 0.0},
            "attributes": {
                "noise_accel_stddev_x": 0.0,
                "noise_accel_stddev_y": 0.0,
                "noise_accel_stddev_z": 0.0,
                "noise_gyro_bias_x": 0.0,
                "noise_gyro_bias_y": 0.0,
                "noise_gyro_bias_z": 0.0,
                "noise_gyro_stddev_x": 0.0,
                "noise_gyro_stddev_y": 0.0,
                "noise_gyro_stddev_z": 0.0,
                "noise_seed": 0
            }
        }
    ]
}

ros2_native.pyの変更

デフォルトのままだと速度が早すぎてSLAMがうまく行かないので、ros2_native.pyを変更して元速度の1/10にします。

ros2_native.py
ros2_native.py
#!/usr/bin/env python

# Copyright (c) 2024 Computer Vision Center (CVC) at the Universitat Autonoma de
# Barcelona (UAB).
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.

# Allows controlling a vehicle with a keyboard. For a simpler and more
# documented example, please take a look at tutorial.py.

import argparse
import json
import logging

import carla
import math


def _setup_vehicle(world, config):
    logging.debug("Spawning vehicle: {}".format(config.get("type")))

    bp_library = world.get_blueprint_library()
    map_ = world.get_map()

    bp = bp_library.filter(config.get("type"))[0]
    bp.set_attribute("role_name", config.get("id"))
    bp.set_attribute("ros_name", config.get("id")) 

    index = 3

    return  world.spawn_actor(
        bp,
        map_.get_spawn_points()[1],
        attach_to=None)


def _setup_sensors(world, vehicle, sensors_config):
    bp_library = world.get_blueprint_library()

    sensors = []
    for sensor in sensors_config:
        logging.debug("Spawning sensor: {}".format(sensor))

        bp = bp_library.filter(sensor.get("type"))[0]
        bp.set_attribute("ros_name", sensor.get("id")) 
        bp.set_attribute("role_name", sensor.get("id")) 
        for key, value in sensor.get("attributes", {}).items():
            bp.set_attribute(str(key), str(value))

        wp = carla.Transform(
            location=carla.Location(x=sensor["spawn_point"]["x"], y=-sensor["spawn_point"]["y"], z=sensor["spawn_point"]["z"]),
            rotation=carla.Rotation(roll=sensor["spawn_point"]["roll"], pitch=-sensor["spawn_point"]["pitch"], yaw=-sensor["spawn_point"]["yaw"])
        )

        sensors.append(
            world.spawn_actor(
                bp,
                wp,
                attach_to=vehicle
            )
        )

        sensors[-1].enable_for_ros()

    return sensors


def main(args):

    world = None
    vehicle = None
    sensors = []
    original_settings = None

    try:
        client = carla.Client(args.host, args.port)
        client.set_timeout(100.0)

        world = client.get_world()

        original_settings = world.get_settings()
        settings = world.get_settings()
        settings.synchronous_mode = True
        settings.fixed_delta_seconds = 0.05
        world.apply_settings(settings)

        traffic_manager = client.get_trafficmanager()
        traffic_manager.set_synchronous_mode(True)


        with open(args.file) as f:
            config = json.load(f)

        vehicle = _setup_vehicle(world, config)
        ### この行を追加 制限速度を9割減
        traffic_manager.vehicle_percentage_speed_difference(vehicle, 90)

        sensors = _setup_sensors(world, vehicle, config.get("sensors", []))

        _ = world.tick()

        vehicle.set_autopilot(True)

        logging.info("Running...")

        while True:
            _ = world.tick()

    except KeyboardInterrupt:
        print('\nCancelled by user. Bye!')

    finally:
        if original_settings:
            world.apply_settings(original_settings)

        for sensor in sensors:
            sensor.destroy()

        if vehicle:
            vehicle.destroy()


if __name__ == '__main__':
    argparser = argparse.ArgumentParser(description='CARLA ROS2 native')
    argparser.add_argument('--host', metavar='H', default='localhost', help='IP of the host CARLA Simulator (default: localhost)')
    argparser.add_argument('--port', metavar='P', default=2000, type=int, help='TCP port of CARLA Simulator (default: 2000)')
    argparser.add_argument('-f', '--file', default='', required=True, help='File to be executed')
    argparser.add_argument('-v', '--verbose', action='store_true', dest='debug', help='print debug information')

    args = argparser.parse_args()

    log_level = logging.DEBUG if args.debug else logging.INFO
    logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level)

    logging.info('Listening to server %s:%s', args.host, args.port)

    main(args)

run_rviz.shの変更

Docker内でLIO-SAMをダウンロードするために、run_rviz.shをbashシェルを起動するよう変更します。

run_rviz.sh
run_rviz.sh
#!/bin/bash

SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"

function transfer_x11_permissions() {
    # store X11 access rights in temp file to be passed into docker container
    XAUTH=/tmp/.docker.xauth
    touch $XAUTH
    xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge -
}

transfer_x11_permissions
docker run \
    -it \
    --rm \
    --net=host \
    --env="DISPLAY=$DISPLAY" \
    --env="XAUTHORITY=$XAUTH" \
    --env="FASTRTPS_DEFAULT_PROFILES_FILE=/config/fastrtps-profile.xml" \
    --volume="${SCRIPT_DIR}/config:/config:ro" \
    --volume="${SCRIPT_DIR}/rviz:/rviz:rw" \
    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
    --volume="$XAUTH:$XAUTH" \
    osrf/ros:humble-desktop \
    # ros2 run rviz2 rviz2 -d /rviz/ros2_native.rvizの代わりにbashを起動
    bash

Docker内でLIO-SAMをビルドする

コンテナ内に移動

./run_rviz.sh

バッケージリストの更新

sudo apt update 

パッケージのインストール

sudo apt install software-properties-common
sudo add-apt-repository ppa:borglab/gtsam-release-4.1
sudo apt install ros-humble-perception-pcl \
   ros-humble-pcl-msgs \
   ros-humble-vision-opencv \
   ros-humble-xacro
sudo apt install libgtsam-dev libgtsam-unstable-dev

LIO-SAMのビルド

mkdir -p ~/slam_ws/src
cd ~/slam_ws/src
git clone https://github.com/TixiaoShan/LIO-SAM.git -b ros2
cd ..
colcon build --symlink-install --packages-select lio_sam 

params.yamlの変更

シミュレーター上に搭載したセンサーの設置状況に合わせて、対応するトピック名やフレーム名を設定します。

geditのインストール

sudo apt install gedit
sudo gedit src/LIO-SAM/config/params.yaml 

params.yamlの変更

params.yaml
params.yaml
/**:
  ros__parameters:
    # Topic名の変更
    pointCloudTopic: "/carla/hero/lidar"                   # Point cloud data
    imuTopic: "/carla/hero/imu"                        # IMU data
    odomTopic: "odometry/imu"                    # IMU pre-preintegration odometry, same frequency as IMU
    gpsTopic: "odometry/gpsz"                    # GPS odometry topic from navsat, see module_navsat.launch file
    # Frameの変更
    lidarFrame: "lidar"
    baselinkFrame: "hero"
    odometryFrame: "odom"
    mapFrame: "map"
    # GPS Settings
    useImuHeadingInitialization: false           # if using GPS data, set to "true"
    useGpsElevation: false                       # if GPS elevation is bad, set to "false"
    gpsCovThreshold: 2.0                         # m^2, threshold for using GPS data
    poseCovThreshold: 25.0                       # m^2, threshold for using GPS data
    # Export settings
    savePCD: true                               # https://github.com/TixiaoShan/LIO-SAM/issues/3
    savePCDDirectory: "/move/src"         # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
    # Sensor Settings
    # センサー名の変更
    sensor: velodyne                               # lidar sensor type, either 'velodyne', 'ouster' or 'livox'
    N_SCAN: 64                                   # number of lidar channels (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
    Horizon_SCAN: 512                            # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
    downsampleRate: 1                            # default: 1. Downsample your data if too many
    # points. i.e., 16 = 64 / 4, 16 = 16 / 1
    lidarMinRange: 1.0                           # default: 1.0, minimum lidar range to be used
    lidarMaxRange: 1000.0                        # default: 1000.0, maximum lidar range to be used
    # IMU Settings
    imuAccNoise: 3.9939570888238808e-03
    imuGyrNoise: 1.5636343949698187e-03
    imuAccBiasN: 6.4356659353532566e-05
    imuGyrBiasN: 3.5640318696367613e-05
    imuGravity: 9.80511
    imuRPYWeight: 0.01
    # 座標軸の変更
    extrinsicTrans:  [ 0.0,  0.0,  0.0 ]
    extrinsicRot:    [ 1.0,  0.0,  0.0,
                       0.0,  1.0,  0.0,
                       0.0,  0.0,  1.0 ]
    extrinsicRPY: [ 1.0,  0.0,  0.0,
                    0.0,  1.0,  0.0,
                    0.0,  0.0,  1.0 ]
    # LOAM feature threshold
    edgeThreshold: 1.0
    surfThreshold: 0.1
    edgeFeatureMinValidNum: 10
    surfFeatureMinValidNum: 100
    # voxel filter paprams
    odometrySurfLeafSize: 0.4                     # default: 0.4 - outdoor, 0.2 - indoor
    mappingCornerLeafSize: 0.2                    # default: 0.2 - outdoor, 0.1 - indoor
    mappingSurfLeafSize: 0.4                      # default: 0.4 - outdoor, 0.2 - indoor
    # robot motion constraint (in case you are using a 2D robot)
    z_tollerance: 1000.0                          # meters
    rotation_tollerance: 1000.0                   # radians
    # CPU Params
    numberOfCores: 4                              # number of cores for mapping optimization
    mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency
    # Surrounding map
    surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
    surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
    surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses
    surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization
    # (when loop closure disabled)
    # Loop closure
    loopClosureEnableFlag: true
    loopClosureFrequency: 1.0                     # Hz, regulate loop closure constraint add frequency
    surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
    historyKeyframeSearchRadius: 15.0             # meters, key frame that is within n meters from
    # current pose will be considerd for loop closure
    historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be
    # considered for loop closure
    historyKeyframeSearchNum: 25                  # number of hostory key frames will be fused into a
    # submap for loop closure
    historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment
    # Visualization
    globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
    globalMapVisualizationPoseDensity: 10.0       # meters, global map visualization keyframe density
    globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density

LIO-SAMの実行

  1. LIO-SAMの起動

    現在のディレクトリが~/slam_wsであることを前提としています。LIO-SAMを起動します。

    source install/setup.bash
    ros2 launch lio_sam run.launch.py
    
  2. CARLAの起動(未起動の場合)

    CARLAがまだ起動していない場合、ROS2統合機能を有効にして起動します。すでに起動済みの場合はこのステップはスキップしてください。

    ./CarlaUnreal.sh --ros2 -RenderOffScreen
    
  3. ros2_native.pyの起動

    別のターミナルを開き、JSONファイルからセンサー情報を読み込み、車両にセンサーを追加してスポーンさせます。

    conda activate carla
    cd PythonAPI/examples/ros2
    python3 ros2_native.py -f stack.json
    
  4. mapフレームとodomフレームの間の変換を設定

    LIO-SAMが動作するために、各座標フレーム間の変換が必要なので、LIO-SAMをビルドしたコンテナに接続し、mapフレームとodomフレームの間に固定の変換を設定します。

    LIO-SAMをビルドしたコンテナに入る。

    別ターミナルを開き、起動中のコンテナを確認するために以下のコマンドを実行し、対象のコンテナIDを確認します。

     docker ps
    

    例えば、以下のような表示が出た場合、

    CONTAINER ID   IMAGE                     COMMAND                  CREATED       STATUS       PORTS     NAMES
    cdec563c5ed3   osrf/ros:humble-desktop   "/ros_entrypoint.sh …"   3 hours ago   Up 3 hours             unruffled_wing
    

    確認したコンテナIDを使用して、コンテナ内に入ります。

    docker exec -it cdec563c5ed3 /bin/bash
    

    static_transform_publisherの起動

    コンテナ内でROS2の環境をセットアップした後、map→odomの静的な変換をpubします。

    source /opt/ros/humble/setup.bash
    ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 map odom
    
  5. mapフレームとodomフレームの間の変換を設定
    更に別のターミナルを開き、先ほどと同様にコンテナに入り、odom→heroの静的な変換をpubします。

    docker exec -it cdec563c5ed3 /bin/bash
    source /opt/ros/humble/setup.bash
    ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 odom hero
    

これでrviz2の画面を見るとLIO-SAMが始まっています。

おわりに

今回はCARLA0.10.0を利用してLIO-SAMをしてみました。
手間が少なく非常に綺麗で、ROS2ネイティブインターフェースのありがたさと、UE5のグラフィックの美しさを実感できました。

Discussion