【自作ロボットROS2化】Nav2で自律走行を試す
どうもこんにちは、
前回の記事では、slam_toolboxを使って、地図を生成することができました。
今回はその地図を使って、Nav2で自律走行させてみたので備忘録としてまとめておきます。
これまでのROS2化の作業は以下ブログを参照ください。
【自作ロボットROS2化】Raspberry Pi5+ROS2 JazzyでRplidarを動かす
【自作ロボットROS2化】Raspberry Pi5(Ubuntu24.04)+ROS2 JazzyでUART通信を動かす
【自作ロボットROS2化】ROS2 slam_toolboxを試す
目次
Nav2とは、与えられた地図に対してセンサー等で自己推定を行い、目標位置へ移動する機能を提供するパッケージです。
ROS1ではナビゲーションスタックと呼ばれていたと思います。
以下がNav2のドキュメントページです。
Nav2
Nav2の構成は以下のようになっています。
上記に出てくる各Serverについて調べました。
| Nav2 モジュール | 概要 |
|---|---|
| BT Navigator Server | Nav2の中核となるモジュール。BT(Behavior Tree)を用いてナビゲーションプロセスを制御する。 |
| Planner Server | ゴール位置に到達するまでにグローバル経路（Global Path)を計画する。 地図データやコストマップを用いて、スタートからゴールまでの最適な経路を生成。 |
| Controller Server | Planner Serverが生成した経路に従い、パス追従（Path Following)制御を行う。ここで実際の速度指令値が計算され通知される。 |
| Behavior Server | 特定の状況に応じた動作（リカバリ等）を管理する。 ロボットが予期しない状況になったときに最適な行動を実行する。 |
| Smoother Server | 生成されたパスを平準化して、コントローラーが追従するための滑らかなパスを作成する。 |
| Route Server | 調査中・・・ |
かなりざっくりしたイメージとしては、BT Navigatorが全体の核となるモジュールでこれが目標位置を受けると、Planner Serverでゴールまでのグローバル経路を生成し、なるべくその経路に追従するようにController Serverが制御していくような感じだと思います。（おそらく・・）
その他のサーバーがどのように絡んでるのかは詳細がまだわかりませんが、また色々調査しながら随時まとめていきます。
sudo apt install ros-jazzy-navigation2
sudo apt install ros-jazzy-nav2-bringup
navigation2とnav2-bringupという２つのパッケージをインストールしました。
bringupというのは、navigation2に必要なノードや設定をまとめて起動するためのパッケージのようです。
以下の手順でNav2を動かしてみました。
sllidarノードを起動
ros2 launch sllidar_ros2 sllidar_a1_launch.py
モーター駆動用ノード、オドメトリ発行ノードを動かす
モーター駆動用のノードと、ホイールオドメトリ用のノードを起動します。（自作のノードです）
ご自身のロボットのオドメトリを通知するノードに置き換えて起動してください。
※まだ自作のノードは公開できてませんが、そのうち整理して公開しようとは思っています。
ros2 run inobo_dcmotor motor_node
ros2 run inobo_odom odom_publisher
laserフレームのTFを出力
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_footprint laser
マップは以前slam_toolboxで作った地図を使用します。また、nav2用のパラメータファイルを指定します。
ros2 launch nav2_bringup bringup_launch.py params_file:=/home/ubuntu/ros2_ws/src/nav2_params.yaml map:=/home/ubuntu/ros2_ws/map/test_map2.yaml
RVIZ2で、「2D Pose Estimate」で初期姿勢を合わせると、マップとロボットの位置が表示されました。
RVIZ2の「2D Goal Pose」でゴールを指定してみます。
経路が生成され、自律走行が動きました。
パラメータ設定
まだまだパラメータ調整ができていないため、簡単な経路しか成功しません。
とりあえず、今回重要そうと思ったパラメータをメモしておきます。
まずは、amcl関係から。
最初自己位置の精度が悪くいろいろ試行錯誤しましたが、とりあえずlase_model_typeを”likelihood_field_prob”にして、do_beamskipをtrueにしたら少し改善しました。
おそらく、beamモデルのほうが計算負荷が高いと思われます。
amcl:
ros__parameters:
alpha1: 0.2
alpha2: 0.2
alpha3: 0.2
alpha4: 0.2
alpha5: 0.2
base_frame_id: "base_footprint"
beam_skip_distance: 0.5
beam_skip_error_threshold: 0.9
beam_skip_threshold: 0.3
do_beamskip: true #false change inomata
global_frame_id: "map"
lambda_short: 0.1
laser_likelihood_max_dist: 2.0
laser_max_range: 13.0 #100.0 change inomata
laser_min_range: -1.0
laser_model_type: "likelihood_field_prob" #"beam" #"likelihood_field" change inomata
max_beams: 60
次は、Nav関連のパラメータ。
Controller ServerのFollowPathプラグインは、
RegulatedPurePursuitControllerを使用。
lookahead_distを少し短くして、細かい経路追従ができるように調整しました。
1. ルックアヘッド距離が長い場合
効果:
直線的でスムーズな動作が可能になります。
多少のパスの曲がりを無視して、大まかな追従が可能です。
デメリット:
急なカーブで経路から外れやすくなります。
障害物回避が難しくなることがあります。
2. ルックアヘッド距離が短い場合
効果:
細かい経路追従が可能になります。
急カーブや狭い場所でも正確に追従できます。
デメリット:
動作がギクシャクする可能性があります。
ロボットの進行が不安定になりやすく、滑らかさが失われます。
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
desired_linear_vel: 0.4 #0.5 cahnge inomata
lookahead_dist: 0.4 #0.6 change inomata
min_lookahead_dist: 0.2 #0.3 change inomata
max_lookahead_dist: 0.6 #0.9 change inomata
コストマップ関連のパラメータ
2025.2.22
コストマップ関連のパラメータを見直し再調整しました。
まず、local_costmap関連から。
今までpluginsは、voxel_layerとinflation_layerを指定していましたが、voxel_layerは、3Dの障害物の情報を管理するためレイヤーのようなので、obstacle_layerを指定するように変更。
あとは、その場旋回時にコストマップがずれるのでマップの更新周期は高くしたり、global_frameをodomに変更したり、色々調整。（動かしながら試行錯誤）
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 10.0 #20.0 #5.0 #10.0 #5.0 #change inomata
publish_frequency: 10.0 #2.0 #10.0 #2.0 #2.0 #change inomata
global_frame: odom #map #odom
robot_base_frame: base_footprint #base_link
rolling_window: true
width: 1 #2 #3
height: 1 #2 #3
resolution: 0.05
robot_radius: 0.12 #0.12 #0.22 change inomata
plugins: ["obstacle_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.20 #0.30 #0.1 #0.70 # change inomata 2024.12.26
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
publish_voxel_map: False
origin_z: 0.0
z_resolution: 0.05
z_voxels: 16
max_obstacle_height: 2.0
mark_threshold: 0
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 2.0 #3.0 #10.0 #3.0 # change inomata 2024.1226
raytrace_min_range: 0.0
obstacle_max_range: 1.5 #2.5
obstacle_min_range: 0.0
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
track_unknown_space: true #false
max_obstacle_height: 0.0 #2.0
observation_sources: scan
scan:
# observation_persistence: 0.0
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 1.5 #2.5
raytrace_min_range: 0.0
obstacle_max_range: 1.0 #1.5 #2.5
obstacle_min_range: 0.0
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
always_send_full_costmap: True #False #True # change inomata 2024.12.26
続いてglobal_costmapの設定。
こちらは基本local_costmapの設定を真似て良い感じになるように設定しました。
いや～ほんとパラメータが多くて調整が大変です。この設定が最適解ではなさそうですが、それなりに自律できるようになったので一旦これでいこうと思います。
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 10.0 #1.0 #1.0 #change inomata
publish_frequency: 10.0 #1.0 #1.0 #cahnge inomata
global_frame: map
robot_base_frame: base_footprint #base_link
robot_radius: 0.12 #0.22 change inomata
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True #False #True
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 0.0 #2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 1.5 #3.0
raytrace_min_range: 0.0
obstacle_max_range: 1.0 #2.5
obstacle_min_range: 0.0
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.20 #0.1 #0.7 # change inomata 2024.12.26
always_send_full_costmap: True #False #True # change inomata 2024.12.26
その場旋回のスピードを落とす
色々パラメータを調整しても、どうもロボットが回転するときにコストマップがズレてルートが安定しません。
私が使っているRPLidar A1M8は、スキャンの周期は最大10Hzなのでそもそもロボットの早い回転には追従できなさそうです。
ということで、その場旋回のスピードを落としました。
実際は、cmd_velの中身を解析し、その場回転指示であったら、出力を下げてモーター指示を送るようにしました。
これにより、少しはコストマップのズレが解消された気がします。
実際に動かした時の動画です。↓
最後に
Nav2を使って自律走行を動かしてみました。
ROS1のナビゲーションスタックとかなり構成が変わったため、構成やパラメータなどまだまだ不明点が多いので、色々調査してまたブログにまとめていこうと思います。
それでは！
