AIチャレンジしたいけどubuntu～autowareは初めて！な方へその２ #ROS2

目的

前回の続き（ROS2，autoware）です。今回は、現在開催中の2023年度AIチャレンジsimulation大会をベースに書いていきます。

下記に自作node追加に関して書かれてます。ChatGPTに聞く時代なんですね。私は相変わらずgoogleさんにちょこちょこ聞いてます。あとmsg型の確認にros2 interface showを教えていただいたので下記で使ってみます。

1. パラメータ変更

あんまり書くこと無いのですが（私もよくわかってない…また今回はパラメータ変更より制御構築？）、パラメータの多くはyamlファイルに記載されています。通常はpackage.xmlのあるディレクトリ（パッケージ：制御等のひとかたまり）内のconfigにあるみたいですが、AIチャレンジでは提出ファイルがaichallenge_submit以下に制限されていますので、運営側で編集が必要となりそうなファイルについては当該ディレクトリ下[aichallenge_submit/autoware_launch/autoware_launch]のconfigにまとめられています。
パラメータの仕様や変更方法は各パッケージ内のREADME.mdにいくらか載っている程度で理解するのは大変（とりあえず変更し動き見て推測するとか）でしたが、iASL-Gifuさんがこの夏の大会時パラメータをコメント付きでupしてくださってます。

Dockerコンテナ内など提出範囲外にあるyamlファイルを変更したい時も、launchファイルが編集できる場合は読込ファイルを変えることができるのかもしれませんが、私はしたことがありません。
あと、たまにlaunchファイルで設定するものもあります。

2.自作node追加

パラメータ変更では対応できない場合、新たな制御を織り込むことになりますが、既存nodeのカスタマイズは、パラメータ同様Dockerコンテナ内など提出範囲外にあると編集内容を反映させることができませんし、複雑な仕様であれば正しく改良するのは困難と思いますので、新たに自作nodeを追加することになります。

launchファイルを編集して、必要なpackageをaichallenge_submit以下に移動させることも考えられますが、関連するpackageとの調整が必要みたいで難しそうです（一度試みたができず…）。

AI Challenge入門ガイドで紹介されているpath_to_trajectoryのように、aichallenge_submit以下にあり、カスタマイズしやすいように作られているものがあれば、nodeを追加しなくてもいいですね。

1. package作成

今回は、raw_vehicle_cmd_converterの代わりを作ってみます。
raw_vehicle_cmd_converterは、autoware.universe/vehicle/raw_vehicle_cmd_converterとDockerコンテナ内にあるみたいで直接編集はできません。

まず、packageをaichallenge_submit直下に作成します。package名はmy_cntlにしました。
package名のディレクトリとその下にいろいろできますので、srcディレクトリにmy_cntl.cppを作ります（最近やっとcppとhppを分けることができたところで自信ないので、includeは使わずmy_cntl.cppだけで進めます。最低限動けばいいやレベルなので、ちょこちょこ怪しかったり手抜きだったりします）。

#aichallenge2023-racingディレクトリで
cd docker/aichallenge/aichallenge_ws/src/aichallenge_submit
ros2 pkg create my_cntl --build-type ament_cmake --dependencies rclcpp std_msgs

2. topic送受信

raw_vehicle_cmd_converterが送受信しているtopicのうち、最低限必要な下記２つを追加します。
受信topic
　topic名：/control/command/control_cmd
　［msg型：autoware_auto_control_msgs/msg/AckermannControlCommand］
送信topic
　topic名：/control/command/actuation_cmd
　［msg型：tier4_vehicle_msgs/msg/ActuationCommandStamped］

教えてもらったros2 interface show msg型(topic名ではない)と、ros2 topic echo topic名で内容を確認します。
（なぜか、autoware_auto_control_msgs/msg/AckermannControlCommandは取れなかった。）

*受信側

$ ros2 topic echo /control/command/control_cmd                              
stamp:
  sec: 152
  nanosec: 738007254
lateral:
  stamp:
    sec: 152
    nanosec: 738007254
  steering_tire_angle: -0.1867808699607849
  steering_tire_rotation_rate: 0.0
longitudinal:
  stamp:
    sec: 152
    nanosec: 738007254
  speed: 11.11111068725586
  acceleration: -0.12292861938476562
  jerk: 0.0

*送信側

$ ros2 interface show tier4_vehicle_msgs/msg/ActuationCommandStamped
std_msgs/Header header
	builtin_interfaces/Time stamp
		int32 sec
		uint32 nanosec
	string frame_id
tier4_vehicle_msgs/ActuationCommand actuation
	float64 accel_cmd
	float64 brake_cmd
	float64 steer_cmd

