SLAM on M1KE BL0G

差動二輪ロボットの駆動系とAMCL・SLAM・Nav2

Wed, 14 Oct 2026 11:00:00 +0900

背景

ROS2の概念を理解するためのメモでTF2・SLAM・Nav2・Gazeboの概念をざっと整理したが、「ホイールのオドメトリとLiDARが、具体的にAMCL・SLAM・Nav2のどこに入り、どう使われるのか」という駆動系（ドライブトレイン）側の繋がりが曖昧だった
そこで、Gazebo上での差動二輪駆動（Differential Drive）のシミュレーションを起点に、オドメトリ・LiDARがAMCL・SLAM・Nav2にどう流れていくのかを整理する

全体像: 駆動系からナビゲーションまでのデータの流れ

大まかな処理の流れは以下の通り

順序	段階	内容
1	駆動系シミュレーション	Gazeboが、ホイールの関節の回転からロボットの移動をシミュレートし、オドメトリを計算する
2	センサーシミュレーション	Gazeboが、LiDARセンサーをシミュレートし、距離測定データを生成する
3	自己位置推定	AMCL（既知の地図がある場合）またはSLAM（地図がない場合）が、オドメトリとLiDARデータから、地図上での自己位置を推定する
4	経路計画・制御	Nav2が、推定された自己位置とLiDARベースのコストマップを使って、経路を計画し、速度指令（`cmd_vel`）を出す
5	駆動系への反映	`cmd_vel`がGazeboの駆動系シミュレーションに戻され、ロボットが実際に動く

つまり、駆動系（1・5）とセンサー（2）がGazebo側の入出力、自己位置推定（3）とナビゲーション（4）がROS 2側の処理という役割分担になっており、この2つがオドメトリ・LiDARデータ・cmd_velを通じて閉じたループを作っている

TFツリー全体像

このループを理解する上で軸になるのが、TF2が管理する座標フレームのツリー
代表的な構成は以下の通り

1

map -> odom -> base_footprint -> base_link -> base_laser（LiDARなど各センサーのフレーム）

変換	発行元	内容
`odom` -> `base_footprint`（実質的に`base_link`）	駆動系シミュレーション（Gazeboのプラグイン、実機ではモータードライバ側）	ホイールの回転から計算した、ロボット自身の移動量（オドメトリ）
`map` -> `odom`	AMCLまたはSLAM	オドメトリの誤差を、LiDARと地図の照合で補正した分のズレ
`base_footprint` -> `base_link`、`base_link` -> `base_laser`など	`robot_state_publisher`（URDFから自動計算）	ロボット本体の構造に対する、各フレーム・センサーの固定された取り付け位置

重要なのは、AMCL・SLAMはodom->base_link系の変換を書き換えるのではなく、それとは別にmap->odomの変換を発行するという点
odom->base_link系は駆動系が出し続けるオドメトリそのもの（ドリフトを含む、短期的には滑らかだが長期的にはズレていく値）であり、AMCL・SLAMはそのズレを、別のレイヤーで補正する役割分担になっている

TF2の仕組み: 分散システム前提の座標変換ライブラリ

TF2は、複数のノードに分かれて動く分散システムであることを前提にした、座標変換のためのライブラリ
各ノードが、自分の知っている2つのフレーム間の位置・姿勢関係をtransform_broadcasterで発行し、それらを集約することで、ロボット全体の全フレームが繋がった1つのツリーが構築される
位置・姿勢を知りたい側は、transform_listenerでこのツリーをたどり、任意の2フレーム間の変換を取得する
位置は$x, y, z$（メートル単位）、姿勢はクォータニオン（四元数）で表現される

ROS座標フレームの一覧

ここまで出てきたmap・odom・base_footprint・base_link・base_laserは、ROSのコミュニティで広く使われている座標フレームの命名規約（移動ロボット向けの標準規約REP-105ほか）に沿ったもので、それぞれの役割は以下の通り

フレーム	種類	内容
`map`	ワールド座標	ロボットが位置する世界座標系。REP-105によれば、Z軸が上向きで、地図に対する位置は長期的にほとんどドリフトしないが、再位置推定のタイミングで不連続にジャンプすることがある
`odom`	ワールド座標（ローカルに連続）	オドメトリ（車輪エンコーダなどの距離計）を積分した座標系。REP-105によれば連続的でなめらかな一方、実際の世界座標（`map`）とは長期的にズレ（ドリフト）ていく。このズレを補正するのが、前述の`map`->`odom`変換
`base_footprint`	ローカル座標	`base_link`を地面に投影したフレーム。XY座標・向きは`base_link`と連動するが、Z座標は常に地面の高さ（0）に固定される。2Dのナビゲーションアルゴリズムが、ロボットを平面上の点として扱えるようにするためのフレーム
`base_link`	ローカル座標	ロボット本体に固定された基準フレーム。車体の実際の高さ・姿勢を含む、ロボットそのものの位置
`base_laser`（本記事では`laser_link`と表記）	ローカル座標	LiDARなどのレーザースキャナが取り付けられている位置を表すフレーム。具体的な名前はロボットごとに異なる

