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 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, referee_branch="PENALTY_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()
nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(active_enemy_ids)[0]
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 = "move_linear"
status.pid_goal_pos_x = -4.0
status.pid_goal_pos_y = 4.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]))
elif robot.get_role() == "RDF":
#固定値へ移動
status.status = "move_linear"
status.pid_goal_pos_x = -3.5
status.pid_goal_pos_y = 4.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]))
elif robot.get_role() == "LFW":
#固定値へ移動
status.status = "move_linear"
status.pid_goal_pos_x = -3.0
status.pid_goal_pos_y = 4.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]))
elif robot.get_role() == "RFW":
#固定値へ移動
status.status = "move_linear"
status.pid_goal_pos_x = -2.5
status.pid_goal_pos_y = 4.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 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 calcurate(self, strategy_context=None):
# type: (StrategyContext) -> StrategyBase
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")
active_robot_ids = self._get_active_robot_ids()
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]
second_nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(active_enemy_ids)[1]
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"
# if second_nearest_enemy_id != None:
if True:
# 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)
target_pos_xy = functions.calculate_internal_dividing_point_vector_args(config.GOAL_CENTER, self._ball_params.get_current_position(), 1, 1)
status.pid_goal_pos_x = target_pos_xy[0]
status.pid_goal_pos_y = target_pos_xy[1]
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 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] - 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 _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 _defence_strategy(self, strategy_context=None):
#ball取得時、攻撃へ移行
if self._get_who_has_a_ball() == "robots":
strategy_context.update("defence_or_attack", True, 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":
#ballの位置に移動
status.status = "move_linear"
status.pid_goal_pos_x = self._ball_params.get_current_position()[0]
status.pid_goal_pos_y = self._ball_params.get_current_position()[1]
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 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] - 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 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 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 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 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 _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 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_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 calcurate(self, strategy_context=None, place_ball_position=[0, 0]):
# type: (StrategyContext) -> StrategyBase
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")
active_robot_ids = self._get_active_robot_ids()
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]
second_nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(
active_enemy_ids)[1]
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"
# if second_nearest_enemy_id != None:
if True:
# 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)
target_pos_xy = functions.calculate_internal_dividing_point_vector_args(
config.GOAL_CENTER,
self._ball_params.get_current_position(), 1, 1)
status.pid_goal_pos_x = target_pos_xy[0]
status.pid_goal_pos_y = target_pos_xy[1]
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 robot.get_role() == "RFW":
status.status = "move_linear"
# ボールプレースメントの場所とボールの場所でゴールに近い方のゴール側に陣取る
goal_pos = np.array(config.GOAL_CENTER)
place_pos = np.array(place_ball_position)
ball_pos = np.array(self._ball_params.get_current_position())
if np.linalg.norm(goal_pos -
place_pos) < np.linalg.norm(goal_pos -
ball_pos):
target_pos = place_pos + (
goal_pos - place_pos) * 1 / np.linalg.norm(goal_pos -
place_pos)
else:
target_pos = ball_pos + (
goal_pos - ball_pos) * 1 / np.linalg.norm(goal_pos -
ball_pos)
status.pid_goal_pos_x = target_pos[0]
status.pid_goal_pos_y = target_pos[1]
status.pid_goal_theta = np.arctan2(
(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