def update(self): Observer.update_ball_is_moving(self._ball_info) Observer.update_role_is_exist(self._roledecision._rolestocker._role_is_exist) self._roledecision.set_disappeared([i.disappeared for i in self._robot_info['our']]) if tool.is_in_defense_area(self._ball_info.pose, 'our') is False \ and Observer.ball_is_moving() is False: # ボールが自チームディフェンスエリア外にあり # ボールが動いていないとき、アタッカーの交代を考える self._roledecision.check_ball_dist([i.pose for i in self._robot_info['our']], self._ball_info) self._roledecision.event_observer() defense_num = self._roledecision._rolestocker._defense_num self._obstacle_avoidance.update_obstacles(self._ball_info, self._robot_info) for our_info in self._robot_info['our']: robot_id = our_info.robot_id target = ControlTarget() # ロールの更新 self._robot_node[robot_id]._my_role = self._roledecision._rolestocker._my_role[robot_id] if our_info.disappeared: # ロボットが消えていたら停止 target = self._robot_node[robot_id].get_sleep() else: # ロボットの状態を更新 self._robot_node[robot_id].set_state( our_info.pose, our_info.velocity) # 目標位置を生成 target = self._robot_node[robot_id].get_action( self._decoded_referee, self._obstacle_avoidance, self._ball_info, self._robot_info, defense_num) self._pubs_control_target[robot_id].publish(target)
def update_receive_ball(ball_info, my_pose, zone_id): # ボール位置 ball_pose = ball_info.pose # ボールスピード ball_vel = ball_info.velocity # 受け取れると判断する距離 _can_receive_dist = 1.0 # ヒステリシス _can_receive_hysteresis = 0.3 result = False target_pose = Pose2D() # ボールが動いている if Observer.ball_is_moving(): # ボール速度ベクトルの角度 angle_velocity = tool.get_angle_from_center(ball_vel) trans = tool.Trans(ball_pose, angle_velocity) tr_pose = trans.transform(my_pose) # ボール速度の線と垂直な距離 fabs_y = math.fabs(tr_pose.y) # 受け取れる判定 if Receiving.receiving(zone_id) == False and \ fabs_y < _can_receive_dist - _can_receive_hysteresis: Receiving.update_receiving(zone_id, True) # 受け取れない判定 elif Receiving.receiving(zone_id) == True and \ fabs_y > _can_receive_dist + _can_receive_hysteresis: Receiving.update_receiving(zone_id, False) # 受け取れるかつボールが向かう方向にいる if Receiving.receiving(zone_id) and tr_pose.x > 0.0: tr_pose.y = 0.0 inv_pose = trans.inverted_transform(tr_pose) angle_to_ball = tool.get_angle(inv_pose, ball_pose) target_pose = Pose2D(inv_pose.x, inv_pose.y, angle_to_ball) result = True return result, target_pose
def receive_ball(ball_info, my_pose): ball_pose = ball_info.pose _can_receive_dist = 1.0 _can_receive_hysteresis = 0.3 target_pose = Pose2D() if Observer.ball_is_moving(): angle_velocity = tool.get_angle_from_center(ball_info.velocity) trans = tool.Trans(ball_pose, angle_velocity) tr_pose = trans.transform(my_pose) fabs_y = math.fabs(tr_pose.y) tr_pose.y = 0.0 inv_pose = trans.inverted_transform(tr_pose) angle_to_ball = tool.get_angle(inv_pose, ball_pose) target_pose = Pose2D(inv_pose.x, inv_pose.y, angle_to_ball) return target_pose