Rover ROS2環境への移行(まずはL298N足回りから)

0. 目的と前提

Arduino Uno + nRF24L01 + 左右2個ESCで動作しているスキッドステア型Roverを、段階的にROS2構成へ移行する。 今回もCodexを利用しバイブコーディングメインで開発を行う。（本当に完成度が高く便利です。）

今回のPhaseではESCとArduinoをいったん外し、Raspberry Pi(3B)のGPIOからL298Nを直接制御する方式とした。PC側のROS2 teleopから /cmd_vel を送信し、Wi-Fi LAN経由でRasPi側ROS2ノードが受信して左右DCモータをPWM制御する。将来的にはESC制御に戻す予定。

環境制約(家にある機材の流用)からPC側はROS2 Jazzy(Ubuntu24.04)、RasPi側はROS2 Humble(Ubuntu22.04)とし、同一環境内でROS2 distributionを混載しない。Humble/Jazzy間の通信互換性を優先し、初期Phaseでは標準メッセージ geometry_msgs/msg/Twist のみを使う。

目標構成:

PC ROS2 Jazzy
  rover_operator
  teleop / test publisher / limiter / monitor
  -> /cmd_vel geometry_msgs/msg/Twist
  -> Wi-Fi LAN
Raspberry Pi ROS2 Humble
  rover_base
  -> GPIO direction + PWM
  -> L298N
  -> 左右DCモータ

単体テストではL298N経由の始動にduty 0.45以上が必要だったため、始動時だけ start_boost_duty=0.55 を使い、エンコーダtick検出後は run_min_duty=0.32 からの低速走行プロファイルへ落とす。通信断時は約0.5秒で自動停止する。

1. 実装済みパッケージ構成

PC側Jazzy用:

pc_rover_ws/
  src/
    rover_operator/
      package.xml
      setup.py
      rover_operator/
        cmd_vel_limiter.py
        cmd_vel_test_publisher.py
        topic_monitor.py
      launch/
        teleop_rover.launch.py
        monitor_rover.launch.py
      config/
        teleop_limits.yaml
      README.md

PC側の役割

  • cmd_vel_test_publisher.py: 低速テスト用の /cmd_vel をpublishする。
  • cmd_vel_limiter.py: /cmd_vel_raw を受け、速度上限をかけて /cmd_vel へpublishする。
  • topic_monitor.py: /cmd_vel の受信状況をログ表示する。将来 /wheel_ticks や /odom 監視を追加する置き場。
  • teleop_rover.launch.py: PC側速度リミッタと、標準teleop_twist_keyboardを起動する。

RasPi側Humble用:

raspi_rover_ws/
  src/
    rover_base/
      package.xml
      setup.py
      rover_base/
        l298n_driver.py
        diff_drive_mixer.py
        l298n_cmd_vel_node.py
        encoder_counter.py
      scripts/
        l298n_gpio_test.py
        l298n_pwm_sweep.py
        encoder_count_test.py
      launch/
        l298n_rover.launch.py
      config/
        l298n_rover.yaml
      test/
        test_diff_drive_mixer.py
      README.md

RasPi側の役割:

  • l298n_driver.py: GPIO方向ピンとPWMピンを直接扱う低レベルドライバ。
  • diff_drive_mixer.py: /cmd_vel から左右モータ指令への変換ロジック。
  • l298n_cmd_vel_node.py: /cmd_vel 購読、タイムアウト監視、安全停止。
  • encoder_counter.py: フォトインタラプタ式エンコーダの左右カウント。
  • scripts/l298n_gpio_test.py: ROS2を使わないGPIO単体テスト。
  • scripts/l298n_pwm_sweep.py: 低速PWM始動しきい値の確認。
  • scripts/encoder_count_test.py: エンコーダカウント確認。

2. /cmd_vel から左右モータPWMへの変換仕様

入力は geometry_msgs/msg/Twist とする。

  • linear.x: 前後方向。正で前進、負で後退。
  • angular.z: 旋回方向。ROS標準に合わせ、正を左旋回、負を右旋回とする。

