概要
自動運転AIチャレンジの初期状態のコードを動かすと，以下のように障害物で止まってしまいます．(一個目の障害物には何故が突撃していますが...)
ここで，初期のコードとはCustomizeでAutoware-Miniを使っている状態を指します．
そこで，障害物を回避する周辺のコードをオリジナルなものに置き換えることで，いいかんじにしていきたいです．今回は，Autowareで，どのあたりをどう変えればいいのかを見てみようと思います．
プランニング周りの概観
Autoware-Miniのドキュメントを見ると、プランニング周りの構造は以下のようになっているようです。
-
ここでは指定されたgoalに辿り着くためのrouteを求めているようです．ここで，goalはPoseのようなもので指定されており，routeとは経路や軌道ではなく，以下の図のようなマップにある領域のIDの列のようなものだそうです．
-
先程のルートから経路を生成しているようです．
lane_followingというのはドキュメントの説明をみると，laneletのセンターラインから経路を生成するもののようです． -
これは，交通ルールから速度を修正する機能があるらしいです．
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
-
このパッケージでは，入力された経路、走行可能な領域に基づいて、運動学的に走行可能で衝突のない軌道を生成しているようです．ただ，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>
実行結果
こんな感じで，すべての障害物を無視して突き進むようになりました．ただ，この先で障害物に関係なくカーブするところで止まってしまいます．
障害物回避以外にも修正すべき点がありそうですね．