def interpose(target_info,
control_target,
dist_from_goal=None,
dist_from_target=None):
# 自チームのゴール中心とtarget_info.poseを直線で結び、その直線上に移動する
# dist_from_goal is not Noneなら、ゴール中心からdist分離れた位置に移動する
# dist_from_target is not Noneなら、target_info.poseからdist分離れた位置に移動する
# 到達姿勢の計算とcontrol_targetの更新(path以外)
control_target.kick_power = 0.0
control_target.dribble_power = 0.0
# 両方設定されてなかったらdist_from_targetを優先する
if dist_from_goal is None and dist_from_target is None:
dist_from_target = 0.6 # 適当な値
OUR_GOAL_POSE = Field.goal_pose('our', 'center')
angle_to_target = tool.get_angle(OUR_GOAL_POSE, target_info.pose)
new_goal_pose = Pose2D()
if dist_from_goal is not None:
trans = tool.Trans(GOAL_POSE, angle_to_target)
tr_goal_pose = Pose2D(dist_from_goal, 0, 0)
new_goal_pose = trans.inverted_transform(tr_goal_pose)
else:
angle_to_goal = tool.get_angle(target_info.pose, OUR_GOAL_POSE)
trans = tool.Trans(target_info.pose, angle_to_goal)
tr_goal_pose = Pose2D(dist_from_target, 0, 0)
new_goal_pose = trans.inverted_transform(tr_goal_pose)
new_goal_pose.theta = angle_to_target
# ---------------------------------------------------------
remake_path = False
# pathが設定されてなければpathを新規作成
if control_target.path is None or len(control_target.path) == 0:
remake_path = True
# 現在のpathゴール姿勢と、新しいpathゴール姿勢を比較し、path再生成の必要を判断する
if remake_path is False:
current_goal_pose = control_target.path[-1]
if not tool.is_close(current_goal_pose, new_goal_pose,
Pose2D(0.1, 0.1, math.radians(10))):
remake_path = True
# remake_path is Trueならpathを再生成する
# pathを再生成すると衝突回避用に作られた経路もリセットされる
if remake_path:
control_target.path = []
control_target.path.append(new_goal_pose)
return control_target
def simple_kick(my_pose, ball_info, control_target, kick_power=0.5):
# ボールへまっすぐ向かい、そのまま蹴る
# シュート目標位置を設定しないシンプルなaction
# 目標位置はボールの前方にし、目標角度は、自己位置からみたボール方向にする
angle_robot_to_ball = tool.get_angle(my_pose, ball_info.pose)
trans = tool.Trans(ball_info.pose, angle_robot_to_ball)
tr_position = Pose2D(0.2, 0, 0) # ボールより少し前を目標位置にする
position = trans.inverted_transform(tr_position)
new_goal_pose = Pose2D()
new_goal_pose = position
new_goal_pose.theta = angle_robot_to_ball
# ボールに近づいたらキックフラグをON
if tool.distance_2_poses(my_pose, ball_info.pose) < 0.5:
control_target.kick_power = kick_power
else:
control_target.kick_power = 0.0
# ドリブルは常にオフ
control_target.dribble_power = 0.0
# ---------------------------------------------------------
remake_path = False
# pathが設定されてなければpathを新規作成
if control_target.path is None or len(control_target.path) == 0:
remake_path = True
# 現在のpathゴール姿勢と、新しいpathゴール姿勢を比較し、path再生成の必要を判断する
if remake_path is False:
current_goal_pose = control_target.path[-1]
if not tool.is_close(current_goal_pose, new_goal_pose,
Pose2D(0.1, 0.1, math.radians(10))):
remake_path = True
# remake_path is Trueならpathを再生成する
# pathを再生成すると衝突回避用に作られた経路もリセットされる
if remake_path:
control_target.path = []
control_target.path.append(new_goal_pose)
return control_target
def interpose(ball_info, robot_info, control_target):
# ボールの位置
ball_pose = ball_info.pose
# ボールの速度
ball_vel = ball_info.velocity
# ゴールの位置
OUR_GOAL_POSE = Field.goal_pose('our', 'center')
xr = OUR_GOAL_POSE.x + 0.3
goalie_threshold_y = 1.5
# 敵のロボットの情報
robot_info_their = robot_info['their']
dist = []
for their_info in robot_info_their:
if their_info.detected:
dist.append(tool.distance_2_poses(their_info.pose, ball_pose))
else:
dist.append(100)
# 一番近い敵ロボットのID
min_dist_id = dist.index(min(dist))
# 一番近い敵ロボットの座標
robot_pose = robot_info_their[min_dist_id].pose
# ボールの速度
if ball_vel is None:
v = 0
dx = 0
dy = 0
da = 0
else:
# ボールの速度
vx = ball_vel.x
vy = ball_vel.y
v = math.hypot(ball_vel.x, ball_vel.y)
# ボールの進む角度
angle_ball = math.atan2(vy, vx)
# ボールの変化量を計算（正確ではなく方向を考慮した単位量）
dvx = math.cos(angle_ball)
dvy = math.sin(angle_ball)
# ボールの次の予測位置を取得
ball_pose_next = Pose2D(ball_pose.x + dvx, ball_pose.y + dvy, 0)
if dist[min_dist_id] < 0.15 and robot_pose.x < 0:
rospy.logdebug('their')
angle_their = robot_pose.theta
if angle_their == 0:
a = 1e-10
else:
a = math.tan(angle_their)
b = robot_pose.y - a * robot_pose.x
elif 0.02 < v and dvx < 0:
rospy.logdebug('ball move')
a, b = line_pram(ball_pose, ball_pose_next)
else:
rospy.logdebug('ball stop')
a, b = line_pram(ball_pose, OUR_GOAL_POSE)
# 位置を決める
yr = a*xr + b
# 位置
if yr > goalie_threshold_y:
yr = goalie_threshold_y
elif yr < -goalie_threshold_y:
yr = -goalie_threshold_y
# elif ball_pose.x < xr:
# ボールが後ろに出たらy座標は0にする
# yr = 0
new_goal_pose = Pose2D(xr, yr, 0)
# ---------------------------------------------------------
remake_path = False
# pathが設定されてなければpathを新規作成
if control_target.path is None or len(control_target.path) == 0:
remake_path = True
# 現在のpathゴール姿勢と、新しいpathゴール姿勢を比較し、path再生成の必要を判断する
if remake_path is False:
current_goal_pose = control_target.path[-1]
if not tool.is_close(current_goal_pose, new_goal_pose, Pose2D(0.1, 0.1, math.radians(10))):
remake_path = True
# remake_path is Trueならpathを再生成する
# pathを再生成すると衝突回避用に作られた経路もリセットされる
if remake_path:
control_target.path = []
control_target.path.append(new_goal_pose)
# print(goalie_pose.x, goalie_pose.y, ball_pose.x, ball_pose.y)
return control_target
def interpose(ball_info, robot_info, control_target):
# ボールが動いていると判断するしきい値[m/s]
MOVE_BALL_VELOCITY_THRESHOLD = 0.5
# ロボットがボールを持っていると判断するしきい値[m]
DIST_ROBOT_TO_BALL_THRESHOLD = 0.2
# ゴールライン上ではなく一定距離[m]前を守るための変数
MARGIN_DIST_X = 0.1
# ゴールの位置(自陣のゴールのx座標は絶対負になる)
OUR_GOAL_POSE = Field.goal_pose('our', 'center')
OUR_GOAL_UPPER = Field.goal_pose('our', 'upper')
OUR_GOAL_LOWER = Field.goal_pose('our', 'lower')
# ボールの位置
ball_pose = ball_info.pose
# 敵のロボットの情報
robot_info_their = robot_info['their']
# 敵ロボットとボールの距離
dist = []
for their_info in robot_info_their:
if their_info.disappeared is False:
dist.append(tool.distance_2_poses(their_info.pose, ball_pose))
else:
dist.append(100)
# 一番近い敵ロボットのID
min_dist_id = dist.index(min(dist))
# 一番近い敵ロボットの座標
robot_pose = robot_info_their[min_dist_id].pose
# ボールの速度
ball_velocity_x = ball_info.velocity.x
ball_velocity_y = ball_info.velocity.y
ball_velocity = math.hypot(ball_velocity_x, ball_velocity_y)
# ボールの進む角度
angle_ball = math.atan2(ball_velocity_y, ball_velocity_x)
# ボールの進む変化量を計算（方向を考慮した単位量）
var_ball_velocity_x = math.cos(angle_ball)
var_ball_velocity_y = math.sin(angle_ball)
# ボールの次の予測位置を取得
ball_pose_next = Pose2D(ball_pose.x + var_ball_velocity_x,
ball_pose.y + var_ball_velocity_y, 0)
# 敵ロボットとボールの距離が近い場合は敵の向いている直線を使う
if dist[min_dist_id] < DIST_ROBOT_TO_BALL_THRESHOLD and robot_pose.x < 0:
slope = math.tan(robot_pose.theta)
intercept = robot_pose.y - slope * robot_pose.x
# ボールの速度がある場合かつ近づいてくる場合はボールの向かう直線を使う
elif MOVE_BALL_VELOCITY_THRESHOLD < ball_velocity and ball_velocity_x < 0:
slope, intercept = _get_line_parameters(ball_pose, ball_pose_next)
# その他はゴール中心とボールを結ぶ直線を使う
else:
slope, intercept = _get_line_parameters(ball_pose, OUR_GOAL_POSE)
# ゴーリの新しい座標
goalie_pose_x = OUR_GOAL_POSE.x + MARGIN_DIST_X
goalie_pose_y = slope * goalie_pose_x + intercept
# ゴールから飛び出さないようる上下限を設ける
if OUR_GOAL_UPPER.y < goalie_pose_y:
goalie_pose_y = OUR_GOAL_UPPER.y
elif goalie_pose_y < OUR_GOAL_LOWER.y:
goalie_pose_y = OUR_GOAL_LOWER.y
# ゴーリの新しい座標
new_goalie_pose = Pose2D(goalie_pose_x, goalie_pose_y, 0)
# ---------------------------------------------------------
remake_path = False
# pathが設定されてなければpathを新規作成
if control_target.path is None or len(control_target.path) == 0:
remake_path = True
# 現在のpathゴール姿勢と、新しいpathゴール姿勢を比較し、path再生成の必要を判断する
if remake_path is False:
current_goalie_pose = control_target.path[-1]
if not tool.is_close(current_goalie_pose, new_goalie_pose,
Pose2D(0.1, 0.1, math.radians(10))):
remake_path = True
# remake_path is Trueならpathを再生成する
# pathを再生成すると衝突回避用に作られた経路もリセットされる
if remake_path:
control_target.path = []
control_target.path.append(new_goalie_pose)
return control_target