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

はじめに

前回はgoal setterや，ルートを変更するには?についてまとめました．今回は，いよいよレーンの最適化についてまとめようと思います.まずは，前知識と準備から見ていきましょう．

前提知識

前回軽くlanelet2について触れました．マップ情報はここを基準に出力され，中心線も定義できました．

座標を変更仕様とした際，画像のようにx,yがとても大きいことに気づいたと思います．

なぜ，これほど大きいのか？の理由はおそらく今回の位置推定方法にあると考えられます.
今回使用されているのは，GNSSとIMU,wheel encoderを用いたekf_localizeなので，GNSSの座標に対応した結果これほど大きい数値になってしまったと考えられます.

Mapの原点

マップの最適化を行うにしても数値が大きいのは困ります．故に，初期位置をoffsetとして，相対地図にすることを考えました．初期位置を取得する方法は色々あるのですが，今回はtfを利用することで取得しました.tfについて知りたい場合は，以下を参考にしてください.

map->baselinkを参照するとoffsetは以下のようになりました.

ofset_x = 89633.15625
ofset_y = 43127.796875

lanelet2データの描画

rvizでMapデータを見ることができますが，実際のデータはどのような構造になっているのでしょうか?中身の一部を以下に示します.

lanelet2_map.osm

<?xml version="1.0" encoding="UTF-8"?>
<osm generator="VMB">
  <MetaInfo format_version="1" map_version="10"/>
  <node id="8" lat="35.62581952966983" lon="139.78142932797388">
    <tag k="mgrs_code" v="54SUE896431"/>
    <tag k="local_x" v="89653.9564"/>
    <tag k="local_y" v="43131.2322"/>
    <tag k="ele" v="43.217"/>
  </node>
  <node id="9" lat="35.625788527958306" lon="139.78142310661156">
    <tag k="mgrs_code" v="54SUE896431"/>
    <tag k="local_x" v="89653.3504"/>
    <tag k="local_y" v="43127.8006"/>
    <tag k="ele" v="43.148"/>
  </node>
  <node id="11" lat="35.625798395853955" lon="139.78150047282446">
    <tag k="mgrs_code" v="54SUE896431"/>
    <tag k="local_x" v="89660.3701"/>
    <tag k="local_y" v="43128.8083"/>
    <tag k="ele" v="43.217"/>
  </node>
  <node id="12" lat="35.62576282858205" lon="139.7815025463708">
    <tag k="mgrs_code" v="54SUE896431"/>
    <tag k="local_x" v="89660.509"/>
    <tag k="local_y" v="43124.861"/>
    <tag k="ele" v="43.148"/>
---------------------------------------------------------------
  <relation id="1483">
    <member type="way" role="left" ref="1480"/>
    <member type="way" role="right" ref="1482"/>
    <member type="way" role="centerline" ref="2249"/>
    <tag k="type" v="lanelet"/>
    <tag k="subtype" v="road"/>
    <tag k="speed_limit" v="100"/>
    <tag k="location" v="urban"/>
    <tag k="one_way" v="yes"/>
  </relation>
</osm>

このように，点の座標とidを保持しているデータだということがわかります.つまり，mapを表示しているというよりは点と点を結んだ線を描画することで，きれいなMapを表示していた事がわかりました.あとでこのことがあっているか確認していきます.それでは，これらを踏まえて準備を進めていきます．

準備

まず，lanelet2からどのようにデータをとるか考えましたが，とにかく扱いが難しいと感じてしまいました．そこで，代替案としてrviz上にmapを表示するのに使用する/map/vector_map_markertopicを利用することを考えました.msgの型は,visualization_msgs/msg/MarkerArrayです．重要部分を抜粋したものが以下のようになります．

self.subscription = self.create_subscription(MarkerArray, '/map/vector_map_marker', self.map_callback, 10)
def map_callback(self, msg):
    print('Received map')
    self.save_map(msg)
def save_map(self, msg):
    print('Saving map')

    for marker in msg.markers:
        filename = f'{marker.ns}.csv'
        filepath = os.path.join(self.map_dir, filename)
        with open(filepath, mode='w') as file:
            writer = csv.writer(file)
            writer.writerow(['x', 'y', 'z'])
            for point in marker.points:
                writer.writerow([point.x, point.y, point.z])

何をしているかというと，MakerArrayのsubscriptionを定義して，取得したmsgをMakerに解体してその中にあるpointsをcsvに書き出しただけです.すると，複数のファイルが生成されると思います．その中にleft_lane_bound.csvとright_lane_bound.csvの２種類があると思います.これがコースの外枠を示した点群の座標となります．また，center_lane_line.csvは中心線を保持しているデータです.こちらにデータを共有しておきます.

データの解析

csvのデータ取得ができたので次は中身を可視化しながら解析していきます．前回大会で使用されているレース最適化も便利そうだったので，以下のツールを試せないか検討してみます.

まず，最初にそれぞれのfile_pathを変数化します.その後,pandasのデータとして読み込みます.どれもヘッダーは同じなので，読み込み自体はシンプルになると思います.