$ /aichallenge$ ros2 topic echo /control/command/actuation_cmd
header:
  stamp:
    sec: 219
    nanosec: 968010447
  frame_id: base_link
actuation:
  accel_cmd: 0.0
  brake_cmd: 0.16234322441948787
  steer_cmd: -0.013832184486091137

以上から、アクセル、ブレーキ、操舵角を送信する必要がありますので、受信した加速度とタイヤ角から演算して最低限走行できるコードを作成してみました（my_cntl.cpp）。
以下、topic追加を中心に説明していきます。

my_cntl.cpp

// include　…[1]
#include <rclcpp/rclcpp.hpp>
#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>   // 送信topic用
#include <tier4_vehicle_msgs/msg/actuation_command_stamped.hpp>           // 受信topic用

// node class定義　…[2]
class MyCntl : public rclcpp::Node{
  public:
    MyCntl();
  private:
    // publisher
    rclcpp::Publisher<tier4_vehicle_msgs::msg::ActuationCommandStamped>::SharedPtr pub_act_cmd;
    // subscriber
    rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr sub_ctl_cmd;
    // 受信時callback
    void cb_ctl_cmd(const autoware_auto_control_msgs::msg::AckermannControlCommand msg);
};

// constructor　…[3]
MyCntl::MyCntl() : Node("my_cntl_node"){
  // publisher
  pub_act_cmd = this->create_publisher<tier4_vehicle_msgs::msg::ActuationCommandStamped>("/control/command/actuation_cmd", 1);
  // subscriber
  sub_ctl_cmd = this->create_subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>(
    "/control/command/control_cmd", 1, std::bind(&MyCntl::cb_ctl_cmd, this, std::placeholders::_1));
}

// 受信時callback　…[4]
void MyCntl::cb_ctl_cmd(const autoware_auto_control_msgs::msg::AckermannControlCommand msg){
  // msg：受信したtopic(/control/command/control_cmd)
  tier4_vehicle_msgs::msg::ActuationCommandStamped act_cmd;     // 送信用変数（これに送信内容を書き込んでいく）

  // header
  act_cmd.header.stamp = msg.stamp;  // stampは同じなのでcopy（frame_idは空になるが大丈夫みたい…）

  // steer_cmd
  auto steer_cmd = msg.lateral.steering_tire_angle;
  act_cmd.actuation.steer_cmd = steer_cmd;          // 単純にcopy（一応それらしく動いている）

  // accel_cmd,brake_cmd
  double acc = msg.longitudinal.acceleration;
  double accel_cmd = 0;
  double brake_cmd = 0;
  if(acc > 0.1){
    accel_cmd = 0.3;
  }else if(acc < -0.1){
    brake_cmd = 2;
  }
  act_cmd.actuation.accel_cmd = accel_cmd;
  act_cmd.actuation.brake_cmd = brake_cmd;

  // 送信
  pub_act_cmd->publish(act_cmd);

  // debug
  double t_now = (double)msg.stamp.sec + (double)msg.stamp.nanosec * 0.000000001;
  static double t_old = 0;
  if((t_now - t_old) > 1){
    auto vsp = msg.longitudinal.speed;
    std::cout << t_now << "[s] sub(vsp/acc/str):" << vsp << "/" << acc << "/" << steer_cmd;
    std::cout << ",pub(aps/brk/str)" << accel_cmd << "/" << brake_cmd << "/" << steer_cmd << std::endl;
    t_old = t_now; 
  }
}

// main　…[5]
int main(int argc, char const* argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MyCntl>());
  rclcpp::shutdown();
  return 0;
}

[1] include
送受信するmsg型のheaderを追加します。header名やpathは、同じmsg型を使用しているソースファイルを探してコピーするのが確実と思いますが、規則はあるらしいです（msg型の単語中の大文字前にアンダースコアを入れて小文字にする）。
[2] node class定義
自作nodeのクラスをMyCntlとしました（アンダースコア使用不可？）。この中で、constructor、publisher,subscriberと受信時callbackを宣言します。
publisher,subscriberの<>内にそれぞれ送受信msg型を書き込み、callbackも<>内に受信msg型を、名前は下記[3]のsubscriberで設定したものです。
[3] constructor
node名（my_cntl_node）を設定して、publisher,subscriberを初期化します。
上記[2]同様<>内にmsg型を、()内にはtopic名を書き込みます。更にsubscriberは、受信時に実行されるcallback(&MyCntl::cb_ctl_cmd)を設定します。
[4] 受信時callback
受信時に実行されますので、ここに実際に演算させたい内容を書きます。
送信用変数（送信msg型と同じ型）としてact_cmdを作り、操舵は受信した値をそのまま代入、アクセルとブレーキは、加速度でそれぞれ一定値を入れてます。
topicの各値は、ros2 topic echo等で調べたラベルをそのまま繋げば取得できます（たまに配列あり）。
最後に、pub_act_cmd->publish(act_cmd)で送信用変数をpublisherに渡して送信します。