mapとodomはどちらも世界座標だが、mapは長期的に正確な代わりに不連続、odomは連続的だが長期的にはズレていくという、正反対の性質を持つ
base_footprint・base_link・base_laserはどれもロボット本体を基準にしたローカル座標で、odom->base_footprintの変換さえ分かれば、そこから先は固定されたロボットの構造（URDF）だけで計算できる

Gazeboでの駆動系シミュレーション

差動二輪駆動の仕組み

差動二輪駆動（Differential Drive）は、左右2つの独立した駆動輪の回転速度の差で、前進・後退・旋回を実現する機構
左右輪の角速度を$\omega_L$、$\omega_R$、ホイール半径を$r$、ホイール間距離（トレッド）を$L$とすると、ロボット本体の並進速度$v$と角速度$\omega$は以下の式で計算される

$$ v = \frac{r(\omega_R + \omega_L)}{2}, \quad \omega = \frac{r(\omega_R - \omega_L)}{L} $$

Nav2やteleopノードがcmd_vel（並進速度$v$・角速度$\omega$）を指令すると、駆動系はこの式を逆に解いて、左右輪それぞれの目標角速度を求める

gz-simのDiffDriveシステムプラグイン

現行のGazebo（gz-sim、旧Ignition Gazebo）では、DiffDriveというシステムプラグインが、この差動二輪駆動のシミュレーションとオドメトリ計算をまとめて担う
実際にソースを確認したところ、役割は以下の通り

項目	内容
入力トピック	`/model/<モデル名>/cmd_vel`（`geometry_msgs/Twist`、トピック名は`<topic>`タグで変更可能）
出力トピック（オドメトリ）	`/model/<モデル名>/odometry`（`<odom_topic>`タグで変更可能）
出力トピック（TF）	`/model/<モデル名>/tf`（`<tf_topic>`タグで変更可能）
主なパラメータ	`<left_joint>`・`<right_joint>`（ホイールの関節名）、`<wheel_separation>`（ホイール間距離、デフォルト1.0m）、`<wheel_radius>`（ホイール半径、デフォルト0.2m）、`<odom_publish_frequency>`（オドメトリ配信周波数、デフォルト50Hz）
発行するTF	`frame_id`は`<モデル名>/odom`、`child_frame_id`は`<モデル名>/<ホイールが接続されたリンク名>`がデフォルト

つまり、SDF（Gazeboのロボット記述形式）やURDFの中でこのプラグインに左右ホイールの関節名と、ホイール半径・間距離を指定するだけで、cmd_velを受け取って物理的にホイールを回転させ、その回転量から上の式を使ってオドメトリを逆算し、odom->base_link相当のTFとトピックを配信してくれる
ROSとの橋渡し（ros_gz_bridge）を使うことで、これらのGazebo側トピックを、ROS 2標準の/cmd_vel・/odomトピックに変換して繋ぎ込む

オドメトリのドリフトという前提

このDiffDriveプラグインが計算するオドメトリは、ホイールの回転量だけから並進・回転を積分しているため、実機ではスリップ（ホイールの空転・横滑り）やエンコーダのノイズによって、時間が経つほど実際の位置とズレていく（ドリフトする）
Gazeboでも、タイヤと地面の摩擦パラメータ次第でこのスリップが再現されるため、オドメトリ単体では長時間・長距離の自己位置推定には使えない、という実機と同じ制約がシミュレーション上でも成り立つ
このドリフトを補正するのが、後述のAMCL・SLAMの役割になる

GazeboでのLiDARシミュレーション

GazeboのLiDARセンサープラグインは、仮想空間内でレイを飛ばして周囲のオブジェクトとの交差点を計算することで、実際のLiDARと同じ形式の距離測定データ（sensor_msgs/LaserScan、各角度ごとの距離値の配列）を生成する
このセンサーも、URDF上でbase_linkからの相対位置・向きを指定したリンク（例えばlaser_link）に取り付ける形で定義し、robot_state_publisherがそのリンクのTFフレームを自動的に配信する
出力は、ROS 2標準では/scanトピックとして配信され、このトピックを後述のAMCL・SLAM・Nav2のコストマップが共通して利用する

AMCL: オドメトリとLiDARで自己位置を補正する

パーティクルフィルタの基本

AMCL（Adaptive Monte Carlo Localization）は、既知の地図上でのロボットの自己位置を、パーティクルフィルタで推定する手法
「ロボットがここにいるかもしれない」という仮説を、数百〜数千個の粒子（パーティクル、それぞれが位置・向きの候補）として保持し、センサー情報が入るたびに、尤度の高い粒子を生き残らせ、低い粒子を淘汰する、というリサンプリングを繰り返す

動作モデル（オドメトリ）と観測モデル（LiDAR）

実際にリポジトリで確認したところ、AMCLはodom（オドメトリ）・scan（LiDARのスキャン）・map（既知の地図）・initialpose（初期位置の手動指定）をsubscribeし、particlecloud（粒子群の可視化用）・amcl_pose（推定された自己位置）をpublishする
動作モデルは、直前のオドメトリの変化量を使って、各粒子を同じだけ移動させる。ただし、ドリフトを考慮して、移動量にはランダムなノイズを加える
観測モデルは、各粒子の位置から見たときに、今回のLiDARスキャンがどれだけ「地図と一致していそうか」を計算し、一致度が高い粒子ほど尤度を高くする
この2つを組み合わせることで、「オドメトリでおおよその動きを予測し、LiDARと地図の照合でその予測を補正する」というベイズフィルタ的な推定を、観測が入るたびに繰り返す

