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