現在のArduino 2ESC版では、ジョイスティック値から T と S を作り、通常時に次の差動ミキシングを行っている。

L = T + k_mix * S
R = T - k_mix * S

ROS2版でもこの考え方を踏襲する。ただし入力はジョイスティック生値ではなく /cmd_vel なので、まず正規化する。

T = clamp(linear.x / max_linear, -1.0, 1.0)
S = clamp(angular.z / max_angular, -1.0, 1.0)

if abs(T) < pivot_linear_threshold and abs(S) >= pivot_angular_threshold:
    if pivot_mode == "one_wheel":
        if S > 0:
            L = k_pivot * S
            R = 0
        else:
            L = 0
            R = -k_pivot * S
    else:
        L =  k_pivot * S
        R = -k_pivot * S
else:
    L = T + k_mix * S
    R = T - k_mix * S

L = clamp(L, -1.0, 1.0)
R = clamp(R, -1.0, 1.0)

初期パラメータ:

パラメータ初期値意味
max_linear0.30/cmd_vel.linear.x を100%指令とみなす値
max_angular1.50/cmd_vel.angular.z を100%指令とみなす値
k_mix0.25通常旋回時の旋回混合率。キーボード操作向けに弱める
k_pivot0.25ピボット時の混合率。大きく回りすぎないよう弱める
pivot_modeone_wheelピボット時は左右逆回転ではなく片輪のみ駆動する
pivot_linear_threshold0.10ほぼ停止中とみなす前後指令
pivot_angular_threshold0.60その場旋回へ入る旋回指令。意図しないピボットを減らす
deadband0.05微小指令を停止扱いにする幅
invert_angulartrue現車体で旋回方向を合わせるため角速度符号を反転
left_gain / right_gain1.00左右dutyの手動補正
min_command0.05正規化後の左右指令がこれ未満なら停止扱い
start_boost_duty0.55停止状態から動かすときの始動duty
start_boost_sec0.20エンコーダがない場合の時間ベースboost期間
run_max_duty0.50始動後の通常走行上限
run_min_duty0.32エンコーダtick検出後の最低走行duty
accel_step0.04制御周期ごとのduty増加上限
brake_step0.12停止・減速側のduty変化上限

L298NではESCのような1000-2000usのサーボパルスではなく、方向ピンとPWM dutyで制御する。

cmd > 0:
  IN_FORWARD = HIGH
  IN_REVERSE = LOW
  PWM = start_boost_duty until encoder ticks increase,
        then map(abs(cmd), min_command..1.0, run_min_duty..run_max_duty)

cmd < 0:
  IN_FORWARD = LOW
  IN_REVERSE = HIGH
  PWM = start_boost_duty until encoder ticks increase,
        then map(abs(cmd), min_command..1.0, run_min_duty..run_max_duty)

abs(cmd) < min_command or cmd == 0:
  IN_FORWARD = LOW
  IN_REVERSE = LOW
  PWM = 0

左右のモータ取り付け向きで前後が逆になる可能性が高いので、invert_left と invert_right をパラメータ化している。

キーボード操作では旋回量が大きくなりやすいため、現在の初期設定では k_mix=0.25, k_pivot=0.25, pivot_angular_threshold=0.60 として旋回を弱める。さらにピボット時は pivot_mode=one_wheel とし、左右逆回転ではなく片輪のみを駆動する。i で直進しない場合は、エンコーダ同期制御の前段階として left_gain/right_gain を0.05刻みで手動調整する。

3. L298N GPIOピン割り当て

BCM番号で定義する。物理ピン番号ではない。

L298N端子RasPi BCM用途
IN118左モータ方向A
IN217左モータ方向B
ENA13左モータPWM
IN323右モータ方向A
IN422右モータ方向B
ENB12右モータPWM

エンコーダ入力:

信号RasPi BCM用途
ENC_L5左フォトインタラプタ入力
ENC_R6右フォトインタラプタ入力

L298Nの注意点:

  • RasPi GNDとL298N GNDは必ず共通化する。
  • L298Nのモータ電源はRasPiから取らない。
  • L298Nの5V端子をRasPi 5Vへ逆給電しない構成にする。
  • 初回テストは車体を浮かせ、タイヤが空転できる状態で行う。