map -> odom変換の補正という役割

AMCLは、odom->base_linkのTFを書き換えるのではなく、map->odomのTFを配信することで、地図座標系での自己位置を表現する
つまり、駆動系（DiffDriveプラグイン）が出し続ける生のオドメトリはそのまま残しつつ、AMCLがその上に「このオドメトリ座標系は、実際の地図上ではこれだけズレている」という補正値を重ねる、というレイヤー構造になっている
このため、AMCLが何らかの理由で推定に失敗しても、odom->base_link自体は駆動系から配信され続けるため、システム全体が完全に座標情報を失うことはない

SLAM: 地図がない場合はslam_toolboxが代わりを担う

AMCLは「既知の地図」が前提になっているため、未知の環境ではそもそも使えない
そこで使われるのが、2D LiDARベースのSLAMとしてよく使われるslam_toolbox
AMCLとの違いは以下の通り

項目	AMCL	slam_toolbox
前提	既知の地図が必要	地図は不要（走行しながら作る）
役割	自己位置推定のみ	自己位置推定＋地図作成（SLAM）を同時に行う
使う情報	オドメトリ＋LiDARスキャン＋既存の地図	オドメトリ＋LiDARスキャンのスキャンマッチングで地図を構築・更新
内部構造	パーティクルフィルタ	ポーズグラフ（過去の位置同士の関係をグラフ化）とループクロージャ（同じ場所に戻ったことを検出して誤差を補正）の最適化
発行するTF・トピック	`map`->`odom`	`map`->`odom`、および`/map`（占有格子地図）、`/slam_toolbox/pose`

slam_toolboxも、AMCLと同様にオドメトリとLiDARスキャンを使ってスキャンマッチングを行い、その結果をもとにmap->odomのTFを配信する、という同じレイヤー構造を取る
つまり、AMCLとSLAMは「地図を使うか作るか」の違いであり、駆動系（オドメトリ）・LiDARからの入力の受け方、map->odomという出力の形は共通している
作成した地図を保存しておけば、以降はAMCLで既知の地図として使い回せる、という使い分けになる

SLAMとAMCLは同時に動かさない

AMCLとslam_toolboxは、どちらもmap->odomのTFを配信する役割を持つため、両者を同時に動かすとこの変換が競合してしまう
通常の運用では、地図作成中はslam_toolboxが地図と自己位置の両方を提供し、地図が完成した後はAMCLに切り替えて、既知の地図上での自己位置推定に専念するという使い分けが基本になる
つまり「地図作成フェーズ（SLAM）」と「運用フェーズ（AMCL）」を明確に切り分け、同じタイミングでは片方しか動かさないのが前提

発展: 意味情報を加えるOWL/RDFとSemantic SLAM

ここまでのSLAM・AMCLは、あくまで「空間のどこに壁や障害物があるか」という幾何学的な情報を扱う技術で、「その場所が何をする場所なのか」という意味までは扱わない
OWL（Web Ontology Language）・RDF（Resource Description Framework）は、場所やオブジェクトに「101号室は病室」「薬品庫はスタッフ専用エリア」といった意味・関係をオントロジー・知識グラフとして記述するための標準規格で、SLAM・AMCLとは独立した、意味情報を載せるためのレイヤーにあたる
通常のSLAMが「そこに壁や物体がある」という幾何学的な地図しか作らないのに対し、Semantic SLAM（意味的SLAM）は「それはドアなのか机なのか部屋なのか」といった意味ラベルも地図に結びつける研究分野で、OWL/RDFはこの意味情報を整理する手段の1つとして使われることがあるが、必須というわけではない
ROS4HRIの仕組みのLLM対応チュートリアルで触れたknowledge_coreも、OWL/RDFベースの記号的な知識ベースであり、位置情報そのものではなく、その位置・場所が持つ意味を扱う用途で使われている
整理すると、SLAM（地図を作る）→AMCL（地図上で位置を知る）→OWL/RDF（その位置・場所に意味を与える）という積み重ねで理解すると分かりやすいが、OWL/RDFはSLAM・AMCLの必須要素ではなく、必要に応じて追加する別レイヤー

Nav2との接続: コストマップと速度指令

AMCL・SLAMがamcl_pose（またはそれに相当するTF）として自己位置を確定させると、Nav2はそれをもとにグローバルプランナーで大まかな経路を計画し、コントローラー（ローカルプランナー）が直近の障害物を避けながら実際の速度指令を計算する
この「直近の障害物」を表すのがコストマップで、/scanのLiDARデータがリアルタイムに反映される。地図上の静的な障害物（グローバルコストマップ）と、今まさにLiDARが検出している動的な障害物（ローカルコストマップ）の両方に、同じ/scanデータが使われる
最終的にコントローラーが計算した速度指令がcmd_velとして配信され、それが駆動系シミュレーション（DiffDriveプラグイン）に渡って実際にホイールが回り、ロボットが移動する。この移動がまた新しいオドメトリ・LiDARスキャンを生み、ループが続いていく

まとめ

