🚗

自動運転AIチャレンジ　予測経路をコントローラで走行するノードを作成

2024/01/08に公開

ROS2

背景

自動運転AIチャレンジ2023（シミュレーション)に参加しており、自作AIの構築とAutoware環境への組み込みにチャレンジしたいと考えております。まずは、生成された経路に対して、車両を制御するAIの構築を目指したいと思います。前回の記事ではあらかじめ設定したthrottleで、Autowareによって生成された経路を走行しました。
しかし、 VehicleInputs の制御では throttle と brake の両方を調整する必要があり、かなり複雑になります。今回はより上位のレイヤで車両を制御するために、生成された経路を予め設定した速度で走行させることを目指します。また、今後AIに用いる訓練データの収集を行うためにコントローラを用いて手動で速度を調整できるようにします。

環境

前回と同じく以下のaichallenge2023-racingのリポジトリを使用しております。

方針

今回も本大会のベースのフレームワークの一つとなる Autoware microを使用します。
※Autoware microの詳細についてはこちらをご参照ください

Autoware-micro

前回の記事と同じく、簡易的な制御方法を考えます。
今回は、simple_pure_pursuit からトピックを受信し、新たにトピックを送信するノードを作成します。これにより、作成したノード内で、simpure_pure_pursuitから受信した目標の速度を変更することができます。
作成するノード

実装

トピックの送受信の編集

simple_pure_pursuit が送信するトピック名を変更します。具体的には下記のようにautoware_micro_awsim.launch.xml を修正しビルドします。

編集したautoware_micro_awsim.launch.xmlの全体は以下になります。

