設計 - seigot/burger_war_dev GitHub Wiki

概要

p-robotics-hub/burger_war_devが提供するロボコン競技に向けて開発した機能について記載する。

対象リポジトリ

前提条件

本リポジトリに実装している機能は以下を前提としている。

開発環境：ubuntu18.04/ROS melodic環境
rulebook記載のルール下で勝利する事を目的としている
- burger war ルール
- フィールド
- ターゲット
  - 相手背後：５ポイント
  - 相手左右：３ポイント
  - フィールド：１ポイント

これらの前提が変更になった場合、変更内容に応じてバージョンアップが必要となる。

設計思想

競技の性質上、単位時間当たりの得失点差を最大化することが重要だと考えている。
- やや分かりづらい表現になっているが、「得失点差=得点-失点」を指しており、得点を増やしつつ失点を減らしたいという意味で使っている。
- 得点を増やすため、走行機能の向上によりフィールドのターゲットからの得点効率化を図る。
- 失点を減らすため、敵からの攻撃を回避する機能を実装している。また、ただ回避するだけでなく隙があれば相手背後、左右のターゲットからの得点を図る。
今後のルール変更に備えて、システムとしてなるべく変更容易性を担保するように努めている。基本機能である以下もなるべくこの思想で実装している。
- 起動処理
- 走行
- 敵機検出
- 安定化

全体設計

起動処理
- seigoRun2.py init()関数が該当
- 各種topicのsubscribe/publisherの初期化
  - callback関数の詳細は後述
- ゲーム状態の管理変数の初期化
- 外部パラメータの取得
- 起動後はstatus_transition関数を数msec間隔で定期実行する
  - status_transition関数の処理は以下の通り
    - mode_desicion()により現在の状態を決定する。決定した状態に応じて処理を分岐する。
    - BASIC: 敵を見つけていない場合、もしくは遠い場合。
    - DEFENCE: 敵が近くにいて、得点が勝っている場合、かつ敵のターゲットをほぼ取得済の場合。（攻撃するメリットが少ない場合は守りたい）
    - ATTACK: 敵が近くにいて、得点が勝っている場合。（攻撃するメリットが多い場合は攻めたい）
    - ESCAPE: 敵が近くにいて、得点が負けている場合。（攻撃するメリットが多いが負けてる場合は更に工夫して攻めたい）
    - ATTACKかESCAPEかはrosparam(enable_escape_approach)で決める。ESCAPEは現状有効化している。
走行
- BASIC状態での処理。基本的にフィールド上の予め指定した巡回ルートを走行する。
- 巡回ルート
  - waypoint.csvをwaypoint.pyで読み取って制御している。
  - 既に得点しているターゲットは巡回不要のためスキップする。
  - 得点したターゲットについてはWarState_timerCallbackで取得している。
  - ターゲットの番号と位置はこちらにコメントを残している
  - 巡回ルートは現状、手動で決定した値だがチューニングの余地がある。
- 走行アルゴリズムとパラメータ、地図
  - burger_navigation.launchで設定可能
  - mapはmapsにある、高解像度版＋壁の線を細くしてマッチング性能向上した版を使用している。
  - ただしlocal_plannerは独自に実装している（@d_kuboさん）
- local_planner
  - pure pursueアルゴリズムを採用
  - 実装はここ
敵機検出
- BASICから、DEFENCE/ATTACK/ESCAPEに状態遷移するための条件としている
- 敵機検出の方式はlidar方式とcamera方式を検討した
  - lidar方式は、enemy_detector.py及びdetect_from_lidarにより実施
  - camera方式は、誤検出を克服できず非採用
- DEFENCE
  - defence関数が該当
  - 自身の左右、背後のターゲットを取られないようにする
- ATTACK
  - attack関数が該当
  - 敵機の左右、背後のターゲットを取るべく、敵に向かって進んでいく
- ESCAPE
  - escape関数が該当
  - ATTACK同様だが、相手も敵検出機能を実装している場合、状況によってはお見合い状態になる。場合に応じてはそのまま時間切れになり敗北に繋がる。
  - これを解決するために以下のお見合い対策を実施
