👏

カチャカでemcl2を動かす

2024/07/28に公開

この記事ではカチャカでROS 2のLocalizationパッケージを動かしてみる方法について書いていきます。

方法

今回はPFRさんが公開してくれてるkachaka-apiを使います。
普通にROS 2で動かせるようにする場合はREADMEの手順通りros2 bridgeを立ち上げれば動かせますが、Localizationモジュールを別途動かしたい場合懸念となるのがtf情報がかぶってしまうことです。ros2 bridgeが位置情報と合わせてmapのtfを出してくれるんですが手元で別途Localizationを動かそうとすると同じくmapのtfを出してしまうので競合してしまいます。
とりあえず動作させるためには、

ros2 bridgeからmapのtfを出さないようにする
tfにprefixをつけ、競合しないようにする

のどちらかをやってあげれば動きそう。

今回は(あまり良くはないかもですが)一行継ぎ足すだけで終わるので、mapのtfを出さないようにする方針でやってみます。

ros2 bridgeを書き換え

kachaka-apiをcloneしてきます。

git clone https://github.com/pf-robotics/kachaka-api

ros2/kachaka_grpc_ros2_bridge/src/dynamic_tf_bridge.cppを開いて、30行目辺りの後に以下を追記します。

if (transform_ros.header.frame_id == "map")　continue;

あんま賢くないですがframe_idがmapの場合はスキップさせてます。こうすると手元で別のLocalizationノードを動かしてもtf情報がぶつからずに動かすことが出来ます。

Dockerイメージビルド

ここまでできたら、dockerイメージをビルドしていきます。自分でイメージをビルドして立ち上げる方法に関しては、方法をREADMEに記載するPRを投げてくれた方がいたのでそちらを参考にしています(https://github.com/pf-robotics/kachaka-api/pull/109)。

cd kachaka-api
docker buildx build -t kachaka-api --target kachaka-grpc-ros2-bridge -f Dockerfile.ros2 . --build-arg BASE_ARCH=x86_64 --load

完了したら、kachaka-api/tools/ros2_bridge/docker-compose.yamlを開き、イメージ名を変更します(デフォルトは配布されてるもので、今回自分でビルドして生成してきたイメージ名にすり替えます)。

-    image: "asia-northeast1-docker.pkg.dev/kachaka-api/docker/kachaka-grpc-ros2-bridge:${TAG}"
+    image: kachaka-api:latest

ここまでできたらカチャカ側の準備はOKです。

emcl2のビルド

動かしたいLocalizationのROSパッケージをビルドします。今回は千葉工大の上田研究室から提供されているemcl2を使用してみます。

今回動かすためにいくつか修正をします。
1つ目はLaserScanのsubscriberのqosを以下のように修正しておきます。

laser_scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
	  "scan",
	  rclcpp::QoS(rclcpp::KeepLast(1)).best_effort().durability_volatile(), // <- new
	  std::bind(&EMcl2Node::cbScan, this, std::placeholders::_1));

launch/emcl2.launch.pyを以下のものに変更します。

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction
from launch.substitutions import LaunchConfiguration, TextSubstitution
from launch_ros.actions import Node, SetParameter


def generate_launch_description():
    params_file = LaunchConfiguration('params_file')
    use_sim_time = LaunchConfiguration('use_sim_time')

    declare_use_sim_time = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use simulation (Gazebo) clock if true')
    declare_params_file = DeclareLaunchArgument(
        'params_file',
        default_value=[
            TextSubstitution(text=os.path.join(
                get_package_share_directory('emcl2'), 'config', '')),
            TextSubstitution(text='emcl2.param.yaml')],
        description='emcl2 param file path')

    launch_node = GroupAction(
        actions=[
            SetParameter('use_sim_time', use_sim_time),
            Node(
                name='emcl2',
                package='emcl2',
                executable='emcl2_node',
                parameters=[params_file],
                remappings=[('scan', '/kachaka/lidar/scan'), ('map', '/kachaka/mapping/map')],
                output='screen'),
        ]
    )

    ld = LaunchDescription()
    ld.add_action(declare_use_sim_time)
    ld.add_action(declare_params_file)

    ld.add_action(launch_node)

    return ld

修正が出来たらビルドします。

mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src
git clone https://github.com/CIT-Autonomous-Robot-Lab/emcl2_ros2
cd ../
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release

実行

ここまでできれば後は実行するだけです。まずros2 bridgeを動かすため、docker composeを立ち上げます。これはREADMEに書いてある手順通りです。

cd kachaka-api/tools/ros2_bridge
./start_bridge.sh  <カチャカのIPアドレス>

emcl2を立ち上げます。今回mapデータはカチャカ側で作成したmapがros2 bridgeより配信されるので、そちらを使用します。

ros2 launch emcl2 emcl2.launch.py

あとはrvizを立ち上げれば動作している様子が見えます↓

方法

ros2 bridgeを書き換え

Dockerイメージビルド

emcl2のビルド

実行

Discussion