Gazebo上の駆動系シミュレーション（差動二輪駆動の場合はgz-simのDiffDriveプラグイン）は、cmd_velを受けてホイールを回転させ、その回転量からオドメトリを逆算し、odom->base_linkのTFとトピックを配信する
このオドメトリは、ホイールの回転だけから積分した値のため、実機・シミュレーション問わずドリフトする。この誤差を補正するのが、既知の地図があるときのAMCL、地図がないときのslam_toolboxの役割
AMCL・slam_toolboxはどちらも、オドメトリ（動作モデル／スキャンマッチングの初期値）とLiDARスキャン（観測モデル／地図とのマッチング）を使い、odom->base_link自体は書き換えず、別レイヤーのmap->odomというTFを配信することで、地図座標系での自己位置を補正する
両者ともmap->odomを配信するため、同時には動かさず、地図作成フェーズ（SLAM）と運用フェーズ（AMCL）を切り分けて使う
Nav2は、この自己位置と、LiDARスキャンから作られるコストマップをもとに経路計画・障害物回避を行い、最終的な速度指令cmd_velを駆動系に戻すことで、駆動系→センサー→自己位置推定→ナビゲーション→駆動系という1つの閉じたループが成立する
SLAM・AMCLが扱うのはあくまで幾何学的な位置情報で、「その場所が何をする場所か」という意味はOWL/RDFのような別レイヤー（Semantic SLAMという研究分野も含む）で扱う、必須ではない拡張領域になる

参考文献

ROS2の概念を理解するためのメモ

Tue, 25 Aug 2026 12:00:00 +0900

背景

業務でROS2を使う必要が出てきた
しかし、正直あまりROS2を触っていない
そこで、自分向けにROS2の基本的な考え方と用語をAIで整理したログ
さくっと全体概念を理解するのが目的

ROS2とは

Robot Operating System 2の略だが、OS（オペレーティングシステム）そのものではない
ロボット向けのソフトウェアを作るためのミドルウェア兼フレームワーク
「センサーで取得した値を処理して、モーターに指令を送る」といった、ロボットの各機能をバラバラのプログラム（後述のノード）として作り、それらを通信でつなげて1つのロボットとして動かす、という考え方が中心にある
前身のROS1に対して、以下のような改善が入っている
- 通信の基盤にDDSという業界標準の仕組みを採用し、単一障害点（後述）をなくした
- リアルタイム性や、Windows・組み込み環境への対応が強化された
- セキュリティ機能が追加された

ディストリビューションとインストール

ROS2は、毎年1回コードネームつきのバージョン（ディストリビューション）がリリースされる
全てのバージョンがLTS（長期サポート）というわけではなく、偶数年リリースのものがLTS（サポート期間5年）、奇数年リリースのものは短期サポート（サポート期間約1年半）になっている主なLTS版は以下の通り。

コードネーム	リリース年	対応Ubuntu
Humble Hawksbill	2022年	Ubuntu 22.04
Jazzy Jalisco	2024年	Ubuntu 24.04

迷ったら、その時点の最新LTS版を、対応するUbuntuのバージョンに入れるのが基本
ROS1と違い、ROS2はOSのバージョンとの結びつきが強いため、Ubuntuのバージョンを先に決めてから、対応するROS2のバージョンを選ぶことになる

全体アーキテクチャ

DDS（Data Distribution Service）

ROS2の通信を裏で支えているミドルウェアの規格
「マスターレス」という特徴があり、ROS1にあったroscoreのような、通信を仲介する中央サーバーが存在しない
各ノードが自律的にお互いを見つけて（Discoveryと呼ぶ）、直接データをやり取りする
中央サーバーがないため、その1台が落ちるとシステム全体が止まる、という単一障害点の問題が起きにくい

ノード（Node）

ROS2における処理の最小単位
1つのノードが、1つのプログラム（プロセス）に対応する
例えば「カメラ画像を取得するノード」「物体検出をするノード」「モーターを制御するノード」のように、機能ごとにノードを分けて作る
ノード同士は、後述のTopic・Service・Actionという3つの方式で通信する

ノード間の通信

ノード同士の通信には、目的に応じて3つの方式がある。

方式	通信の形	用途の例
Topic	非同期のpub/sub	センサーの値を流し続ける
Service	同期的なリクエスト/レスポンス	一度だけの問い合わせ・設定変更
Action	非同期＋進捗報告＋キャンセル可能	時間のかかる動作（アーム移動など）

Topic（トピック）

特定の名前（トピック名）に対して、データを送り続ける側（Publisher）と、受け取り続ける側（Subscriber）が非同期に繋がる仕組み
PublisherとSubscriberはお互いの存在を意識しなくてよく、同じトピック名にさえ繋がれば、何対何でも通信できる
センサーの値のように、継続的に流れてくるデータに向いている

Service（サービス）

関数呼び出しのように、リクエストを送って、その場でレスポンスを待つ同期的な通信
「今の設定値を教えて」「このパラメータを変更して」のような、1回のやり取りで完結する処理に向いている
レスポンスが返るまで基本的に待つことになるので、時間のかかる処理には向かない

Action（アクション）

Serviceに、進捗報告（フィードバック）とキャンセル機能を足したような仕組み
「アームを目標位置まで動かす」のように、時間がかかり、途中経過を知りたかったり、途中で中断したくなったりする処理に向いている
内部的にはTopicとServiceの組み合わせで実現されている