autoware_micro_awsim.launch.xml

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <!-- Essential parameters -->
  <arg name="vehicle_id" default="$(env VEHICLE_ID default)" description="vehicle specific ID"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="map_path" default="/aichallenge/mapfile"/>
  <arg name="vehicle_model" default="dallara"  description="vehicle model name"/>

  <!-- Optional parameters -->
  <!-- Map -->
  <arg name="lanelet2_map_file" default="lanelet2_map.osm" description="lanelet2 map file name"/>
  <arg name="pointcloud_map_file" default="pointcloud_map.pcd" description="pointcloud map file name"/>
  <!-- Control -->
  <!-- Vehicle -->
  <arg name="launch_vehicle_interface" default="false"/>
  <!-- System -->
  <arg name="system_run_mode" default="online" description="run mode in system"/>

  <arg name="model_file" default="$(find-pkg-share dallara_description)/urdf/dallara.xacro" description="path to the file of model settings (*.xacro)"/>
  <node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
      <param name="robot_description" value="$(command 'xacro $(var model_file)' 'warn')"/>
  </node>
  <node pkg="dallara_interface" exec="dallara_interface_node" name="dallara_interface" output="screen"/>

  <!-- Localization -->
  <group>
    <push-ros-namespace namespace="localization"/>
    <node pkg="topic_tools" exec="relay" name="relay" output="screen">
      <param name="input_topic" value="/awsim/ground_truth/localization/kinematic_state"/>
      <param name="output_topic" value="/localization/kinematic_state"/>
     </node>
    <node pkg="odom2tf" exec="odom2tf_node" name="odom2tf" output="screen">
      <remap from="in_odom" to="/awsim/ground_truth/localization/kinematic_state"/>
      <remap from="out_twist" to="/localization/twist_estimator/twist_with_covariance"/>
    </node>
    <!-- twist2accel -->
    <group>
      <node pkg="twist2accel" exec="twist2accel" name="twist2accel" output="screen">
        <param name="accel_lowpass_gain" value="0.9"/>
        <param name="use_odom" value="true"/>
        <remap from="input/odom" to="/localization/kinematic_state"/>
        <remap from="input/twist" to="/localization/twist_estimator/twist_with_covariance"/>
        <remap from="output/accel" to="/localization/acceleration"/>
      </node>
    </group>
  </group>


  <!-- Map -->
  <group>
    <push-ros-namespace namespace="map"/>

    <!-- map_container -->
    <node_container pkg="rclcpp_components" exec="component_container" name="map_container" namespace="">

      <!-- map_loader::Lanelet2MapLoaderNode -->
      <composable_node pkg="map_loader" plugin="Lanelet2MapLoaderNode" name="lanelet2_map_loader" namespace="">
        <remap from="output/lanelet2_map" to="vector_map" />
        <param name="lanelet2_map_path" value="$(var map_path)/$(var lanelet2_map_file)" />
        <param from="$(find-pkg-share autoware_launch)/config/map/lanelet2_map_loader.param.yaml" />
        <extra_arg name="use_intra_process_comms" value="false" />
      </composable_node>

      <!-- map_loader::Lanelet2MapVisualizationNode -->
      <composable_node pkg="map_loader" plugin="Lanelet2MapVisualizationNode" name="lanelet2_map_visualization" namespace="">
        <remap from="input/lanelet2_map" to="vector_map" />
        <remap from="output/lanelet2_map_marker" to="vector_map_marker" />
        <param name="lanelet2_map_path" value="$(var map_path)/$(var lanelet2_map_file)" />
        <param from="$(find-pkg-share autoware_launch)/config/map/lanelet2_map_loader.param.yaml" />
        <extra_arg name="use_intra_process_comms" value="false" />
      </composable_node>

      <!-- map_tf_generator::VectorMapTFGeneratorNode -->
      <composable_node pkg="map_tf_generator" plugin="VectorMapTFGeneratorNode" name="vector_map_tf_generator" namespace="">
        <param name="map_frame" value="map" />
        <param name="viewer_frame" value="viewer" />
        <extra_arg name="use_intra_process_comms" value="false" />
      </composable_node>
     
    </node_container>
  </group> <!-- map -->

  <!-- Perception -->
  <group>
    <push-ros-namespace namespace="perception"/>

    <!-- object_recognition -->
    <group>
      <push-ros-namespace namespace="object_recognition"/>

      <!-- detection -->
      <group>
        <push-ros-namespace namespace="detection"/>
        <node pkg="topic_tools" exec="relay" name="object_relay" output="screen">
          <param name="input_topic" value="/awsim/ground_truth/perception/object_recognition/detection/objects"/>
          <param name="output_topic" value="objects"/>
        </node>
      </group> <!-- detection -->

      <!-- tracking -->
      <group>
        <push-ros-namespace namespace="tracking"/>
        <!-- multi_object_tracker -->
        <node pkg="multi_object_tracker" exec="multi_object_tracker" name="multi_object_tracker" output="screen">
          <remap from="input" to="/awsim/ground_truth/perception/object_recognition/detection/objects"/> <!-- autoware_auto_perception_msgs/DetectedObjects -->
          <remap from="output" to="objects"/> <!-- autoware_auto_perception_msgs/TrackedObjects -->
          <param name="world_frame_id" value="map"/>
          <param name="publish_rate" value="10.0"/>
          <param name="enable_delay_compensation" value="false"/>
          <param from="$(find-pkg-share multi_object_tracker)/config/default_tracker.param.yaml"/>
          <param from="$(find-pkg-share multi_object_tracker)/config/data_association_matrix.param.yaml"/>
        </node>
      </group> <!-- tracking -->

      <!-- prediction -->
      <group>
        <push-ros-namespace namespace="prediction"/>
        <!-- map_based_prediction -->
        <node pkg="map_based_prediction" exec="map_based_prediction" name="map_based_prediction" output="screen">
          <remap from="/vector_map" to="/map/vector_map"/>
          <remap from="input" to="/perception/object_recognition/tracking/objects"/>
          <remap from="objects" to="/perception/object_recognition/objects"/> <!-- autoware_auto_perception_msgs/PredictedObjects -->
          <param from="$(find-pkg-share map_based_prediction)/config/map_based_prediction.param.yaml"/>
        </node>
      </group> <!-- prediction -->
    </group> <!-- object_recognition -->
  </group> <!-- Perception -->

  <!-- Planning -->
  <group>
    <push-ros-namespace namespace="planning"/>

    <!-- mission_planning -->
    <group>
      <push-ros-namespace namespace="mission_planning"/>

      <!-- mission_planner -->
      <node pkg="mission_planner" exec="mission_planner" name="mission_planner" output="screen">
        <remap from="input/modified_goal" to="/planning/scenario_planning/modified_goal"/>
        <remap from="input/vector_map" to="/map/vector_map"/>
        <!-- <remap from="/localization/kinematic_state" to="/awsim/ground_truth/localization/kinematic_state"/> -->
        <remap from="debug/route_marker" to="/planning/mission_planning/route_marker"/>
        <param from="$(find-pkg-share autoware_launch)/config/planning/mission_planning/mission_planner/mission_planner.param.yaml"/>
      </node>

      <!-- goal_pose_visualizer -->
      <node pkg="mission_planner" exec="goal_pose_visualizer" name="goal_pose_visualizer" output="screen">
        <remap from="input/route" to="/planning/mission_planning/route"/>
        <remap from="output/goal_pose" to="/planning/mission_planning/echo_back_goal_pose"/>
      </node>

    </group> <!-- mission_planning -->

    <!-- scenario_planning -->
    <group>
      <push-ros-namespace namespace="scenario_planning"/>

      <!-- scenario_selector -->
      <group>
        <arg name="cmd" default="ros2 topic pub /planning/scenario_planning/scenario tier4_planning_msgs/msg/Scenario '{current_scenario: LaneDriving, activating_scenarios: [LaneDriving]}'"/>
        <executable cmd="$(var cmd)" name="scenario_pub" shell="true"/>
      </group> <!-- scenario_selector -->

      <!-- operation_mode -->
      <group>
        <arg name="cmd" default="ros2 topic pub /system/operation_mode/state autoware_adapi_v1_msgs/msg/OperationModeState '{
          mode: 1,
          is_autoware_control_enabled: true,
          is_in_transition: false,
          is_stop_mode_available: true,
          is_autonomous_mode_available: true,
          is_local_mode_available: true,
          is_remote_mode_available: true
          }'"/>
        <executable cmd="$(var cmd)" name="operation_mode_pub" shell="true"/>
      </group> <!-- operation_mode -->

      <!-- lane_driving -->
      <group>
        <push-ros-namespace namespace="lane_driving"/>

        <!-- behavior_planning -->
        <group>
          <push-ros-namespace namespace="behavior_planning"/>

          <!-- behavior_planning_container -->
          <node_container pkg="rclcpp_components" exec="component_container" name="behavior_planning_container" namespace="">

            <!-- behavior_path_planner::BehaviorPathPlannerNode -->
            <composable_node pkg="behavior_path_planner" plugin="behavior_path_planner::BehaviorPathPlannerNode" name="behavior_path_planner" namespace="">
              <remap from="~/input/route" to="/planning/mission_planning/route" />
              <remap from="~/input/vector_map" to="/map/vector_map" />
              <remap from="~/input/perception" to="/perception/object_recognition/objects" /> <!-- autoware_auto_perception_msgs/PredictedObjects -->
              <remap from="~/input/occupancy_grid_map" to="/perception/occupancy_grid_map/map" />
              <remap from="~/input/costmap" to="/planning/scenario_planning/parking/costmap_generator/occupancy_grid" />
              <remap from="~/input/odometry" to="/localization/kinematic_state" />
              <remap from="~/input/accel" to="/localization/acceleration" />
              <remap from="~/input/scenario" to="/planning/scenario_planning/scenario" />
              <remap from="~/output/path" to="path_with_lane_id" />
              <remap from="~/output/turn_indicators_cmd" to="/planning/turn_indicators_cmd" />
              <remap from="~/output/hazard_lights_cmd" to="/planning/hazard_lights_cmd" />
              <remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal" />
              <param name="bt_tree_config_path" value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml"/>
              <param name="lane_change.enable_abort_lane_change" value="false"/>
              <param name="lane_change.enable_collision_check_at_prepare_phase" value="false"/>
              <param name="lane_change.use_predicted_path_outside_lanelet" value="false"/>
              <param name="lane_change.use_all_predicted_path" value="false"/>
              <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/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml" />
              <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml" />
              <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml" />
              <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml" />
              <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml" />
              <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml" />
              <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml" />
              <extra_arg name="use_intra_process_comms" value="false" />
            </composable_node>

          </node_container>
        </group> <!-- behavior_planning -->
      </group> <!-- lane_driving -->

      <!-- Customizable -->
      <node pkg="path_to_trajectory" exec="path_to_trajectory_node" name="path_to_trajectory" output="screen">
        <remap from="input" to="/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id"/>
        <remap from="output" to="/planning/scenario_planning/trajectory"/>
      </node>

    </group>
  </group>

  <node pkg="simple_pure_pursuit" exec="simple_pure_pursuit" name="simple_pure_pursuit_node" output="screen">
    <param name="use_external_target_vel" value="false"/>
    <param name="external_target_vel" value="100.0"/>
    <param name="lookahead_gain" value="0.4"/>
    <param name="lookahead_min_distance" value="5.0"/>
    <param name="speed_proportional_gain" value="1.0"/>
    
    <remap from="input/kinematics" to="/localization/kinematic_state"/>
    <remap from="input/trajectory" to="/planning/scenario_planning/trajectory"/>
    <remap from="output/control_cmd" to="/control/command/pre_control_cmd"/>
  </node>

  <!-- vehicle -->
  <include file="$(find-pkg-share raw_vehicle_cmd_converter)/launch/raw_vehicle_converter.launch.xml">
    <arg name="max_throttle" value="1.0"/>
    <arg name="max_brake" value="1.0"/>
    <arg name="max_steer" value="300.0"/>
    <arg name="min_steer" value="-300.0"/>    
    <arg name="csv_path_accel_map" value="$(find-pkg-share dallara_launch)/config/accel_map.csv"/>
    <arg name="csv_path_brake_map" value="$(find-pkg-share dallara_launch)/config/brake_map.csv"/>
    <arg name="convert_accel_cmd" value="true" />
    <arg name="convert_brake_cmd" value="true" />
    <arg name="convert_steer_cmd" value="false" />
  </include>

  <!-- API -->
  <group>
    <!-- default_ad_api -->
    <include file="$(find-pkg-share default_ad_api)/launch/default_ad_api.launch.py" />

    <!-- ad_api_adaptors -->
    <include file="$(find-pkg-share ad_api_adaptors)/launch/rviz_adaptors.launch.xml" />
  </group>
