自動運転AIチャレンジでローカルプランニングを自作する

## 概要

自動運転AIチャレンジの初期状態のコードを動かすと，以下のように障害物で止まってしまいます．(一個目の障害物には何故が突撃していますが...)
![Screenshot from 2023-08-10 19-04-42.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320782/66b03e48-451f-6364-ceaa-8e5a269d0393.png)
ここで，初期のコードとは[CustomizeでAutoware-Mini](https://automotiveaichallenge.github.io/aichallenge2023-sim/customize/index.html)を使っている状態を指します．

そこで，障害物を回避する周辺のコードをオリジナルなものに置き換えることで，いいかんじにしていきたいです．今回は，Autowareで，どのあたりをどう変えればいいのかを見てみようと思います．

## プランニング周りの概観

Autoware-Miniのドキュメントを見ると、プランニング周りの構造は以下のようになっているようです。

![autoware-mini-node-diagram.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320782/dca2bc28-65b6-9ce6-ca5c-a62ded803ad1.png)

- [mission_planner](https://autowarefoundation.github.io/autoware.universe/main/planning/mission_planner/)

    ここでは指定されたgoalに辿り着くためのrouteを求めているようです．ここで，goalはPoseのようなもので指定されており，routeとは経路や軌道ではなく，以下の図のようなマップにある領域のIDの列のようなものだそうです．
    ![](https://autowarefoundation.github.io/autoware.universe/main/planning/mission_planner/media/route_sections.svg)

- [behavior_path_planner](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_planner/)

    先程のルートから経路を生成しているようです．`lane_following`というのはドキュメントの説明をみると，laneletのセンターラインから経路を生成するもののようです．

- [behavior_velocity_planner](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_velocity_planner/)

    これは，交通ルールから速度を修正する機能があるらしいです．
    ![](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_velocity_planner/docs/set_stop_velocity.drawio.svg)
    [intersection](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_velocity_intersection_module/)は交差点の交通ルールを，[stop_line](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_velocity_stop_line_module/)は止まるべき線で止まるためのものなのでしょうか．今回のシミュレーションには，あまり関係なさそうなので全ての機能をOFFにしたいですね．機能のON/OFFは恐らく[このファイル](https://github.com/AutomotiveAIChallenge/aichallenge2023-sim/blob/main/docker/aichallenge/aichallenge_ws/src/aichallenge_submit/autoware_launch/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml)で書き換えることが出来そうです．
```yaml
    launch_stop_line: false
    launch_crosswalk: false
    launch_traffic_light: false
    launch_intersection: false
    launch_blind_spot: false
    launch_detection_area: false
    launch_virtual_traffic_light: false
    launch_occlusion_spot: false
    launch_no_stopping_area: false
    launch_run_out: false
    launch_speed_bump: false
```
- [obstacle_avoidance_planner](https://autowarefoundation.github.io/autoware.universe/main/planning/obstacle_avoidance_planner/)
    
    このパッケージでは，入力された経路、走行可能な領域に基づいて、運動学的に走行可能で衝突のない軌道を生成しているようです．ただ，[input topics](https://autowarefoundation.github.io/autoware.universe/main/planning/obstacle_cruise_planner/#input-topics)に動的に検出された障害物のobjectの情報などは含まれておらず，ここで生成するtrajectoryは静的な障害物を避けるだけのようです．

- [obstacle_stop_planner](https://autowarefoundation.github.io/autoware.universe/main/planning/obstacle_stop_planner/)
    このパッケージでは，動的な障害物が前にあると，車速を落としたり停止するといった処理を行っているようです．
    

## 変えるべきところ

障害物回避がうまくいってないので，そこを変えたいです．障害物を検知して処理しているところは，どうやら`obstacle_stop_planner`のようです．なので，ここを自作コードに取り替えたいです．`obstacle_stop_planner`の代わりに以下のようなイメージのパッケージを自作したいと思います！
![ファイル_000.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320782/4b514a7d-fff5-d3b4-bcc9-d16188e4c775.png)



## 自作パッケージの雛形

とりあえず上記の目標を達成するために，`obstacle_stop_planner`の代わりとなる，trajectoryやobstacle，自己位置の情報をsubscribeしてtrajectoryをpublishするNodeを作りました．ただし，このNodeでは取得したtrajectoryをそのままpublishしているので，障害物を無視して突き進むようになっています．

```python
import rclpy
from autoware_auto_perception_msgs.msg import PredictedObject
from autoware_auto_planning_msgs.msg import Trajectory
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
from nav_msgs.msg import Odometry

class OriginalLocalPlanner(Node):
    def __init__(self) -> None:
        super().__init__("original_local_planner")

        self.pub_trj = self.create_publisher(
            Trajectory,
            "~/output/trajectory",
            10,
        )

        self.sub_trj = self.create_subscription(
            Trajectory,
            "~/input/trajectory",
            self.listener_callback,
            10,
        )

        self.sub_obj = self.create_subscription(
            PredictedObject,
            "~/input/objects",
            lambda _: ...,
            10,
        )

        self.sub_pc = self.create_subscription(
            PointCloud2,
            "~/input/pointcloud",
            lambda _: ...,
            10,
        )

        self.sub_odm = self.create_subscription(
            Odometry,
            "~/input/odometry",
            lambda _: ...,
            10,
        )

    def listener_callback(self, msg: Trajectory):
        self.pub_trj.publish(msg)


def main(args=None):
    rclpy.init(args=args)

    node = OriginalLocalPlanner()

    rclpy.spin(node)

    node.destroy_node()

    rclpy.shutdown()


if __name__ == "__main__":
    main()
```

また，launchファイルを書き換えて，`obstacle_stop_planner`をlaunchしていたところを自作パッケージに置き換えます．変えるべきファイルは[docker/aichallenge/aichallenge_ws/src/aichallenge_submit/aichallenge_submit_launch/launch/autoware_mini_awsim.launch.xml](https://github.com/AutomotiveAIChallenge/aichallenge2023-sim/blob/main/docker/aichallenge/aichallenge_ws/src/aichallenge_submit/aichallenge_submit_launch/launch/autoware_mini_awsim.launch.xml)です．ここの`obstacle_stop_planner`をコメントアウトして自作ノードを起動します．

```xml
            <!-- motion_planning::ObstacleStopPlannerNode -->
            <!-- <composable_node pkg="obstacle_stop_planner" plugin="motion_planning::ObstacleStopPlannerNode" name="obstacle_stop_planner" namespace="">
              <remap from="~/input/acceleration" to="/localization/acceleration" />
              <remap from="~/input/pointcloud" to="/perception/obstacle_segmentation/pointcloud" />
              <remap from="~/input/objects" to="/perception/object_recognition/objects" />
              <remap from="~/input/odometry" to="/localization/kinematic_state" />
              <remap from="~/input/trajectory" to="obstacle_avoidance_planner/trajectory" />
              <remap from="~/output/trajectory" to="/planning/scenario_planning/trajectory" />
              <remap from="~/output/velocity_limit_clear_command" to="/planning/scenario_planning/clear_velocity_limit" />
              <remap from="~/output/max_velocity" to="/planning/scenario_planning/max_velocity_candidates" />
              <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons" />
              <remap from="~/output/stop_reason" to="/planning/scenario_planning/status/stop_reason" />
              <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml" />
              <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml" />
              <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/common/nearest_search.param.yaml" />
              <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/common/common.param.yaml" />
              <param from="$(find-pkg-share $(var vehicle_model)_description)/config/vehicle_info.param.yaml" />
              <extra_arg name="use_intra_process_comms" value="false" />
            </composable_node> -->

          </node_container> <!-- motion_planning_container -->

          <node pkg="original_local_planner" exec="original_local_planner" name="original_local_planner" output="screen">
            <remap from="~/input/pointcloud" to="/perception/obstacle_segmentation/pointcloud" />
            <remap from="~/input/objects" to="/perception/object_recognition/objects" />
            <remap from="~/input/odometry" to="/localization/kinematic_state" />
            <remap from="~/input/trajectory" to="obstacle_avoidance_planner/trajectory" />
            <remap from="~/output/trajectory" to="/planning/scenario_planning/trajectory" />
          </node>
```

### 実行結果

![Screenshot from 2023-08-10 18-55-37.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320782/05138317-6e6c-323b-b141-312fcb119d67.png)

こんな感じで，すべての障害物を無視して突き進むようになりました．ただ，この先で障害物に関係なくカーブするところで止まってしまいます．
![Screenshot from 2023-08-10 19-18-05.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320782/e0675776-2bda-9b95-6c48-3b856eccfef3.png)
障害物回避以外にも修正すべき点がありそうですね．