メッセージ型・インターフェース

Topic・Service・Actionでやり取りするデータの型（インターフェース）は、それぞれ.msg・.srv・.actionという専用のファイル形式で定義される標準で用意されている型は、目的別のパッケージにまとまっている。

パッケージ	主な型
`std_msgs`	Int32・String・Boolなど、基本的な型
`geometry_msgs`	Twist（速度）・Pose（位置・姿勢）など、幾何情報の型
`sensor_msgs`	Image・LaserScan・PointCloud2など、センサーデータの型

標準の型で足りない場合は、自分のパッケージ内に.msgファイルなどを作って独自の型を定義できる
ros2 interface show <型名>で、その型がどんなフィールドを持つか確認できる

cmd_vel（速度指令）

ロボットを動かすための速度指令に、事実上の標準として使われているトピック名
geometry_msgs/Twist（またはTwistStamped）という、並進速度（linear）と回転速度（angular）を持つメッセージ型を使う
Nav2の出力や、キーボード操作用のteleopノードなど、様々なノードがこの/cmd_velにpublishし、ロボット側のモーター制御ノードがそれをsubscribeして実際に動かす、という流れが一般的
複数のノードが同時に/cmd_velを出そうとすると競合するため、優先順位をつけて1つの指令にまとめるtwist_muxのようなノードがよく使われる

QoS（Quality of Service）

DDSが持つ、通信の品質を細かく設定できる仕組み
ROS1にはなかった、ROS2特有の重要な概念代表的な設定項目は以下の通り。

設定項目	内容
Reliability	通信の信頼性。多少データが欠けてもいいから速さを優先する（Best Effort）か、確実に届ける（Reliable）か
Durability	新しく繋がったSubscriberに対して、過去のデータも渡すかどうか
History	直近何件分のデータを保持しておくか

PublisherとSubscriberでQoSの設定が噛み合っていないと、通信自体が繋がらないことがあるため、初心者がハマりやすいポイントの1つ

開発の基本的な流れ

ワークスペース

ROS2のプロジェクト全体を置く作業ディレクトリ
中にsrc/ディレクトリを作り、その下に複数のパッケージ（後述）を置く構成が基本
colcon buildというビルドツールで、ワークスペース内の全パッケージをまとめてビルドする

パッケージ

ROS2における、機能をまとめる単位（1つ以上のノードやライブラリをまとめたもの）
各パッケージにはpackage.xml（パッケージの情報・依存関係を書くファイル）が必須
ビルド方式によって、以下のように使うファイルが変わる
- C++の場合: CMakeLists.txt
- Pythonの場合: setup.py（ament_pythonというビルドタイプを使う）

rosdepとワークスペースのsource

パッケージが依存する外部ライブラリを、OSのパッケージマネージャ経由で解決してくれるツールがrosdep
ワークスペースのsrc/以下にある全パッケージの依存関係を、rosdep install --from-paths src -y --ignore-srcのようなコマンドでまとめて解決できる
colcon buildでビルドした後は、source install/setup.bashを実行しないと、ビルドしたパッケージがシェルから認識されない
「ビルドは通ったのにros2 runできない」というのは、大抵このsourceのし忘れが原因で、初心者が最初につまずきやすいポイントの1つ

基本的なコマンド

ros2 run <パッケージ名> <実行ファイル名>: 1つのノードを実行する
ros2 launch <パッケージ名> <launchファイル名>: 複数のノードをまとめて起動する
ros2 node list: 今動いているノードの一覧を見る
ros2 topic list / ros2 topic echo <トピック名>: トピックの一覧確認・流れているデータの確認
ros2 service list: サービスの一覧を見る
rviz2: センサーの値やロボットの状態を可視化するツール
rqt: ノードの構成やログをGUIで確認できるツール

最小限のコード例（rclpy）

ここまでは概念の説明だったので、実際に最小限のノードを書いてみる
以下は、Pythonのクライアントライブラリrclpyを使った、シンプルなPublisherの例

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27


import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class MinimalPublisher(Node):
 def __init__(self):
 super().__init__('minimal_publisher')
 self.publisher_ = self.create_publisher(String, 'topic', 10)
 self.timer = self.create_timer(0.5, self.timer_callback)
 self.count = 0

 def timer_callback(self):
 msg = String()
 msg.data = f'Hello World: {self.count}'
 self.publisher_.publish(msg)
 self.get_logger().info(f'Publishing: "{msg.data}"')
 self.count += 1

def main(args=None):
 rclpy.init(args=args)
 node = MinimalPublisher()
 rclpy.spin(node)
 node.destroy_node()
 rclpy.shutdown()

if __name__ == '__main__':
 main()

ポイント
- Nodeを継承したクラスを作るのが基本形
- create_publisherでPublisherを作り、create_timerで一定間隔ごとにtimer_callbackを呼ぶ
- rclpy.spin(node)で、ノードを動かし続ける（この中で、後述のExecutorがコールバックを回している）
Subscriberの場合はcreate_subscriptionを使い、メッセージが届くたびに呼ばれるコールバック関数を登録する形になる

Executor

