自動運転AIチャレンジ2024 コース内判定＆壁までの距離計算

はじめに

自動運転AIチャレンジ2024の予選も残すところ1ヶ月を切りました。私はチームTLABのメンバーとして参加しており、上位に食い込むべくトライアンドエラーを繰り返しています。さて、今大会で良い結果を残すためには、コース内にランダムに現れる仮想障害物をいかに避けるかが鍵になりそうです。私も障害物回避の実装に取り組んでいますが、回避させたいあまり壁に接触してしまうなど、一筋縄ではいかないと感じています。そんなわけで、生成した経路がコース内に収まっているのか、壁までどのくらい余裕があるのかを判定したいと思い、実装に取り組んでみました。

結果

コース内判定と距離計算の結果を可視化したものが以下です。コース内が、壁に近いほど濃く、遠いほど薄く描画されています。

実装

コースマップのインポート

提出用プログラムに組み込むためにはコースや位置の情報はROSトピックから取得する必要がありますが、まずはそれらを考えずにマップファイルからコースデータを取得することにします。コースを定義しているファイルであるlanelet2_map.osmはXML形式で書かれており、Pythonで読み込むにはElementTreeが使えます。コースの壁は外側内側それぞれ2本のwayからなっており、各wayはnodeのリストであり、各nodeは座標をもつ、という構造になっています。

コース内判定

今回のコースは自己交差を持たないため、任意の一点が与えられたとき、「外側の壁より内側にあり、かつ内側の壁より外側にある」という条件でその点がコース内にあることを判定できます。したがって一点$P$と自己交差を持たない閉曲線$C$に対してその点が閉曲線の内側にあるかどうかを判定する関数を実装できればよいということになります。

閉曲線の内側判定

そのための方法として「平面内の自己交差を持たない閉曲線は平面を内側と外側の2つの領域に分ける」という主張であるジョルダンの閉曲線定理に基づいたアルゴリズムがあります。内側の点から無限遠点に向けて半直線を引くと閉曲線と交わる回数は奇数回、外側の点の場合は偶数回、となるのでこれで判定します。

線分と線分の交差判定

上のアルゴリズムを実装するために、壁を構成する線分と、点$P$から十分遠くに引いた線分とが交差するか否かを判定する必要があります。線分と線分の交差を判定する方法として、3つの点の位置関係を求める関数であるCCW(CounterClockWise)関数を使った手法が知られています。詳しい説明はここでは省略します。

壁までの距離計算

コースの壁を構成する各線分について、点と線分の距離を計算し、最小値を取ればOKです。

コード

import xml.etree.ElementTree as ET
import pandas as pd
import matplotlib.pyplot as plt
import math
import numpy as np

file_path = 'lanelet2_map.osm'
outer_wall_id_list = [1480, 1479]
inner_wall_id_list = [1482, 1481]
eps = 1e-8
xmin = 89600
xmax = 89700
ymin = 43100
ymax = 43200
N = 200
dx = (xmax - xmin) / N
dy = (ymax - ymin) / N

class Point: # 2次元平面上の点を表すクラス
    def __init__(self, _x, _y):
        self.x = _x
        self.y = _y

    def __sub__(self, p):
        return Point(self.x - p.x, self.y - p.y)
    
    def __add__(self, p):
        return Point(self.x + p.x, self.y + p.y)
    
    def dot(self, p):
        return self.x * p.x + self.y * p.y
    
    def norm(self):
        return math.sqrt(self.dot(self))
    
    def cross(self, p):
        return self.x * p.y - self.y * p.x

def get_waypoints(way_id_list): # lanelet2のwayを構成する点のリストを取得する
    tree = ET.parse(file_path)
    root = tree.getroot()
    nodes = {}
    for node in root.findall('node'):
        node_id = node.get('id')
        x = None
        y = None
        for tag in node.findall('tag'):
            if tag.get('k') == 'local_x':
                x = float(tag.get('v'))
            if tag.get('k') == 'local_y':
                y = float(tag.get('v'))
        nodes[node_id] = Point(x, y)

    way_node_ids = []
    for way in root.findall('way'):
        if int(way.get('id')) in way_id_list:
            for nd in way.findall('nd'):
                if not way_node_ids or nd.get('ref') != way_node_ids[-1]: #wayの境界点が二重に読み込まれないようにしておく
                    way_node_ids.append(nd.get('ref'))
    waypoints = []
    for way_node_id in way_node_ids:
        waypoints.append(nodes[way_node_id])
    return waypoints

def xylist(waypoints): #点のリストを座標のリストに変換
    x = []
    y = []
    for point in waypoints:
        x.append(point.x)
        y.append(point.y)
    return x, y

def ccw(p0, p1, p2): # 3点p0, p1, p2の位置関係
    v = p1 - p0
    w = p2 - p0
    if v.cross(w) > eps: return 1           #COUNTER_CLOCKWISE
    if v.cross(w) < -eps: return -1         #CLOCKWISE
    if v.dot(w) < -eps: return 2            #ONLINE p2 - p0 - p1
    if v.norm() + eps < w.norm(): return -2 #ONLINE p0 - p1 - p2
    return 0                                #ONLINE p1 - p2 - p0

def intersect(p0, p1, q0, q1): # 線分p0p1と線分q0q1の交差判定
    t = ccw(p0, p1, q0) * ccw(p0, p1, q1)
    u = ccw(q0, q1, p0) * ccw(q0, q1, p1)
    return (t == 0 or t == -1 or t == -4) and (u == 0 or u == -1 or u == -4)

def dist_point_segment(p, q0, q1): # 点pと線分q0q1の距離
    if (q1 - q0).dot(p - q0) < 0.0: return (p - q0).norm()
    if (q0 - q1).dot(p - q1) < 0.0: return (p - q1).norm()
    return abs((p - q0).cross(q1 - q0)) / (q1 - q0).norm()

def inside(p, curve): # 点pが閉曲線curveの内側にあるか判定
    p_infty = Point(0.0, 0.0)
    cross_cnt = 0
    for i in range(len(curve)-1):
        if intersect(p, p_infty, curve[i], curve[i+1]):
            cross_cnt = cross_cnt + 1
    return cross_cnt % 2 == 1

def dist_point_curve(p, curve): #点pと曲線curveの距離
    dist = 1e8
    for i in range(len(curve)-1):
        dist = min(dist, dist_point_segment(p, curve[i], curve[i+1]))
    return dist

def main():
    outer_wall_points = get_waypoints(outer_wall_id_list)
    inner_wall_points = get_waypoints(inner_wall_id_list)

    innerPts_x = []
    innerPts_y = []
    innerPts_dist = []
    for i in range(N):
        for j in range(N):
            p = Point(xmin + i * dx, ymin + j * dy)
            if inside(p, outer_wall_points) and not inside(p, inner_wall_points):
                innerPts_x.append(p.x)
                innerPts_y.append(p.y)
                innerPts_dist.append(min(dist_point_curve(p, outer_wall_points), dist_point_curve(p, inner_wall_points)))

    outer_x, outer_y = xylist(outer_wall_points)
    inner_x, inner_y = xylist(inner_wall_points)
    fig, ax = plt.subplots()
    ax.plot(outer_x, outer_y, c="black")
    ax.plot(inner_x, inner_y, c="black")
    ax.scatter(innerPts_x, innerPts_y, c=innerPts_dist, cmap="gray", s=1)
    ax.set_aspect('equal')
    plt.show()

if __name__ == '__main__':
    main()