CARLA0.10.0 で LIO-SAM してみる
はじめに
今回は、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のブリッジを介さずに、直接センサーデータの送信や制御コマンドの受信を行ってみます。
-
ROS2統合機能を有効にしてCARLAを起動します。
./CarlaUnreal.sh --ros2 -RenderOffScreen
このオプションにより、シミュレーターはROS2のネイティブインターフェースを有効化し、トピックの通信が可能になります。
-
別のターミナルを開き、JSONファイルからセンサー情報を読み込み、車両にセンサーを追加してスポーンします。
conda activate carla cd PythonAPI/examples/ros2 python3 ros2_native.py -f stack.json
ros2_native.pyファイル内の
enable_for_ros()
でROS2データ送信ができるよう設定しています。(condaの環境作成についてはここで書いています)
-
更に別のターミナルを開き、センサーデータの可視化を行います。
./run_rviz.sh
ブリッジを介さなくても簡単にトピックを取得できました!!
すごく便利ですね。
次にこのコードを少し変更して、LIO-SAMをしてみます。
IMUの追加と速度変更
まずは、Carla-0.10.0-Linux-Shippingフォルダ内にあるサンプルコードを変更します。
stack.jsonの変更
LIO-SAMをする上で必要なIMUの情報を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
#!/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
#!/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
/**:
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の実行
-
LIO-SAMの起動
現在のディレクトリが
~/slam_ws
であることを前提としています。LIO-SAMを起動します。source install/setup.bash ros2 launch lio_sam run.launch.py
-
CARLAの起動(未起動の場合)
CARLAがまだ起動していない場合、ROS2統合機能を有効にして起動します。すでに起動済みの場合はこのステップはスキップしてください。
./CarlaUnreal.sh --ros2 -RenderOffScreen
-
ros2_native.pyの起動
別のターミナルを開き、JSONファイルからセンサー情報を読み込み、車両にセンサーを追加してスポーンさせます。
conda activate carla cd PythonAPI/examples/ros2 python3 ros2_native.py -f stack.json
-
mapフレームとodomフレームの間の変換を設定
LIO-SAMが動作するために、各座標フレーム間の変換が必要なので、LIO-SAMをビルドしたコンテナに接続し、mapフレームとodomフレームの間に固定の変換を設定します。
別ターミナルを開き、起動中のコンテナを確認するために以下のコマンドを実行し、対象のコンテナ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
コンテナ内で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
-
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