ノード内の複数のコールバック（タイマー・Subscriberなど）を、実際にどう実行するかを管理する仕組み
rclpy.spin(node)を呼ぶと、内部でデフォルトのExecutor（SingleThreadedExecutor）が動き、登録されたコールバックを順番に処理する
シングルスレッドのExecutorだと、1つのコールバックの処理が終わるまで次のコールバックは実行されない
- 時間のかかる処理を1つのコールバックに書いてしまうと、他のコールバック（他のトピックの受信など）がその間ブロックされてしまう
複数のコールバックを並行して処理したい場合は、MultiThreadedExecutorを使う
「トピックが来ているはずなのに反応が遅い/来ない」という不具合の原因が、実はExecutorのブロッキングだった、というのはよくあるハマりどころ

実行時の設定と制御

パラメータ（Parameters）

ノードに渡す設定値のこと。プログラムを書き換えずに、動作を外から調整できる仕組み
例えば「カメラのフレームレート」「しきい値」のような値を、パラメータとして持たせることが多い
コマンドでの確認・操作
- ros2 param list: あるノードが持つパラメータの一覧を見る
- ros2 param get <ノード名> <パラメータ名>: 値を取得する
- ros2 param set <ノード名> <パラメータ名> <値>: 値を変更する
実際の運用では、パラメータを1つずつsetするのではなく、後述のLaunchファイルからYAMLファイルでまとめて渡すのが一般的

Namespace・Remapping

Namespaceは、ノード名やトピック名の前につける「名前空間」で、同じパッケージ・同じノードを複数のインスタンスとして区別して動かすときに使う
- 例: robot1/cmd_vel、robot2/cmd_velのように、ロボットごとに名前空間を分ける
Remappingは、ノードが元々使っているトピック名・サービス名などを、実行時に別の名前に付け替える仕組み
- 例: ros2 run pkg node --ros-args -r cmd_vel:=robot1/cmd_vel
どちらも、コードを一切変更せずに、起動時のオプションやLaunchファイルの引数だけで名前を切り替えられるのがポイント
複数台のロボットを同時に動かす場合や、他人が作ったパッケージをそのまま自分のシステムに組み込む場合に、ほぼ必須になる知識

Launchファイル

複数のノードを、パラメータ付きでまとめて起動するための設定ファイル
Python・XML・YAMLのいずれかで書ける（最近はPythonで書くことが多い）
実際のロボットは複数のノードが同時に動いて成り立っているため、1つずつros2 runするのではなく、launchファイルでまとめて立ち上げるのが基本的な運用になる

ライフサイクルノード（Managed Nodes）

通常のノードは起動したらすぐ全機能が有効になるが、ライフサイクルノードは明示的な状態管理を持つ主な状態は以下の通り。

状態	内容
Unconfigured	起動直後、まだ設定されていない状態
Inactive	設定は終わっているが、まだ動いていない状態
Active	実際に処理を行っている状態
Finalized	終了処理が終わった状態

状態遷移は、configure・activate・deactivate・cleanup・shutdownといったサービス呼び出しで制御する
Nav2は、内部の各サーバー（Planner、Controllerなど）がこのライフサイクルノードとして作られていて、lifecycle_managerというノードが、それらの状態をまとめて管理・遷移させている
Nav2起動時に「なぜか動かない」場合、実はライフサイクルがActiveまで遷移していないだけ、ということもよくある

ロボット特有の周辺技術

rosbag2

トピックに流れているデータを記録・再生するツール（ROS1のrosbagのROS2版）
実機やシミュレーションで流れたデータをファイルに記録しておき、後から同じデータを何度でも再生できる
実機を毎回動かさなくても、記録済みのデータでノードの開発・デバッグを進められるのが大きな利点
主なコマンド
- ros2 bag record -a: 全トピックを記録する
- ros2 bag record <topic1> <topic2>: 指定したトピックだけ記録する
- ros2 bag play <bagファイル>: 記録したデータを再生する
- ros2 bag info <bagファイル>: 記録内容（トピック・件数・期間など）を確認する
保存形式はsqlite3かmcapが使われる

URDF（Unified Robot Description Format）

ロボットの物理的な構造（リンクとジョイント）を記述するXML形式
リンク（Link）は剛体の部品（腕の一部、車体など）、ジョイント（Joint）はリンク同士をつなぐ関節（回転・スライドなど）を表す
各リンクの見た目（形状・色）や、衝突判定用の形状、質量・慣性などの物理情報もここに書ける
robot_state_publisherというノードが、URDFとジョイントの角度情報から、後述のTF2の座標変換を自動的に計算・配信してくれる
RVizでロボットの3Dモデルを表示したり、Gazeboでシミュレーションしたりする際の元データにもなる

TF2（座標変換）

ロボットに関わる複数の座標系（フレーム）の関係を、時系列で管理する仕組み
例えば「地図（map）」「ロボット本体（base_link）」「カメラ（camera_link）」は、それぞれ別の座標系を持っていて、TF2はこれらの間の相対位置・姿勢を管理する
各フレームは木構造（ツリー）でつながっていて、map -> odom -> base_link -> camera_linkのように親子関係を辿ることで、任意の2点間の座標変換を計算できる
ros2 run rqt_tf_tree rqt_tf_treeで、今のフレーム構成をツリー図として確認できる
Nav2のようなナビゲーションスタックは、TF2が正しく配信されていることが前提になっているため、ロボット開発で最初につまずきやすいポイントの1つ

