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 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 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 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 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 __init__(self):
rospy.init_node("world_model")
self.team_color = str(rospy.get_param("~team_color"))
self.robot_total = 8
self.enemy_total = 8
"""---robot、ballオブジェクトのインスタンス---"""
self.robot = [entity.Robot() for i in range(self.robot_total)]
self.enemy = [entity.Robot() for i in range(self.enemy_total)]
self.ball = entity.Ball()
"""statusの立ち上げ msg"""
self.status = [Status() for i in range(self.robot_total)]
"""statusのパラメータ初期化"""
for id in range(self.robot_total):
self.status[id].status = "None"
self.status[id].pid_goal_pos_x = 0.
self.status[id].pid_goal_pos_y = 0.
self.status[id].pid_goal_theta = 0.
self.status[id].pid_circle_center_x = 0.
self.status[id].pid_circle_center_y = 0.
self.status[id].pass_target_pos_x = 0.
self.status[id].pass_target_pos_y = 0.
#print(self.status)
"""----上の5つの変数、インスタンスをまとめたもの、callbackをもつ---"""
self.objects = Objects(self.robot_total, self.enemy_total, self.robot,
self.enemy, self.ball)
"""---World State(固定パラが多い)---"""
self.world_state = WorldState(self.objects)
"""---Referee---"""
self.referee = Referee(self.world_state)
"""---DecisionMaker---"""
self.decision_maker = DecisionMaker(self.world_state, self.objects,
self.referee, self.status)
"""---status Publisherオブジェクトのインスタンス---"""
self.robot_0_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_0/status",
Status,
queue_size=10)
self.robot_1_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_1/status",
Status,
queue_size=10)
self.robot_2_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_2/status",
Status,
queue_size=10)
self.robot_3_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_3/status",
Status,
queue_size=10)
self.robot_4_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_4/status",
Status,
queue_size=10)
self.robot_5_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_5/status",
Status,
queue_size=10)
self.robot_6_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_6/status",
Status,
queue_size=10)
self.robot_7_status_pub = rospy.Publisher("/" + self.team_color +
"/robot_7/status",
Status,
queue_size=10)
def __init__(self):
rospy.init_node("world_model")
self.team_color = str(rospy.get_param("team_color"))
"""---devisionの選択---"""
self.devision = "A"
self.devision = "B"
"""---味方ロボット台数と敵ロボット台数---"""
self.robot_num = 4
self.enemy_num = 8
"""---フィールドとロボットのパラメータ---"""
self.robot_r = 0.09
if self.devision == "A":
self.field_x_size = 12.
self.field_y_size = 9.
elif self.devision == "B":
self.field_x_size = 9.
self.field_y_size = 6.
self.field_x_min = - self.field_x_size / 2.
self.field_x_max = self.field_x_size / 2.
self.field_y_min = - self.field_y_size / 2.
self.field_y_max = self.field_y_size / 2.
self.goal_y_size = 1.2
self.goal_y_min = -self.goal_y_size / 2.
self.goal_y_max = self.goal_y_size / 2.
"""---team cloorと攻撃方向---"""
#self.color = "Yellow"
self.color = "Blue"
self.goal_direction = "Right"
#self.goal_direction = "Left"
"""---PID制御用パラメータ---"""
self.Kpv = 0.3
self.Kpr = 0.1
"""---Potential法用パラメータ---"""
self.Kpv_2 = 0.5
self.Kpr_2 = 0.1
"""---refereeからの指示をもとにどのループを回すかの指標---"""
self.referee_branch = None
"""---ボール軌道の考慮時間幅(linear Regressionで軌道予測するため)---"""
self.ball_dynamics_window = 5
self.ball_dynamics_x = [0. for i in range(self.ball_dynamics_window)]
self.ball_dynamics_y = [0. for i in range(self.ball_dynamics_window)]
self.goal_keeper_x = self.field_x_min + 0.05
"""---robot、ballオブジェクトのインスタンス---"""
self.robot = [entity.Robot() for i in range(self.robot_num)]
self.enemy = [entity.Robot() for i in range(self.enemy_num)]
self.ball = entity.Ball()
"""---Publisherオブジェクトのインスタンス---"""
self.robot_0_pub = rospy.Publisher("/" + self.team_color + "/robot_0/robot_commands", robot_commands, queue_size=10)
self.robot_1_pub = rospy.Publisher("/" + self.team_color + "/robot_1/robot_commands", robot_commands, queue_size=10)
self.robot_2_pub = rospy.Publisher("/" + self.team_color + "/robot_2/robot_commands", robot_commands, queue_size=10)
self.robot_3_pub = rospy.Publisher("/" + self.team_color + "/robot_3/robot_commands", robot_commands, queue_size=10)
"""
self.robot_4_pub = rospy.Publisher("/robot_4/robot_commands", robot_commands, queue_size=10)
self.robot_5_pub = rospy.Publisher("/robot_5/robot_commands", robot_commands, queue_size=10)
self.robot_6_pub = rospy.Publisher("/robot_6/robot_commands", robot_commands, queue_size=10)
self.robot_7_pub = rospy.Publisher("/robot_7/robot_commands", robot_commands, queue_size=10)
"""
"""---status Publisherオブジェクトのインスタンス---"""
self.robot_0_status_pub = rospy.Publisher("/" + self.team_color + "/robot_0/status", Status, queue_size=10)
self.robot_1_status_pub = rospy.Publisher("/" + self.team_color + "/robot_1/status", Status, queue_size=10)
self.robot_2_status_pub = rospy.Publisher("/" + self.team_color + "/robot_2/status", Status, queue_size=10)
self.robot_3_status_pub = rospy.Publisher("/" + self.team_color + "/robot_3/status", Status, queue_size=10)
self.robot_4_status_pub = rospy.Publisher("/" + self.team_color + "/robot_4/status", Status, queue_size=10)
self.robot_5_status_pub = rospy.Publisher("/" + self.team_color + "/robot_5/status", Status, queue_size=10)
self.robot_6_status_pub = rospy.Publisher("/" + self.team_color + "/robot_6/status", Status, queue_size=10)
self.robot_7_status_pub = rospy.Publisher("/" + self.team_color + "/robot_7/status", Status, queue_size=10)
"""pid用のflag初期化"""
for id in range(self.robot_num):
self.robot[id].pid_init_flag = True
"""cmdの立ち上げ"""
self.cmd = [robot_commands() for i in range(self.robot_num)]
"""statusの立ち上げ"""
self.status = [Status() for i in range(self.robot_num)]
"""cmdのパラメータ初期化"""
for id in range(self.robot_num):
self.cmd[id].vel_surge = 0. #vel_x
self.cmd[id].vel_sway = 0. #vel_y
self.cmd[id].omega = 0. #vel_theta
self.cmd[id].kick_speed_x = 0
self.cmd[id].kick_speed_z = 0
self.cmd[id].dribble_power = 0
"""statusのパラメータ初期化"""
for id in range(self.robot_num):
self.status[id].status = "None"
"""positionの割り振り(動的にPositionが切り替わるのは多分大事、現状使っていない)"""
"""
if self.robot_num == 8:
self.robot[0].position = "GK"
self.robot[1].position = "LCB"
self.robot[2].position = "RCB"
self.robot[3].position = "LSB"
self.robot[4].position = "RSB"
self.robot[5].position = "LMF"
self.robot[6].position = "RMF"
elif self.robot_num == 4:
self.robot[0].position = "GK"
self.robot[1].position = "LCB"
self.robot[2].position = "CCB"
self.robot[3].position = "RCB"
"""
"""publishのrate(多分rospyのを使うからいらない)"""
self.publish_rate = 0.05 #s #
"""strategyの選択(いまはつかっていない)"""
self.strategy = None
"""refereeから受信する情報(現状cmdしか使っていない)"""
self.referee = None
"""---敵と味方どっちがボールをもっているか(現状使ってない)---"""
self.which_has_a_ball = None
"""---攻撃かdefenceか(これをうまく使っていきたい)---"""
self.attack_or_defence = None
"""---ポテンシャル法用のパラメータ（1/x）---"""
self.weight_enemy = 1.0
self.weight_goal = 100.0
self.delta = 0.0000001
"""---ポテンシャル法用のパラメータ（ガウス関数)---"""
self.weight_enemy_2 = 7.0
self.weight_goal_2 = 20000000.0
self.width_enemy = 0.1
self.width_goal = 0.1
self.delta_2 = 0.00001
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 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 _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 _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 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, 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, 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 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 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 _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):
# 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]
third_nearest_enemy_id = self._objects.get_enemy_ids_sorted_by_distance_to_ball(
active_enemy_ids)[2]
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 third_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)
target_pos_xy = functions.calculate_internal_dividing_point_vector_args(
self._enemy[third_nearest_enemy_id].
get_current_position(),
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 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