</launch>

1. 固定の速度で走行

前回のスクリプトを少し変更しただけですが、下記のスクリプトで20km/h で推定されて経路を走行することができました。

command_setter.py

#! /usr/bin/env python
# -*- coding: utf-8 -*-

import time
import rclpy
from rclpy.node import Node
from autoware_auto_control_msgs.msg import AckermannControlCommand


class Command_setter(Node):
    def __init__(self):
        super().__init__('Command_setter')

        # simple_pure_pursuit からトピックを受信
        self.create_subscription(AckermannControlCommand, "/control/command/pre_control_cmd", self.onTrigger, 1)

        # raw_vehicle_cmd_converter にトピックを送信
        self.vehicle_inputs_pub_ = self.create_publisher(AckermannControlCommand, "/control/command/control_cmd", 1)

        # 時速 20km/h で固定
        target_vel = 20.0

        # km/h ->m/s
        self.target_vel =  target_vel / 3.6

        # 比例係数 (simple_pure_pursuitと同値である必要あり)
        self.speed_proportional_gain_ = 1.0

        # log を出力するかどうか
        self.log = False

    def onTrigger(self, msg):
        
        # 現在の速度を逆計算 
        current_longitudinal_vel = msg.longitudinal.speed - (msg.longitudinal.acceleration / self.speed_proportional_gain_) 
                                                             
        if self.log:
            self.get_logger().info("current vel {}".format(current_longitudinal_vel * 3.6))
        
        msg.longitudinal.speed = self.target_vel

        # accel を計算
        msg.longitudinal.acceleration = self.speed_proportional_gain_ * (self.target_vel - current_longitudinal_vel) 

        # トピックを送信
        self.vehicle_inputs_pub_.publish(msg)

