def calculate_goal_kick(self):
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_pass_chip"
status.pass_target_pos_x = self._objects.get_robot_by_role("LFW").get_current_position()[0]
status.pass_target_pos_y = self._objects.get_robot_by_role("LFW").get_current_position()[1]
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 = 1.0
status.pid_goal_pos_y = 1.5
elif robot.get_role() == "RFW":
status.status = "move_linear"
status.pid_goal_pos_x = 1.0
status.pid_goal_pos_y = -1.5
else:
status.status = "none"
self._dynamic_strategy.set_robot_status(robot_id, status)
result = self._dynamic_strategy
return result
def ball_placement_kick(self, kick_robot_id, receive_robot_id):
#placementに関係ない機体はstop
#今後変更予定
status = Status()
for robot_id in self.active_robot_ids:
status.status = "stop"
self._dynamic_strategy.set_robot_status(robot_id, status)
kick_robot_status = Status()
kick_robot_status.status = "ball_place_kick"
kick_robot_status.pass_target_pos_x = self.place_point[0]
kick_robot_status.pass_target_pos_y = self.place_point[1]
self._dynamic_strategy.set_robot_status(kick_robot_id,
kick_robot_status)
receive_robot_status = Status()
receive_robot_status.status = "receive"
receive_robot = self._objects.get_robot_by_id(receive_robot_id)
#機体半径分レシーブ位置を下げる
vector = np.array(self._ball_params.get_current_position()) - np.array(
self.place_point)
vector = (receive_robot.size_r / np.linalg.norm(vector)) * vector
vector = np.array(self.place_point) - vector
receive_robot_status.pid_goal_pos_x = vector[0]
receive_robot_status.pid_goal_pos_y = vector[1]
self._dynamic_strategy.set_robot_status(receive_robot_id,
receive_robot_status)
result = self._dynamic_strategy
return result
def _get_default_status():
# type: () -> Status
status = Status()
status.status = "None"
status.pid_goal_pos_x = 0.
status.pid_goal_pos_y = 0.
status.pid_goal_theta = 0.
status.pid_circle_center_x = 0.
status.pid_circle_center_y = 0.
status.pass_target_pos_x = 0.
status.pass_target_pos_y = 0.
return status
def calculate_corner_kick_left(self):
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":
if self.passed_1_flg:
# パス完了した場合
status.status = "defence4"
else:
# パス完了する前 パス先をposition1にする、パス完了したら完了flgを立てる
status.status = "pass"
status.pass_target_pos_x = self._objects.get_robot_by_role(self.lfw_or_rfw).get_current_position()[0]
status.pass_target_pos_y = self._objects.get_robot_by_role(self.lfw_or_rfw).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.get_role() == "RDF":
status.status = "defence3"
elif robot.get_role() == "LFW":
position = [self.position_2[0] + 0.4, self.position_2[1]]
if self.passed_2_flg:
status.status = "move_linear"
# テキトーに移動
status.pid_goal_pos_x = 2.0
status.pid_goal_pos_y = 1.5
if self.received_2_flg:
status.status = "shoot_left"
if self._objects.get_has_a_ball(robot_id) == False:
self.passed_2_flg = True
else:
status.status = "receive"
status.pid_goal_pos_x = position[0]
status.pid_goal_pos_y = position[1]
if self._objects.get_has_a_ball(robot_id):
self.received_2_flg = True
# else:
# status.status = "receive_direct_shoot_left"
# status.pid_goal_pos_x = position[0]
# status.pid_goal_pos_y = position[1]
# if self._objects.get_has_a_ball(robot_id) == False and self.received_2_flg:
# self.passed_2_flg = True
# if self._objects.get_has_a_ball(robot_id):
# self.received_2_flg = True
elif robot.get_role() == "RFW":
position = [self.position_3[0] + 0.2, self.position_3[1]]
if self.passed_3_flg:
status.status = "move_linear"
# テキトーに移動
status.pid_goal_pos_x = 2.0
status.pid_goal_pos_y = -1.5
# if self.received_3_flg:
# status.status = "shoot_right"
# if self._objects.get_has_a_ball(robot_id) == False:
# self.passed_3_flg = True
# else:
# status.status = "receive"
# status.pid_goal_pos_x = position[0]
# status.pid_goal_pos_y = position[1]
# if self._objects.get_has_a_ball(robot_id):
# self.received_3_flg = True
else:
status.status = "receive_direct_shoot_left"
status.pid_goal_pos_x = position[0]
status.pid_goal_pos_y = position[1]
if self._objects.get_has_a_ball(robot_id) == False and self.received_3_flg:
self.passed_3_flg = True
if self._objects.get_has_a_ball(robot_id):
self.received_3_flg = True
else:
status.status = "none"
self._dynamic_strategy.set_robot_status(robot_id, status)
result = self._dynamic_strategy
return result
def calculate_indirect_right(self, strategy_context=None):
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 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_role(
self.lfw_or_rfw).get_current_position()[0]
status.pass_target_pos_y = self._objects.get_robot_by_role(
self.lfw_or_rfw).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:
position = self.position_3
if self.received_2_flg:
status.status = "shoot_left"
if self._objects.get_has_a_ball(robot_id) == False:
self.passed_2_flg = True
else:
status.status = "receive"
status.pid_goal_pos_x = position[0]
status.pid_goal_pos_y = position[1]
if self._objects.get_has_a_ball(robot_id):
self.received_2_flg = True
elif robot_id == self.position_2_nearest_id:
position = self.position_2
if self.received_3_flg:
status.status = "shoot_right"
if self._objects.get_has_a_ball(robot_id) == False:
self.passed_3_flg = True
else:
status.status = "receive"
status.pid_goal_pos_x = position[0]
status.pid_goal_pos_y = position[1]
if self._objects.get_has_a_ball(robot_id):
self.received_3_flg = True
elif robot.get_role() == "GK":
status.status = "keeper"
elif robot.get_role() == "LDF":
status.status = "defence4"
elif robot.get_role() == "RDF":
status.status = "defence3"
else:
status.status = "none"
self._dynamic_strategy.set_robot_status(robot_id, status)
result = 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 _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