4. 安全停止仕様

安全停止はRasPi側 rover_base の最終責務とする。PC側の速度リミッタは補助であり、RasPi側のtimeout停止を置き換えない。

  • ノード起動直後は必ず停止状態。
  • /cmd_vel を受信するまでモータは動かさない。
  • 最後の /cmd_vel 受信から timeout_sec=0.5 秒を超えたら停止。
  • linear.x=0 かつ angular.z=0 を受信したら停止方向へ制御。
  • Ctrl+C、ROS shutdown、例外発生時はPWMを0、方向ピンをLOWへ戻す。
  • 始動時は start_boost_duty=0.55 とし、単体テストで確認した始動duty 0.45を少し超える範囲に抑える。
  • エンコーダtick検出後は run_min_duty..run_max_duty へ落とし、キーボード操作時の移動量を抑える。
  • start_boost_duty=0.55 でも弱い場合は、コード変更より先にモータ電源電圧、L298Nの電圧降下、配線抵抗、タイヤ負荷を確認する。
  • dutyの急変を避けるため、加速側は遅く、停止・減速側は速くする。

停止時のL298N出力はフリーラン停止を初期仕様にする。

IN1/IN2 = LOW/LOW
IN3/IN4 = LOW/LOW
ENA/ENB PWM = 0

ブレーキ停止 HIGH/HIGH はモータ・L298Nへの負荷が増えるため、初期Phaseでは使わない。

5. 最初の単体テスト手順

ROS2化の前に、必ずRasPi単体でGPIOとL298Nを確認する。

cd ~/raspi_rover_ws/src/rover_base
python3 scripts/l298n_gpio_test.py --motor left --direction forward --duty 0.25 --duration 1.0
python3 scripts/l298n_gpio_test.py --motor left --direction reverse --duty 0.25 --duration 1.0
python3 scripts/l298n_gpio_test.py --motor right --direction forward --duty 0.25 --duration 1.0
python3 scripts/l298n_gpio_test.py --motor right --direction reverse --duty 0.25 --duration 1.0

PWM低速確認:

python3 scripts/l298n_pwm_sweep.py --motor left --start 0.30 --end 0.65 --step 0.05
python3 scripts/l298n_pwm_sweep.py --motor right --start 0.30 --end 0.65 --step 0.05

エンコーダ確認:

python3 scripts/encoder_count_test.py --left-pin 5 --right-pin 6

確認内容:

  • forward で左右とも車体前進方向に回るか。
  • 左右どちらかが逆なら invert_left / invert_right で補正する。
  • duty 45%付近で始動することを確認し、左右差があれば実測値を記録する。
  • エンコーダは左右countが増えることをまず確認する。

6. PC-RasPi間ROS2通信確認手順

RasPi側:

cd ~/raspi_rover_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install
source install/setup.bash
export ROS_DOMAIN_ID=30
ros2 launch rover_base l298n_rover.launch.py

PC側:

cd ~/pc_rover_ws
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install
source install/setup.bash
export ROS_DOMAIN_ID=30

PCから低速テスト:

ros2 run rover_operator cmd_vel_test_publisher --ros-args \
  -p topic:=/cmd_vel \
  -p linear_x:=0.05 \
  -p angular_z:=0.0 \
  -p duration_sec:=3.0

PCからteleop:

ros2 launch rover_operator teleop_rover.launch.py

teleop_rover.launch.py はxterm内で標準teleop_twist_keyboardを起動し、/cmd_vel_rawへpublishする。cmd_vel_limiterが速度上限をかけて/cmd_velへ中継する。VNCでは、開いたxtermをクリックして操作する。

PC側で標準teleopとxtermを設定する。

sudo apt install ros-jazzy-teleop-twist-keyboard xterm

iは前進、,は後退、j/lはその場旋回、u/oは前進旋回、m/.は後退旋回である。標準teleopはキーごとに1つの指令を送るため、iの後にj/lを押すと前進は維持されずその場旋回に切り替わる。