SLAM（Simultaneous Localization and Mapping）

自己位置推定（Localization）と地図作成（Mapping）を同時に行う技術
ロボットが未知の環境を動き回りながら、センサー情報をもとに「今どこにいるか」と「周囲の地図」を同時に構築していく
前述のNav2は自己位置推定を行わない（AMCLなどと組み合わせる前提）ため、Nav2を使うには、事前にSLAMで地図を作っておくか、SLAMをリアルタイムで動かしながらナビゲーションする、という使い方になる
ROS2では、2D LiDARを使ったSLAMの標準的な実装としてslam_toolboxというパッケージがよく使われる

Visual SLAM

LiDARなどの距離センサーではなく、カメラ映像を使って自己位置推定・地図作成を行うSLAM
画像内の特徴点（際立った点）をフレーム間で追跡し、その動きからカメラ（ロボット）自身の移動量と、周囲の3次元構造を推定する
LiDARより安価なセンサー（カメラ）で実現できる一方、照明条件の変化や、特徴点の少ない環境（真っ白な壁など）に弱いという弱点がある
代表的な実装として、ORB-SLAMやRTAB-Map（ROS2にも対応）などがある

cuVSLAM

NVIDIAが開発した、GPU（CUDA）で高速化されたVisual SLAMの実装
Isaac ROS（NVIDIAが提供するROS2向けのGPUアクセラレーションパッケージ群）の一部として提供されている
ステレオカメラの映像を使い、GPU上で並列処理することで、CPUベースのVisual SLAMより高速・低遅延に自己位置推定を行える
Jetsonシリーズのようなエッジデバイス上でのリアルタイム動作を意識して設計されている

Nav2（Navigation2）

ROS2上で自律移動（ナビゲーション）を実現するための標準的なソフトウェアスタック
地図上で目的地を指定すると、経路計画と障害物回避をしながら自律的に移動できるようにする主な構成要素は以下の通り。

構成要素	役割
Global Planner	地図全体を使って、大まかな経路を計画する
Controller（Local Planner）	直近の障害物やロボットの動特性を考慮して、実際の速度指令（前述の`cmd_vel`）を計算する
Costmap	障害物や走行コストを表現する地図。全体用のGlobal Costmapと、直近用のLocal Costmapの2種類がある
Behavior Tree	経路計画→移動→リカバリー動作といった、Nav2全体の動作フローを制御する仕組み
Recovery Behaviors	経路が見つからない・スタックしたときに行う復帰動作（その場回転、後退など）

Nav2自体は「自分が今どこにいるか」は推定しないため、AMCLなどの自己位置推定と組み合わせて使うのが前提
launchファイルで各構成要素をまとめて立ち上げ、パラメータYAMLで細かい挙動をチューニングするのが基本的な使い方

順運動学（FK）と逆運動学（IK）

Nav2・SLAMが移動ロボット向けの話だったのに対し、ここからはロボットアーム（マニピュレータ）向けの基礎知識
順運動学（Forward Kinematics, FK）: 各関節の角度が分かっているときに、アームの先端（エンドエフェクタ）がどの位置・姿勢にあるかを計算すること
- 「関節の角度 → 先端の位置」という向きの計算で、URDFのリンク・ジョイント情報から比較的簡単に求まる
逆運動学（Inverse Kinematics, IK）: 逆に、アームの先端を「この位置・姿勢にしたい」と決めたときに、そのために必要な各関節の角度を計算すること
- 「先端の位置 → 関節の角度」という向きの計算で、同じ先端位置に対して関節角度の組み合わせが複数存在したり、そもそも解が存在しない（届かない位置）こともあり、FKより計算が難しい

DHパラメータとリンク長

FKを計算する際、隣り合う関節同士の座標系の関係を統一的に表現するための方法が、Denavit-Hartenberg（DH）パラメータ
以下4つのパラメータの組み合わせで、隣り合う座標系同士の変換を表現する
- リンク長$a_i$: $x_i$軸に沿って測った、$z_i$軸から$z_{i+1}$軸までの距離。そのリンク（関節と関節の間の剛体部分）の物理的な長さにあたる
- リンクのねじれ角$\alpha_i$: $x_i$軸まわりに測った、$z_i$軸から$z_{i+1}$軸までの角度
- リンクオフセット$d_i$: $z_i$軸に沿って測った、$x_{i-1}$軸から$x_i$軸までの距離
- 関節角$\theta_i$: $z_i$軸まわりに測った、$x_{i-1}$軸から$x_i$軸までの角度
回転関節の場合、可変なのは関節角$\theta_i$だけで、リンク長$a_i$を含む残り3つはロボットの機械構造によって決まる定数になる

PID制御とPD制御

関節や車輪を目標値（角度・速度・位置など）に追従させるための、古典的で最も広く使われるフィードバック制御手法
目標値と現在値の差（誤差）$e(t)$を使って、以下の3項を組み合わせて制御量$u(t)$を計算する

$$ u(t) = K_p e(t) + K_i \int_0^t e(\tau)\,d\tau + K_d \frac{de(t)}{dt} $$

