自動運転AIchallenge-2024の備忘録(2)

はじめに

　前回は環境構築とこれから行うことについてまとめました．今回は，どのように周回しているのか?について，まとめようと思います.

実行&仮説

　それでは，とりあえず実行してみましょう.少し自分用にrvizを変更しました!こちらにrvizを共有しておきます.

さて，よく見ていただけるとわかると思いますが，以下のことがわかります.

• ルートは中心線に沿って生成されている
• ゴールが次々に切り替わっている

以上のことについて，考察していきます.

1. ルート生成

　とりあえずどのようにルートができているのか確認します．
/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xmlを確認すると各機能で何が使用されているのかがわかります.今回はMapに関連する部分の抜粋をします.

reference.launch.xml

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <!-- 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"/>
 
<!-- ----------------------- -->

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


</launch>

　上記を見ればわかるように,/aichallenge_submit_launch/map/lanelet2_map.osmが関連ありそうです．Vector Map Builderというツールがあるので，そこで確認してみましょう.

以下は，vector map builderにlaneletをimportした結果です.

　どうやら，laneletに中心線が定められていることがわかります.これを変更することでルートを変更することができそうです.最適なコース取りになるように変えることができれば，タイムを縮めることが期待できそうです.

2. ゴール変更

　続いて，ゴールが次々と変わることについてaichallenge_submit.launch.xmlを確認していきましょう.

aichallenge_submit.launch.xml

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <log message="This is aichallenge_submit_launch."/>
  <include file="$(find-pkg-share aichallenge_submit_launch)/launch/reference.launch.xml" >
    <arg name="vehicle_model" value="racing_kart"/>
    <arg name="sensor_model" value="racing_kart_sensor_kit"/>
    <arg name="map_path" value="$(find-pkg-share aichallenge_submit_launch)/map"/>
  </include>

  <!-- place a goal pose anywhere you like-->
  <node pkg="goal_pose_setter" exec="goal_pose_setter_node" name="goal_pose_setter" output="screen">
    <param from="$(find-pkg-share goal_pose_setter)/config/default_goal_pose.param.yaml" />
  </node>
</launch>

　ここにあるgoal_pose_setterが怪しそうなので，一旦コメントアウトして実行してみましょう.公式ドキュメントにあったように実行してみます.

# docker内にて
./run_autoware.bash 

# 別ターミナル1にて
./run_simulator.bash

# 別ターミナル2にて
 ros2 topic pub --once /control/control_mode_request_topic std_msgs/msg/Bool '{data: true}'

すると，以下のようなWarningが出て,うまく起動しませんでした．

[WARN] [1719222028.574910069] [localization.ekf_localizer]: The node is not activated. Provide initial pose to pose_initializer

　どうやら，初期位置の初期化ができていないようです.検証した結果，以下のコマンドを打つことで解決されました.

ros2 service call /localization/trigger_node std_srvs/srv/SetBool "{data: true}"

　これにより，自由にゴールを設置することが可能になり，ループしなくなりました．やはり，goal_pose_setterが周回の基盤ということです.
　さらに，ゴールを通過するようになっていました.つまり，ゴールで停止するのではなく，速度を維持したまま通過していたということです.このことから，ゴールの再設定を繰り返すことで周回を再現していたと考えられます.実際に，goal_pose_setterを確認してみると，以下のようにgoalとhalf_goalが存在しますね.paramで決めた座標に変更されるようです.

goal_pose_setter_node.cpp

    goal_position_.position.x = this->get_parameter("goal.position.x").as_double();
    goal_position_.position.y = this->get_parameter("goal.position.y").as_double();
    goal_position_.position.z = this->get_parameter("goal.position.z").as_double();
    goal_position_.orientation.x = this->get_parameter("goal.orientation.x").as_double();
    goal_position_.orientation.y = this->get_parameter("goal.orientation.y").as_double();
    goal_position_.orientation.z = this->get_parameter("goal.orientation.z").as_double();
    goal_position_.orientation.w = this->get_parameter("goal.orientation.w").as_double();


    half_goal_position_.position.x = this->get_parameter("half_goal.position.x").as_double();
    half_goal_position_.position.y = this->get_parameter("half_goal.position.y").as_double();
    half_goal_position_.position.z = this->get_parameter("half_goal.position.z").as_double();
    half_goal_position_.orientation.x = this->get_parameter("half_goal.orientation.x").as_double();
    half_goal_position_.orientation.y = this->get_parameter("half_goal.orientation.y").as_double();
    half_goal_position_.orientation.z = this->get_parameter("half_goal.orientation.z").as_double();
    half_goal_position_.orientation.w = this->get_parameter("half_goal.orientation.w").as_double();

default_goal_pose.param.yaml

/**:
  ros__parameters:
    goal.position.x: 89653.7
    goal.position.y: 43122.5
    goal.position.z: 0.0
    goal.orientation.x: 0.0
    goal.orientation.y: 0.0
    goal.orientation.z: -0.971732
    goal.orientation.w: 0.236088

    half_goal.position.x: 89657.0
    half_goal.position.y: 43175.0
    half_goal.position.z: -28.0
    half_goal.orientation.x: 0.0
    half_goal.orientation.y: 0.0
    half_goal.orientation.z: -0.9
    half_goal.orientation.w: 0.25

    goal_range: 10.0

しかし，現状ではきちんと周回できていますが，今後障害物等が出てきた際など対応できない可能性があるので変更の必要性がありそうです.

まとめ

今回は，laneletによってルートを変更できることと，任意のゴールを設定するためには?ということについてまとめてみました.次回は,中心線の最適化やパラメータ調整についてまとめていこうかと思います!コメント等あればお気軽にお願いします!!