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