送受信の変数は、きっちり型を合わしてなくてもdoubleかintのやりとりで動いてます（手抜き…）。あと、変数の単位等、どのような値を渡さないといけないかは、使いたいtopicを送受信しているソースやros2 topic echoで挙動を確認する必要があると思います（ソース見るの大変なので、値の挙動で推測することが多い）。

[5] main
rclcpp::spin(std::make_shared<MyCntl>())のように、<>内にclass名を設定すればいいみたいです。

細かいところはよくわかってませんが、とりあえずこう書いておけば動くみたい、ですので必要に応じて調べてください。

3. package.xml

不要なstd_msgs部分を消し、使用する送受信のpackage名を追記します。package名(my_cntl)は作成時に書き込まれています（流用する場合は、<name>my_cntl</name>のように変更要）。

package.xml（抜粋）

  <depend>rclcpp</depend>
  <depend>tier4_vehicle_msgs</depend>
  <depend>autoware_auto_control_msgs</depend>

4. CMakeLists.txt

最初に紹介したpath_to_trajectoryで使われているament_cmake_auto，ament_auto_find_build_dependenciesならほとんど追記しなくてよいみたいです（知らなかったので使用する送受信のpackage名を追記してた）。
ament_auto_add_executableのところに、node名とcppファイルを書き込めば、後はよろしくしてくれるようですので、# find dependencies以下を下記のように書き換えてください。
なお、package.xml同様、2行目のpackage名は作成時に書き込まれています（流用する場合は、project(my_cntl)のように変更要）。

CMakeLists.txt（# find dependencies以下）

# find dependencies
find_package(ament_cmake_auto REQUIRED)

ament_auto_find_build_dependencies()

ament_auto_add_executable(my_cntl_node
  src/my_cntl.cpp
)

ament_auto_package()

5. launchファイル

上記4まででbuildが通るはずですので、あとはautoware起動時に作成したnodeを起動するようにlaunchファイルに追記する必要があります。またこのままでは、送信topic(/control/command/actuation_cmd)が、raw_vehicle_cmd_converterの送信と干渉するので、raw_vehicle_cmd_converterを止める必要があります。
raw_vehicle_cmd_converterはautoware_micro_awsim.launch.xml内で起動されているので、この部分をコメントアウトします。
あわせて、追加node(my_cntl_node)を起動するよう追記します。pkg=，exec=にそれぞれpackage名，node名を設定します（output="screen"をつけるとdebugで使用したstd::coutで端末に表示できるようになるそうです。

autoware_micro_awsim.launch.xml（抜粋）

  <!-- vehicle -->
    <node pkg="my_cntl" exec="my_cntl_node" name="my_cntl" output="screen"/>
<!--
  <include file="$(find-pkg-share raw_vehicle_cmd_converter)/launch/raw_vehicle_converter.launch.xml">
    <arg name="max_throttle" value="1.0"/>
    <arg name="max_brake" value="1.0"/>
    <arg name="max_steer" value="300.0"/>
    <arg name="min_steer" value="-300.0"/>    
    <arg name="csv_path_accel_map" value="$(find-pkg-share dallara_launch)/config/accel_map.csv"/>
    <arg name="csv_path_brake_map" value="$(find-pkg-share dallara_launch)/config/brake_map.csv"/>
    <arg name="convert_accel_cmd" value="true" />
    <arg name="convert_brake_cmd" value="true" />
    <arg name="convert_steer_cmd" value="false" />
  </include>
-->

launchファイルは階層化されているので、削除や追記する場合は気をつけてください。
また、nodeを停止する代わりに、topic名を変更する方法もあります（launchファイル内でremapされていると楽）。

これで、通常の手順でRviz2を立ち上げると自作nodeも起動し、端末に送受信内容が表示されます。
挙動としては、加速度に合わせてアクセルとブレーキを交互に操作しつつ指示値付近の車速で走行、カーブ手前でちゃんと減速し操舵もそれなりにしています。

以上、怪しい内容ですが、送受信を備えた自作nodeを作って起動するまでの手順をまとめてみました。
AIチャレンジは昨年冬からの参加ですが自作nodeを使ったのはこの夏の大会からで、ｃ++も半年程度ですので、よくわからないけどまぁ何とか動くものができているといったレベルですが、昨年の私のように何も知らなければちょっとは助かる位の内容にはなったかなーと思ってます。

では、AIチャレンジ楽しみましょう

AIチャレンジしたいけどubuntu～autowareは初めて！な方へ その２

目的

目次