def calcurate(self, strategy_context=None, place_ball_position=[0, 0]):
# type: (StrategyContext) -> StrategyBase
self.place_point = place_ball_position
self.ball_dispersion.append(
functions.distance_btw_two_points(
self.place_point, self._ball_params.get_current_position()))
del self.ball_dispersion[0]
ball_dispersion_average = sum(self.ball_dispersion) / len(
self.ball_dispersion)
#place point,ボールから一番近い機体設定
self.active_robot_ids = self._get_active_robot_ids()
robots_near_to_ball_ids = self._objects.get_robot_ids_sorted_by_distance_to_ball(
self.active_robot_ids)
robots_near_to_placement_point_ids = self._objects.get_robot_ids_sorted_by_distance(
self.place_point, self.active_robot_ids)
receive_robot_id = robots_near_to_placement_point_ids[0]
if robots_near_to_placement_point_ids[0] == robots_near_to_ball_ids[0]:
kick_robot_id = robots_near_to_ball_ids[1]
else:
kick_robot_id = robots_near_to_ball_ids[0]
#ボールがplace pointから0.15m以内ならボールから離れる
if ball_dispersion_average < 0.15:
result = self.leave_ball()
#ボール後方に回り込めるならball placement
elif self.judge_kick_space(kick_robot_id):
result = self.ball_placement(kick_robot_id, receive_robot_id)
#回り込めない場合ボールを移動
else:
result = self.change_ball_position(kick_robot_id)
return result
def _get_who_has_a_ball(self):
# type: () -> str
ball_position = self._objects.ball.get_current_position()
#size_r = self._objects.robot.size_r
flag = 0
area = config.HAS_A_BALL_DISTANCE_THRESHOLD + self._objects.robot[
0].size_r
for robot_id in self._objects.get_robot_ids():
if self._objects.get_has_a_ball(robot_id):
flag = flag + 1
for enemy_id in self._objects.get_enemy_ids():
enemy_position = self._objects.enemy[
enemy_id].get_current_position()
if functions.distance_btw_two_points(ball_position,
enemy_position) < area:
flag = flag - 1
if flag > 0:
who_has_a_ball = "robots"
elif flag < 0:
who_has_a_ball = "enemy"
else:
who_has_a_ball = "free"
return who_has_a_ball
def calcurate(self, strategy_context=None):
# type: (StrategyContext) -> StrategyBase
if self._get_who_has_a_ball() == "free":
pass
elif self._get_who_has_a_ball() == "robots":
self.history_who_has_a_ball.pop(0)
self.history_who_has_a_ball.append("robots")
else:
self.history_who_has_a_ball.pop(0)
self.history_who_has_a_ball.append("enemy")
if self.history_who_has_a_ball.count("enemy") > 5:
strategy_context.update("direct_finish",
True,
namespace="world_model")
strategy_context.update("defence_or_attack",
False,
namespace="world_model")
# ここに入ると次からnormal_startが呼び出される
ball_x, ball_y = self._objects.ball.get_current_position()
if functions.distance_btw_two_points([ball_x, ball_y],
[5.8, 4.3]) < 0.3:
self.ckl_flg = True
self.ckr_flg = False
if functions.distance_btw_two_points([ball_x, ball_y],
[5.8, -4.3]) < 0.3:
self.ckl_flg = False
self.ckr_flg = True
if self._objects.get_ball_in_penalty_area() == "friend":
# ボールが自陣ペナルティエリアに入っているときはGK
result = self.calculate_goal_kick()
elif self.ckl_flg:
# ボールがCK左の位置にあるとき
result = self.calculate_corner_kick_left_2()
elif self.ckr_flg:
# ボールがCK右の位置にあるとき
result = self.calculate_corner_kick_right_2()
elif self._objects.ball.get_current_position()[1] > 0.:
result = self.calculate_indirect_left()
else:
result = self.calculate_indirect_right()
return result
def calculate_defense(self, strategy_context=None):
# type: (StrategyContext) -> StrategyBase
active_robot_ids = self._get_active_robot_ids()
active_enemy_ids = self._get_active_enemy_ids()
status = Status()
nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(
active_enemy_ids)[0]
for robot_id in active_robot_ids:
robot = self._objects.get_robot_by_id(robot_id)
if robot.get_role() == "GK":
status.status = "keeper"
elif robot.get_role() == "LDF":
status.status = "defence4"
elif robot.get_role() == "RDF":
status.status = "defence3"
elif robot.get_role() == "LFW":
#敵kickerとballの延長線上に移動
status.status = "move_linear"
if nearest_enemy_id != None:
status.pid_goal_pos_x, status.pid_goal_pos_y = functions.calculate_internal_dividing_point(
self._enemy[nearest_enemy_id].get_current_position()
[0],
self._enemy[nearest_enemy_id].get_current_position()
[1],
self._ball_params.get_current_position()[0],
self._ball_params.get_current_position()[1],
functions.distance_btw_two_points(
self._enemy[nearest_enemy_id].get_current_position(
), self._ball_params.get_current_position()) +
0.55, -0.55)
status.pid_goal_theta = math.atan2(
(self._ball_params.get_current_position()[1] -
self._robot[3].get_current_position()[1]),
(self._ball_params.get_current_position()[0] -
self._robot[2].get_current_position()[0]))
elif robot.get_role() == "RFW":
#フリーで最もゴールに近い敵idを返す
status.status = "move_linear"
free_enemy_id = self._get_free_enemy_id(4, nearest_enemy_id)
status.pid_goal_pos_x = (
self._ball_params.get_current_position()[0] +
self._enemy[free_enemy_id].get_current_position()[0]) / 2
status.pid_goal_pos_y = (
self._ball_params.get_current_position()[1] +
self._enemy[free_enemy_id].get_current_position()[1]) / 2
status.pid_goal_theta = math.atan2(
(self._ball_params.get_current_position()[1] -
self._robot[4].get_current_position()[1]),
(self._ball_params.get_current_position()[0] -
self._robot[4].get_current_position()[0]))
else:
status.status = "none"
self._dynamic_strategy.set_robot_status(robot_id, status)
result = self._dynamic_strategy
return result
def calcurate(self, strategy_context=None, referee_branch="KICKOFF_DEFENCE"):
# type: (StrategyContext) -> StrategyBase
if referee_branch == "NORMAL_START":
if self._detect_enemy_kick(strategy_context):
strategy_context.update("enemy_kick", True, namespace="world_model")
strategy_context.update("defence_or_attack", False, namespace="world_model")
else:
strategy_context.update("placed_ball_position", self._ball_params.get_current_position(), namespace="world_model")
active_robot_ids = self._get_active_robot_ids()
active_enemy_ids = self._get_active_enemy_ids()
status = Status()
nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(active_enemy_ids)[0]
for idx, robot_id in enumerate(active_robot_ids):
robot = self._objects.get_robot_by_id(robot_id)
if robot.get_role() == "GK":
status.status = "keeper"
elif robot.get_role() == "LDF":
status.status = "defence4"
elif robot.get_role() == "RDF":
status.status = "defence3"
elif robot.get_role() == "LFW":
#敵kickerとballの延長線上に移動
status.status = "move_linear"
if nearest_enemy_id != None:
status.pid_goal_pos_x, status.pid_goal_pos_y = functions.calculate_internal_dividing_point(
self._objects.get_enemy_by_id(nearest_enemy_id).get_current_position()[0],
self._objects.get_enemy_by_id(nearest_enemy_id).get_current_position()[1],
self._ball_params.get_current_position()[0],
self._ball_params.get_current_position()[1],
functions.distance_btw_two_points(self._objects.get_enemy_by_id(nearest_enemy_id).get_current_position(),
self._ball_params.get_current_position()) + 0.8,
-0.8)
status.pid_goal_theta = math.atan2((self._ball_params.get_current_position()[1] - robot.get_current_position()[1]) , (self._ball_params.get_current_position()[0] - robot.get_current_position()[0]))
if status.pid_goal_pos_x >= -0.2 and status.pid_goal_pos_y >= 0:
status.pid_goal_pos_x = -0.2
status.pid_goal_pos_y = 0.8
elif status.pid_goal_pos_x >= -0.2 and status.pid_goal_pos_y < 0:
status.pid_goal_pos_x = -0.2
status.pid_goal_pos_y = -0.8
elif robot.get_role() == "RFW":
#固定位置
status.status = "move_linear"
free_enemy_id = self._get_free_enemy_id(4, nearest_enemy_id)
status.pid_goal_pos_x = -3.
status.pid_goal_pos_y = 0.
status.pid_goal_theta = math.atan2( (self._ball_params.get_current_position()[1] - robot.get_current_position()[1]) , (self._ball_params.get_current_position()[0] - robot.get_current_position()[0]) )
else:
status.status = "none"
self._dynamic_strategy.set_robot_status(robot_id, status)
result = self._dynamic_strategy
return result
def calcurate(self, strategy_context=None):
# (context.StrategyContext) -> strategy.StrategyBase
strategy_context.update("placed_ball_position",
self._ball_params.get_current_position(),
namespace="world_model")
strategy_context.update("enemy_kick", False, namespace="world_model")
self._dynamic_strategy.clone_from(self._static_strategies['initial'])
rospy.set_param("/robot_max_velocity", 1.5)
ball = self._objects.ball
for robot_id in self._get_active_robot_ids():
robot = self._objects.get_robot_by_id(robot_id)
if robot is None:
continue
status = self._dynamic_strategy.get_robot_status(robot_id)
target_pos_xy = (status.pid_goal_pos_x, status.pid_goal_pos_y)
target_distance = 0.5 + self._objects.robot[0].size_r
distance_ball_robot = functions.distance_btw_two_points(
robot.get_current_position(), ball.get_current_position())
distance_ball_target_pos = functions.distance_btw_two_points(
target_pos_xy, ball.get_current_position())
# 閾値を超える/超えないで振動を防ぐためのoffset
offset = 0.05
if distance_ball_robot < (target_distance + offset) \
and distance_ball_target_pos < (target_distance + offset):
status = Status()
status.status = "move_linear"
vector = np.array(target_pos_xy) - np.array(
ball.get_current_position())
vector = (target_distance / np.linalg.norm(vector)) * vector
vector = np.array(ball.get_current_position()) + vector
status.pid_goal_pos_x = vector[0]
status.pid_goal_pos_y = vector[1]
self._dynamic_strategy.set_robot_status(robot_id, status)
return self._dynamic_strategy
def ball_placement(self, kick_robot_id, receive_robot_id):
#place pointからボールが遠ければパス,近ければドリブル
if functions.distance_btw_two_points(
self.place_point,
self._ball_params.get_current_position()) > self.dribble_range:
result = self.ball_placement_kick(kick_robot_id, receive_robot_id)
else:
result = self.ball_placement_dribble(kick_robot_id,
receive_robot_id)
return result
def kick_xz(self, power_x=10.0, power_z=0.0, ignore_penalty_area=False):
area = 1.0
elapsed_time = rospy.Time.now() - self._kick_start_time
if functions.distance_btw_two_points(self.ball_params.get_current_position(),
self.ctrld_robot.get_current_position()) \
> self.ctrld_robot.size_r + area \
or elapsed_time.to_sec() > 5.0 \
or functions.distance_btw_two_points(self.ball_params.get_current_position(), self._dribble_start_pos) > 0.8:
self.cmd.vel_surge = 0
self.cmd.vel_sway = 0
self.cmd.omega = 0
self.cmd.kick_speed_x = 0
self.status.robot_status = "None"
self.pass_stage = 0
return
self.cmd.kick_speed_x = power_x
self.cmd.kick_speed_z = power_z
self.pid.pid_linear(self.ball_params.get_current_position()[0],
self.ball_params.get_current_position()[1],
self.pose_theta,
ignore_penalty_area=ignore_penalty_area)
def get_enemy_ids_sorted_by_distance(self, target_xy, enemy_ids=None):
target_x = target_xy[0]
target_y = target_xy[1]
if enemy_ids is None:
enemys = self.enemy
else:
enemys = [self.enemy[i] for i in enemy_ids]
sorted_enemys = sorted(
enemys,
key=lambda enemy: functions.distance_btw_two_points(
enemy.get_current_position(), (target_x, target_y)))
sorted_ids = map(lambda enemy: enemy.get_id(), sorted_enemys)
return sorted_ids
def judge_kick_space(self, kick_robot_id):
n = self.kick_margin
m = functions.distance_btw_two_points(
self.place_point, self._ball_params.get_current_position()) + n
moving_point = functions.calculate_internal_dividing_point_vector_args(
self.place_point, self._ball_params.get_current_position(), m, n)
kick_robot = self._objects.get_robot_by_id(kick_robot_id)
#ボール後方がフィールド内かで判定
offset = 1.5
if -config.FIELD_SIZE[0] / 2. + (kick_robot.size_r * offset) < moving_point[0] < config.FIELD_SIZE[0] / 2. - (kick_robot.size_r * offset) \
and -config.FIELD_SIZE[1] / 2. + (kick_robot.size_r * offset) < moving_point[1] < config.FIELD_SIZE[1] / 2. - (kick_robot.size_r * offset):
return True
else:
return False
def get_robot_ids_sorted_by_distance(self, target_xy, robot_ids=None):
# type: (typing.List[float], typing.List[int]) -> typing.List[int]
target_x = target_xy[0]
target_y = target_xy[1]
if robot_ids is None:
robots = self.robot
else:
robots = [self.robot[i] for i in robot_ids]
sorted_robots = sorted(
robots,
key=lambda robot: functions.distance_btw_two_points(
robot.get_current_position(), (target_x, target_y)))
sorted_ids = map(lambda robot: robot.get_id(), sorted_robots)
return sorted_ids
def get_has_a_ball(self, robot_id, threshold=None):
robot_ids = self.get_active_robot_ids()
sorted_ids = self.get_robot_ids_sorted_by_distance_to_ball(robot_ids)
if sorted_ids[0] != robot_id:
return False
if threshold is None:
threshold = config.HAS_A_BALL_DISTANCE_THRESHOLD
area = threshold + self.robot[0].size_r
if functions.distance_btw_two_points(
self.robot[robot_id].get_current_position(), self.ball.get_current_position()) \
> area:
return False
return True
def leave_ball(self):
status = Status()
for robot_id in self.active_robot_ids:
robot = self._objects.get_robot_by_id(robot_id)
distance_ball_robot = functions.distance_btw_two_points(
robot.get_current_position(),
self._ball_params.get_current_position())
if distance_ball_robot < self.leave_range:
vector = np.array(
self._ball_params.get_current_position()) - np.array(
robot.get_current_position())
vector = (self.leave_range / np.linalg.norm(vector)) * vector
vector = np.array(robot.get_current_position()) - vector
status.pid_goal_pos_x = vector[0]
status.pid_goal_pos_y = vector[1]
status.status = "move_linear"
else:
status.status = "stop"
self._dynamic_strategy.set_robot_status(robot_id, status)
result = self._dynamic_strategy
return result
def dribble_ball(self, target_x, target_y, ignore_penalty_area=False):
distance = math.sqrt(
(target_x - self.ball_params.get_current_position()[0])**2 +
(target_y - self.ball_params.get_current_position()[1])**2)
if distance != 0:
#ボール後方へ移動
if self.dribble_stage == 0:
prepare_offset = 0.4
pose_x = (
-prepare_offset * target_x + (prepare_offset + distance) *
self.ball_params.get_current_position()[0]) / distance
pose_y = (
-prepare_offset * target_y + (prepare_offset + distance) *
self.ball_params.get_current_position()[1]) / distance
pose_theta = math.atan2(
(target_y - self.ctrld_robot.get_current_position()[1]),
(target_x - self.ctrld_robot.get_current_position()[0]))
a, b, c = functions.line_parameters(
self.ball_params.get_current_position()[0],
self.ball_params.get_current_position()[1], target_x,
target_y)
self.dispersion1.append(
functions.distance_of_a_point_and_a_straight_line(
self.ctrld_robot.get_current_position()[0],
self.ctrld_robot.get_current_position()[1], a, b, c))
#(pose_x - self.ctrld_robot.get_current_position()[0])**2 \
#+ (pose_y - self.ctrld_robot.get_current_position()[1])**2)
del self.dispersion1[0]
self.dispersion2.append(
(pose_x - self.ctrld_robot.get_current_position()[0])**2 \
+ (pose_y - self.ctrld_robot.get_current_position()[1])**2)
del self.dispersion2[0]
dispersion_average1 = sum(self.dispersion1) / len(
self.dispersion1)
dispersion_average2 = sum(self.dispersion2) / len(
self.dispersion2)
self.rot_dispersion.append(
(pose_theta -
self.ctrld_robot.get_current_orientation())**2)
del self.rot_dispersion[0]
rot_dispersion_average = sum(self.rot_dispersion) / len(
self.rot_dispersion)
# print("{} {}".format(dispersion_average, rot_dispersion_average))
if dispersion_average1 < self.dribble_access_threshold1 and dispersion_average2 < self.dribble_access_threshold2 and rot_dispersion_average < self.dribble_rot_access_threshold:
self.dribble_stage = 1
self.pid.pid_linear(pose_x,
pose_y,
pose_theta,
ignore_penalty_area=ignore_penalty_area)
#目標地付近までドリブル
elif self.dribble_stage == 1:
self.ctrld_robot.set_max_velocity(
config.ROBOT_DRIBBLE_VELOCITY)
pose_theta = math.atan2(
(target_y - self.ctrld_robot.get_current_position()[1]),
(target_x - self.ctrld_robot.get_current_position()[0]))
area = 0.5
if functions.distance_btw_two_points(self.ball_params.get_current_position(),
self.ctrld_robot.get_current_position()) \
> self.ctrld_robot.size_r + area:
self.cmd.vel_surge = 0
self.cmd.vel_sway = 0
self.cmd.omega = 0
self.cmd.dribble_power = 0
self.status.robot_status = "None"
self.dribble_stage = 0
return
if functions.distance_btw_two_points(
self.ball_params.get_current_position(),
[target_x, target_y]) < 0.3:
self.dribble_stage = 2
self.cmd.dribble_power = self.dribble_power
self.pid.pid_straight(target_x,
target_y,
pose_theta,
ignore_penalty_area=ignore_penalty_area,
avoid=False)
#目標地付近になったらドリブラーを止めて引きずる(バックスピン防止)
elif self.dribble_stage == 2:
pose_theta = math.atan2(
(target_y - self.ctrld_robot.get_current_position()[1]),
(target_x - self.ctrld_robot.get_current_position()[0]))
area = 0.5
if functions.distance_btw_two_points(self.ball_params.get_current_position(),
self.ctrld_robot.get_current_position()) \
> self.ctrld_robot.size_r + area \
or functions.distance_btw_two_points(self.ball_params.get_current_position(), [target_x, target_y]) < 0.1:
self.cmd.vel_surge = 0
self.cmd.vel_sway = 0
self.cmd.omega = 0
self.cmd.dribble_power = 0
self.status.robot_status = "None"
self.dribble_stage = 0
return
self.cmd.dribble_power = 0
self.pid.pid_straight(target_x,
target_y,
pose_theta,
ignore_penalty_area=ignore_penalty_area,
avoid=False)
def _recieve_ball(self,
target_xy,
next_target_xy,
auto_kick=False,
is_shoot=False,
ignore_penalty_area=False,
is_tip_kick=False):
# type: (typing.Tuple[float], typing.Tuple[float]) -> None
"""
Parameters
----------
target_xy: (x, y) パス目標地点
next_target_xy: (x, y) 受け取り時にロボットが向いているべき方向
auto_kick: boolean Trueならキッカーに当たったらそのままキック、Falseなら受けるだけ
is_shoot: boolean auto_kickがTrueのときにのみ利用され、Trueならshootの威力でキック、Falseならpassの威力でキック
ignore_penalty_area: boolean Trueならペナルティエリアに進入する、Falseなら進入しない
"""
self.cmd.kick_speed_x = 0
self.cmd.kick_speed_z = 0
target_x = target_xy[0]
target_y = target_xy[1]
line_a = self.ball_params.get_line_a()
line_b = self.ball_params.get_line_b()
# 本来のパス目標地点とフィッティング直線Lとの距離計算
d = abs(line_a*target_x - target_y + line_b) \
/ ((line_a**2 + 1)**(1/2)) # ヘッセの公式で距離計算
# 交点H(hx, hy) の座標計算
hx = (line_a * (target_y - line_b) + target_x) \
/ (line_a**2 + 1)
hy = line_a * (line_a * (target_y - line_b) + target_x) \
/ (line_a**2 + 1) + line_b
# TODO: 機体の速度・加速度から間に合うかどうか判断
# 距離だけで諦めるかどうか判断
if (d < 2.0) and (line_a != 0):
pose_theta = math.atan2((next_target_xy[1] - hy),
(next_target_xy[0] - hx))
# pose_theta = math.atan2( (next_target_xy[1]) , (next_target_xy[0]) )
self.pid.pid_linear(hx,
hy,
pose_theta,
ignore_penalty_area=ignore_penalty_area)
else:
pose_theta = math.atan2((next_target_xy[1] - target_y),
(next_target_xy[0] - target_x))
# pose_theta = math.atan2( (next_target_xy[1]) , (next_target_xy[0]) )
self.pid.pid_linear(target_x,
target_y,
pose_theta,
ignore_penalty_area=ignore_penalty_area)
if auto_kick:
distance = functions.distance_btw_two_points((target_x, target_y),
next_target_xy)
if not is_shoot:
kick_power_x = min(
math.sqrt(distance) * self.const, config.MAX_KICK_POWER_X)
else:
# 12.0: フィールドの横幅
kick_power_x = config.MAX_KICK_POWER_X
if is_tip_kick:
kick_power_x = config.MAX_KICK_POWER_X
self.cmd.kick_speed_z = kick_power_x
self.cmd.kick_speed_x = kick_power_x
def calcurate(self, strategy_context=None, should_wait=True):
#print "penalty"
if self._get_who_has_a_ball() == "free":
pass
elif self._get_who_has_a_ball() == "robots":
self.history_who_has_a_ball.pop(0)
self.history_who_has_a_ball.append("robots")
else:
self.history_who_has_a_ball.pop(0)
self.history_who_has_a_ball.append("enemy")
# if self.history_who_has_a_ball.count("enemy") > 5:
# strategy_context.update("penalty_finish", True, namespace="world_model")
# strategy_context.update("defence_or_attack", False, namespace="world_model")
active_robot_ids = self._get_active_robot_ids()
for robot_id in active_robot_ids:
status = Status()
robot = self._objects.get_robot_by_id(robot_id)
if robot.get_role() == "GK":
status.status = "keeper"
elif robot.get_role() == "LDF":
status.status = "defence4"
elif robot.get_role() == "RDF":
status.status = "defence3"
elif robot.get_role() == "LFW":
#敵kickerとballの延長線上に移動
status.status = "move_linear"
status.pid_goal_pos_x = 3.0
status.pid_goal_pos_y = 1.5
elif robot.get_role() == "RFW":
status.status = "move_linear"
status.pid_goal_pos_x = 3.0
status.pid_goal_pos_y = -1.5
else:
status.status = "none"
if robot_id == active_robot_ids[0]:
if should_wait:
status.status = "move_linear"
status.pid_goal_pos_x = 4.5
status.pid_goal_pos_y = 0.0
else:
status.status = "penalty_shoot"
if functions.distance_btw_two_points(
self._objects.ball.get_current_position(),
self._objects.get_robot_by_id(
robot_id).get_current_position()) > 1.0:
strategy_context.update("penalty_finish",
True,
namespace="world_model")
strategy_context.update("defence_or_attack",
False,
namespace="world_model")
self._dynamic_strategy.set_robot_status(robot_id, status)
result = self._dynamic_strategy
return result
def calc_ball_line(self):
#直近nフレームの座標を取得
if self.ball_pos_count < self.ball_frame:
self.ball_pos_x_array[
self.ball_pos_count] = self.ball_params.get_current_position(
)[0]
self.ball_pos_y_array[
self.ball_pos_count] = self.ball_params.get_current_position(
)[1]
# self.ball_vel_x_array[self.ball_pos_count] = self.ball_params.get_current_velosity()[0]
# self.ball_vel_y_array[self.ball_pos_count] = self.ball_params.get_current_velosity()[1]
# self.ball_vel_array[self.ball_pos_count] = math.sqrt(self.ball_params.get_current_velosity()[0]**2 + self.ball_params.get_current_velosity()[1]**2)
# self.ball_vel_time_array[self.ball_pos_count] = 1./WORLD_LOOP_RATE * self.ball_pos_count
# 二回目以降に、前回との偏差を計算し、一定値以下なら動いてない判定とし、カウントを増やす。nフレームの半分までカウントされたら計算フラグをFalseにして
if self.ball_pos_count > 0:
if functions.distance_btw_two_points(
(self.ball_pos_x_array[self.ball_pos_count - 1],
self.ball_pos_y_array[self.ball_pos_count - 1]),
(self.ball_pos_x_array[self.ball_pos_count],
self.ball_pos_y_array[self.ball_pos_count]
)) < self.ball_move_threshold:
self.same_pos_count += 1
if self.same_pos_count >= self.ball_frame / 2:
self.same_pos_count = self.ball_frame / 2
self.ball_pos_count = -1
self.calc_flag = False
else:
self.same_pos_count = 0
self.calc_flag = True
self.ball_pos_count += 1
else:
self.ball_pos_x_array = np.roll(self.ball_pos_x_array, -1)
self.ball_pos_y_array = np.roll(self.ball_pos_y_array, -1)
# self.ball_vel_x_array = np.roll(self.ball_vel_x_array,-1)
# self.ball_vel_y_array = np.roll(self.ball_vel_y_array,-1)
# self.ball_vel_array = np.roll(self.ball_vel_array,-1)
self.ball_pos_x_array[
self.ball_pos_count -
1] = self.ball_params.get_current_position()[0]
self.ball_pos_y_array[
self.ball_pos_count -
1] = self.ball_params.get_current_position()[1]
# self.ball_vel_x_array[self.ball_pos_count-1] = self.ball_params.get_current_velosity()[0]
# self.ball_vel_y_array[self.ball_pos_count-1] = self.ball_params.get_current_velosity()[1]
# self.ball_vel_array[self.ball_pos_count] = math.sqrt(self.ball_params.get_current_velosity()[0]**2 + self.ball_params.get_current_velosity()[1]**2)
if functions.distance_btw_two_points(
(self.ball_pos_x_array[self.ball_pos_count - 2],
self.ball_pos_y_array[self.ball_pos_count - 2]),
(self.ball_pos_x_array[self.ball_pos_count - 1],
self.ball_pos_y_array[self.ball_pos_count -
1])) < self.ball_move_threshold:
self.same_pos_count += 1
if self.same_pos_count >= self.ball_frame / 2:
self.ball_pos_count = 0
self.calc_flag = False
else:
self.same_pos_count = 0
self.calc_flag = True
#x,y座標の分散を計算
x_variance = variance(self.ball_pos_x_array)
y_variance = variance(self.ball_pos_y_array)
#print(x_variance,y_variance)
#分散が1より大きかったらカウントリセット
if (x_variance > 1 or y_variance > 1):
self.ball_pos_count = 0
self.same_pos_count = 0
for i in range(0, self.ball_frame):
self.ball_pos_x_array[i] = 0
self.ball_pos_y_array[i] = 0
#print(self.ball_pos_count,self.same_pos_count)
if self.calc_flag == True:
a, b = self.reg1dim(self.ball_pos_x_array, self.ball_pos_y_array,
self.ball_pos_count)
self.ball_params.set_line_a(a)
self.ball_params.set_line_b(b)
""" #self.ball_vel_x_a, self.ball_vel_x_b = self.reg1dim(self.ball_vel_x_array, self.ball_vel_time_array, self.ball_pos_count)
#self.ball_vel_y_a, self.ball_vel_y_b = self.reg1dim(self.ball_vel_y_array, self.ball_vel_time_array, self.ball_pos_count)
#self.ball_vel_a, self.ball_vel_b = self.reg1dim(self.ball_vel_array, self.ball_vel_time_array, self.ball_pos_count)
#self.ball_params.ball_sub_params.a, self.ball_params.ball_sub_params.b = self.reg1dim(self.ball_vel_x_array, self.ball_vel_time_array, self.ball_pos_count)
# self.ball_params.ball_sub_params.future_x =
# self.ball_params.ball_sub_params.future_y
#rospy.loginfo("vel_x_a:%f\tvel_x_b:%f",self.ball_vel_x_a, self.ball_vel_x_b)
#ボールの予想停止位置を計算
#x,y方向の現在の速度を最小二乗法で求めた直線から計算→式が違う、速度推定が必要
#ball_fit_vel_x = self.ball_vel_x_a*self.ball_vel_time_array[self.ball_pos_count-1] + self.ball_vel_x_b
#ball_fit_vel_y = self.ball_vel_y_a*self.ball_vel_time_array[self.ball_pos_count-1] + self.ball_vel_y_b
#とりあえず現在速度を使う
#ball_fit_vel_x = self.ball_params.get_current_velosity()[0]
#ball_fit_vel_y = self.ball_params.get_current_velosity()[1]
#停止するまでの時間を現在の速度と傾きから計算
if self.ball_vel_x_a != 0 and self.ball_vel_y_a != 0:
self.ball_stop_time_x = -(ball_fit_vel_x / self.ball_vel_x_a)
self.ball_stop_time_y = -(ball_fit_vel_y / self.ball_vel_y_a)
if self.ball_stop_time_x <= 0 or self.ball_stop_time_y <= 0:
# self.ball_params.ball_sub_params.future_x = 0
# self.ball_params.ball_sub_params.future_y = 0
else:
self.ball_params.ball_sub_params.future_x = self.ball_params.get_current_position()[0] + ball_fit_vel_x*self.ball_stop_time_x + 1/2*self.ball_vel_x_a*self.ball_stop_time_x**2
self.ball_params.ball_sub_params.future_y = self.ball_params.get_current_position()[1] + ball_fit_vel_y*self.ball_stop_time_y + 1/2*self.ball_vel_y_a*self.ball_stop_time_y**2
self.ball_params.ball_sub_params.future_x = np.clip(self.ball_params.ball_sub_params.future_x,-5,5)
self.ball_params.ball_sub_params.future_y = np.clip(self.ball_params.ball_sub_params.future_y,-5,5)
#rospy.loginfo("t=(%.3f,%.3f)\t(f_x:n_x)=(%.3f:%.3f)\t(f_y:n_y)=(%.3f:%.3f)",self.ball_stop_time_x,self.ball_stop_time_y,self.ball_params.ball_sub_params.future_x, self.ball_params.get_current_position()[0], self.ball_params.ball_sub_params.future_y, self.ball_params.get_current_position()[1]) """
else:
# self.ball_params.ball_sub_params.a = 0.
# self.ball_params.ball_sub_params.b = 0.
self.ball_params.set_line_a(0.)
self.ball_params.set_line_b(0.)
""" self.ball_vel_x_a = 0.
self.ball_vel_x_b = 0.
self.ball_vel_y_a = 0.
self.ball_vel_y_b = 0.
for i in range(0,self.ball_frame):
self.ball_pos_x_array[i] = 0
self.ball_pos_y_array[i] = 0
self.ball_vel_x_array[i] = 0
self.ball_vel_y_array[i] = 0 """
self.ball_sub_params.a = self.ball_params.get_line_a()
self.ball_sub_params.b = self.ball_params.get_line_b()
def get_distance(point_a, point_b):
# type: (Tuple[float, float], Tuple[float, float]) -> float
return functions.distance_btw_two_points(point_a, point_b)
def main(self, Kpv=None, Kpr=None, Kdv=None, Kdr=None):
robot_id = str(4)
robot_color = 'blue'
status_topic_name = '/'+robot_color+'/robot_'+robot_id+'/status'
self.status_topic_publisher = rospy.Publisher(
status_topic_name, Status, queue_size=1)
pid_service_name = '/'+robot_color+'/robot_'+robot_id+'/set_pid'
self.pid_service_proxy = rospy.ServiceProxy(pid_service_name, pid)
rospy.wait_for_service(pid_service_name)
self.robot = {}
rospy.wait_for_message(
'/'+robot_color+'/robot_'+robot_id+'/odom', Odometry)
self.odom_listener = rospy.Subscriber(
'/'+robot_color+'/robot_'+robot_id+'/odom', Odometry, self.odom_listener_callback)
rospy.wait_for_message(
'/'+robot_color+'/robot_'+robot_id+'/odom', Odometry)
# {'Kdv': 5.436618301696918, 'Kpv': 3.111774552866889}
if Kpv is None:
Kpv = 3.111774552866889
if Kpr is None:
Kpr = 2.2
if Kdv is None:
Kdv = 5.436618301696918
if Kdr is None:
Kdr = 1.0
# kdr 4.78 | 5.037
next_param = {
'Kpv': Kpv,
'Kpr': Kpr,
'Kdv': Kdv,
'Kdr': Kdr,
}
status = Status()
status.status = "move_linear"
status.pid_goal_pos_x = 4.5
status.pid_goal_pos_y = 1.0
status.pid_goal_theta = 0.0
self.pid_service_proxy(2.0,
2.0,
2.0,
2.0)
self.publish(status)
rospy.sleep(3.0)
self.pid_service_proxy(next_param['Kpv'],
next_param['Kpr'],
next_param['Kdv'],
next_param['Kdr'])
loop_rate = rospy.Rate(config.ROBOT_LOOP_RATE)
total_elapsed_time = 0.0
# x, y, theta
global mode
if mode == "position":
positions = [
[4.5, 0.0, 0.3 * np.pi],
[4.5, 2.0, 0.9 * np.pi],
# [0.0, -1.0, 1.8 * np.pi],
# [0.0, 2.0, 0.6 * np.pi],
]
else:
positions = [
[4.5, 1.0, 0.3 * np.pi],
[4.5, 1.0, 0.9 * np.pi],
[4.5, 1.0, 1.8 * np.pi],
[4.5, 1.0, 0.6 * np.pi],
]
total_average_pos = 0.0
total_average_rot = 0.0
for pos in positions:
status.pid_goal_pos_x = pos[0]
status.pid_goal_pos_y = pos[1]
status.pid_goal_theta = pos[2]
# print(str(status))
list_length = 60
hantei_list_pos = [10.0 for _ in range(list_length)]
hantei_list_rot = [1.0 * np.pi for _ in range(list_length)]
area_pos = 0.02
area_rot = np.pi * 3.0 / 180.0
start_time = rospy.Time.now()
# Start measuring
time_limit = 4.0
additional_cost = 0.0
self.publish(status)
while not rospy.is_shutdown():
distance_to_target_pos = \
functions.distance_btw_two_points((status.pid_goal_pos_x, status.pid_goal_pos_y),
(self.robot['x'], self.robot['y']))
distance_to_target_rot = self.diff_theta(status.pid_goal_theta, self.robot['theta'])
hantei_list_pos.append(distance_to_target_pos)
hantei_list_pos.pop(0)
hantei_list_rot.append(distance_to_target_rot)
hantei_list_rot.pop(0)
average_pos = np.average(hantei_list_pos)
average_rot = np.average(hantei_list_rot)
# rospy.loginfo_throttle(1, "pos average: "+str(average_pos)+" rot average: "+str(average_rot))
current_elapsed_time = rospy.Time.now() - start_time
time_over = abs(current_elapsed_time.to_sec()) > time_limit
if mode == "position":
condition = (average_pos < area_pos and average_rot < area_rot) or time_over
else:
condition = average_rot < area_rot or time_over
if condition:
# if current_elapsed_time.to_sec() > 3.0:
# total_average_pos = total_average_pos + average_pos
# total_average_rot = total_average_rot + average_rot
if time_over:
# 評価関数が非線形だろうが不連続だろうがそんなものは気にしない
additional_cost = 10 * (average_pos + average_rot)
break
loop_rate.sleep()
end_time = rospy.Time.now()
elapsed_time = (end_time - start_time).to_sec() + additional_cost
total_elapsed_time = total_elapsed_time + elapsed_time
# print("time: " + str(elapsed_time) + " sec")
# print("")
if rospy.is_shutdown():
break
# print("total_elapsed_time:" + str(total_elapsed_time))
return 10.0 / total_elapsed_time
def _attack_strategy1(self, strategy_context=None):
pass_positions = [
[2.0, 2.0],
[-2.0, -2.0],
[6.0, 0.0],
]
last_state = strategy_context.get_last("normal_strat_state")
self._reset_state()
area = self._objects.robot[0].size_r + 0.3
ball_pos = self._objects.ball.get_current_position()
# if self._receiver_id is None:
target_pos = pass_positions[last_state]
# else:
# # パスの目標ではなく、受け取る側のロボットが実際にリーチした場所を目標とする
# target_pos = self._objects.robot[self._receiver_id].get_current_position()
distance = functions.distance_btw_two_points(ball_pos, target_pos)
cur_state = last_state
if distance < area:
if len(pass_positions) > cur_state:
cur_state = cur_state + 1
is_shoot = False
if len(pass_positions) - 1 == cur_state:
is_shoot = True
should_reset = False
if len(pass_positions) == cur_state:
cur_state = cur_state - 1
should_reset = True
# print("State:"+str(cur_state))
# InitialStaticStrategyを元に組み立てる
self._dynamic_strategy.clone_from(self._static_strategies['initial'])
# 生きてるロボットにだけ新たな指示を出す例
active_robot_ids = self._get_active_robot_ids()
not_assigned_robot_ids = active_robot_ids
# Defence系を先にアサイン
not_assigned_ops = ["keeper", "defence3", "defence4"]
# for文で消えないようコピー
sorted_not_assigned_robot_ids = sorted(not_assigned_robot_ids, reverse=True)
for robot_id in sorted_not_assigned_robot_ids:
if not_assigned_ops == []:
break
status = Status()
status.status = not_assigned_ops[0]
self._dynamic_strategy.set_robot_status(robot_id, status)
not_assigned_ops.pop(0)
not_assigned_robot_ids.remove(robot_id)
robots_near_to_ball = self._objects.get_robot_ids_sorted_by_distance_to_ball(not_assigned_robot_ids)
for idx, robot_id in enumerate(robots_near_to_ball):
status = Status()
if idx == 0:
status.status = "pass"
if self._receiver_id is not None:
receiver_pos = self._objects.robot[self._receiver_id].get_current_position()
receiver_area = 1.0
if functions.distance_btw_two_points(pass_positions[cur_state], receiver_pos) > receiver_area:
status.status = "prepare_pass"
self._kicker_id = robot_id
status.pass_target_pos_x = pass_positions[cur_state][0]
status.pass_target_pos_y = pass_positions[cur_state][1]
not_assigned_robot_ids.remove(robot_id)
elif idx == 1:
if is_shoot:
status.status = "stop"
self._receiver_id = None
else:
status.status = "receive"
self._receiver_id = robot_id
status.pass_target_pos_x = pass_positions[cur_state][0]
status.pass_target_pos_y = pass_positions[cur_state][1]
not_assigned_robot_ids.remove(robot_id)
self._dynamic_strategy.set_robot_status(robot_id, status)
# self._dynamic_strategy.clone_from(self._static_strategies['initial'])
if should_reset:
# シュートしたらリセット
strategy_context.update("normal_strat_state", 0)
self._receiver_id = None
else:
strategy_context.update("normal_strat_state", cur_state)
result = copy.deepcopy(self._dynamic_strategy)
return result
def calculate_1(self, strategy_context=None):
# print 'passed_1_flg:', self.passed_1_flg
# print 'received_1_flg:', self.received_1_flg
# print 'passed_2_flg:',self.passed_2_flg
# print 'received_2_flg:', self.received_2_flg
# print 'passed_3_flg:', self.passed_3_flg
# print 'received_3_flg:', self.received_3_flg
"""ハーフラインより左側の戦略（の予定）"""
ball_x, ball_y = self._objects.ball.get_current_position()
active_enemy_ids = self._get_active_enemy_ids()
nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(
active_enemy_ids)[0]
for robot_id in self._robot_ids:
status = Status()
robot = self._objects.get_robot_by_id(robot_id)
if self.ball_position_nearest_id == robot_id:
if self.passed_1_flg:
# パス完了した場合
if self._objects.robot[robot_id].get_role() == "LDF":
status.status = "defence4"
else:
status.status = "defence3"
else:
# パス完了する前 パス先をposition1にする、パス完了したら完了flgを立てる
status.status = "pass"
status.pass_target_pos_x = self._objects.get_robot_by_id(
self.position_1_nearest_id).get_current_position()[0]
status.pass_target_pos_y = self._objects.get_robot_by_id(
self.position_1_nearest_id).get_current_position()[1]
if self._objects.get_has_a_ball(robot_id):
self.received_1_flg = True
if self.received_1_flg and self._objects.get_has_a_ball(
robot_id) == False:
self.passed_1_flg = True
elif robot_id == self.position_1_nearest_id:
if self.received_3_flg:
#シュートを打つ子がボールを受け取ったあとはこの機体もシュート体制に入る(おこぼれを狙うイメージ)
#status.status = "pass"
if self.position_2[1] > 0.:
status.status = "shoot_left"
# status.pass_target_pos_x = 6.0
# status.pass_target_pos_y = 0.55
else:
status.status = "shoot_right"
# status.pass_target_pos_x = 6.0
# status.pass_target_pos_y = -0.55
elif self.passed_2_flg:
# 自分がパスしたあとはゴール前のposition_3に移動
status.status = "move_linear"
status.pid_goal_pos_x = self.position_1[0]
status.pid_goal_pos_y = self.position_1[1]
status.pid_goal_theta = 0.
elif self.received_2_flg:
# レシーブしたあとはパスに切り替え
status.status = "pass"
status.pass_target_pos_x = self._objects.get_robot_by_id(
self.position_2_nearest_id).get_current_position()[0]
status.pass_target_pos_y = self._objects.get_robot_by_id(
self.position_2_nearest_id).get_current_position()[1]
if self._objects.get_has_a_ball(self.position_2_nearest_id,
threshold=0.20) == True:
self.passed_2_flg
else:
status.status = "receive"
status.pid_goal_pos_x = self.position_1[0]
status.pid_goal_pos_y = self.position_1[1]
if self._objects.get_has_a_ball(robot_id):
self.received_2_flg = True
elif robot_id == self.position_2_nearest_id:
if self.passed_3_flg:
status.status = "move_linear"
if nearest_enemy_id != None:
status.pid_goal_pos_x, status.pid_goal_pos_y = functions.calculate_internal_dividing_point(
self._enemy[nearest_enemy_id].get_current_position(
)[0],
self._enemy[nearest_enemy_id].get_current_position(
)[1],
self._ball_params.get_current_position()[0],
self._ball_params.get_current_position()[1],
functions.distance_btw_two_points(
self._enemy[nearest_enemy_id].
get_current_position(),
self._ball_params.get_current_position()) +
0.55, -0.55)
status.pid_goal_theta = math.atan2(
(self._ball_params.get_current_position()[1] -
robot.get_current_position()[1]),
(self._ball_params.get_current_position()[0] -
robot.get_current_position()[0]))
elif self.received_3_flg:
# status.status = "pass"
if self.position_3[1] > 0.:
status.status = "shoot_left"
# status.pass_target_pos_x = 6.0
# status.pass_target_pos_y = 0.55
else:
status.status = "shoot_right"
# status.pass_target_pos_x = 6.0
# status.pass_target_pos_y = -0.55
if self._objects.get_has_a_ball(robot_id) == False:
self.passed_3_flg = True
else:
status.status = "receive"
status.pid_goal_pos_x = self.position_2[0]
status.pid_goal_pos_y = self.position_2[1]
if self._objects.get_has_a_ball(robot_id):
self.received_3_flg = True
elif robot_id == self.GK_id:
status.status = "keeper"
elif robot.get_role() == "LDF":
status.status = "defence4"
elif robot.get_role() == "RDF":
status.status = "defence3"
else:
print 'error'
self._dynamic_strategy.set_robot_status(robot_id, status)
return self._dynamic_strategy
def _attack_strategy1(self, strategy_context=None):
#ball喪失時、防御へ移行
if self._get_who_has_a_ball() == "enemy":
strategy_context.update("defence_or_attack", False, namespace="world_model")
if self._pass_positions is None:
self._pass_positions = self._generate_pass_positions()
pass_positions = self._pass_positions
succeeded_area = self._objects.robot[0].size_r + 0.62
last_state = strategy_context.get_last("normal_strat_state", namespace="normal_strat")
ball_pos = self._objects.ball.get_current_position()
if self._receiver_id is None:
target_pos = pass_positions[last_state]
else:
# パスの目標ではなく、受け取る側のロボットが実際にリーチした場所を目標とする
target_pos = self._objects.get_robot_by_id(self._receiver_id).get_current_position()
distance = functions.distance_btw_two_points(ball_pos, target_pos)
# print("Receiver: "+ str(self._receiver_id) +" Distance:"+str(distance))
cur_state = last_state
# 目標位置近くにボールが行ったら次のステートへ
change_state = False
if distance < succeeded_area or (rospy.Time.now() - self._last_pass_start_time).to_sec() > 10.0:
cur_state = cur_state + 1
self._random_fake_position = self._generate_pass_position()
change_state = True
self._last_pass_start_time = rospy.Time.now()
# 最後のpass_positionsだったらシュート
is_shoot = False
if len(pass_positions) - 1 == cur_state:
is_shoot = True
# 最後の一つ前のpass_positionsだったらシュートのためのレシーブ
pre_shoot = False
if len(pass_positions) - 2 == cur_state:
pre_shoot = True
# シュートが終了したらリセット
should_reset = False
if len(pass_positions) == cur_state:
cur_state = cur_state - 1
should_reset = True
# print("State:"+str(cur_state))
# InitialStaticStrategyを元に組み立てる
self._dynamic_strategy.clone_from(self._static_strategies['initial'])
not_assigned_robot_ids = self._get_active_robot_ids()
tmp_not_assigned_robot_ids = copy.deepcopy(not_assigned_robot_ids)
# Defence系をアサイン
for robot_id in tmp_not_assigned_robot_ids:
status = Status()
# 2台以下ならとりあえずGKをアサイン
if len(not_assigned_robot_ids) <= 2:
status.status = "keeper"
self._dynamic_strategy.set_robot_status(robot_id, status)
not_assigned_robot_ids.remove(robot_id)
break
role = self._objects.get_robot_by_id(robot_id).get_role()
if role == 'GK':
status.status = "keeper"
self._dynamic_strategy.set_robot_status(robot_id, status)
not_assigned_robot_ids.remove(robot_id)
elif role == 'LDF':
status.status = "defence4"
self._dynamic_strategy.set_robot_status(robot_id, status)
not_assigned_robot_ids.remove(robot_id)
elif role == 'RDF':
status.status = "defence3"
self._dynamic_strategy.set_robot_status(robot_id, status)
not_assigned_robot_ids.remove(robot_id)
# ステートが変わるときに近い順を更新
if self._robots_near_to_ball is None or change_state:
self._robots_near_to_ball = self._objects.get_robot_ids_sorted_by_distance_to_ball(not_assigned_robot_ids)
if len(self._robots_near_to_ball) >= 2: # 2台以上攻撃に割ける場合
# 残りをボールに近い順にアサイン
for idx, robot_id in enumerate(self._robots_near_to_ball):
status = Status()
if idx == 0:
if is_shoot:
status.status = "shoot"
else:
status.status = "pass"
if self._receiver_id is not None:
receiver_pos = self._objects.get_robot_by_id(self._receiver_id).get_current_position()
receiver_area = 1.0
if functions.distance_btw_two_points(pass_positions[cur_state], receiver_pos) > receiver_area:
if not self._prepare_pass:
self._prepare_pass_start_time = rospy.Time.now()
self._prepare_pass = True
if (rospy.Time.now() - self._prepare_pass_start_time).to_sec() > 1.0:
status.status = "pass"
else:
status.status = "prepare_pass"
else:
self._prepare_pass = False
self._kicker_id = robot_id
status.pass_target_pos_x = pass_positions[cur_state][0]
status.pass_target_pos_y = pass_positions[cur_state][1]
not_assigned_robot_ids.remove(robot_id)
elif idx == 1:
if is_shoot:
status.status = "receive"
if self._kicker_id is not None:
# 適当な位置に移動
x, y = self._random_fake_position
status.pid_goal_pos_x = x
status.pid_goal_pos_y = y
self._receiver_id = None
else:
# if pre_shoot:
# status.status = "receive_direct_shoot"
# else:
# status.status = "receive"
status.status = "receive"
self._receiver_id = robot_id
if not should_reset:
status.pass_target_pos_x = pass_positions[cur_state+1][0]
status.pass_target_pos_y = pass_positions[cur_state+1][1]
status.pid_goal_pos_x = pass_positions[cur_state][0]
status.pid_goal_pos_y = pass_positions[cur_state][1]
not_assigned_robot_ids.remove(robot_id)
self._dynamic_strategy.set_robot_status(robot_id, status)
else: # 1台以下の場合あまりがいればとりあえずゴールにシュート
for robot_id in self._robots_near_to_ball:
status = Status()
status.status = "shoot"
status.pass_target_pos_x = pass_positions[-1][0]
status.pass_target_pos_y = pass_positions[-1][1]
self._dynamic_strategy.set_robot_status(robot_id, status)
# self._dynamic_strategy.clone_from(self._static_strategies['initial'])
if should_reset:
# シュートしたらリセット
strategy_context.update("normal_strat_state", 0, namespace="normal_strat")
self._receiver_id = None
self._pass_positions = None
else:
strategy_context.update("normal_strat_state", cur_state, namespace="normal_strat")
result = self._dynamic_strategy
return result
def calcurate(self, strategy_context=None):
pass_pos = [-0.3, -2.0]
succeeded_area = self._objects.robot[0].size_r + 0.3
ball_pos = self._objects.ball.get_current_position()
target_pos = pass_pos
distance = functions.distance_btw_two_points(ball_pos, target_pos)
distance_from_center = functions.distance_btw_two_points(ball_pos, [0.0, 0.0])
if distance < succeeded_area or distance_from_center > 1.0:
strategy_context.update("kickoff_complete", True, namespace="world_model")
strategy_context.update("defence_or_attack", True, namespace="world_model")
self._dynamic_strategy.clone_from(self._static_strategies['initial'])
not_assigned_robot_ids = self._get_active_robot_ids()
tmp_not_assigned_robot_ids = copy.deepcopy(not_assigned_robot_ids)
for robot_id in tmp_not_assigned_robot_ids:
if len(not_assigned_robot_ids) <= 2:
break
status = Status()
role = self._objects.get_robot_by_id(robot_id).get_role()
if role == 'GK':
status.status = "keeper"
self._dynamic_strategy.set_robot_status(robot_id, status)
not_assigned_robot_ids.remove(robot_id)
elif role == 'LDF':
status.status = "defence4"
self._dynamic_strategy.set_robot_status(robot_id, status)
not_assigned_robot_ids.remove(robot_id)
elif role == 'RDF':
status.status = "defence3"
self._dynamic_strategy.set_robot_status(robot_id, status)
not_assigned_robot_ids.remove(robot_id)
for idx, robot_id in enumerate(not_assigned_robot_ids):
status = Status()
if idx == 0:
if len(not_assigned_robot_ids) == 1:
status.status = "shoot"
else:
status.status = "pass"
if self._receiver_id is not None:
receiver_pos = self._objects.get_robot_by_id(self._receiver_id).get_current_position()
receiver_area = 1.0
if functions.distance_btw_two_points(pass_pos, receiver_pos) > receiver_area:
if not self._prepare_pass:
self._prepare_pass_start_time = rospy.Time.now()
self._prepare_pass = True
if (rospy.Time.now() - self._prepare_pass_start_time).to_sec() > 3.0:
status.status = "pass"
else:
status.status = "prepare_pass"
else:
self._prepare_pass = False
self._kicker_id = robot_id
status.pass_target_pos_x = pass_pos[0]
status.pass_target_pos_y = pass_pos[1]
elif idx == 1:
status.status = "receive"
self._receiver_id = robot_id
status.pid_goal_pos_x = pass_pos[0]
status.pid_goal_pos_y = pass_pos[1]
self._dynamic_strategy.set_robot_status(robot_id, status)
result = self._dynamic_strategy
return result
def _clip_penalty_area(self, goal_pos_x, goal_pos_y, offset=0.0):
"""
Parameters
----------
goal_pos_x, goal_pos_y 省略
offset: float offsetメートル分ペナルティエリアが各辺に対して広いと仮定して計算する
Return
----------
goal_pos_x, goal_pos_y: (float, float) 自分-目的地の直線とペナルティエリアの交点の座標を返す
"""
in_penalty_area = functions.in_penalty_area((goal_pos_x, goal_pos_y),
offset=offset)
if not in_penalty_area:
return goal_pos_x, goal_pos_y
penal_area_points = {}
penal_area_points['friend'] = {
'left_bot': [-6.0 - offset, -1.2 - offset],
'right_bot': [-4.8 + offset, -1.2 - offset],
'right_top': [-4.8 + offset, 1.2 + offset],
'left_top': [-6.0 - offset, 1.2 + offset],
}
penal_area_points['enemy'] = {
'right_top': [6.0 + offset, 1.2 + offset],
'left_top': [4.8 - offset, 1.2 + offset],
'left_bot': [4.8 + offset, -1.2 - offset],
'right_bot': [6.0 + offset, -1.2 - offset],
}
robo_x, robo_y = self.ctrld_robot.get_current_position()
line_robot_to_goal = \
functions.line_parameters(robo_x, robo_y, goal_pos_x, goal_pos_y)
line_penal_dict = {}
line_penal_dict['friend'] = {}
line_penal_dict['friend'][
'front'] = functions.line_parameters_vector_args(
penal_area_points['friend']['right_bot'],
penal_area_points['friend']['right_top'])
line_penal_dict['friend'][
'top'] = functions.line_parameters_vector_args(
penal_area_points['friend']['left_top'],
penal_area_points['friend']['right_top'],
)
line_penal_dict['friend'][
'bot'] = functions.line_parameters_vector_args(
penal_area_points['friend']['left_bot'],
penal_area_points['friend']['right_bot'],
)
line_penal_dict['enemy'] = {}
line_penal_dict['enemy'][
'front'] = functions.line_parameters_vector_args(
penal_area_points['enemy']['left_bot'],
penal_area_points['enemy']['left_top'])
line_penal_dict['enemy'][
'top'] = functions.line_parameters_vector_args(
penal_area_points['enemy']['left_top'],
penal_area_points['enemy']['right_top'])
line_penal_dict['enemy'][
'bot'] = functions.line_parameters_vector_args(
penal_area_points['enemy']['left_bot'],
penal_area_points['enemy']['right_bot'])
if in_penalty_area == "friend":
line_penal = line_penal_dict['friend']
else:
line_penal = line_penal_dict['enemy']
clip_pos_xys = []
for key in line_penal.keys():
tmp_cross_point = functions.cross_point(line_penal[key],
line_robot_to_goal)
if functions.in_penalty_area(tmp_cross_point, offset=offset + 0.1):
clip_pos_xys.append(tmp_cross_point)
sorted_clip_pos_xys = sorted(
clip_pos_xys,
key=lambda clip_pos_xy: functions.distance_btw_two_points(
clip_pos_xy, self.ctrld_robot.get_current_position()))
return sorted_clip_pos_xys[0]
