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

概要

自動運転AIチャレンジの初期状態のコードを動かすと，以下のように障害物で止まってしまいます．(一個目の障害物には何故が突撃していますが...)

ここで，初期のコードとはCustomizeでAutoware-Miniを使っている状態を指します．

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

プランニング周りの概観

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

mission_planner

ここでは指定されたgoalに辿り着くためのrouteを求めているようです．ここで，goalはPoseのようなもので指定されており，routeとは経路や軌道ではなく，以下の図のようなマップにある領域のIDの列のようなものだそうです．
behavior_path_planner

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

これは，交通ルールから速度を修正する機能があるらしいです．

intersectionは交差点の交通ルールを，stop_lineは止まるべき線で止まるためのものなのでしょうか．今回のシミュレーションには，あまり関係なさそうなので全ての機能をOFFにしたいですね．機能のON/OFFは恐らくこのファイルで書き換えることが出来そうです．

    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

このパッケージでは，入力された経路、走行可能な領域に基づいて、運動学的に走行可能で衝突のない軌道を生成しているようです．ただ，input topicsに動的に検出された障害物のobjectの情報などは含まれておらず，ここで生成するtrajectoryは静的な障害物を避けるだけのようです．
obstacle_stop_planner
このパッケージでは，動的な障害物が前にあると，車速を落としたり停止するといった処理を行っているようです．

変えるべきところ

障害物回避がうまくいってないので，そこを変えたいです．障害物を検知して処理しているところは，どうやらobstacle_stop_plannerのようです．なので，ここを自作コードに取り替えたいです．obstacle_stop_plannerの代わりに以下のようなイメージのパッケージを自作したいと思います！

自作パッケージの雛形

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

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です．ここのobstacle_stop_plannerをコメントアウトして自作ノードを起動します．

            <!-- 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>

実行結果

こんな感じで，すべての障害物を無視して突き進むようになりました．ただ，この先で障害物に関係なくカーブするところで止まってしまいます．

障害物回避以外にも修正すべき点がありそうですね．