データのロード

import pandas as pd

# Load the provided CSV files
outer_track_path = # path to left_lane_bound.csv
inner_track_path = # path to right_lane_bound.csv
center_track_path = # path to center_lane_line.csv

# Defining column names
column_names = ['x', 'y','z']

# Re-load the CSV files with column names
outer_track_df = pd.read_csv(outer_track_path, names=column_names, header=0)
inner_track_df = pd.read_csv(inner_track_path, names=column_names, header=0)
center_track_df = pd.read_csv(center_track_path, names=column_names, header=0)

次に，コースを可視化してみます.

データの可視化

import matplotlib.pyplot as plt

# plot the track
plt.plot(outer_track_df['x'], outer_track_df['y'], 'r')
plt.plot(inner_track_df['x'], inner_track_df['y'], 'b')
plt.plot(center_track_df['x'], center_track_df['y'], 'g')
# plt.plot(outer_track_df['x'], outer_track_df['y'], 'ro')
# plt.plot(inner_track_df['x'], inner_track_df['y'], 'bo')
# plt.plot(center_track_df['x'], center_track_df['y'], 'go')
plt.show()

おそらく，以下のようなグラフが出力されると思います.(r: outer, b: inner, g: center)

line plot	dot plot

この可視化方法では，点と点を線で結ぶようにplotされます．コメントアウトを外していただくと右図のようなドットで可視化できます.この図からわかるとおり等間隔というわけではないので注意しないといけなさそうです.
先程のtoolを使うためには,centerlineに，xy座標とそこから壁への距離情報が必要なので，その処理の例を以下に示します.

データの作成例

def search_nearest_index(x, y, bound_map_x, bound_map_y, index=0, limit=10):
    min_dist = 1000000
    min_index = index
    bound_map_x = bound_map_x + bound_map_x
    bound_map_y = bound_map_y + bound_map_y
    for i in range(index - limit, index + limit):
        dist = np.sqrt((x - bound_map_x[i])**2 + (y - bound_map_y[i])**2)
        if dist < min_dist:
            min_dist = dist
            min_index = i
    return min_index

def calc_width(x, y, bound_x, bound_y):
    return np.sqrt((x - bound_x)**2 + (y - bound_y)**2)

def calculate_center_line(outer_track_df, inner_track_df, limit=10, skip=5):
    # Calculate the center line
    outer_track_x = list(outer_track_df['x'])
    outer_track_y = list(outer_track_df['y'])
    inner_track_x = list(inner_track_df['x'])
    inner_track_y = list(inner_track_df['y'])
    center_x = []
    center_y = []
    w_tr_right_m = []
    w_tr_left_m = []
    nearest_index = 0
    for i in range(0, len(outer_track_x), skip):
        nearest_index = search_nearest_index(outer_track_x[i], outer_track_y[i], inner_track_x, inner_track_y, nearest_index, limit)

        # Calculate the center line
        if nearest_index > len(inner_track_x) - 1:
            nearest_index -= len(inner_track_x)
        center_x.append((outer_track_x[i] + inner_track_x[nearest_index]) / 2)
        center_y.append((outer_track_y[i] + inner_track_y[nearest_index]) / 2)
        width = calc_width(outer_track_x[i], outer_track_y[i], inner_track_x[nearest_index], inner_track_y[nearest_index])
        w_tr_right_m.append((width/2)-0.3)
        w_tr_left_m.append((width/2)-0.3)
    
    return pd.DataFrame({
        'x_m': center_x,
        'y_m': center_y,
        'w_tr_right_m': w_tr_right_m,  # Assuming the width is evenly divided
        'w_tr_left_m': w_tr_left_m
    })


# center_df = calculate_center_line(inner_track_df,outer_track_df, 50)
center_df = calculate_center_line(outer_track_df, inner_track_df,limit=40, skip=3)
# Define the output path
output_corrected_path = './ai_challenge.csv'

# Define the custom header string
custom_header = '# x_m, y_m, w_tr_right_m, w_tr_left_m\n'

# Open the file in write mode and write the custom header
with open(output_corrected_path, 'w') as f:
    f.write(custom_header)

    # Append the DataFrame content without the default header
    center_df.to_csv(f, index=False, header=False)

このアルゴリズムは,outer_laneの点から一番近いinner_laneを探し，中点と長さを求めます．その後，決まった形のヘッダーに合うように整形して出力するといったコードになります.以下は，laneletの中心線と計算で出したものの比較図になります.(r: lane, g: lanelet, b: center_line)

あとは，最適化を順路に従って動かすだけで問題ないと思いますが，現状のパラメータでは動かすことはできませんでした.なので，その部分を変更する必要がありそうです．ぜひ，最適な方法を検討してみてください．

まとめ

今回は，laneletをcsvに変換して解析する方法を示しました.この部分はチームによって最適化方法は変わるのでぜひ検討してみてください．また，aichallengeのslackにiaslのへやで定期的に情報を公開していますのでぜひ見に来てください!!