level_3_clubhouse_ike_comment.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

# level_3_clubhouse.py
# write by yamaguchi takuya @dashimaki360
## GO around field by AMCL localizer, and find enemy.

import rospy
import random
import math

from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseWithCovarianceStamped
from sensor_msgs.msg import LaserScan

import tf

PI = math.pi
# 8x8  [rad]
# TARGET_THはTeriyakiと同じ
    # このルートは絶対に最適じゃない
TARGET_TH = (
    (-PI/4, -PI/4, -PI/2, -PI/2, -PI*3/4, -PI*3/4, -PI*3/4, -PI*3/4),
    (-PI/4, -PI/4, -PI/3, -PI/2, -PI*3/5, -PI*3/4, -PI*3/4, -PI*3/4),
    (-PI/4, -PI/4, -PI/6,     0,   -PI/2, -PI*3/4,     -PI,  PI*3/4),
    (-PI/4, -PI/5,     0,     0,      PI,  PI*6/10,  PI*3/4,    PI/2),
    (    0,     0,  PI/2,  PI/2,      PI,  PI*3/4,  PI*3/4,    PI/2),
    (    0,  PI/4,  PI/3,  PI/2,  PI*5/6,  PI*3/4,  PI*3/4,  PI*3/4),
    ( PI/4,  PI/4,  PI/4,  PI/3,  PI*5/6,    PI/2,  PI*3/4,  PI*3/4),
    ( PI/4,  PI/4,  PI/4,  PI/4,    PI/3,    PI/2,  PI*3/4,  PI*3/4),
)
WIDTH = 1.2 * (2 **0.5) # [m]


