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")