feat: to avoide obstacles, add state lattice planner

[masahiro-kubota]

追加機能

• 取得した点群を用いて障害物を回避するために、state lattice plannerを実装したtrajectory_plannerを作成。
• Trajectoryがrvizで表示されるように変更。

マージ後修正予定箇所

• Trajectoryの終端で動作が不安定になりますが、シンプルさのために対処せずにマージしたいです。
• コード内のコメントはマージ後これまでのコードと一緒に修正します。

確認していただきたいこと

以下を行って画像のように回避動作を行うことを確認していただきたいです。

1. src/autoware_practice_lidar_simulator/config/object_centers.csvを以下のように変更

x_center,y_center
47.0,-1.0
30.0,1.0
15.0,-1.0

2. src/autoware_practice_course/src/velocity_planning/trajectory_loader.cppの28行目を以下のように変更

- pub_trajectory_ = create_publisher<Trajectory>("/planning/scenario_planning/trajectory", rclcpp::QoS(1));
+ pub_trajectory_ = create_publisher<Trajectory>("/planning/trajectory_loader/trajectory", rclcpp::QoS(1));

3. 以下のコマンドをそれぞれ別端末で実行

• ros2 launch autoware_practice_launch practice.launch.xml
• ros2 run autoware_practice_course trajectory_loader --ros-args -p path_file:=src/autoware_practice_course/config/trajectory.csv
• ros2 run autoware_practice_course trajectory_follower --ros-args -p kp:=5.0 -p lookahead_distance:=5.0
• ros2 run autoware_practice_course trajectory_planner --ros-args -p state_num:=9 -p target_interval:=1.0

[masahiro-kubota]

障害物が点群だけだと分かりづらいので、rviz上では箱を置くように後ほど修正しようと思います

[isamu-takagi]

LGTM 動作確認できました。章の内容がまとまったら後で以下の対応ができると良さそうです。

• trajectory_loaderのtopic名をremapで対応する
• FloatGridはFloat32MultiArrayで汎用化できるかも
• 幾何関連の処理をライブラリ化

[KariControl]

確認遅くなり申し訳ございません。作成ありがとうございます。

maker.cppの187に関して、auto node = std::make_shared< TrajectoryVisualizer>(options) だとビルドが通りませんでした。

auto node = std::make_shared<autoware_practice_visualization::TrajectoryVisualizer>(options)に修正するとビルドがとおりました。

[isamu-takagi]

レビュー中に入ったコミットでビルドできなくなっているようですので修正お願いします。f324738

[masahiro-kubota]

@KariControl
レビューありがとうございます！大変ありがたいです！
ビルドチェックできておらず申し訳ありませんでした。
KariControlさんのアドバイスを元に修正しビルドできることを確認しましたのでマージします。