Feature/add path avoidance

[uchikun2493]

game.pyの中で，アクション実行後（pathが生成されたcontrol_target）にpath_avoidを使うことで中間パスを生成してcontrol_target.pathに追加してくれるようにした

[ShotaAk]

consai2/consai2_game/scripts/example/avoidance.py

Lines 85 to 86 in 811b276

	# ロボットが検出できている場合
	if info.detected:

ロボットの存在判定にはdetected is Trueではなく、disappeared is Falseを使ってください。

detectedはノイズやデータ欠落で頻繁にFalseになります。

[ShotaAk]

後の調整を考え、これらはメンバ変数（self._{SOMETHING}）にしてください。

consai2/consai2_game/scripts/example/avoidance.py

Lines 90 to 92 in 811b276

	# 検出の幅
	threshold_tr_y = 0.4
	margin_tr_x = 0.2

consai2/consai2_game/scripts/example/avoidance.py

Lines 115 to 118 in 811b276

	# 一番近い敵ロボットの座標
	tr_robot_pose = trans.transform(robot_pose)
	tr_robot_pose.x += 0.2
	tr_robot_pose.y -= 0.5

[ShotaAk]

ここはfloat値同士が等しいかどうかチェックしているので修正してください。

consai2/consai2_game/scripts/example/avoidance.py

Lines 61 to 63 in 811b276

	# 敵と味方それぞれどちらが近い障害物か比較して近い方を優先する
	if min_dist_our == min_dist_their:
	avoid_pose = None

やりたいことは、障害物がないときは回避位置を生成しない、だと思うので

consai2/consai2_game/scripts/example/avoidance.py

Lines 50 to 55 in 811b276

	dist_our, obst_dist_our, obst_id_our = self.detect_dist_and_id(
	trans, my_pose, goal_pose, self._robot_info['our'])
	dist_their, obst_dist_their, obst_id_their = self.detect_dist_and_id(
	trans, my_pose, goal_pose, self._robot_info['their'])
	# 進路上の障害物から避ける経路を生成

detect_dist_and_id()の後に

if len(obst_id_our) == 0 and len(obst_id_their) == 0:
    avoid_pose = None
else:
    # avoid_poseの計算

return avoid_pose

とするのはどうでしょうか？

[uchikun2493]

コメント反映してコミットしました

ちなみに変数名が
threshold_tr_y = 0.4 → self._range_tr_y = 0.4
margin_tr_x = 0.2 → self._range_tr_x = 0.2

tr_robot_pose.x += 0.2
tr_robot_pose.y -= 0.5
の加算する値を
self._tr_move_x = 0.2
self._tr_move_y = 0.5
としてinitに追加しました

def __init__(self):

        # ボール情報の初期化
        self._ball_info = BallInfo()
        # ロボット情報の初期化
        self._robot_info = RobotInfo()

        # 検出の幅
        self._range_tr_y = 0.4
        self._range_tr_x = 0.2

        # 経路生成時の相対的に移動する位置(ロボットから見た座標系)
        self._tr_move_x = 0.2
        self._tr_move_y = 0.5

[ShotaAk]

良いと思います。マージします。