def main(args=None):
    print('Hi from Command_setter.')
    rclpy.init(args=args)
    node = Command_setter()
    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

実行方法

実行のためには３つのターミナルを起動します。また、事前にAIチャレンジ2023のdockerコンテナの中に入っていることが前提になります。AIチャレンジ2023のdockerコンテナへの入り方は下記になります。

cd ~/aichallenge2023-racing/docker/train/
bash build_docker.sh
bash run_container.sh

AIチャレンジ2023のdocker コンテナの詳細についてはこちらをご参照ください。

実行方法(1)

１つ目のターミナルでAWSIMを起動します。

cd /aichallenge/
bash build_autoware.sh
bash run_awsim.sh

2つ目のターミナルでAutowareを起動します。dallara_interface を変更したため、この時点では車両にトピックが送信されてないので車両は停止したままです。

cd /aichallenge/
bash run_autoware.sh

3つ目のターミナルで作成したノードを起動します。起動後に車両が走行を開始します。

cd /aichallenge/
source aichallenge_ws/install/setup.bash 
ros2 run control_command_setter command_setter

2. コントローラで速度を変更しながら走行

次にコントローラーを使って、経路に合わせて手動で速度を変更できるようにします。
今回はNintendo Switch コントローラーを用います。
接続方法についてはこちらの記事をご覧ください。

