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