富岡が下ノ村研究室で開発を物を集約したリポジトリです。
本研究開発は、乗馬施設等における馬房清掃作業の自動化を目的とする。馬房清掃は作業者にとって大きな身体的負担となっており、本研究では自律移動ロボットによる清掃作業の代替を目指す。本ドキュメントは、その実現に向けたシステム設計、技術的アプローチ、および開発計画を定義する。
馬房の衛生状態は馬の健康に直結する。従来の人手による清掃は、フォークでボロ（馬糞）と寝藁をすくい上げ、篩い分ける作業であり、非常に重労働である。本研究では、この作業の中でも特に「ボロの発見とマッピング」および「自律的な移動」に焦点を当てる。
最終目標は、「人間による馬房清掃タスクを代替可能な自律移動ロボットのプロトタイプを開発し、その有効性を実証すること」である。
-
自律移動: LiDARによるSLAMと自己位置推定に基づき、馬房内を自律的に移動する。
-
ボロ認識: YOLOモデルを用い、カメラ画像からボロをリアルタイムに検出する。
-
ボロ位置マッピング: 認識したボロの位置を地図上に記録し、「ボロ分布マップ」を生成する。
-
Sim-to-Real: シミュレーションで開発したソフトウェアを実機実装し、実環境との差異（リアリティギャップ）と解決策を検証する。
- OS: Ubuntu 24.04 LTS
- Middleware: ROS 2 Jazzy Jalisco
- Simulator: Gazebo Harmonic
- Robot PC: Raspberry Pi 5
- PC: Thinkpad P14s Gen5
.
├── documents
│ └── ThesisB ← 卒論
├── pre-project-2025 ← プレプロジェクト
├── README.MD
└── stable_cleaning_robot
├── archive
├── interval_camera ← 定期撮影ユーティリティ
├── robot_slam_nav
│ ├── eval_outputs ← 評価ログや rosbag 出力
│ ├── rione_map.yaml / *.pgm / *.data / *.posegraph ← 保存した地図
│ └── src
│ ├── robot_bringup ← 実機 bringup の中核
│ ├── cugo_ros2_control2_ ← 車体制御と URDF
│ ├── ldlidar_stl_ros2 ← LiDAR ドライバ
│ ├── waypoint_generator ← 被覆走行と評価記録
│ └── cugo_ros_simulations ← シミュレーション系
├── stable_robot_mecha ← メカ設計データ
└── turtlebot3_gazebo_sim ← Gazebo ベースの別系統シミュレータ
このリポジトリで実際に馬房環境の走行実験に使っていた主系統のソフトウェアは stable_cleaning_robot/robot_slam_nav である。説明の基準は documents/ThesisB の第3章・第4章に置きつつ、ここでは現行の launch / package / 設定ファイルに対応する形でシステム構成を整理する。
本システムは単一ノードで完結する構成ではない。ロボット側 Raspberry Pi と、SLAM / AMCL / Nav2 を動かす PC を Wi-Fi で接続した分散 ROS 2 システムとして運用し、さらに PC 側でも地図作成・自律走行・評価記録を複数のターミナルに分けて起動する。論文では両ホストとも Ubuntu 24.04 + ROS 2 Jazzy を用い、同一ネットワーク上で Fast DDS と NTP を前提にしている。つまり、robot_slam_nav は「単一ノードを立ち上げれば動くシステム」ではなく、「複数ホスト・複数ノード・複数ターミナルを前提にした実験系」である。
flowchart LR
subgraph Robot["ロボット側 PC (Raspberry Pi)"]
LD["ldlidar_stl_ros2<br/>LD19 driver<br/>publishes /scan"]
TF["static_transform_publisher<br/>base_footprint -> base_laser"]
CG["cugo_ros2_control2<br/>subscribes /cmd_vel<br/>publishes /odom"]
RSP1["robot_state_publisher<br/>URDF / TF"]
PICO["Raspberry Pi Pico<br/>motor driver + encoder"]
CG <-- serial --> PICO
end
subgraph PCMap["SLAMサーバー用 PC: 地図作成フェーズ"]
SLAM["pc_slam.launch.py<br/>slam_toolbox<br/>lifecycle_manager<br/>RViz2"]
end
subgraph PCNav["SLAMサーバー用 PC: 自律走行フェーズ"]
NAV["pc_nav.launch.py<br/>map_server + amcl + Nav2 + RViz2"]
WP["waypoint_generator<br/>coverage_path_planner"]
EVAL["evaluation_recorder<br/>rosbag + metrics"]
end
LD -- "/scan" --> SLAM
CG -- "/odom" --> SLAM
TF --> SLAM
RSP1 --> SLAM
LD -- "/scan" --> NAV
CG -- "/odom" --> NAV
TF --> NAV
RSP1 --> NAV
NAV -- "/cmd_vel" --> CG
WP -- "/coverage_waypoints" --> NAV
WP -- "FollowWaypoints action" --> NAV
NAV -- "/amcl_pose, /tf" --> EVAL
CG -- "/odom" --> EVAL
WP -- "/coverage_waypoints" --> EVAL
-
地図作成フェーズでは
pc_slam.launch.pyを使い、slam_toolboxで地図を生成する。 -
自律走行フェーズでは
pc_nav.launch.pyに切り替え、保存済み地図をmap_serverから配信し、amclと Nav2 で走行する。 -
ロボット側はセンサ入力と低レイヤ制御を担当し、PC 側は高レイヤの自己位置推定・経路計画・可視化を担当する。
-
論文では Robot1 / Robot2 の2台を使い分けているが、ソフトウェア上は
/scan、/odom、/cmd_vel、TF を共通インタフェースとして扱う。
実験時に必要だった構成を要約すると次の通りである。
-
ロボット側では
raspi_bringup.launch.pyにより、LiDAR ドライバ、静的 TF、オドメトリ配信、車体制御を同時に起動する。ロボット側にはディスプレイは乗っていないので、手元のPCからSSHでラズベリーパイに接続し、手元のターミナルからノードを起動する。このときSSHするためにはWiFiルータ等でロボットとPCを接続しなければならない。 -
PC 側では、地図作成時は
pc_slam.launch.py、自律走行時はpc_nav.launch.pyを使い分ける。 -
自律走行時には Nav2 本体とは別に
waypoint_generatorを起動し、/coverage_waypointsの生成と/follow_waypointsアクション送信を行う。 -
評価時はさらに
evaluation_recorder.launch.pyを別ターミナルで起動し、ros2 bag recordと評価ノードを同時に走らせる。実際に移動したパスなどが書き込まれた画像が生成されたりする。
したがって、最低限でも以下のような端末分担が必要になる。
sequenceDiagram
participant R as Robot terminal
participant P1 as PC terminal 1
participant P2 as PC terminal 2
participant P3 as PC terminal 3
Note over R,P1: 地図作成フェーズ
R->>R: ros2 launch robot_bringup raspi_bringup.launch.py
P1->>P1: ros2 launch robot_bringup pc_slam.launch.py
Note over P1: 地図を保存したら pc_slam を終了
Note over R,P3: 自律走行フェーズ
R->>R: ros2 launch robot_bringup raspi_bringup.launch.py
P1->>P1: ros2 launch robot_bringup pc_nav.launch.py map:=...
P2->>P2: ros2 launch waypoint_generator coverage_path_planner.launch.py
P3->>P3: ros2 launch waypoint_generator evaluation_recorder.launch.py
P2->>P1: /follow_waypoints を送信
P1->>R: /cmd_vel
R-->>P1: /scan, /odom
重要:
pc_slam.launch.pyとpc_nav.launch.pyは同じ役割ではない。前者は地図生成用、後者は保存済み地図を使った自己位置推定・ナビゲーション用であり、実験中はフェーズに応じて切り替える。
| 名前 | 主な発行元 | 用途 |
|---|---|---|
/scan |
ldlidar_stl_ros2 |
slam_toolbox、amcl、Nav2 costmap の入力 |
/odom |
cugo_ros2_control2 |
slam_toolbox、amcl、Nav2 の運動予測入力 |
/cmd_vel |
Nav2 controller | ロボット側の速度指令 |
map -> odom TF |
slam_toolbox または amcl |
地図座標とオドメトリ座標の整合 |
odom -> base_footprint TF |
cugo_ros2_control2 |
ロボット姿勢の基準 |
base_footprint -> base_laser TF |
static_transform_publisher |
LiDAR の取り付け位置・姿勢 |
/coverage_waypoints |
waypoint_generator |
被覆走行用の waypoint 列 |
/follow_waypoints |
waypoint_generator から Nav2 action server へ送信 |
Nav2 waypoint follower による逐次追従 |
この topic / TF 構成により、ロボット側の /scan と /odom を PC 側へ送り、PC 側で map -> odom を推定し、その結果として生成された /cmd_vel を再びロボット側へ返す閉ループを構成している。
robot_slam_nav/src 以下の主なパッケージは次の通りである。
| パッケージ | 主な役割 | 主な launch / node | 主な参照先・依存先 |
|---|---|---|---|
robot_bringup |
実機 bringup 全体の入口 | raspi_bringup.launch.py、pc_slam.launch.py、pc_nav.launch.py |
ldlidar_stl_ros2、cugo_ros2_control2、slam_toolbox、nav2_*、rviz2、robot_state_publisher |
cugo_ros2_control2 |
車体制御とオドメトリ配信 | cugo_ros2_control2 node |
Raspberry Pi Pico、robot_state_publisher、TF |
ldlidar_stl_ros2 |
LD19 LiDAR ドライバ | ldlidar_stl_ros2_node |
/scan を上位へ提供 |
waypoint_generator |
ジグザグ waypoint 生成、Nav2 への送信、評価記録 | coverage_path_planner、evaluation_recorder |
nav2_msgs、/follow_waypoints action、/coverage_waypoints |
cugo_ros_simulations |
Gazebo / シミュレーション系 | simulation launch 群 | 実験本番ではなくシミュレーション検証用 |
パッケージ間の関係を大づかみに描くと次のようになる。
flowchart TB
RB["robot_bringup<br/>launch / config / maps / rviz"] --> LD["ldlidar_stl_ros2"]
RB --> CG["cugo_ros2_control2"]
RB --> SL["slam_toolbox"]
RB --> N2["Nav2 stack<br/>map_server / amcl / planner / controller / bt_navigator / waypoint_follower / velocity_smoother"]
RB --> RV["rviz2 / robot_state_publisher / xacro"]
WG["waypoint_generator"] --> N2
WG --> RV
SIM["cugo_ros_simulations"] -. simulation only .-> N2
ここで重要なのは、robot_bringup と waypoint_generator は一体化した単一パッケージではなく、別パッケージとして協調動作している点である。robot_bringup がロボット・SLAM・Nav2 の基盤を立ち上げ、waypoint_generator がその上に被覆走行タスクを載せる。
| ファイル | 役割 |
|---|---|
robot_bringup/config/robot_config.yaml |
ロボット側の LiDAR、シリアル、TF、URDF の設定 |
robot_bringup/config/slam_real_robot.yaml |
slam_toolbox の mapping 用パラメータ |
robot_bringup/config/nav2_real_robot.yaml |
map_server、amcl、Nav2 controller / planner / costmap の設定 |
waypoint_generator/config/coverage_params.yaml |
被覆走行領域、path pitch、wall margin、評価出力先の設定 |
論文では Robot1 と Robot2 で LiDAR 搭載高さや機体寸法が異なるため、URDF や base_footprint -> base_laser の値を機体ごとに切り替える前提になっている。したがって、robot_config.yaml と URDF は「固定値」ではなく、実験対象の機体に合わせて調整する設定点として扱う必要がある。
地図ファイルの扱いも重要である。pc_nav.launch.py や coverage_visualizer.launch.py に渡すのは json ではなく *.yaml であり、この YAML が対応する *.pgm を参照する。保存結果として *.data や *.posegraph が残る場合もあるが、Nav2 の map_server に直接渡すのは *.yaml である。
waypoint の設定については、現状の実装では waypoint_generator/config/coverage_params.yaml が基準ファイルである。coverage_path_planner.launch.py は params_file を受けられる一方、coverage_visualizer.launch.py は waypoint 用の params_file を公開しておらず、package 内の coverage_params.yaml を固定で読む。
本システムの運用は、まず SLAM モードで地図を作成し、その後に Navigation モードへ切り替える二段階構成である。AMCL は事前地図がないと自己位置推定できないため、最初に pc_slam.launch.py で地図を作る必要がある。
地図作成フェーズでは次のように起動する。
# Terminal 1: ロボット側 Raspberry Pi
ros2 launch robot_bringup raspi_bringup.launch.py
# Terminal 2: SLAMサーバー用PC
ros2 launch robot_bringup pc_slam.launch.pyこの状態では pc_slam.launch.py が slam_toolbox を動かし、ロボットから受け取った /scan と /odom を使って地図を構築する。RViz2 上で地図が十分に取れるまでロボットを動かして環境をスキャンする。
地図が取れたら、RViz2 上の slam_toolbox plugin あるいは対応する保存操作で地図を保存する。ここで次の点だけ押さえておけばよい。
-
自律走行フェーズで指定するのは
map_name.yamlである。 -
map_name.yamlの中でmap_name.pgmが参照される。 -
map_name.dataやmap_name.posegraphはslam_toolboxのシリアライズ情報であり、保持しておくと便利だが、pc_nav.launch.pyのmap:=...に直接渡す対象ではない。
地図作成が終わったら pc_slam.launch.py は終了し、保存済み地図を指定して pc_nav.launch.py を起動する。
# Terminal 1: ロボット側 Raspberry Pi
ros2 launch robot_bringup raspi_bringup.launch.py
# Terminal 2: SLAMサーバー用PC
ros2 launch robot_bringup pc_nav.launch.py \
map:=/absolute/path/to/map.yaml※もちろん/absolute/path/to/map.yamlは例なので、自分が保存した場所を絶対パスで指定するなどしてください。
ここで pc_nav.launch.py は内部で map_server の yaml_filename にこの引数を渡している。そのため、引数として必要なのは *.yaml である。これにより、AMCL による自己位置推定と Nav2 による経路追従が可能になる。
waypoint の確認には2通りの使い方がある。
1つ目は、pc_nav.launch.py がすでに map と RViz2 を立ち上げている前提で、coverage_path_planner.launch.py だけを追加起動する方法である。第三者が再現するときは、まずこの経路を使うのが安全である。
# Terminal 3: SLAMサーバー用PC
ros2 launch waypoint_generator coverage_path_planner.launch.py \
use_params_file:=true \
params_file:=/path/to/coverage_params.yamlこの場合、地図表示は pc_nav.launch.py 側の RViz2 を使い、coverage_params.yaml で waypoint の領域や pitch を決める。coverage_path_planner は /coverage_waypoints を publish する。
現在の実装では、coverage_path_planner.launch.py は次の2通りで使える。
-
use_params_file:=trueのとき:params_fileで指定した YAML を読む。デフォルトでは package 内のcoverage_params.yamlを使う。 -
use_params_file:=falseのとき:area_width、area_height、path_pitch、origin_x、origin_y、origin_theta、wall_margin、show_filled_areaを launch 引数で直接渡す。
なお、coverage_params.yaml にある use_robot_pose_as_origin は現行の coverage_path_planner_node.py では経路生成に使われていない。そのため、実際に効く設定としては origin_x、origin_y、origin_theta を使う前提で運用した方が混乱が少ない。
2つ目は、waypoint 設計だけを独立して見たいときに coverage_visualizer.launch.py を使う方法である。
ros2 launch waypoint_generator coverage_visualizer.launch.py \
map_file:=/absolute/path/to/map.yamlこちらは launch 引数 map_file:=... で *.yaml を読み込ませ、専用の map_server と RViz2 を起動する。そのため、waypoint を事前に決める用途には分かりやすい。
ただし、現在の coverage_visualizer.launch.py は waypoint 側の params_file を launch 引数として受けていない。つまり、waypoint の大きさ、pitch、origin などは package 内の coverage_params.yaml を固定で読む。また、map_file のデフォルト値は開発者のホームディレクトリを指しているため、他の人が使うときは map_file:=/absolute/path/to/map.yaml を必ず明示した方がよい。
このため、第三者が確実に再現する用途では、coverage_visualizer.launch.py を主経路にせず、pc_nav.launch.py と coverage_path_planner.launch.py を組み合わせる運用を推奨する。
waypoint の処理は2段階である。
-
coverage_path_plannerが/coverage_waypointsを publish する。 -
/start_coverage_navigationサービスを呼ぶと、その waypoint 列がFollowWaypointsaction として Nav2 に送られる。
実際の開始操作は次のコマンドで行う。
ros2 service call /start_coverage_navigation std_srvs/srv/Triggerこれにより Nav2 の waypoint_follower が waypoint を順次巡回し始める。巡回中は、pc_nav.launch.py で動いている AMCL が読み込んだ地図に対して自己位置推定を行い、その推定結果を使って Nav2 controller が /cmd_vel を出し、ロボット側の cugo_ros2_control2 が実機を駆動する。
# Terminal 4: SLAMサーバー用PC（任意）
ros2 launch waypoint_generator evaluation_recorder.launch.pyこの launch は ros2 bag record と評価ノードを同時に起動し、/coverage_waypoints、/amcl_pose、/cmd_vel、/odom、/tf などを記録する。
実験で最も典型的なのは次の流れである。
raspi_bringup.launch.pyとpc_slam.launch.pyで地図を作る。- 地図を保存し、
map_name.yamlを得る。 pc_nav.launch.py map:=/absolute/path/to/map_name.yamlで Navigation モードに切り替える。- まずは
coverage_path_planner.launch.pyを使って waypoint を確認する。独立可視化が必要なときだけcoverage_visualizer.launch.py map_file:=...を使う。 /start_coverage_navigationを呼んで Nav2 に巡回させる。- 必要なら
evaluation_recorder.launch.pyで評価ログを残す。
-
実験系の中心は
stable_cleaning_robot/robot_slam_navである。 -
ロボット側と SLAMサーバー用 PC 側で役割を分離した分散 ROS 2 構成である。
-
地図作成と自律走行で launch ファイルが異なる。
-
AMCL で自己位置推定するため、最初に
pc_slam.launch.pyで地図を作る必要がある。 -
pc_nav.launch.pyとcoverage_visualizer.launch.pyに渡す地図は*.yamlである。 -
自律走行時は Nav2 だけでなく
waypoint_generatorも別プロセスで必要になる。 -
coverage_path_planner.launch.pyはparams_fileまたは launch 引数で waypoint パラメータを指定できる。 -
coverage_visualizer.launch.pyはmap_fileは指定できるが、waypoint パラメータは現在 package 内のcoverage_params.yaml固定である。 -
waypoint は「生成して publish する」段階と、「Nav2 へ送って巡回を開始する」段階が分かれている。
-
評価時は
evaluation_recorderにより rosbag 記録と指標算出を追加する。 -
したがって、本システムは単一ノード起動では再現できない。