デバイスをrocker 内にマウントする必要があるため、rocker コンテナの起動スクリプトを下記のように変更してください。

結果的に作成したスクリプトは以下になります。今回は pygame を用いて一定間隔でコントローラからの入力を受取り、AckermannControlCommand に格納しています。

controller_cmd_setter.py

#! /usr/bin/env python
# -*- coding: utf-8 -*-

import time
import sys
import rclpy
import pygame
from rclpy.node import Node
from autoware_auto_control_msgs.msg import AckermannControlCommand


class Controller_cmd_setter(Node):
    def __init__(self, joystick=None):
        super().__init__('Controller_cmd_setter')

        # simple_pure_pursuit からトピックを受信
        self.create_subscription(AckermannControlCommand, "/control/command/pre_control_cmd", self.onTrigger, 1)

        # raw_vehicle_cmd_converter にトピックを送信
        self.vehicle_inputs_pub_ = self.create_publisher(AckermannControlCommand, "/control/command/control_cmd", 1)

        # 初期値を時速 20km/h とする
        self.target_vel = 20.0

        # 比例係数 (simple_pure_pursuitと同値である必要あり)
        self.speed_proportional_gain_ = 1.0

        # log を出力するかどうか
        self.log = False

        # 0.1 秒ごとにコントローラーの入力を更新
        timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.timer_callback)

        if joystick is None:
            print("ジョイスティックが見つかりません。")
            sys.exit()
        self.joystick = joystick

    # コントローラーからの入力を更新
    def timer_callback(self):
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                pygame.quit()
                sys.exit()

        self.target_vel  =  self.target_vel + self.joystick.get_axis(3) * (-1.0)
        
        # Aボタンで速度を +10 km/s
        if self.joystick.get_button(0): 
            self.target_vel += 10.0
        
        # Aボタンで速度を -10 km/s
        if self.joystick.get_button(1): 
            self.target_vel -= 10.0

        self.target_vel = min(max( self.target_vel, 0), 150)
        self.get_logger().info("target_vel:{}".format(self.target_vel))

    # 制御コマンドを送受信
    def onTrigger(self, msg):
        # km/h ->m/s
        target_vel = self.target_vel / 3.6

        # 現在の速度を逆計算 
        current_longitudinal_vel = msg.longitudinal.speed - (msg.longitudinal.acceleration / self.speed_proportional_gain_) 

        if self.log:
            self.get_logger().info("current vel {}".format(current_longitudinal_vel * 3.6))
        
        msg.longitudinal.speed = float(target_vel)

        # accel を計算
        msg.longitudinal.acceleration = float(self.speed_proportional_gain_ * (target_vel - current_longitudinal_vel))

        # トピックを送信
        self.vehicle_inputs_pub_.publish(msg)

def main(args=None):
    print('Hi from Controller_cmd_setter.')

    # rosノードを初期化
    rclpy.init(args=args)

    # Pygameの初期化
    pygame.init()

    # pygame を初期化
    pygame.joystick.init()

    # 利用可能なジョイスティックの数を取得
    joystick_count = pygame.joystick.get_count()

    if joystick_count == 0:
        print("ジョイスティックが見つかりません。")
        sys.exit()

    # 0番のジョイスティックを取得
    joystick = pygame.joystick.Joystick(0)
    joystick.init()

    node = Controller_cmd_setter(joystick=joystick)
    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

実行方法(2)

実行方法(1)の３つ目のターミナルで下記を実行する。

cd /aichallenge/
source aichallenge_ws/install/setup.bash 
ros2 run control_command_setter controller_cmd_setter

結果

下記がコントローラで速度調整しながら走行した例です。
動画では１つ目のシケインまでを120km/h に設定し、１つ目のシケイン直前で30km/h まで速度を落としています。その後、また150km/hまで加速して、２つ目のシケイン手前で50km/h まで減速しています。ただ、２つ目のシケイン前で他の車両に追いついてしまい、衝突してしまいました。そのため、150km/h 近くで走行させたい場合は障害物を回避するように経路を立てることも重要になりそうです。

ROS2のパッケージ

作成したパッケージの全体は以下になります。

まとめ

今回はAIチャレンジ2023(シミュレーション）のAI構築に向けた準備として、推定された経路をコントローラから入力した速度で走行させる方法を記載しました。今後はこちらのノードを使用し、AI構築のための訓練データの収集を行って行きたいと思います。