def update(self, data):
"""
Update method, starts dbscan filtering
:param data: BitBots data object
"""
# If no new frame is recored, than cancel exection because new information is needed
if not data[DATA_KEY_IS_NEW_FRAME]:
return 0
# Cancel execution if no goal is found, because goal information is needed
if not data[DATA_KEY_GOAL_FOUND]:
return 1
# store goal information in local variable
goal_info = data[DATA_KEY_GOAL_INFO]
# only start calculation if exactly two goal posts are found
if len(goal_info) > 0:
self.__dbscanStart(data)
# debug stuff
debug_m(2, "GOAL_INFO_FILTERED", DATA_KEY_GOAL_INFO_FILTERED)
# Print debug infos to terminal
if self.printDebugToConsole and self.lastPrintDebugToConsole < time.time() - 1:
DebugString = "[GoalFilter] => "
DebugString += "| u: " + str(round(data[DATA_KEY_GOAL_INFO_FILTERED].u_center, 2)).rjust(8) + " | "
DebugString += "| v: " + str(round(data[DATA_KEY_GOAL_INFO_FILTERED].v_center, 2)).rjust(8) + " |"
print(DebugString)
self.lastPrintDebugToConsole = time.time()
def post(self, data):
if self.stack != self.old_representation:
self.old_representation = self.stack
a = [str(e).split(" ")[1][0:-1] for e in self.stack]
b = [str(c).split(".")[-1] for c in a]
debug_m(3, "Stack1", "\n".join(b[:4]))
debug_m(3, "Stack2", "\n".join(b[4:]))
def update(self, data):
#print data[DATA_KEY_MINIMAL_BALL_TIME] , data[DATA_KEY_BALL_TIME]
if data[DATA_KEY_MINIMAL_BALL_TIME] < data[DATA_KEY_BALL_TIME]:
debug_m(2, "Other is nearer")
data['Animation'] = "goalie_mitte2"
data[DATA_KEY_ROLE] = ROLE_SUPPORTER
else:
debug_m(2, "Striker")
data['Animation'] = "goalie_left_shoulder"
data[DATA_KEY_ROLE] = ROLE_STRIKER
def update(self, data):
ipc = data[DATA_KEY_IPC]
accel = ipc.get_accel()
self.accel_y = self.accel_y * 0.98 + accel.y * 0.02
debug_m(4, "MitAccelY", self.accel_y)
#print self.accel_y
hip_pitch_change = (self.accel_y + 45) * 0.1
if data[DATA_KEY_WALKING_ACTIVE]:
#data["Walking.HipPitch.In"] = hip_pitch_change + data["Walking.HipPitch.Out"]
debug_m(4, "HipPitch", data.get("Walking.HipPitch.In", -9999))
def vote_for_switch_to_striker(self, connector):
if (connector.raw_vision_capsule().ball_seen() and
connector.raw_vision_capsule().get_ball_info("u") < self.go_striker_range):
self.ball_distance_history.append(True)
else:
self.ball_distance_history.append(False)
if len(self.ball_distance_history) > self.ball_history_length:
self.ball_distance_history = self.ball_distance_history[1:]
number_trues = len([e for e in self.ball_distance_history if e])
debug_m(3, "DefenderDecision.number_trues", number_trues)
return number_trues
def vote_accel_z_method(self, accel):
if len(self.accel_lst) > 30:
self.accel_lst = self.accel_lst[1:]
self.accel_lst.append(accel.z)
number_valid = sum(100 < v < 170 for v in self.accel_lst)
debug_m(3, "Number Valid Accel", number_valid)
return number_valid > 28
def update(self, data):
if not data[DATA_KEY_IS_NEW_FRAME]:
return -1
if self.i < 10:
self.i += 1
return
self.i = 11
#self.publish_ball_info(data)
#self.publish_goal_info(data)
self.publish_ball_info_prediction(data)
#self.publish_obstacle_info(data)
debug_m(3, "draw", 1)
def update(self, data):
"""
Read status and update informations
"""
ipc = data[DATA_KEY_IPC]
if ipc.get_button1() and not self.button1:
debug_m(2, "Button 1 pressed")
data[DATA_KEY_BUTTON_1] = True
self.button1 = True
self.send_event(EVENT_BUTTON1_DOWN)
else:
data[DATA_KEY_BUTTON_1] = False
if self.button1 and not ipc.get_button1():
self.button1 = False
debug_m(2, "Button 1 released")
self.send_event(EVENT_BUTTON1_UP)
if ipc.get_button2() and not self.button2:
debug_m(2, "Button 2 pressed")
data[DATA_KEY_BUTTON_2] = True
self.button2 = True
self.send_event(EVENT_BUTTON2_DOWN)
else:
data[DATA_KEY_BUTTON_2] = False
if self.button2 and not ipc.get_button2():
self.button2 = False
debug_m(2, "Button 2 released")
self.send_event(EVENT_BUTTON2_UP)
def update(self, data):
# minimal distance to fieldie
minimal_ball_walk_time = 99999999999999
fieldie_ball_time_list = []
for robot in data[DATA_KEY_TEAM_MATES].itervalues():
if robot.get_state() == STATE_ACTIVE:
robot_ball_time = robot.get_ball_time()
if robot_ball_time:
if robot_ball_time < minimal_ball_walk_time:
minimal_ball_walk_time = robot_ball_time
if robot.get_role() == ROLE_GOALIE:
# provide distance from goalie to ball
data[DATA_KEY_GOALIE_BALL_RELATIVE_POSITION] = (
robot.get_relative_ball_x(),
robot.get_relative_ball_y(),
)
# fieldie_ball_time_list.append((robot.get_id(), robot.get_ball_time())) #%TODO FIELDIE<- bennenung
else:
fieldie_ball_time_list.append((robot.get_id(), robot.get_ball_time()))
if not robot.get_id() == int(config["PLAYER"]):
data[DATA_KEY_OPPONENT_ROBOT] = (
robot.get_opponent_robot_x(),
robot.get_opponent_robot_y(),
robot.get_opponent_robot_3(),
robot.get_opponent_robot_4(),
robot.get_opponent_robot_5(),
robot.get_opponent_robot_6(),
robot.get_opponent_robot_7(),
robot.get_opponent_robot_8(),
)
data[DATA_KEY_TEAM_MATE] = (
robot.get_team_mate_x(),
robot.get_team_mate_y(),
robot.get_team_mate_3(),
robot.get_team_mate_4(),
robot.get_team_mate_5(),
robot.get_team_mate_6(),
robot.get_team_mate_7(),
robot.get_team_mate_8(),
)
# data[DATA_KEY_OWN_POSITION_GRID] = (
# robot.get_own_position_grid_x(),
# robot.get_own_position_grid_y())
if robot.get_kickoff_offence_side() != 0:
debug_m(3, str(robot), robot.get_kickoff_offence_side())
data[DATA_KEY_KICKOFF_OFFENSE_SIDE_RECEIVED] = (robot.get_kickoff_offence_side(), time.time())
data[DATA_KEY_MINIMAL_BALL_TIME] = minimal_ball_walk_time
data[DATA_KEY_FIELDIE_BALL_TIME_LIST] = fieldie_ball_time_list
def perform(self, connector, reevaluate=False):
# get the u and v values to position
u, v = connector.world_model_capsule().get_uv_from_xy(self.position[0], self.position[1])
#print "u:", u, " v: ", v
if u is None:
debug_m(3, "No position information")
return
if self.track_goal:
connector.blackboard_capsule().schedule_enemy_goal_tracking()
else:
connector.blackboard_capsule().schedule_both_tracking()
#there are no addional obstacles
obstacles = []
self.go_to_pos(connector, u, v, obstacles, self.align)
def publish_obstacle_info(self, data):
debug_m(3, "ObstacleFound", data[DATA_KEY_OBSTACLE_FOUND])
# Send the first 5 obstacles
if data[DATA_KEY_OBSTACLE_FOUND]:
obstacles = data[DATA_KEY_OBSTACLE_INFO]
for i in range(5):
if i < len(obstacles):
u, v, x, y, h, w, colour = obstacles[i]
debug_m(3, "ObstacleInfo.%i.u" % i, u)
debug_m(3, "ObstacleInfo.%i.v" % i, v)
debug_m(3, "ObstacleInfo.%i.colour" % i, colour)
def post(self, data):
"""
This module does its work in :func:`post` to directly play anitmaitons which were provided in this cycle.
"""
ipc = data[DATA_KEY_IPC]
if self.next is None and data.get(DATA_KEY_ANIMATION, None) is not None:
# Animation merken
self.next = data[DATA_KEY_ANIMATION]
debug_m(2, "Next", str(self.next))
data[DATA_KEY_ANIMATION] = None
if self.next is None:
# Wir haben bisher keine Animation
return
if (not ipc.controlable) and ipc.get_motion_state() != STATE_WALKING:
# Wenn wir im state Walking sind müssen wir einfach noch etwas
# Warten, das stoppen wir dann gleich.
debug_m(1, "Could not start Annimation %s." %
str(self.next) + "Ipc is in state %s" % ipc.get_motion_state())
# Wir haben keine Kontrolle oder spielen schon was,
# ignoriere die aktuelle Animation
if isinstance(self.next, (tuple, list)):
# callback mit fail aufruffen
self.next[1](False)
self.next = None
return
# Walking jetzt stoppen
data[DATA_KEY_WALKING_ACTIVE] = False
# Wenn Walking nicht mehr läuft, Animation starten
if self.next is not None and \
not data.get(DATA_KEY_WALKING_RUNNING, False):
# debug.log("Animation abspielen")
if isinstance(self.next, (tuple, list)):
# da ist ein callback dabei
if not play_animation(self.next[0], ipc, self.next[1]):
# schon beim starten gescheitert, wir ruffen das callback
# mit False auf
self.next[1](False)
else:
#ohne callback
play_animation(self.next, ipc)
self.next = None
debug_m(2, "Next", str(self.next))
elif data.get(DATA_KEY_WALKING_RUNNING, False):
debug_m(2, "Wait for stopping of the walking")
def update(self, data):
if DATA_KEY_CAMERA_CAPTURE_TIMESTAMP not in data:
return
# If no Ball is found there can't be valid data
if not data[DATA_KEY_BALL_FOUND]:
data[DATA_KEY_BALL_SPEED] = -1
data[DATA_KEY_BALL_VECTOR] = (0, 0)
self.last_frame_time = -1
return
if self.last_frame_time == -1:
# wir hatten im latzen frme keinen ball
# da wir jetzt einen haben können wir es
# jetzt messen
self.last_frame_time = data[DATA_KEY_CAMERA_CAPTURE_TIMESTAMP]
self.last_u = data[DATA_KEY_BALL_INFO].u
self.last_v = data[DATA_KEY_BALL_INFO].v
return
# time since last software-cycle is added to the time spent waiting for
# a new frame as long as we do not receive a new image.
if not data[DATA_KEY_IS_NEW_FRAME]:
return
# Actually it is just working while the robot is not moving
# TODO testen obs vielleicht trotzdem geht
if data["Walking.Active"]:
data[DATA_KEY_BALL_SPEED] = -1
data[DATA_KEY_BALL_VECTOR] = (0, 0)
self.last_frame_time = -1
return
latest_time_frame = data[DATA_KEY_CAMERA_CAPTURE_TIMESTAMP]
try:
# Umwandlung von m der Distance in m/s des Ballspeeds
delta_u = data[DATA_KEY_BALL_INFO].u - self.last_u
delta_v = data[DATA_KEY_BALL_INFO].v - self.last_v
# Differenzberechnung wie viel Zeit zwischen dem aktuellen und dem letzten Bild vergangen ist
delta_timestamp = latest_time_frame - self.last_frame_time
rolled_distance = math.sqrt(math.pow(delta_u, 2) + math.pow(delta_v, 2))
data[DATA_KEY_BALL_SPEED] = round(rolled_distance / delta_timestamp, 5)
data[DATA_KEY_BALL_VECTOR] = (
delta_u / delta_timestamp,
delta_v / delta_timestamp)
except ZeroDivisionError:
data[DATA_KEY_BALL_SPEED] = -1
# Reset time after a new frame update
self.last_frame_time = latest_time_frame
self.last_u = data[DATA_KEY_BALL_INFO].u
self.last_v = data[DATA_KEY_BALL_INFO].v
debug_m(3, "Speed.delta_u", data[DATA_KEY_BALL_VECTOR][0])
debug_m(3, "Speed.delta_v", data[DATA_KEY_BALL_VECTOR][1])
debug_m(3, "Speed ", data[DATA_KEY_BALL_SPEED])
def ball_moving(self, ball_info, data):
# Add it
self.ball_info_speed_buffer_list.append([ball_info.u, ball_info.v])
# If we have to much elements we slice the first one off
if len(self.ball_info_speed_buffer_list) > self.speed_buffer_list_counter:
self.ball_info_speed_buffer_list = self.ball_info_speed_buffer_list[1:]
us = [e[0] for e in self.ball_info_speed_buffer_list]
vs = [e[1] for e in self.ball_info_speed_buffer_list]
np_array_us = np.array(us)
np_array_vs = np.array(vs)
#umean = np_array_us.mean()
uvar = np_array_us.var()
#vmean = np_array_vs.mean()
vvar = np_array_vs.var()
debug_m(3, DATA_KEY_BALL_INFO_FILTERED + ".BallUVListVariance.u", uvar)
debug_m(3, DATA_KEY_BALL_INFO_FILTERED + ".BallUVListVariance.v", vvar)
data[DATA_KEY_BALL_INFO_FILTERED]["uvvariance"] = [uvar, vvar]
def extract_horizon_obstacles(self, data, obstacles):
obstacles = [self.transformer.transform_point_to_location(obs[0], obs[1], 0.0) for obs in obstacles]
data[DATA_KEY_HORIZON_OBSTACLES] = obstacles
i = 0
for obstacle in obstacles:
debug_m(4, "Obstacle.%d.u" % i, obstacle[0])
debug_m(4, "Obstacle.%d.v" % i, obstacle[1])
i += 1
debug_m(4, "AnzObstacles", len(obstacles))
data[DATA_KEY_OBSTACLE_FOUND] = len(obstacles) > 0
def load_color_config(self, which):
# Farbraum-Konfiguration laden
conf = numpy.zeros((256, 768), dtype=numpy.uint8)
for idx, name in enumerate(("ball", "cyan", "yellow", "carpet", "white", "magenta")):
if name != "carpet": # ignoring carpet, because of autoconfig
debug_m(DebugLevel.LEVEL_4, "searching vision-color-config/%s/%s.png" % (which, name))
name = find_resource(
"vision-color-config/%s/%s.png" % (which, name))
debug_m(DebugLevel.LEVEL_4, "Loading %s" % name)
im = PngImage(name)
mask = im.get_png_as_numpy_array()
mask[mask != 0] = (1 << idx)
conf += mask
debug_m(DebugLevel.LEVEL_4, "vision-color-config/%s/%s.png geladen" % (which, name))
else:
debug_m(DebugLevel.LEVEL_4, "vision-color-config/%s/%s.png ignoriert (autocolorconfig)" % (which, name))
return conf
def man_unpenelize(self, event_data):
debug_m(2, "Man_Unpenalize")
self.ipc.set_state(STATE_CONTROLABLE)
self.send_event(EVENT_GLOBAL_INTERRUPT)
self.man_pen = False
def run(self):
#Später import, da das ZMPWalking nich mehr gebaut wird, sondern ein Teil der Motion ist @Robert 04.08.2014
from bitbots.motion.zmpwalking import ZMPWalkingEngine
ipc = self.get(DATA_KEY_IPC)
config = self.get(DATA_KEY_CONFIG)
zmp_config = config["ZMPConfig"]
ipc_was_controllable = False
self.walking = ZMPWalkingEngine()
self.walking.r_shoulder_roll_offset = zmp_config["r_shoulder_roll"]
self.walking.l_shoulder_roll_offset = zmp_config["l_shoulder_roll"]
self.walking.hip_pitch_offset = zmp_config["hip_pitch"]
self.walking.hip_pitch = zmp_config["HipPitch"]
max_speed = zmp_config["MAX_SPEED"]
update_intervall = zmp_config["UPDATE_INTERVALL"]
speed_delta = zmp_config["SPEED_DELTA"]
debug_m(
2,
"Hip Pitch auf %d gesetzt" % config["ZMPConfig"]["HipPitch"])
# wir wollen maximal mögliche aufmerksamkeit von scheduler
self.nice.set_realtime()
log_counter = -1
last_time = time.time()
velocity = (0, 0, 0)
lastforward = 0
lastsideward = 0
lastangular = 0
forward = 0
sideward = 0
angular = 0
updateslower = time.time()
stop_request = 0
while True:
log_counter += 1
if log_counter > 60:
log_counter = 0
if not ipc.controlable or \
self.get(DATA_KEY_GAME_STATUS) != DATA_VALUE_STATE_PLAYING:
time.sleep(0.5)
if not ipc.controlable:
debug_m(4, "IPC not Controllable -> continue")
ipc_was_controllable = False
continue
debug_m(4, "Game State not STATE_PLAYING -> continue")
self.stop_walking()
continue
if self.get("Walking.Active") is False:
self.stop_walking()
if self.walking.running is False:
if log_counter is 0:
debug_m(4, "Walking not active -> Continue")
time.sleep(self.sleeptime)
# Damit bei Walking.active=false nicht getrippelt wird
continue
#this code prevents the robot to run too fast
#and prevents too abrupt canges of speeds
if time.time() - updateslower > update_intervall:
newforward = int(self.get("Walking.Forward"))
if newforward == lastforward:
forward = newforward
elif lastforward < newforward:
if lastforward < 0:
forward = min(
lastforward + speed_delta,
newforward)
else: # newforward >= 0
forward = min(
lastforward + speed_delta,
newforward,
max_speed)
else: # lastforward >= newforward:
if lastforward < 0:
forward = max(
lastforward - speed_delta,
newforward,
-max_speed)
else: # lastforward >= 0
forward = max(
lastforward - speed_delta,
newforward)
newsideward = int(self.get("Walking.Sideward"))
if newsideward == lastsideward:
sideward = newsideward
elif lastsideward < newsideward:
if lastsideward < 0:
sideward = min(
lastsideward + speed_delta,
newsideward)
else: # newforward >= 0
sideward = min(
lastsideward + speed_delta,
newsideward,
max_speed)
else: # lastforward >= newforward:
if lastsideward < 0:
sideward = max(
lastsideward - speed_delta,
newsideward,
-max_speed)
else: # lastforward >= 0
sideward = max(
lastsideward - speed_delta,
newsideward)
newangular = int(self.get("Walking.Angular"))
if newangular == lastangular:
angular = newangular
elif lastangular < newangular:
if lastangular < 0:
angular = min(lastangular + speed_delta, newangular)
else: # newforward >= 0
angular = min(
lastangular + speed_delta,
newangular,
max_speed)
else: # lastforward >= newforward:
if lastangular < 0:
angular = max(
lastangular - speed_delta,
newangular,
-max_speed)
else: # lastforward >= 0
angular = max(
lastangular - speed_delta,
newangular)
updateslower = time.time()
lastforward = forward
lastsideward = sideward
lastangular = angular
if not (velocity == (forward, sideward, angular)):
debug_m(2, "Set new velocity (%.2d %.2d %.2d)" % (forward,
sideward,
angular))
velocity = (forward, sideward, angular)
if self.walking.running is False:
self.reset_walking()
self.walking.velocity = (forward / self.forward_factor,
sideward / self.sideward_factor,
angular / self.angle_factor)
x_move_amplitude, y_move_amplitude, a_move_amplitude = self.walking.velocity
self.set(
"Walking.Forward.Real",
x_move_amplitude * self.forward_factor)
self.set(
"Walking.Sideward.Real",
y_move_amplitude * self.sideward_factor)
self.set(
"Walking.Angular.Real",
a_move_amplitude * self.angle_factor)
if log_counter is 0:
debug_m(3, "Running", self.walking.running)
debug_m(3, "XAmplitude", x_move_amplitude)
debug_m(3, "YAmplitude", y_move_amplitude)
debug_m(3, "AAmplitude", a_move_amplitude)
gyro = ipc.get_gyro()
gyro_x = gyro.x
gyro_y = gyro.y
gyro_z = gyro.z
self.walking.gyro = (gyro_x, gyro_y, gyro_z)
with Timer("Walking", self.debug):
# Etwas bewegen
if ipc_was_controllable is False or \
self.walking.running is False:
self.reset_walking()
self.walking.process()
if self.walking.running:
self.set("Walking.Running", True)
pose = self.walking.pose
# Pose updaten
with Timer("Update IPC Pose", self.debug):
ipc.update(pose)
pose.reset()
else:
stop_request = stop_request + 1
if stop_request is 100:
self.set("Walking.Running", False)
stop_request = 0
# Etwas warten und geht weiter!
t = time.time()
dt = t - last_time
last_time = t
time.sleep(max(0, self.sleeptime - dt))
ipc_was_controllable = ipc.controlable
def publish_ball_info_prediction(self, data):
upred, vpred = data[DATA_KEY_BALL_INFO_FILTERED]["uvprediction"]
if upred: # wenn das None ist geht das nicht
debug_m(3, "Ballinfo.Prediction.u", upred)
debug_m(3, "Ballinfo.Prediction.v", vpred)
def publish_ball_info(self, data):
# Ball Info Part
debug_m(3, "BallFound", data[DATA_KEY_BALL_FOUND])
if data[DATA_KEY_BALL_FOUND]:
u, v, x, y, radius, rating, distance = data[DATA_KEY_BALL_INFO]
debug_m(3, "Ballinfo.x", x)
debug_m(3, "Ballinfo.y", y)
debug_m(3, "Ballinfo.u", u)
debug_m(3, "Ballinfo.v", v)
debug_m(3, "Ballinfo.Distance", distance)
def publish_goal_info(self, data):
# Goal Info Part
debug_m(3, "GoalFound", data[DATA_KEY_GOAL_FOUND])
if data[DATA_KEY_GOAL_FOUND]:
goal_info = data[DATA_KEY_GOAL_INFO]
debug_m(3, DATA_KEY_GOAL_INFO + ".1.u", goal_info[0].u)
debug_m(3, DATA_KEY_GOAL_INFO + ".1.v", goal_info[0].v)
if len(goal_info.keys()) == 2:
debug_m(3, DATA_KEY_GOAL_INFO + ".2.u", goal_info[1].u)
debug_m(3, DATA_KEY_GOAL_INFO + ".2.v", goal_info[1].v)
#if data[DATA_KEY_RELATIVE_TO_GOAL_POSITION_AVERAGED] is not None:
# debug_m(3, "PositionAveraged.x", data[DATA_KEY_RELATIVE_TO_GOAL_POSITION_AVERAGED][0])
# debug_m(3, "PositionAveraged.y", data[DATA_KEY_RELATIVE_TO_GOAL_POSITION_AVERAGED][1])
else:
debug_m(3, DATA_KEY_GOAL_INFO + ".2.u", 0)
debug_m(3, DATA_KEY_GOAL_INFO + ".2.v", 0)
# local GoalModel
goal_model = data[DATA_KEY_GOAL_MODEL]
for i in range(4):
u, v = convert_polar2uv(goal_model.goal_posts[i].distance, goal_model.goal_posts[i].angel)
debug_m(3, "GoalModel.%d.u" % i, u)
debug_m(3, "GoalModel.%d.v" % i, v)
def __exit__(self, *ignore):
duration = 1000 * (time.time() - self.start)
debug_m(2, self.name, "brauchte %1.2fms" % duration)
def update(self, data):
# time.sleep(0.1)
""" Hier werden die Ballkandidaten bewertet """
if not data[DATA_KEY_IS_NEW_FRAME]:
return
elif data.get(DATA_KEY_RAW_GOAL_DATA, None) is None:
data[DATA_KEY_GOAL_FOUND] = False
else:
goalpost_candidate = []
goal_posts, color = data[DATA_KEY_RAW_GOAL_DATA]
for goalpost in goal_posts:
x, y, pixelwidth, width, height = goalpost
(u, v) = self.transformer.transform_point_to_location(x, y, 0)
distance = sqrt(u ** 2 + v ** 2)
u = int(u)
v = int(v)
print "Höhe y:", y, " h:", height
print "breite x:", x, " w:", width
print "Dist:", distance
print "u:", u, " v:", v
if distance > self.max_goalpost_distance:
print "Entfernung Weg"
text = "D %.0f" % (distance)
data[VIZ_KEY_SORTED_OUT_GOALS].extend([(text, x, y, width, height)])
continue
print " "
width_to_be_on_picture_max = atan((self.goalpost_width_max) / distance) / (self.cam_winkel * 3 / 4)
width_to_be_on_picture_min = atan((self.goalpost_width_min) / distance) / (self.cam_winkel * 3 / 4)
# width_to_be_on_picture_max = self.goalpost_width_max * self.focal_length/(distance * 6)
# width_to_be_on_picture_min = self.goalpost_width_min * self.focal_length/(distance * 6)
print "Width max:", width_to_be_on_picture_max, "Width min:", width_to_be_on_picture_min
# todo hier bedacht, dass der öffnungswinkel in verticaler richtung anders is als horizontal
# heigth_to_be_on_picture_max = self.goalpost_height_max * self.focal_length/(distance * 6)
# heigth_to_be_on_picture_min = self.goalpost_height_min * self.focal_length/(distance * 6)
heigth_to_be_on_picture_max = atan((self.goalpost_height_max) / distance) / self.cam_winkel
heigth_to_be_on_picture_min = atan((self.goalpost_height_min) / distance) / self.cam_winkel
print "height max:", heigth_to_be_on_picture_max, "heit min:", heigth_to_be_on_picture_min
print " Mittelpunkt X ist", abs(x) + width / 2
if width_to_be_on_picture_min >= width and (abs(x) + width / 2) < 0.48:
# Überprüfung der Breite, nur wenn Vollständig auf Bild
text = "WK %.0f < %.0f" % (width, width_to_be_on_picture_min)
data[VIZ_KEY_SORTED_OUT_GOALS].extend([(text, x, y, width, height)])
continue
elif width_to_be_on_picture_max < width:
text = "WL %.0f > %.0f" % (width, width_to_be_on_picture_max)
data[VIZ_KEY_SORTED_OUT_GOALS].extend([(text, x, y, width, height)])
continue
elif heigth_to_be_on_picture_min >= height and (y + height) < 0.48 * 3 / 4:
# Überprüfung der Höhe, nur wenn Vollständig auf Bild TODO: Parameter evaluieren?, Da abs(x) der Mittelpunkt des Fußpunkts ist
text = "HK %.0f < %.0f" % (height, heigth_to_be_on_picture_min)
data[VIZ_KEY_SORTED_OUT_GOALS].extend([(text, x, y, width, height)])
continue
elif heigth_to_be_on_picture_max < height:
text = "HL %.0f > %.0f" % (height, heigth_to_be_on_picture_max)
data[VIZ_KEY_SORTED_OUT_GOALS].extend([(text, x, y, width, height)])
continue
else:
goalpost_candidate.append(GoalInfo(x, y, u, v, width, height))
print "\n"
if not goalpost_candidate:
data[DATA_KEY_GOAL_FOUND] = False
debug_m(4, "All Goalpost Candidates removed in distance/width and height test")
# time.sleep(1)
debug_m(3, "GoalFound", 0)
return
elif len(goalpost_candidate) == 1:
data[DATA_KEY_GOAL_FOUND] = True
data[DATA_KEY_GOAL_INFO] = {0: goalpost_candidate[0]}
data[DATA_KEY_ANY_GOALPOST_LAST_SEEN] = time.time()
data[VIZ_KEY_SORTED_OUT_GOALS].extend(
[
(
"XE",
goalpost_candidate[0].x,
goalpost_candidate[0].y,
goalpost_candidate[0].width,
goalpost_candidate[0].height,
)
]
)
else:
best_match_goal_posts = {}
best_match_goal_value = 999999
for i in range(len(goalpost_candidate)):
goalpost1 = goalpost_candidate[i]
for j in range(i + 1, len(goalpost_candidate)):
goalpost2 = goalpost_candidate[j]
print "Pfosten1 u:", goalpost1.u, " v:", goalpost1.v, "Pfosten2 u:", goalpost2.u, " v:", goalpost2.v
dist_posts = sqrt((goalpost1.u - goalpost2.u) ** 2 + (goalpost1.v - goalpost2.v) ** 2)
print "Dist:", dist_posts
if dist_posts >= (self.goal_width - self.goal_width_tolerance) and dist_posts <= (
self.goal_width + self.goal_width_tolerance
):
print "in Tolerance"
if abs(dist_posts - self.goalpost_width) < best_match_goal_value:
best_match_goal_posts = {
0: goalpost1,
1: goalpost2,
} # TODO: Prüfen ob Reihenfolge relevant
best_match_goal_value = abs(dist_posts - self.goalpost_width)
if goalpost1 not in best_match_goal_posts.values():
print " So wir haben wegen P : ", dist_posts
data[VIZ_KEY_SORTED_OUT_GOALS].extend(
[("P", goalpost1.x, goalpost1.y, goalpost1.width, goalpost1.height)]
)
if (
not best_match_goal_posts
): # TODO Wollen wir wirklich nichts publizieren wenn wir keinen ordentlichen Match gefunden haben?
data[DATA_KEY_GOAL_FOUND] = False
debug_m(4, "All Goalpost Candidates removed in distance/width and height test")
# time.sleep(1)
debug_m(3, "GoalFound", 0)
return
else:
data[VIZ_KEY_SORTED_OUT_GOALS].extend(
[
(
"X",
best_match_goal_posts[0].x,
best_match_goal_posts[0].y,
best_match_goal_posts[0].width,
best_match_goal_posts[0].height,
)
]
)
data[VIZ_KEY_SORTED_OUT_GOALS].extend(
[
(
"X",
best_match_goal_posts[1].x,
best_match_goal_posts[1].y,
best_match_goal_posts[1].width,
best_match_goal_posts[1].height,
)
]
)
data[DATA_KEY_GOAL_FOUND] = True
data[DATA_KEY_GOAL_INFO] = best_match_goal_posts
data[DATA_KEY_ANY_GOALPOST_LAST_SEEN] = time.time()
debug_m(3, "GoalLastSeen", data[DATA_KEY_ANY_GOALPOST_LAST_SEEN])
debug_m(3, "GoalFound", 1)
def update(self, data):
#time.sleep(0.1)
""" Hier werden die Ballkandidaten bewertet """
visualiization_active = data.get(VIZ_KEY_VIZ_ACTIVE, False)
if visualiization_active:
data[VIZ_KEY_BALL_RATING_OUT] = []
data[VIZ_KEY_BALL_BODY_OUT] = []
data[VIZ_KEY_BALL_SMALL_OUT] = []
data[VIZ_KEY_BALL_HUGE_OUT] = []
data[VIZ_KEY_BALL_FAR_OUT] = []
if not data[DATA_KEY_IS_NEW_FRAME]:
return
elif data[DATA_KEY_RAW_BALL] is None:
data[DATA_KEY_BALL_FOUND] = False
else:
ballinfos = []
#iterate over all candidates
for ball in data[DATA_KEY_RAW_BALL]:
rating, (x, y, radius) = ball #y is the FOOT POINT of the ball in the picture
# Sort out bad rated balls
if rating > self.rating_max:
debug_m(4, "Ignore ball candidate with rating: %f" % (rating))
if visualiization_active:
data[VIZ_KEY_BALL_RATING_OUT].extend([(x, y + radius, radius, rating)])
continue
# Compute distance
(u, v) = self.transformer.transform_point_to_location(x, y, 0 if self.ball_pos_is_ball_footpoint else self.ball_radius)
distance = sqrt(u ** 2 + v ** 2)
if self.transformer.convex_hull([(x,y)])[0]:
debug_m(4, "Zu nah im Körper: konvexe Hülle")
#body_counter += 1
#data[DATA_KEY_SORTED_OUT_BALLS].extend([("B", x, y, radius)])
continue
#TODO this is a hack for vision stuff with near balls. refactor. doppelung
# For further information according to this "special ball rating" read the comments above.
if rating == -2:
if self.activate_orange_ball_hack:
#fußpunkt zu mittelpunbkt
y = y + radius
data[DATA_KEY_BALL_FOUND] = True
data[DATA_KEY_BALL_INFO] = BallInfo(*[u, v, x, y, radius, rating, distance])
data[DATA_KEY_BALL_LAST_SEEN] = time.time()
debug_m(3, "BallLastSeen", data[DATA_KEY_BALL_LAST_SEEN])
debug_m(3, "BallFound", 1)
data[DATA_KEY_BALL_HEAD_POSE] = (data[DATA_KEY_IMAGE_POSE]["HeadPan"].position,
data[DATA_KEY_IMAGE_POSE]["HeadTilt"].position)
debug_m(2, "BallInfo", "u=%1.2f, v=%1.2f, r=%1.2f, d=%1.2f" % (u, v, distance * tan(radius*self.cam_winkel), distance))
return
else:
continue
# Compute distance to our own motors, to eleminate balls on ourself
trans_dist = self.transformer.ray_motor_distance((x, y))
u = int(u)
v = int(v)
if trans_dist < self.max_distance_to_motor and u < 200 and distance < 300: #todo nich schön, hack von GO15
debug_m(4, "Zu nah im Körper: %f" % trans_dist)
if visualiization_active:
data[VIZ_KEY_BALL_BODY_OUT].extend([(x, y + radius, radius, trans_dist)])
continue
#fußpunkt zu mittelpunkt
y = y + radius #todo check if it is a good idea to do this, in relation to the small/huge filtering
ballinfos.append(BallInfo(*[u, v, x, y, radius, rating, distance]))
ball_candidate = []
for ball in ballinfos: #todo diese schleife kann man mit der dadrüber mergen
#todo check
# The distance to calculate the angle the ball should cover in the picture dramatically
# Increases when the ball is close, due to the robot's own height, this is approximated here with 320 mm
distance = sqrt(
ball.distance**2 +
get_camera_position_p(data[DATA_KEY_TRANSFORMER].robot, data[DATA_KEY_CAMERA_FOOT_PHASE])[2]**2)
# the radius form the vision is relative to the picture! #todo check if camera winkel is right
radius_to_be_on_picture_max = atan(self.ball_raidus_max / distance) / self.cam_winkel #todo check if formular is correct
radius_to_be_on_picture_min = atan(self.ball_radius_min / distance) / self.cam_winkel
# To small
if not radius_to_be_on_picture_min < ball.radius:
debug_m(3,"Ball to small. Min: %f is %f: u:%f, v:%f, r:%f" %(radius_to_be_on_picture_min, ball.radius, ball.u, ball.v, ball.rating))
if visualiization_active:
data[VIZ_KEY_BALL_SMALL_OUT].extend([(ball.x, ball.y, ball.radius, radius_to_be_on_picture_min)])
# To huge
elif not ball.radius < radius_to_be_on_picture_max:
debug_m(4,
"Ball to big. Max: %f is %f" % (
radius_to_be_on_picture_max, ball.radius))
if visualiization_active:
data[VIZ_KEY_BALL_HUGE_OUT].extend([(ball.x, ball.y, ball.radius, radius_to_be_on_picture_max)])
else:
if ball.distance > self.max_ball_distance:
debug_m(4, "Verwerfe Balconfigl wegen großer entfernung: %f" % ball.distance)
if visualiization_active:
data[VIZ_KEY_BALL_FAR_OUT].extend([(ball.x, ball.y, ball.radius, ball.distance)])
else:
ball_candidate.append(ball)
# check if there are sstill candidates
if not ball_candidate:
data[DATA_KEY_BALL_FOUND] = False
debug_m(4, "All Ball Candidates removed in distance/Radius test")
debug_m(3, "BallFound", 0)
return
# find the best ball from remaining candidates
min_ball_delta = 999999999
best_ball = None
# only use the best canidates, sorted by their rating
ball_candidate.sort(key=lambda x: x.rating)
ball_candidate = ball_candidate[:self.number_rated_candidates]
# for vision script debug
if visualiization_active:
data[VIZ_KEY_BALL_POSSIBLE_LIST] = ball_candidate
number = 0
best_number = -1
# die entfernung testen
for ball in ball_candidate:
radius_desired = atan(self.ball_radius / ball.distance) / self.cam_winkel
delta = abs(ball.radius - radius_desired)
if delta < min_ball_delta:
min_ball_delta = delta
best_ball = ball
best_number = number
number += 1
# ausgewähler ball
info = best_ball
if visualiization_active:
data[VIZ_KEY_BEST_BALL_NUMBER] = best_number
data[DATA_KEY_BALL_FOUND] = True
data[DATA_KEY_BALL_INFO] = info
data[DATA_KEY_BALL_LAST_SEEN] = time.time()
debug_m(3, "BallLastSeen", data[DATA_KEY_BALL_LAST_SEEN])
debug_m(3, "BallFound", 1)
data[DATA_KEY_BALL_HEAD_POSE] = (data[DATA_KEY_IMAGE_POSE]["HeadPan"].position,
data[DATA_KEY_IMAGE_POSE]["HeadTilt"].position)
debug_m(2, "BallInfo", "u=%1.2f, v=%1.2f, r=%1.2f, d=%1.2f" % (u, v, distance * tan(radius*self.cam_winkel), distance))
def __dbscanCalcAndNeuralNetFilter(self, data):
"""
Start dbscan calculation to filter raw data,
initialize the neural net to filter/calibrate data
:param data: BitBots data object
"""
#####################
### initilization ###
#####################
post1u_filtered = None
post1v_filtered = None
post2u_filtered = None
post2v_filtered = None
goalcenteru = None
goalcenterv = None
goalcenterunfilteredu = None
goalcenterunfilteredv = None
#########################
### DBSCAN parameters ###
#########################
minPoints = 5 # minimum number of points to build a cluster
epsilon = 75.0 # range to search for neighboring points
##############
### Post 1 ###
##############
if len(self.dbscanBufferPost1) > 0:
# Post 1 - raw u, v values
post1rawx = numpy.array([elem["u"] for elem in self.dbscanBufferPost1])
post1rawv = numpy.array([elem["v"] for elem in self.dbscanBufferPost1])
# Post 1 - create a DBScan object and start calculation
post1scan = DBScan(post1rawx, post1rawv, minPoints, epsilon)
# Post 1 - extract filtered data out of the DBScan object
post1u_values, post1v_values = post1scan.getFilteredData()
# Post 1 - calculate the mean out of the filtered data
post1u_filtered = numpy.mean(post1u_values)
post1v_filtered = numpy.mean(post1v_values)
##############
### Post 2 ###
##############
if len(self.dbscanBufferPost2) > 0:
# Post 2 - raw u, v values
post2rawx = numpy.array([elem["u"] for elem in self.dbscanBufferPost2])
post2rawv = numpy.array([elem["v"] for elem in self.dbscanBufferPost2])
# Post 2 - create a DBScan object and start calculation
post2scan = DBScan(post2rawx, post2rawv, minPoints, epsilon)
# Post 2 - extract filtered data out of the DBScan object
post2u_values, post2v_values = post2scan.getFilteredData()
# Post 2 - calculate the mean out of the filtered data
#todo hier kommen irgendwo warnings wegen aufruf von mean auf was leerem
post2u_filtered = numpy.mean(post2u_values)
post2v_filtered = numpy.mean(post2v_values)
###################
### Goal center ###
###################
if (post1u_filtered is not None) and\
(post1v_filtered is not None) and\
(post2u_filtered is not None) and\
(post2v_filtered is not None):
goalcenteru = (post1u_filtered + post2u_filtered) / 2.0
goalcenterv = (post1v_filtered + post2v_filtered) / 2.0
#######################
### Log/debug stuff ###
#######################
if self.pipeDebugIntoFile:
exportLog = ExportLog("GoalDBScan.log")
exportData = OrderedDict()
x1, y1, u1, v1 = data[DATA_KEY_GOAL_INFO][0] # collect current raw values for debugging/logging - post 1
x2, y2, u2, v2 = data[DATA_KEY_GOAL_INFO][1] # collect current raw values for debugging/logging - post 2
goalcenterunfilteredu = (u1 + u2) / 2.0
goalcenterunfilteredv = (v1 + v2) / 2.0
exportData["post1_u_raw"] = str(u1)
exportData["post1_v_raw"] = str(v1)
exportData["post2_u_raw"] = str(u2)
exportData["post2_v_raw"] = str(v2)
exportData["post1_u_filtered"] = str(post1u_filtered)
exportData["post1_v_filtered"] = str(post1v_filtered)
exportData["post2_u_filtered"] = str(post2u_filtered)
exportData["post2_v_filtered"] = str(post2v_filtered)
exportData["goalcenter_u_filtered"] = str(goalcenteru)
exportData["goalcenter_v_filtered"] = str(goalcenterv)
exportData["goalcenter_u_unfiltered"] = str(goalcenterunfilteredu)
exportData["goalcenter_v_unfiltered"] = str(goalcenterunfilteredv)
exportLog.addDataRecord(exportData)
exportLog.writeLog()
######################################
### Goal information - named tuple ###
######################################
# prepare an object for filtered information
goalinfo = namedtuple("goalinfo", ["u_center", "v_center", "u_post1", "v_post1", "u_post2", "v_post2", "time"])
##########################################
### Neural net - calibration/filtering ###
##########################################
#inputLength = 2 # input length of input layer
#outputLength = 2 # output length of output layer
#hiddenLayer = [10, 5] # hidden layer configuration (amount of hidden layers & neurons for each layer)
# Create the neural net (MLP)
#net = NeuralNetworkClass.NeuralNetwork(inputLength, hiddenLayer, outputLength)
# Load saved configuration (weights & bias)
#net.loadWeightsAndBias("/home/darwin/logfiles/")
# filter/calibrate the goal center information via the neural net
#goalcenteru, goalcenterv = net.calculate([goalcenteru/10000.0, goalcenterv/10000.0])["output"]
# normalize the goal information again
#goalcenteru *= 10000
#goalcenterv *= 10000
#########################
### Set filtered data ###
#########################
data[DATA_KEY_GOAL_INFO_FILTERED] = goalinfo(u_center=goalcenteru,
v_center=goalcenterv,
u_post1=post1u_filtered,
v_post1=post1v_filtered,
u_post2=post2u_filtered,
v_post2=post2v_filtered,
time=time.time())
debug_m(3, "goal_center_u", str(goalcenteru))
debug_m(3, "goal_center_v", str(goalcenterv))
debug_m(3, "post1_u", str(post1u_filtered))
debug_m(3, "post1_v", str(post1v_filtered))
debug_m(3, "post2_u", str(post2u_filtered))
debug_m(3, "post2_v", str(post2v_filtered))
def post(self, data):
"""
Hier werden die Daten an die Motion durchgereicht, vorher wird auf
Beschleunigung geachtet. Das passiert in :func:`postz` und nicht in
:func:`update` damit das walking die aktuellen Daten des Zyklus
auslesen kann, trotz dem es required wird
..warning:: Es wird nur in STATE_PLAYING gelaufen
"""
data[DATA_KEY_WALKING_RUNNING] = self.ipc.get_motion_state() == STATE_WALKING
self.log_counter += 1
if self.log_counter > 60:
self.log_counter = 0
if self.ipc.get_motion_state() != STATE_WALKING:
self.lastforward = 0
self.lastsideward = 0
self.lastangular = 0
if data[DATA_KEY_GAME_STATUS] != DATA_VALUE_STATE_PLAYING or data[DATA_KEY_WALKING_ACTIVE] is False:
if self.log_counter is 0:
debug_m(4, "Walking not active -> Continue")
if self.lastforward == 0 and self.lastsideward == 0 and self.lastangular == 0:
# nur wenn wir stehen aus machen
self.ipc.set_walking_activ(False)
return
# sonst stop anfordern
newforward = 0
newsideward = 0
newangular = 0
else:
newforward = int(data[DATA_KEY_WALKING_FORWARD])
newsideward = int(data[DATA_KEY_WALKING_SIDEWARD])
newangular = int(data[DATA_KEY_WALKING_ANGULAR])
self.ipc.set_walking_activ(True)
# this code prevents the robot to run too fast
# and prevents too abrupt canges of speeds
# Änderungen nur in definierten Intervallen
if time.time() - self.updateslower > self.update_intervall:
forward_delta = newforward - self.lastforward
forward_stopfactor = self.stop_factor if newforward > 0 and newforward < self.lastforward else 1
sideward_delta = newsideward - self.lastsideward
sideward_stopfactor = (
self.stop_factor
if (newsideward > 0 and newsideward < self.lastsideward)
or (newsideward < 0 and newsideward > self.lastsideward)
else 1
)
angular_delta = newangular - self.lastangular
delta_sum = (
abs(forward_delta * self.angularfaktor) + abs(sideward_delta * self.angularfaktor) + abs(angular_delta)
) / (self.angularfaktor * self.speed_delta)
self.updateslower = time.time()
if delta_sum != 0:
self.lastforward += sign(forward_delta) * min(
abs(forward_delta), abs(forward_delta / delta_sum * forward_stopfactor)
)
self.lastsideward += sign(sideward_delta) * min(
abs(sideward_delta), abs(sideward_delta / delta_sum * sideward_stopfactor)
)
self.lastangular += sign(angular_delta) * min(
abs(angular_delta), abs(angular_delta * self.angularfaktor / delta_sum)
)
self.lastforward = self.maxspeed(self.lastforward, self.max_forward_speed)
self.lastangular = self.maxspeed(self.lastangular, self.max_angular_speed)
self.lastsideward = self.maxspeed(self.lastsideward, self.max_sidewards_speed)
# TODO schneller Stoppen?
debug_m(4, "RealForward", self.lastforward)
debug_m(4, "RealSideward", self.lastsideward)
debug_m(4, "RealAngular", self.lastangular)
data[DATA_KEY_WALKING_FORWARD_REAL] = self.lastforward
data[DATA_KEY_WALKING_SIDEWARD_REAL] = self.lastsideward
data[DATA_KEY_WALKING_ANGULAR_REAL] = self.lastangular
self.ipc.set_walking_forward(self.lastforward)
self.ipc.set_walking_sidewards(self.lastsideward)
self.ipc.set_walking_angular(self.lastangular)
if self.log_counter is 0:
debug_m(3, "Running", (self.ipc.get_motion_state() == STATE_WALKING))
debug_m(3, "XAmplitude", self.lastforward)
debug_m(3, "YAmplitude", self.lastsideward)
debug_m(3, "AAmplitude", self.lastangular)
def man_penelize(self, event_data):
self.ipc.set_state(STATE_PENALTY)
debug_m(2, "Set State Man_Penalized")
self.send_event(EVENT_GLOBAL_INTERRUPT)
self.man_pen = True
def perform(self, connector, reevaluate=False):
# todo refactor in more decisions
""" This is the root for the head stack machine """
if connector.raw_vision_capsule().ball_seen():
self.ball_prio = max(0, self.ball_prio - 3)
else:
self.ball_prio = min(120, self.ball_prio + 5)
if connector.raw_vision_capsule().any_goal_seen():
self.goal_prio = max(0, self.goal_prio - 2)
else:
self.goal_prio = min(100, self.goal_prio + 3)
debug_m(4, "GoalPrio", self.goal_prio)
debug_m(4, "BallPrio", self.ball_prio)
debug_m(4, "BallLastCOnfirmed", time.time() - connector.blackboard_capsule().get_confirmed_ball())
debug_m(4, "BallLastStratedconfirm", time.time() - connector.blackboard_capsule().get_started_confirm_ball())
if connector.blackboard_capsule().is_no_head_movement_at_all():
debug_m(4, "Headdoes", "Nothing")
return self.interrupt()
if connector.blackboard_capsule().is_ball_tracking_still_active():
debug_m(4, "Headdoes", "BallTracking")
return self.push(SearchAndConfirmBall)
if connector.blackboard_capsule().is_enemy_goal_tracking_still_active():
debug_m(4, "Headdoes", "GoalTracking")
return self.push(SearchAndConfirmEnemyGoal)
if connector.blackboard_capsule().is_tracking_both_still_active(): # todo to be tested
debug_m(4, "TrackbothTime", time.time())
if time.time() - connector.blackboard_capsule().get_confirmed_ball() > 5:
debug_m(4, "Headdoes", "TrackBothBall")
return self.push(SearchAndConfirmBall)
# ball long enough seen
elif time.time() - connector.blackboard_capsule().get_confirmed_goal() > 6:
debug_m(4, "Headdoes", "TrackBothGoal")
return self.push(SearchAndConfirmEnemyGoal)
elif self.trackjustball_aftergoal:
debug_m(4, "Headdoes", "TrackBothElse")
return self.push(SearchAndConfirmBall)
if self.toggle_switch_ball_goal:
debug_m(4, "Headdoes", "Priorities")
if self.ball_prio >= self.goal_prio:
return self.push(SearchAndConfirmBall)
else:
return self.push(SearchAndConfirmEnemyGoal)
# Default Head Behaviour
debug_m(4, "Headdoes", "Standardsearch")
return self.push(ContiniousSearch)
def unpenelize(self, event_data):
if not self.man_pen:
# wir unpenelizen nur wenn wir nicht manuel gepenelized sind
debug_m(2, "Unpenalize")
self.ipc.set_state(STATE_CONTROLABLE)
self.send_event(EVENT_GLOBAL_INTERRUPT)
def run(self):
reciver = GameStateReceiver(team=self.team, player=self.player,
addr=(self.address[0], self.address[1]))
debug_m(2, "Teamnumber", str(self.team))
debug_m(2, "Playernumber: " + str(self.player))
debug_m(2, "Playernumber", str(self.player))
# Manual penalty events will be passed to the receiver
# to give it the ability to pass it to the Gamecontroller
# There is no problem if the ManualPenaltyModule is not loaded, it just happens nothing
self.register_to_event(
EVENT_MANUAL_PENALTY,
lambda data: reciver.set_manual_penalty(True))
self.register_to_event(
EVENT_NO_MANUAL_PENALTY,
lambda data: reciver.set_manual_penalty(False))
old_game_state = None
old_penalty_status = False
old_own_goals = -1
block_missed = True
while True:
reciver.receive_once()
state = reciver.get_last_state()
if state is not None:
block_missed = False
if state.teams[0].team_number == self.team or state.teams[1].team_number == self.team:
self.set(DATA_KEY_GAME_STATUS, state.game_state)
debug_m(2, DATA_KEY_GAME_STATUS, state.game_state)
self.set(DATA_KEY_SECONDS_REMAINING, state.seconds_remaining)
self.set(DATA_KEY_SECONDAR_SECONDS_REMAINING, state.secondar_seconds_remaining)
self.set(DATA_KEY_DROP_IN_TIME, state.drop_in_time)
if state.game_state != old_game_state:
debug_m(2, "Gamestate has Changed: %s --> %s" % (
str(old_game_state), str(state.game_state)))
if old_game_state in (DATA_VALUE_STATE_SET, DATA_VALUE_STATE_READY, DATA_VALUE_STATE_INITIAL) \
and state.game_state == DATA_VALUE_STATE_PLAYING:
if 0 < DATA_KEY_DROP_IN_TIME < 30:
self.set(DATA_KEY_DROP_BALL_TIME, time.time())
else:
self.set(DATA_KEY_KICK_OFF_TIME, time.time())
old_game_state = state.game_state
self.send_event(EVENT_GAME_STATUS_CHANGED, state.game_state)
i = 0
for team in state.teams:
if team.team_number == self.team:
self.set(DATA_KEY_OWN_GOALS, team.score)
penalty = team.players[self.player - 1].penalty
if old_penalty_status != penalty:
if penalty:
self.send_event(EVENT_PENALTY)
self.set(DATA_KEY_PENALTY, True)
debug_m(2, "Penalize")
else:
self.send_event(EVENT_NO_PENALTY)
self.set(DATA_KEY_PENALTY, False)
debug_m(2, "Unpenalize")
self.set(DATA_KEY_PENALTY, penalty != 0)
goals = team.score
if goals != old_own_goals:
if old_own_goals != -1:
self.send_event(EVENT_GOAL, goals)
old_own_goals = goals
debug_m(2, DATA_KEY_PENALTY, team.players[
self.player - 1].penalty != 0)
old_penalty_status = penalty
self.set(DATA_KEY_OWN_KICK_OF, state.kick_of_team == i)
debug_m(2,
DATA_KEY_OWN_KICK_OF, str(state.kick_of_team == i))
else:
self.set(DATA_KEY_ENEMY_GOALS, team.score)
i += 1
else:
if not block_missed and reciver.get_time_since_last() > 10:
block_missed = True
# nach 10 sekunden kein packet setzen wir einige dinge
say("Lost Gamecontroler, will Play now")
self.set(DATA_KEY_GAME_STATUS, DATA_VALUE_STATE_PLAYING)
self.send_event(EVENT_GAME_STATUS_CHANGED, DATA_VALUE_STATE_PLAYING)
debug_m(2, DATA_KEY_GAME_STATUS, "STATE_PLAYING (Contorller Lost)")
if DATA_VALUE_STATE_PLAYING != old_game_state:
debug_m(2, "Gamestate has Changed due to Controler loss: %s --> %s" % (
str(old_game_state), DATA_VALUE_STATE_PLAYING))
old_game_state = DATA_VALUE_STATE_PLAYING
if old_penalty_status:
old_penalty_status = False
self.send_event(EVENT_NO_PENALTY)
self.set(DATA_KEY_PENALTY, False)
debug_m(2, "Unpenalize")
debug_m(2, DATA_KEY_GAME_STATUS, "GameStatus is None")
