simple_pure_pursuitのパラメータを動的に変更する

# はじめに

自動運転AIチャレンジ2024決勝 @ 11月初旬 に向けて開発を進めています。

https://www.jsae.or.jp/jaaic/2024ver/summary/

2024決勝大会では、本番までに数回、実際の車両を使った練習の機会が設けられます。

限られた時間の中で車両適合を効率的に進めるためには、パラメータ変更を素早く行い試行回数を増やす必要があります。

`simple_pure_pursuit`ノードに、ROS の `dynamic_reconfigure` に相当する処理を追加し、パラメータの動的な変更を実現してみます。

# 動的なパラメータ変更方法

以下を参考にすれば簡単にできました。`add_parameter_callback`を使用する方法です。

https://docs.ros.org/en/humble/Tutorials/Intermediate/Monitoring-For-Parameter-Changes-CPP.html

例えば、`simple_pure_pursuit` の `external_target_vel` を動的に変更したい場合、
以下の変更を行います。

```diff_python:simple_pure_pursuit.hpp
...
  const bool use_external_target_vel_;
-  const double external_target_vel_;
+  double external_target_vel_; // constを外す
  const double steering_tire_angle_gain_;


 private:
  void onTimer();
  bool subscribeMessageAvailable();
+  std::shared_ptr<rclcpp::ParameterEventHandler> param_subscriber_;
+  std::shared_ptr<rclcpp::ParameterCallbackHandle> cb_handle_;
...
```

```diff_python:simple_pure_pursuit.cpp
...
  using namespace std::literals::chrono_literals;
  timer_ =
    rclcpp::create_timer(this, get_clock(), 30ms, std::bind(&SimplePurePursuit::onTimer, this));
  
+  param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(this);
+  auto cb = [this](const rclcpp::Parameter & p) {
+    external_target_vel_ = p.as_double();
+    RCLCPP_INFO(this->get_logger(), "external_target_vel changed to %f", external_target_vel_);
+  };
+  cb_handle_ = param_subscriber_->add_parameter_callback("external_target_vel", cb);
}

AckermannControlCommand zeroAckermannControlCommand(rclcpp::Time stamp)
{
...
```

ビルドしてAutowareを起動します。

別ターミナルから以下コマンドを打つと目標車速値が変更され、
速く走るようになったと思います。

```bash:Terminal
ros2 param set /simple_pure_pursuit_node external_target_vel 11.1111
```

GUIで変更したい場合は以下で、`external_target_vel` を変えると同様のことができます。
```bash:Terminal
ros2 run rqt_reconfigure rqt_reconfigure
```

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/aa794024-15e7-4fc5-c880-c731cab17f74.png)

他にも`add_on_set_parameters_callback`を使用する方法もあり、
どちらかというと、こちらの実装が多いです。

https://qiita.com/srs/items/837958ea11d586a637ee

https://qiita.com/porizou1/items/8f8efe5d21e2602bcb4e

どちらのほうがベターか、は実はよくわかっていません…
ただ公式ドキュメントのチュートリアルでは`add_parameter_callback`を使っていたので、今回はこちらを採用しました。またコミュニティでは以下のような言及がありましたので載せておきます。

https://discourse.ros.org/t/c-parameters-api-feels-overly-complex/27968/3

(追記)
`add_parameter_callback`
**特定のパラメータ** の変更に対してコールバックを登録する。

`add_on_set_parameters_callback`
**複数のパラメータ** の変更に対してコールバックを登録する。
変更のバリデーションや複数パラメータをまとめて更新する用途と理解。

# (参考) Pythonで書かれたノードの場合…

上記はC++(rclcpp)の1例ですが、Python(rclpy)はというと…

まずhumbleの公式ドキュメントには、同様の`add_parameter_callback`を使ったPython(rclpy)チュートリアルが存在しませんでした。rollingにはありますので参考までに載せます。

https://docs.ros.org/en/rolling/Tutorials/Intermediate/Monitoring-For-Parameter-Changes-Python.html

既にマージされているようですが…

https://github.com/ros2/rclpy/pull/1135

ただ、公式ドキュメント以外の以下記事などを参考に、
`add_on_set_parameters_callback`を使うことで実現はできそうなところまでは確認しています。
皆さんトライしてみてください！

https://zenn.dev/tasada038/articles/f2f5b500cdc36a#1.3%3A-python%E7%89%88-ros-2-%E3%83%91%E3%83%A9%E3%83%A1%E3%83%BC%E3%82%BF%E3%81%AE%E5%AE%A3%E8%A8%80%E3%80%81%E5%8F%96%E5%BE%97

https://note.com/hafnium/n/nf7bf937d3172

https://roboticsbackend.com/ros2-rclcpp-parameter-callback/

https://qiita.com/NeK/items/37cbae948f8a4178044b


# おわりに

実はこれまで個人的に避けてきた動的なパラメータ変更ですが… 意外と簡単にできることがわかりました。
これらの処理を動的に変更したいパラメータ全てに実施すれば、効率的に実車適合が行えそうな感触です。

練習時間をフル活用して、実車適合頑張るぞ〜！

※ パラメータコールバックに関するhumbleのコンセプトは以下で確認できます。

https://docs.ros.org/en/humble/Concepts/Basic/About-Parameters.html#parameter-callbacks

この記事は、AI文章校正ツール「ちゅらいと」で校正されています。