比例項（P, Proportional）: 誤差の大きさに比例した制御量。誤差が大きいほど強く補正する
積分項（I, Integral）: 誤差の累積に比例した制御量。比例項だけでは埋まらずにじわじわ残る誤差（定常偏差）を解消する
微分項（D, Derivative）: 誤差の変化率に比例した制御量。急激な変化を抑え、目標値を行き過ぎるオーバーシュートを防ぐ
PID制御: 上記3項全てを使う、最も一般的な構成
PD制御: 積分項を使わず比例項と微分項だけを使う構成。積分項による巻き込み（積分ワインドアップ）を避けたい場合や、定常偏差が問題になりにくい速度制御などで使われる
ROS2では、ros2_controlフレームワークのcontrol_toolboxパッケージにPID制御の実装があり、ros2_controllersのpid_controllerから直接利用できる

クランプ（Clamp）

値を決められた上限・下限の範囲内に収まるように制限する処理のこと
制御出力$u(t)$が、モーターの実際の駆動範囲を超えないように上限・下限で切り詰める、といった使い方をする
積分ワインドアップへの対策の1つに、クランプ方式のアンチワインドアップ（Clamping Anti-Windup）がある
- 出力が上限・下限に達している間は、積分項の蓄積を止める（またはその値自体を制限する）ことで、誤差が解消された後に出力が行き過ぎるのを防ぐ

Command InterfaceとState Interface（ActVel）

PID制御が誤差$e(t)$を計算するには、目標値だけでなく「今どうなっているか」という実測値が必要になる
ros2_controlでは、コントローラーからハードウェアへの指令をCommand Interface、ハードウェアからコントローラーへのフィードバックをState Interfaceとして抽象化している
代表的な種類にはHW_IF_POSITION（位置）・HW_IF_VELOCITY（速度）・HW_IF_EFFORT（トルク・力）があり、同じ関節でもCommand用とState用で別々のインターフェースとして扱われる
速度のState Interface（実測速度）は、しばしば「ActVel（Actual Velocity）」と呼ばれる。モータードライバの標準規格であるCiA402（CANopenのドライブ用プロファイル）でも、指令速度とは別に「Velocity actual value」というオブジェクトが定義されており、ActVelという略称はここに由来することが多い
前述のcmd_velが、ナビゲーションスタックなど上位からロボット全体への速度指令であるのに対し、ActVelは個々の関節・モーターレベルでの実測値であり、抽象化のレイヤーが異なる点に注意

MoveIt

ROS2上でロボットアームの動作計画（モーションプランニング）を行うための標準的なソフトウェアスタック
Nav2が移動ロボット向けの標準スタックなのに対し、MoveItはアーム向けの標準スタックという位置づけ
前述のIKを解いて目標姿勢までの関節角度を求めつつ、障害物を避けながらの経路計画や衝突検知も行う
URDFで定義したロボットの構造をそのまま利用でき、Gazeboのようなシミュレータ上での動作確認にも対応している

Gazebo（シミュレータ）

ロボットの物理シミュレーションを行うためのソフトウェア
前述のURDFで記述したロボットモデルを仮想空間に配置し、重力・摩擦・衝突などを考慮した動きをシミュレートできる
実機がまだない、あるいは実機を壊すリスクがある動作を試したいときに、ROS2のノードをそのまま実機と同じように動かして検証できる
カメラ・LiDARなどのセンサーのシミュレーションデータも出力できるので、Nav2のようなセンサー入力が前提のスタックのテストにもよく使われる
最近は従来のGazebo Classicから、後継のGazebo（旧Ignition Gazebo）への移行が進んでいる

まとめ

ROS2は、ロボットの各機能をノードという単位に分けて、通信でつなげて動かすためのフレームワーク
インストールは、その時点の最新LTSディストリビューションを、対応するUbuntuのバージョンに入れるのが基本
通信の基盤はDDSで、ROS1にあった中央サーバー（単一障害点）がない
ノード間の通信は、Topic（非同期の流し込み）・Service（同期的な問い合わせ）・Action（進捗報告つきの長時間処理）の3種類を目的に応じて使い分ける。QoSはその通信の品質を細かく設定する仕組み
実際のコードは、Nodeを継承したクラスにPublisher/Subscriberやコールバックを持たせる形で書き、rclpy.spinが内部のExecutorでコールバックを回す
開発は、ワークスペースの中にパッケージを作り、rosdepで依存関係を解決してcolconでビルドし、setup.bashをsourceして使う、という流れが基本
動作の調整はパラメータで行い、Namespace・Remappingで名前を実行時に切り替えられる。複数ノードとパラメータをまとめて起動するのがLaunchファイルの役割
rosbag2でデータを記録・再生できるので、実機を毎回動かさなくても開発・デバッグを進められる
URDFでロボットの構造を記述し、そこからTF2が座標変換を計算・配信する。この2つはNav2などの前提になる基礎的な仕組み
SLAM（Visual SLAM・cuVSLAMも含む）で自己位置推定と地図作成を行い、その地図をもとにNav2が自律移動する、という役割分担になっている
速度指令はcmd_velという標準的なトピック名でやり取りされ、ライフサイクルノードで状態管理されたNav2が、そのcmd_velを出す自律移動用の標準スタック
ロボットアームの場合は、関節角度から先端位置を求めるFK、先端位置から関節角度を求めるIKが基本になり、MoveItがそれを担うNav2の対になる標準スタック
Gazeboを使えば、実機なしでもロボットのシミュレーションができる