この記事は筆者オンリーのAdvent Calendar 202311日目の記事です。
前回までのあらすじ
C++/PythonでAutowareのmsgをサブスクライブしmsgをそのままAutowareにパブリッシュする処理を追加しました。
今回やること
OSS連携について、pursuitの処理をPythonRoboticsのソースコードに置き換えて実行してみたいと思います。
そこで、現在のAutowareの入出力、内部処理について調査してみたいと思います。
pursuitの各処理について
Autoware、PythonRoboticsの処理はリンクの通りになります。
Autowareのpursuitの構成
pursuitの構成を見てみます。
引数/サブスクリプション/パブリッシャー
引数
・wheel_base
・lookahead_gain
・lookahead_min_distance
・speed_proportional_gain
・use_external_target_vel
・external_target_vel
のfloatがあります。
サブスクリプション
・Odometry
・Trajectory
の2つのmsgをサブスクライブしているようです。
Trajectory内には最大100000のベクターのTrajectoryPoiontを持つことができるようです。
パブリッシャー
・AckermannControlCommand
のmsgをパブリッシュしているようです。
AckermannControlCommandは
・Time型のstamp
・AckermannLateralCommand型のlateral
（メンバーはsteering_tire_angle/steering_tire_rotation_rateで構成されている）
・LongitudinalCommand型のlongitudinal
(メンバーはspeed、acceleration、jerkで構成されている)
で構成されているようです。
メッセージをパブリッシュ/サブスクライブする契機
30ms毎にOdometry/Trajectoryのデータを受領してmsgを作成しているようです。
パブリッシャーの作成
上記のAckermannControlCommandにパラメータを入力しているようです。
・AckermannControlCommand.stamp
・AckermannControlCommand.longitudinal.stamp
・AckermannControlCommand.lateral.stamp
には現在のタイムスタンプが入っているようです。
・AckermannControlCommand.longitudinal.speed
・AckermannControlCommand.longitudinal.acceleration
・AckermannControlCommand.lateral.steering_tire_angle
にpure pursuitで計算したパラメータを入れているようです
のパラメータを作り込んでいるようです。
処理としては
Trajectroryの複数点とOdometoryの点を比較し、Trajectory内の最も近い点(インデックス)を取得する。
上記で取得したTrajectoryの縦方向のスピードをlongitudinal.speedに代入する。
proportinal gainと縦方向の速度差分をlongitudinal.accelerationに代入する。
先読み点を検索し、計算を行い
ateral.steering_tire_angleに代入する。
ということをを行っているようです。
まとめ
いろいろOSS連携してみたかったけど面倒くさい予感がしてきた。。。