通信確認で見ること:

  • PCから ros2 topic list で /cmd_vel が見えるか。
  • RasPi側ノードログに受信時刻と変換後の左右dutyが出るか。
  • publishを止めて0.5秒程度で停止するか。
  • teleop_twist_keyboardでkまたは未割当キーを押したとき、停止指令またはtimeoutで止まるか。

ネットワークで見えない場合の確認:

  • PCとRasPiが同じLANにいるか。
  • ROS_DOMAIN_ID が一致しているか。
  • firewallがDDS通信を遮っていないか。
  • geometry_msgs/msg/Twist の標準型だけで試しているか。

7. ESC x2 / Pixhawk / ArduPilot / MAVLinkへの拡張方針

今回のL298N制御を最終形にしない。ROS2上の入力と足回り出力の境界を分け、後からESCやPixhawkへ戻せる構成にする。

上位ROS2インターフェースは維持する。

/cmd_vel geometry_msgs/msg/Twist

下位出力だけを差し替える。

DriveBackend
  - L298NBackend: GPIO direction + PWM
  - EscServoBackend: 50Hz, 1000-2000us pulse
  - PixhawkMavlinkBackend: MAVLink MANUAL_CONTROL / RC override / setpoint

ESC x2へ戻す場合:

  • 中立: 1500us
  • 前進: 1500-2000us
  • 後退: 1500-1000us
  • 左右個別に neutral_us を調整可能にする。
  • ESC初期化・キャリブレーションモードは通常走行ノードと分ける。
  • L298Nのduty制御とは別物として扱い、同じPWM関数を流用しない。

Pixhawk / ArduPilot / MAVLink連携:

  • RasPiがGPIOを直接叩く代わりに、ROS2ノードからMAVLinkへ指令を渡す。
  • mavros またはMAVLink Pythonライブラリを候補にする。
  • failsafeはPixhawk/ArduPilot側にも持たせ、ROS2ノード側timeoutと二重化する。
  • L298N PhaseではMAVLink依存を入れない。

8. エンコーダによる自立走行支援

フォトインタラプタ式エンコーダは各モータ軸に取り付け済みで、スリット穴は20個。将来の自立走行では左右速度のフィードバック、走行距離推定、オドメトリ生成に使う。

このPhaseでは閉ループ制御には入れず、まずカウント確認と始動判定ログまでに留める。

  • 左右エンコーダの立ち上がり/立ち下がりをカウント。
  • 一定周期で左右count、差分count、推定回転数をログ出力。
  • モータ指令とカウント増減の向きを確認する。
  • /cmd_vel で非ゼロdutyを出した後、startup_check_sec=0.20 秒以内に startup_min_ticks=1 以上増えたら始動済みとしてログ出力する。
  • tickが増えない場合は未始動警告を出し、始動boost dutyを維持する。
  • tick検出後は通常走行状態として run_min_duty..run_max_duty の低速走行プロファイルに移行する。
  • 左右回転同期補正やPID制御はこのPhaseでは行わない。

将来のROS2インターフェース:

  • /wheel_ticks: 左右累積tickをpublish。
  • /wheel_speed: 左右車輪速度をpublish。
  • /odom: nav_msgs/msg/Odometry をpublish。
  • tf: odom -> base_link をpublish。

最初の閉ループ制御は左右速度差の補正から始める。車輪径、トレッド幅、ギヤ比、スリット数、エッジ数をパラメータ化する。

9. Phase完了条件

  • RasPi単体GPIOテストで左右モータの前進/後退/停止が確認できる。
  • duty 45-60%の範囲で低速走行できる。
  • RasPi内の ros2 topic pub で /cmd_vel 走行確認ができる。
  • PC JazzyからRasPi Humbleへ /cmd_vel を送って動作する。
  • teleop_twist_keyboardで前進、後退、左右旋回、その場旋回ができる。
  • publish停止またはWi-Fi断相当で0.5秒程度以内に停止する。
  • 左右反転、旋回方向、PWM上限をパラメータで調整できる。
  • エンコーダは少なくとも左右カウント確認まで実施できる。
タイトルとURLをコピーしました