- お見合い対策
  - お見合い状態から抜け出すために、一定時間間隔で前進したり後退したりする機能を実装している。これはパケット通信方式の CSMA/CD方式を参考にしている。また自動運転のお見合い処理でも同様のアイデアがあるらしい。
安定化
- 衝突回避
  - detect_collision関数が該当する
  - 走行中に壁や障害物に衝突すると自己位置がずれて異常停止することがある。競技中にこれが起こると失点による敗北に繋がる。
  - これを解決するために、障害物に接近しないように回避処理を加えている。

自動評価

評価時間の短縮を目的とした自動化の取り組み
使い方はこちらのwikiに記載している通り
デフォルトの敵だけでなく、強い敵を独自に用意している

特に工夫しているポイント

走行機能：
- following plannerによる走行速度向上
- 巡回ルートの最適化
- 得点済のターゲットは巡回スキップする
敵機検出：
- 負けている時でも敵の得点を取るための工夫、ESCAPE状態の処理
自動評価：
- 性能見える化の取り組み、勝利確率の定量化
- 評価のための強い敵をたくさん準備していること最新の結果は以下

以下、補足

callback関数

@KotaOhishiさんありがとうございます


def imageCallback:
	subしたimage topicをdetect_from_cameraに投げる
	#subscriberがコメントアウトされているので使われていない関数？

def imuCallback
	subしたimu_topicをインスタンス変数(imu)に格納
	もし、z方向の加速度が閾値を超えた場合はprintfに表示

def amclposeCallback
	subしたpose_topicのx,y成分をインスタンス変数（my_pose_x, my_pose_y)に格納
	subしたposeをクォータ二オンからオイラー角に変換
	yaw軸の値をインスタンス変数(my_direction_th)に格納

def get_position_from_tf(c1, c2)
	c1から見たc2の座標を計算して返す
	返り値の3つ目は座標変換に成功したらTrue,失敗したらFalse

def enemy_position_callback
	subしたenemy_position topicをインスタンス変数(enemy_position)に格納

def lidar_callback
	subしたscan topicをインスタンス変数(scan)に格納

def detect_enemy
	detect_from_lidarを呼びだす。
	その結果を返す
	#cameraの情報をandをとるような処理もあるがコメントアウトされている

def detect_from_lidar
	最新のenemy_positionを取得した時間からどれだけ経過したか計算
	もし、経過時間が許容範囲を超えていたら（敵情報が古い）
		インスタンス変数（detect_counter）に０を代入
		False, 0 ,0 を返す

	もし、許容範囲内なら
		インスタンス変数（detect_counter）をインクリメント
		もし、detect_counterが閾値よりも小さければ
			False, 0, 0を返す

	
	map_topicとbaselink_topicをインスタンス変数から読み込む
	mapから見たbaselinkの座標を計算する
	
	もし、座標変換に失敗したら
		False,0,0を返す
	
	enemy_positionの値からenemy_robotまでの距離（x、y）を計算
	
	baselinkのyaw軸の値を計算
	enemy_robotのyaw軸を計算
	enemy_robotとの成す角度を計算
	
	True、enemyまでの距離、enemyとの角度を返す


def detect_from_camera
	image_topicを引数としてcamera_detector.detect_enemy関数に渡すと、red,green,blueのangleが返ってくる
	もし、red_angleが-360ではない
		インスタンス変数(is_camera_detect)をTrueにする
		インスタンス変数(camera_detect_angle)をreg_angleにする
		return 
	
	もし、red_angleが-360の場合	
		もし、green_angleが-360でない場合
			インスタンス変数(is_camera_detect)をTrueにする
			インスタンス変数(camera_detect_angle)をgreen_angleにする
		
		もし、green_angleが-360の場合
			インスタンス変数(is_camera_detect)をFalseにする
			インスタンス変数(camera_detect_angle)を-360にする

def detect_collision
	#読んだだけでは理解できなかったので、動かしながら確認中


def WarState_timerCallback
	0.1秒間隔で、getWarState関数を実行する

