Nav2 講座 6: amcl の初期位置を設定する Python ノードを作成する

ROS

ROS 2 に関して筆者は以下の本で学びました。ROS1 との比較を含めかなりわかりやすく ROS 2 について書かれており、おすすめの一冊です。

Amazon.co.jp
amzn.to

この記事で学べる概念

  • initialpose について
  • ROS 2 で python パッケージ作成
  • ライフサイクル

過去記事

PC 環境

  • Thinkpad X1 Intel Core i7
  • Ubuntu 24.04

概要

前回までの記事では、2D Pose Estimate を使ってロボットの自己位置合わせをしていました。

RViz: 2D Pose Estimate

シミュレーション空間上でのロボットの初期位置は以下のように launch ファイルで (x, y, yaw) = (0, 0, 0) と定義されています。これまでは、ロボットにこの初期位置を教えるのを 2D Pose Estimate で手動でやっていたというわけです。

    pose = {'x': LaunchConfiguration('x_pose', default='0.00'),
            'y': LaunchConfiguration('y_pose', default='0.00'),
            'z': LaunchConfiguration('z_pose', default='0.01'),
            'R': LaunchConfiguration('roll', default='0.00'),
            'P': LaunchConfiguration('pitch', default='0.00'),
            'Y': LaunchConfiguration('yaw', default='0.00')}

ロボットに位置を教えるのに、/initial_pose トピックを使うこともできます。amcl ノードがこのトピックを受け取り、位置合わせが完了します（ソースコードリンク）。

簡易的には以下のコマンド（/initialpose トピックの publish）を実行することにより、実際にロボットが原点に位置合わせされることを確かめられます。

/workspace$ ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "
header:
  frame_id: 'map'
pose:
  pose:
    position: {x: 0.0, y: 0.0, z: 0.0}
    orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
               0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
               0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
               0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
               0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
               0.0, 0.0, 0.0, 0.0, 0.0, 0.0]"
Publish /initialpose topic manually

ただ、毎回シミュレーション起動時に上記のコマンドを実行するのはめんどいので、自動でやってくれる ROS 2 ノードを作ってしまおうというのが本記事の趣旨です。

ROS 2 で python パッケージを作成

git checkout article_8266

※ 上記コマンドを実行して、こちらの節まで飛んでも大丈夫です。

ROS 2 で python パッケージを作成するには、docker コンテナが立ち上がった状態で以下を実行してください。docker コンテナの立ち上げからやりたい場合はこの記事を参考してください。

$ docker exec -it ros2_nav2 bash
/workspace$ cd src
/workspace/src$ ros2 pkg create initialpose_publisher --build-type ament_python --dependencies rclpy geometry_msgs

これにより、initialpose_publisher というノードの雛形ができました。

ros2_nav2/workspace/src$ tree initialpose_publisher/
initialpose_publisher/
├── initialpose_publisher
│   └── __init__.py
├── package.xml
├── resource
│   └── initialpose_publisher
├── setup.cfg
├── setup.py
└── test
    ├── test_copyright.py
    ├── test_flake8.py
    └── test_pep257.py

initialpose_publisher/initialpose_publisher 配下に initialpose_publisher.py を作成し、以下のコードを書き込みます。

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node

from geometry_msgs.msg import PoseWithCovarianceStamped
from lifecycle_msgs.srv import GetState

import time

class InitialposePublisher(Node):
    def __init__(self):
        super().__init__('initialpose_publisher')
        self.amcl_service_name = '/amcl/get_state'
        self.pose_pub = self.create_publisher(PoseWithCovarianceStamped, '/initialpose', 10)

        self.cli = self.create_client(GetState, self.amcl_service_name)

        self.timer = self.create_timer(1.0, self.check_amcl_state)

    def check_amcl_state(self):
        if not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Waiting for /amcl/get_state service...')
            return
    
        self.get_logger().info('Requesting AMCL state...')
        request = GetState.Request()
        future = self.cli.call_async(request)
        future.add_done_callback(self.handle_amcl_state_response)
    
    def handle_amcl_state_response(self, future):
        try:
            response = future.result()
            state = response.current_state.label
            self.get_logger().info(f'AMCL current state: {state}')
            if state == 'active':
                self.publish_initial_pose()
                self.destroy_timer(self.timer)
                rclpy.shutdown()
        except Exception as e:
            self.get_logger().error(f'Failed to get AMCL state: {e}')

    def publish_initial_pose(self):
        msg = PoseWithCovarianceStamped()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'map'

        msg.pose.pose.position.x = 0.0
        msg.pose.pose.position.y = 0.0
        msg.pose.pose.position.z = 0.0
        msg.pose.pose.orientation.w = 1.0
        msg.pose.covariance = [0.0] * 36

        self.pose_pub.publish(msg)
        self.get_logger().info('Initial pose published')

def main(args=None):
    rclpy.init(args=args)
    node = InitialposePublisher()
    rclpy.spin(node)

if __name__ == '__main__':
    main()

簡単に説明すると以下のようになります。

  • /amcl/get_state サービスを 1 秒ごとにポーリングして amcl のライフサイクル状態を取得する
  • 状態が “active” になったら、/initialpose に (x=0, y=0, yaw=0) の初期位置を publish する
  • orientation は (x=0, y=0, z=0, w=1) のクォータニオンを指定する
  • 一度 publish したら状態監視を終了し、ノードも終了する（rclpy.shutdown）

ros2 run コマンドで上記 python スクリプトを実行するために、setup.py に以下を追記してください。

    entry_points={
        'console_scripts': [
            'initialpose_publisher = initialpose_publisher.initialpose_publisher:main',
        ],

initialpose_publisher の実行

上記の python スクリプトを以下のコマンドにより実行すると、ロボットの自己位置が原点に設定されることが確認できます。

/workspace$ colcon build
/workspace$ source install/setup.bash
/workspace$ ros2 launch custom_nav2_bringup tb3_simulation_launch.py headless:=False &
/workspace$ ros2 run initialpose_publisher initialpose_publisher  # another terminal
[INFO] [1745047471.644227930] [initialpose_publisher]: Requesting AMCL state...
[INFO] [1745047471.650659221] [initialpose_publisher]: AMCL current state: active
[INFO] [1745047471.652015678] [initialpose_publisher]: Initial pose published
Execute initialpose_publisher and correct a robot’s pose

ライフサイクルとは

ROS1 にはなかった概念ですが、ROS 2 にはライフサイクルというものがあります。これは簡単に言うと、ノードの状態を確認する仕組みであり、以下のような状態があります。

状態説明
UNCONFIGUREDノードが起動直後（まだ準備できてない）
INACTIVE設定済みだがまだ動作してない
ACTIVE通常動作中
FINALIZEDシャットダウン

上記のようにノードの状態がわかることにより、たとえばノード A の状態をノード B が確認し、その確認結果に応じて動作を変えることができます。

今回の例では、initialpose_publisher が amcl ノードが ACTIVE 状態になったときに一度だけ /initialpose トピックを publish する、ということが実現できています。

まとめ

今回は、以下を学びました！やっていること自体はシンプルですが、それぞれ意味を理解すると nav2 での開発が捗るかと思います。

  • /initialpose トピックをコールすることで、ロボットの自己位置を変えられること
  • ROS 2 python パッケージの作り方
  • ライフサイクルの概念と簡単な使い方

コメント