# respect is_point_enemy freom team rabbit
# https://github.com/TeamRabbit/burger_war
class EnemyDetector:
    '''
    Lidarのセンサ値から簡易的に敵を探す。
    obstacle detector などを使ったほうがROSらしいがそれは参加者に任せます。
    いろいろ見た感じ Team Rabit の実装は綺麗でした。
    方針
    実測のLidarセンサ値 と マップと自己位置から期待されるLidarセンサ値 を比較
    ズレている部分が敵と判断する。
    この判断は自己位置が更新された次のライダーのコールバックで行う。（フラグで管理）
    0.7m 以上遠いところは無視する。少々のズレは許容する。
    '''
    def __init__(self):
        self.max_distance = 0.7
        self.thresh_corner = 0.25
        self.thresh_center = 0.35

        self.pose_x = 0
        self.pose_y = 0
        self.th = 0
    
    # 運営側がコメントしてくれている通り．
    def findEnemy(self, scan, pose_x, pose_y, th):
        '''
        input scan. list of lider range, robot locate(pose_x, pose_y, th)
        return is_near_enemy(BOOL), enemy_direction[rad](float)
        '''
        if not len(scan) == 360:
            return False
        
        # update pose
        # AMCLアルゴリズムによって，更新された自己位置を使用．
        self.pose_x = pose_x
        self.pose_y = pose_y
        self.th = th

        # drop too big and small value ex) 0.0 , 2.0 
        # 0.1以下と"max_distance"以上は関係ないと判断して，切り捨て（0に設定，配列の要素数自体は不変）
            # "LaserScan" Messageの"data.ranges"は配列の要素番号がレーザの角度と対応しているため，要素数を変えてはいけない．
        near_scan = [x if self.max_distance > x > 0.1 else 0.0 for x in scan]

        # 上で格納されたLidarからの距離データ（"near_scan"）を要素番号付きでまずは格納．
        # "is_point_emnemy"を用いて，障害物が敵だと判断できれば1を格納．
        enemy_scan = [1 if self.is_point_emnemy(x,i) else 0 for i,x in  enumerate(near_scan)]

        # Noiseも結構乗るので，ある程度の範囲で障害物が検出された場合に敵と判断する．
        is_near_enemy = sum(enemy_scan) > 5  # if less than 5 points, maybe noise


        # 敵が近くにいた場合，まずはどの角度に敵がいるかを判断する必要がある．
            # "enemy_scan" の要素で1の要素番号を取得（∴ 敵がいる角度）
            # 基本，10~20度などの一定の範囲にいるので，ちょうど真ん中の角度を取得（例で言うと，15度．ここに敵ロボットのロボット中心があると考えられる）
            # 敵がいる角度"enemy_direction"と距離"enemy_dist"を取得
        # 敵が近くにいなかった場合はNoneを返す -> Teriyakiの移動ベクトル決定方法と全く同じ．
            # "LidarCallback"を見れば分かる．
        if is_near_enemy:
            idx_l = [i for i, x in enumerate(enemy_scan) if x == 1]
            idx = idx_l[len(idx_l)/2]
            enemy_direction = idx / 360.0 * 2*PI
            enemy_dist = near_scan[idx]
        else:
            enemy_direction = None
            enemy_dist = None

        print("Enemy: {}, Direction: {}".format(is_near_enemy, enemy_direction))
        print("enemy points {}".format(sum(enemy_scan)))
        return is_near_enemy, enemy_direction, enemy_dist
        
    # 距離と角度（要素番号）を引数に，まずはグローバル座標系における壁以外の障害物の位置を算出．
    # フィールド上の障害物でなければ（敵であれば），Trueを返す．
    def is_point_emnemy(self, dist, ang_deg):
        if dist == 0:
            return False

        ang_rad = ang_deg /360. * 2 * PI

        # 超重要！
        # 座標変換
        # Lidar情報（距離と角度）はロボット座標系での値（∴ ロボットから見た障害物までの距離と角度 ≠　グローバル座標系（フィールド中心が原点の座標系）における値）
        # AMCLなどの自己位置推定をするアルゴリズムは，基本的にグローバル座標系での地図データを参照する．
        # -> 地図データとの比較を行うため（敵がいるかどうかを判断するため）に，Lidarから得られた情報をグローバル座標系における値に変換する必要がある．
        # 実態はただの簡単な幾何計算．移動量を足して，回転行列をかけているだけ．
        point_x = self.pose_x + dist * math.cos(self.th + ang_rad)
        point_y = self.pose_y + dist * math.sin(self.th + ang_rad)

        #フィールド内かチェック
        if   point_y > (-point_x + 1.53):
            return False
        elif point_y < (-point_x - 1.53):
            return False
        elif point_y > ( point_x + 1.53):
            return False
        elif point_y < ( point_x - 1.53):
            return False

        #フィールド内の物体でないかチェック
        len_p1 = math.sqrt(pow((point_x - 0.53), 2) + pow((point_y - 0.53), 2))
        len_p2 = math.sqrt(pow((point_x - 0.53), 2) + pow((point_y + 0.53), 2))
        len_p3 = math.sqrt(pow((point_x + 0.53), 2) + pow((point_y - 0.53), 2))
        len_p4 = math.sqrt(pow((point_x + 0.53), 2) + pow((point_y + 0.53), 2))
        len_p5 = math.sqrt(pow(point_x         , 2) + pow(point_y         , 2))

        if len_p1 < self.thresh_corner or len_p2 < self.thresh_corner or len_p3 < self.thresh_corner or len_p4 < self.thresh_corner or len_p5 < self.thresh_center:
            return False
        else:
            #print(point_x, point_y, self.pose_x, self.pose_y, self.th, dist, ang_deg, ang_rad)
            #print(len_p1, len_p2, len_p3, len_p4, len_p5)
            return True

# End Respect