def getWarState
	judge_serverから情報を取得
	情報を辞書型に変換
	
	敵と自分のスコアを取得し、インスタンス変数(my_score, enemy_score)に格納
	
	タイムスタンプをインスタンス変数(game_timestamp)に格納

	もし、ゲームステートが”running”なら
		インスタンス変数(last_game_timestamp)にgame_timestampを格納

	18個のフィールドターゲットに対して
		最新のall_field_scoreをall_field_score_prevに格納
		誰がgetしているかを確認してインスタンス変数(all_field_score[idx])に格納
		
		all_field_scoreに更新があれば、
			enemyが新たにtargetを取得している場合
				インスタンス変数(enemy_get_target_no)にターゲットのインデックス番号を格納
				インスタンス変数(enemy_get_target_no_timestap)にgame_timestampを格納

			自分が新たにtargetを取得している場合
				インスタンス変数(my_get_target_no)にターゲットのインデックス番号を格納
				インスタンス変数(my_get_target_no_timestap)にgame_timestampを格納
			
			
			ログに出力

		waypointにall_field_scoreの値を渡す（idxが6以降の要素）（0~5はロボットのボディーのマーカ？）

	
	ボディーのマーカーのポイントをインスタンス変数(my_body_remain, enemy_body_remain)に格納

	my_scoreとenemy_scoreを比較して、インスタンス変数(Is_lower_score）をTrue/Falseを更新
		


def mode_decision
	#状態を決定する
	detect_enemy関数を実行し、敵を

	もし、敵がいなければ
		return BASIC 巡回モード
	
	もし、敵がいれば
		もし、両プレーヤーともにボディーマーカの点数が残っていない場合
			return BASIC 巡回

		もし、敵のボディーマーカの点数が1点以下 かつ 敵までの距離が1.0以下 かつ スコアで勝っている状態の時
			return DEFFENCE 

		もし、距離がsnipe_thより短い かつ enable_escape_approach == True かつ スコアで負けている状態の時
			return ESCAPE

		もし、距離がsnipe_thよりも短い場合
			return ATTACK

		return BASIC #敵を見つけたが距離が遠い場合
      

def status_transition
	#解読中

def basic
	#解読中

def send_goal
	#解読中

def cancel_goal
	#解読中

def get_move_base_status
	#解読中

def attack
	#解読中

def escape
	#解読中

def defense
	#解読中

def turn_to_enemy
	#解読中

def recovery
	#解読中

topicに関して補足(navigation)

■入力 tf: 自己位置 enemy_position: 敵っぽいものの座標情報（LIDARから点群を取得して、そこから敵位置） scan: LIDARの点群（前方の障害物衝突回避の為） ■出力 move base

LIDAR,障害物のコストマップ
コストマップから経路を引く global plannerはこちら/open source
どんな速度、角速度を出すか local plannerはこちら/

topicに関して補足(seigoRun2.py)

main nodeであるseigoRun2.pyが利用しているtopic
- Subscribe topic (IN)
  - /tf: 座標情報
  - /scan: Lidar
  - /move_base/status: move_baseを使った移動が完了したかどうかの情報
  - /enemy_position: 敵位置
- Publish topic (OUT)
  - /move_base/goal: 移動先座標を指定(BASIC状態)
  - /move_base/cancel: move_baseを使った移動をcancelする(DEFENCE/ATTACK/ESCAPE状態)
  - /cmd_vel: 移動量を直接指定(DEFENCE/ATTACK/ESCAPE状態)
following_planner
- move_baseのlocal planner pluginとして実装されている
- defaultで使用されるOSSのlocal plannerは、大きく迂回してしまう課題があったため、 global plannerが生成する経路に比較的忠実に沿って動くlocal plannerを自作した
enemy_detector
- obstacle_trackerが検出した障害物から、既知の障害物を除いて敵っぽい物体を選別
- Subscribe topic (IN)
  - /obstacles: obstacle_tracker nodeが検出した障害物
- Publish topic (OUT)
  - /enemy_position: 敵の位置情報

rqt_graph

今後のアップデート

ルート最適化（グリッドサーチ）
想定外対策（突っ込んでくる敵、索敵範囲外に忍ぶ敵）