class TeriyakiBurger():
    def __init__(self, bot_name):
        # bot name 
        self.name = bot_name
        # robot state 'inner' or 'outer'
        self.state = 'inner' 
        # robot wheel rot 
        self.wheel_rot_r = 0
        self.wheel_rot_l = 0
        self.pose_x = 0
        self.pose_y = 0
        self.th = 0

        self.k = 0.5
        self.near_wall_range = 0.2  # [m]

        # speed [m/s]
        self.speed = 0.07

        self.is_near_wall = False
        
        # lidar scan
        self.scan = []

        # publisher
        self.vel_pub = rospy.Publisher('cmd_vel', Twist,queue_size=1)

        # subscriber
        # "self.pose_sub" はLidarから得られるセンサー情報（"scan"）とオドメトリ情報を使って，
        # "self.lidar_sub" はLidarから得られるセンサー情報（"scan"）と自己位置を使って，「移動方向」をCallbackするSubscriber
        self.pose_sub = rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, self.poseCallback)
        self.lidar_sub = rospy.Subscriber('scan', LaserScan, self.lidarCallback)

        self.pose_twist = Twist()
        self.pose_twist.linear.x = self.speed; self.pose_twist.linear.y = 0.; self.pose_twist.linear.z = 0.
        self.pose_twist.angular.x = 0.; self.pose_twist.angular.y = 0.; self.pose_twist.angular.z = 0.

        # Teriyakiからの変更点
        # 敵が近く（前方）にいる場合といない場合（通常時，Teriyaki）で移動ベクトルの決定方法を変更するため．
        self.is_near_enemy = False
        self.enemy_direction = None
        self.enemy_dist = None
        self.near_enemy_twist = Twist()
        self.near_enemy_twist.linear.x = self.speed; self.near_enemy_twist.linear.y = 0.; self.near_enemy_twist.linear.z = 0.
        self.near_enemy_twist.angular.x = 0.; self.near_enemy_twist.angular.y = 0.; self.near_enemy_twist.angular.z = 0.

        self.is_initialized_pose = False
        self.enemy_detector = EnemyDetector()
        

    # Teriyakiと全く同じ．
    # TARGET_THと現在の実際の姿勢の差を補正するように次の移動ベクトルを決定するところを関数化 "updatePoseTwist" しているだけ．
    # 一点重要なのが，フラグ "self.is_initialized_pose"
        # AMCLアルゴリズムを使って自己位置を更新できているかをフラグで管理している．
        # 自己位置を更新できていた場合にのみ，敵検知を行う．（"lidarCallback"を見れば分かる）
    def poseCallback(self, data):
        '''
        pose topic from amcl localizer
        update robot twist
        '''
        self.pose_x = data.pose.pose.position.x
        self.pose_y = data.pose.pose.position.y
        quaternion = data.pose.pose.orientation
        rpy = tf.transformations.euler_from_quaternion((quaternion.x, quaternion.y, quaternion.z, quaternion.w))

        self.th = rpy[2]
        th_xy = self.calcTargetTheta(self.pose_x,self.pose_y)
        
        self.updatePoseTwist(self.th, th_xy)
        self.is_initialized_pose = True

    def updatePoseTwist(self, th, th_xy):
        # update pose twist
        th_diff = th_xy - th
        while not PI >= th_diff >= -PI:
            if th_diff > 0:
                th_diff -= 2*PI
            elif th_diff < 0:
                th_diff += 2*PI
        
        delta_th = self.calcDeltaTheta(th_diff)
        new_twist_ang_z = max(-0.3, min((th_diff + delta_th) * self.k , 0.3))
        
        # 敵がいるかどうか判断する前に，一旦次の移動方向を仮決め
        self.pose_twist.angular.z = new_twist_ang_z
        self.pose_twist.linear.x = self.speed
        #print("th: {}, th_xy: {}, delta_th: {}, new_twist_ang_z: {}".format(th, th_xy, delta_th, new_twist_ang_z))

    def calcTargetTheta(self, pose_x, pose_y):
        x = self.poseToindex(pose_x)
        y = self.poseToindex(pose_y)
        th = TARGET_TH[x][y]
        #print("POSE pose_x: {}, pose_y: {}. INDEX x:{}, y:{}".format(pose_x, pose_y, x, y))
        return th

    def calcDeltaTheta(self, th_diff):
        if not self.scan:
            return 0.
        R0_idx = self.radToidx(th_diff - PI/8)
        R1_idx = self.radToidx(th_diff - PI/4)
        L0_idx = self.radToidx(th_diff + PI/8)
        L1_idx = self.radToidx(th_diff + PI/4)
        R0_range = 99. if self.scan[R0_idx] < 0.1 else self.scan[R0_idx]
        R1_range = 99. if self.scan[R1_idx] < 0.1 else self.scan[R1_idx]
        L0_range = 99. if self.scan[L0_idx] < 0.1 else self.scan[L0_idx]
        L1_range = 99. if self.scan[L1_idx] < 0.1 else self.scan[L1_idx]

        #print("Ranges R0: {}, R1: {}, L0: {}, L1: {}".format(R0_range, R1_range, L0_range, L1_range))
        if R0_range < 0.3 and L0_range > 0.3:
            return PI/4
        elif R0_range > 0.3 and L0_range < 0.3:
            return -PI/4
        elif R1_range < 0.2 and L1_range > 0.2:
            return PI/8
        elif R1_range > 0.2 and L1_range < 0.2:
            return -PI/8
        else:
            return 0.
    
    def radToidx(self, rad):
        deg = int(rad / (2*PI) * 360)
        while not 360 > deg >= 0:
            if deg > 0:
                deg -= 360
            elif deg < 0:
                deg += 360
        return deg

    def poseToindex(self, pose):
        i = 7 - int((pose + WIDTH) / (2 * WIDTH) * 8)
        i = max(0, min(7, i))
        return i

    def lidarCallback(self, data):
        '''
        lidar scan use for bumper , and find enemy
        controll speed.x
        '''
        scan = data.ranges
        self.scan = scan
        self.is_near_wall = self.isNearWall(scan)
        
        # enemy detection
        # 現在位置がAMCLにより更新されていれば，敵検知アルゴリズム（"EnemyDetector" CLassの"findEnemy" method）に移る
        if self.is_initialized_pose:
            self.is_near_enemy, self.enemy_direction, self.enemy_dist = self.enemy_detector.findEnemy(scan, self.pose_x, self.pose_y, self.th)
        
        # 敵が近くにいるかどうかを判断（いれば"self.is_near_enemy"がTrue）
        # 近くにいた場合には，Teriyakiとは違うアルゴリズム，つまり敵を追跡するアルゴリズムに移る．
        if self.is_near_enemy:
            self.updateNearEnemyTwist()

    # 敵がいた場合には，移動方向を変更する
    # シンプルに敵のいる方向 "self.enemy_direction" に向かう（ド正面ではなく，半分くらいの角度）
    # 敵までの距離が近い場合には，逆に逃げる．
    def updateNearEnemyTwist(self):
        # update pose twist
        th_diff = self.enemy_direction
        while not PI >= th_diff >= -PI:
            if th_diff > 0:
                th_diff -= 2*PI
            elif th_diff < 0:
                th_diff += 2*PI
        new_twist_ang_z = max(-0.3, min((th_diff) * self.k , 0.3))

        if self.enemy_dist > 0.36:
            speed = self.speed
        else:
            speed = -self.speed
        #print("enemy_dist {}".format(self.enemy_dist))
        
        self.near_enemy_twist.angular.z = new_twist_ang_z
        self.near_enemy_twist.linear.x = speed
        #print("Update near Enemy Twist")

    def isNearWall(self, scan):
        if not len(scan) == 360:
            return False
        forword_scan = scan[:15] + scan[-15:]
        # drop too small value ex) 0.0
        forword_scan = [x for x in forword_scan if x > 0.1]
        if min(forword_scan) < 0.2:
            return True
        return False

    def strategy(self):
        '''
        calc Twist and publish cmd_vel topic
        Go and Back loop forever
        '''
        r = rospy.Rate(10) # change speed 10fps

        while not rospy.is_shutdown():
            # publish twist topic
            twist = Twist()
            if self.is_near_enemy:
                twist.linear.x = self.near_enemy_twist.linear.x
                twist.angular.z = self.near_enemy_twist.angular.z
            else:
                twist.linear.x = self.pose_twist.linear.x
                twist.angular.z = self.pose_twist.angular.z

            if self.is_near_wall:
                twist.linear.x = -self.speed / 2 
            self.vel_pub.publish(twist)
            # for debug
            print("POSE TWIST: {}, {}".format(self.pose_twist.linear.x, self.pose_twist.angular.z))
            print("ENEMY TWIST: {}, {}".format(self.near_enemy_twist.linear.x, self.near_enemy_twist.angular.z))
            print("wall: {}, Enemy: {}, X: {}, Z: {}".format(self.is_near_wall, self.is_near_enemy, twist.linear.x, twist.angular.z))

            r.sleep()


if __name__ == '__main__':
    rospy.init_node('enemy')
    bot = TeriyakiBurger('teriyaki_burger')
    bot.strategy()