def action(self, connector, reevaluate): if connector.raw_vision_capsule().get_ball_info("v") <= 0: say("Kick Strong") return self.push(KickBall, init_data="RP") else: say("Kick Strong") return self.push(KickBall, init_data="LP")
def perform(self, connector, reevaluate=False): """ Diese Methode richtet den Goalie aus, erstmal alles stueck fuer stueck This Method repositionates the goalie, for now step by step. """ if time.time() - self.time > 15: self.debug("Mehr als 15 Sekunden das Tor gesucht, ich gebe es auf") connector.blackboard_capsule().set_priorities(own_goal_priority=0, ball_priority=1000, align_priority=0) connector.blackboard_capsule().set_thrown(False) return self.interrupt() if connector.raw_vision_capsule().own_goal_seen(): # todo testen ob own_goal_seen geht, sonst schauen ob tor gesehen und näherdran als hälfte des spielfeldes # aktivate the tracking connector.blackboard_capsule().schedule_own_goal_tracking() goal_u = connector.filtered_vision_info_capsule().get_center_of_seen_goal()[0] goal_v = connector.filtered_vision_info_capsule().get_center_of_seen_goal()[1] distance = (goal_u ** 2 + goal_v ** 2) if goal_u != 0: winkel = math.degrees((math.atan(goal_u / goal_v) / 3.1415) * 180) else: winkel = 0 capsule_obj = connector.walking_capsule() capsule_fwd = capsule_obj.ZERO capsule_ang = capsule_obj.ZERO capsule_swd = capsule_obj.ZERO if distance < 300: # wir stehen nahe genug say("Im in the center of my goal") self.pop() elif distance < 5000: # es ist nicht das entfernte (vermutlich gegnerische)tor capsule_fwd = capsule_obj.FAST_FORWARD if abs(winkel) > 10: capsule_fwd = capsule_obj.MEDIUM_FORWARD if winkel > 0: if winkel > 45: capsule_ang = capsule_obj.MEDIUM_ANGULAR_RIGHT else: capsule_ang = capsule_obj.SLOW_ANGULAR_RIGHT elif winkel: if winkel < -45: capsule_ang = capsule_obj.MEDIUM_ANGULAR_LEFT else: capsule_ang = capsule_obj.SLOW_ANGULAR_LEFT else: capsule_ang = capsule_obj.ZERO else: say("i dont want to go to this goal. its to far away") self.interrupt() # todo bessere fehlerbehandlung, zB genau so laufen, dass man weit weg davon ist oder dadran ausrichten capsule_obj.start_walking(capsule_fwd, capsule_ang, capsule_swd) """
def switch_manual_penalty(self, data): if not self.penalty: self.send_event(EVENT_MANUAL_PENALTY) say("Set Manual Penalty") self.penalty = True else: self.send_event(EVENT_NO_MANUAL_PENALTY) say("Revoke Manual Penalty") self.penalty = False
def kick_normal(self, connector): """ Pushes a normal kick, depending on side of the Ball """ if connector.filtered_vision_capsule().get_local_goal_model_ball()[1] <= 0: say("Normal Kick") return self.push(KickBall, init_data="R") else: say("Normal Kick") return self.push(KickBall, init_data="L")
def kick_side_goal(self, connector): """ Pushes a sidekick, depending on the side of the enemy goal """ if connector.filtered_vision_capsule().get_local_goal_model_opp_goal()[1] > 0: say("Kick Ball right") return self.push(KickBall, init_data="SRK") else: say("Kick Ball left") return self.push(KickBall, init_data="SLK")
def start(self, data): self.SPEAK = data[DATA_KEY_CONFIG]["SPEAK"] self.oldgamestate = None if self.SPEAK: self.register_to_event(EVENT_PENALTY, lambda data: say("Nooo. I am Penalized")) self.register_to_event(EVENT_NO_PENALTY, lambda data: say("Jeah! I can play again")) self.register_to_event(EVENT_GAME_STATUS_CHANGED, lambda data: say(data))
def act(self, connector): number_of_times_voted_for_yes = 0 if connector.raw_vision_capsule().is_new_frame() and self.is_waiting_period_over(): number_of_times_voted_for_yes = self.vote_for_switch_to_striker(connector) if number_of_times_voted_for_yes > self.required_number: say("Going to Striker! Attack!") connector.set_duty("Striker") return self.interrupt() else: return self.push(Wait)
def hard_kick(self, connector): """ Pushes a hard kick, depending on side of the ball :param connector: :return: """ if connector.filtered_vision_capsule().get_local_goal_model_ball()[1] <= 0: say("Kick Strong") return self.push(KickBall, init_data="RP") else: say("Kick Strong") return self.push(KickBall, init_data="LP")
def perform(self, connector, reevaluate=False): if not self.kicked: self.direction = -1 if random.random() < 0.5 else 1 connector.team_data_capsule().publish_kickoff_strategy(self.direction) direction_string = "left" if self.direction == -1 else "right" say("Kick off to the " + direction_string) self.kicked = True if self.direction == -1: return self.push(KickBall, "SRK") else: return self.push(KickBall, "SLK") return self.interrupt()
def update(self, data): """ Checks buttons and changes state corespondingly """ if data[DATA_KEY_BUTTON_1]: if self.actual_state == 'walking': self.actual_state = 'anim' say("Play Anim Mode") elif self.actual_state == 'init' or \ self.actual_state == 'anim': self.actual_state = 'walking' say("Walking Mode") if data[DATA_KEY_BUTTON_2]: if self.actual_state == 'walking': # if nothing was setted, return false if data.get(DATA_KEY_WALKING_ACTIVE, False): say("stop") data[DATA_KEY_WALKING_ACTIVE] = False data[DATA_KEY_WALKING_FORWARD] = 0 else: say("walk") data[DATA_KEY_WALKING_ACTIVE] = True data[DATA_KEY_WALKING_FORWARD] = 3 if self.actual_state == 'anim': if self.conf_hands: data[DATA_KEY_ANIMATION] = "test_arm" else: data[DATA_KEY_ANIMATION] = "headstand_new_head"
def perform(self, connector, reevaluate=False): number_of_times_voted_for_yes = 0 if connector.raw_vision_capsule().is_new_frame() and self.is_waiting_period_over(): number_of_times_voted_for_yes = self.vote_for_switch_to_striker(connector) if number_of_times_voted_for_yes > self.required_number: say("Going to Striker! Attack!") become_one_time_kicker(connector) return self.interrupt() else: # Standard defender is moving in front of own goal if self.toggle_one_time_defender: return self.push(Wait, 1) else: return self.push(DefenderCorridor)
def perform(self, connector, reevaluate=False): if not connector.gamestatus_capsule().is_game_state_equals(DATA_VALUE_STATE_PLAYING): return self.interrupt() if not self.initilized: self.initilized = True self.ball_distance_saved = connector.raw_vision_capsule().get_ball_info("distance") seconds_remaining = connector.gamestatus_capsule().get_secondary_seconds_remaining() ball_distance_now = connector.raw_vision_capsule().get_ball_info("distance") say("Wait") if seconds_remaining == 0 or abs(ball_distance_now - self.ball_distance_saved) > 200: say("Enemy moved ball. Can play myself") connector.blackboard_capsule().set_enemy_kick_off_done() return self.pop() else: return self.push(Wait, 0.1)
def post(self, data): """ Dieses Modul tut seine Arbeit in :func:`post` damit von Modulen eingereichte Animationen direkt gespielt werden können auch wenn diese Animation requiren und das Modul sonst zuerst ausgeführt wird """ if data.get("Animation", None) is not None: if data["Animation"][0] in ["rk_go14", "lk_io14"]: self.worldServiceClient.kick_ball(self.player) if data["Animation"][0] in ["r-pass"]: say("Kick Right Side") self.worldServiceClient.kick_ball_side(self.player, "right") if data["Animation"][0] in ["l-pass"]: say("Kick Left Side") self.worldServiceClient.kick_ball_side(self.player, "left") print data["Animation"] data["Animation"][1](False) data["Animation"] = None
def perform(self, connector, reevaluate=False): duty = connector.get_duty() if duty == "Goalie": position = (self.goalie_position[0] * self.length, self.goalie_position[1] * self.width) elif duty == "TeamPlayer": position = (self.teamplayer_position[0] * self.length, self.teamplayer_position[1] * self.width) elif duty == "Defender": position = (self.defender_position[0] * self.length, self.defender_position[1] * self.width) elif duty == "Center": position = (self.center_position[0] * self.length, self.center_position[1] * self.width) else: position = (-0.25 * self.length, 0) # this is the middle point of our own half if connector.world_model_capsule().get_distance_to_xy(position[0], position[1]) > self.threshold: say("Go to duty position") return self.push(GoToPositionIntelligent, (position, True)) else: say("I am at position") return self.pop()
def perform(self, connector, reevaluate=False): # When reevaluate isnt set, set stopReEvaluate to False # (otherwise stopReEvaluate would be true forever, if set once) if not reevaluate: self.stopReEvaluate = False # Get the estimated values from the BallDataInfoFilterModule uestimate, vestimate = connector.filtered_vision_capsule().get_uv_estimate() # If they are not valid - return - we can't do anything if uestimate is None or vestimate is None: say("No valid Estimates") # todo by Marc: better error handling. Why can there be None values? return self.richtung = self.get_throw_direction_on_vestimate(vestimate) if uestimate <= self.u_throw_threshold: return self.push(Throw, self.richtung) else: if self.richtung != connector.blackboard_capsule().get_arm_pos(): # todo problems with wrong data in capsule after falling robot? return self.push(RaiseArm, self.richtung)
def action(self, connector, reevaluate): if not connector.blackboard_capsule().get_throwin_turned(): connector.blackboard_capsule().set_throwin_turned() say("Turn") return self.push( PlainWalkAction, [ [WalkingCapsule.ZERO, WalkingCapsule.SLOW_ANGULAR_RIGHT, WalkingCapsule.MEDIUM_SIDEWARDS_RIGHT, 2], [WalkingCapsule.ZERO, WalkingCapsule.SLOW_ANGULAR_RIGHT, WalkingCapsule.ZERO, 15], [WalkingCapsule.ZERO, WalkingCapsule.SLOW_ANGULAR_RIGHT, WalkingCapsule.MEDIUM_SIDEWARDS_RIGHT, 15], [WalkingCapsule.ZERO, WalkingCapsule.SLOW_ANGULAR_RIGHT, WalkingCapsule.ZERO, 15], [WalkingCapsule.ZERO, WalkingCapsule.SLOW_ANGULAR_RIGHT, WalkingCapsule.MEDIUM_SIDEWARDS_RIGHT, 15], [WalkingCapsule.ZERO, WalkingCapsule.SLOW_ANGULAR_RIGHT, WalkingCapsule.ZERO, 15], ], ) elif not connector.blackboard_capsule().get_throwin_aligned(): connector.blackboard_capsule().set_throwin_aligned() o_u, o_v = connector.blackboard_capsule().get_obstacle_position() o_winkel = math.degrees(math.atan2(o_u, o_v)) say("align") turntime = o_winkel / 3 say(str(turntime)) if o_winkel > 0: return self.push( PlainWalkAction, [ [ WalkingCapsule.SLOW_BACKWARD, WalkingCapsule.MEDIUM_ANGULAR_RIGHT, WalkingCapsule.MEDIUM_SIDEWARDS_LEFT, turntime, ] ], ) else: return self.push( PlainWalkAction, [ [ WalkingCapsule.SLOW_BACKWARD, WalkingCapsule.MEDIUM_ANGULAR_LEFT, WalkingCapsule.MEDIUM_SIDEWARDS_RIGHT, turntime, ] ], ) return self.push(ThrowBall)
def up(self, down): if self.begin_press: if time.time() - self.begin_press > 2: say("Manual Start") self.send_event(EVENT_MANUAL_START)
def error(self, e, prefix_msg="", speak=True): if speak: from bitbots.util.speaker import say say(prefix_msg + " " + str(e)) self.warning(prefix_msg + " " + str(e)) self.warning(traceback.format_exc())
def start(self): hostname = socket.gethostname() say("Hello my name is " + hostname, False) self.play_anim("hi", True) say("I'm a Darwin robot. I play for the Hamburg Bit Bots. We are a group of students of the university of hamburg that participates in the robocup.", False) self.play_anim("talk1", True) say("Now I will show you what I can do.", False) self.play_anim("KopfStand", True) self.play_anim("freddy", True) self.play_anim("talk1", False) say("I know this was really impressiv, but I can do many other things too", True) say("for example I can do a kick", True) self.play_anim("rk", True) say("and with the other leg too of course", True) self.play_anim("lk", True) self.play_anim("talk1", False) say("I think I'm a bit out of shape. I better do some push ups", True) self.play_anim("liegestuez", True) self.play_anim("init", True) self.play_anim("brucelee", True) say("now I will do a litle dance") self.play_anim("jogram", True) say("It was nice to meet you. I hope you liked my show. I'm finished. I'm going to sleep now", True) self.play_anim("hinsetzen")
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")
def say_error_message(self, motors): """ Liest einen mit :py:func:`get_error_message` generierten String vor """ message = self.get_error_message(motors) say(message)
def update(self, data): if data[DATA_KEY_BUTTON_1] and data.get(DATA_KEY_MANUAL_PENALTY, False): data[DATA_KEY_RECALIBRATE_BALL] = True say("Recalibrate Ball") else: data[DATA_KEY_RECALIBRATE_BALL] = False
def perform(self, connector, reevaluate=False): if connector.blackboard_capsule().is_frozen(): return if not connector.get_duty(): if duty is not None: # get information about his duty which was set by the startup script connector.set_duty(duty) else: connector.set_duty("TeamPlayer") if not connector.gamestatus_capsule().is_game_state_equals(DATA_VALUE_STATE_PLAYING): # resets all behaviours if the gamestate is not playing, because the robots are positioned again if duty is not None: connector.set_duty(duty) ############################ # # Gamestate related Stuff# ############################ # If we do not Play or Ready we do nothing if connector.gamestatus_capsule().get_gamestatus() in [DATA_VALUE_STATE_INITIAL, DATA_VALUE_STATE_SET, DATA_VALUE_STATE_FINISHED]: return self.push(Wait, 0.1) # Positioning ourself on the Field if self.toggle_self_positioning: if connector.gamestatus_capsule().is_game_state_equals(DATA_VALUE_STATE_READY): # Todo checking if working return self.push(GoToDutyPosition) ################################ # #load cetain part of behaviour ################################ if connector.get_duty() in ["TeamPlayer"]: return self.push(KickOff) elif connector.get_duty() == "Goalie": return self.push(GoalieDecision) elif connector.get_duty() == "OneTimeKicker": return self.push(OneTimeKickerDecision) # this should be normally not used just for debug or emergency elif connector.get_duty() in ["Defender", "Striker", "Center", "Supporter"]: return self.push(RoleDecider, connector.get_duty()) elif connector.get_duty() == "PenaltyKickFieldie": return self.push(PenaltyKickerDecision) ########################### # # Tecnical Chelanges ########################### elif connector.get_duty() == "ThrowIn": return self.push(ThrowInEntryPoint) ########################### # # Other TestStuff ########################### elif connector.get_duty() == "GoToPosition": return self.push(GoToDutyPosition) elif connector.get_duty() == "Positionate": return self.push(GoToAbsolutePosition, [50, 50, 30]) elif connector.get_duty() == "Nothing": return self.push(Wait) elif connector.get_duty() == "FocusGoal": return self.push(FocusEnemyGoal) elif connector.get_duty() == "CalibrateVision": return self.push(CalibrateVision) else: say("Overridden duty not found: %s" % connector.get_duty()) raise NotImplementedError
def perform(self, connector, reevaluate=False): # search complete goal connector.blackboard_capsule().set_priorities(enemy_goal_priority=0, ball_priority=0, own_goal_priority=0, align_priority=1510) connector.blackboard_capsule().cancel_ball_tracking() connector.walking_capsule().stop_walking() self.do_not_reevaluate() # waits for result from head if not connector.blackboard_capsule().get_complete_goal_found() and \ not connector.blackboard_capsule().get_cant_find_complete_goal(): return self.push(Wait, 0.1) # if found if connector.blackboard_capsule().get_complete_goal_found(): connector.blackboard_capsule().unset_complete_goal_found() goal_center = connector.blackboard_capsule().get_complete_goal_center() goal_distance = math.sqrt(goal_center[0] ** 2 + goal_center[1] ** 2) goal_polar = convert_uv2polar(goal_center[0], goal_center[1]) goal_angle = math.degrees(goal_polar[1]) # test if own goal right_goal = self.test_if_right_goal(connector, goal_distance) goal_v = goal_center[1] # compute angle to move if not right_goal: say("Do not shot on own goal") sign = self.sign(goal_angle) goal_angle += 180 * sign # ausgleich, wenns über 180 rüber geht, weil unsere polarwerte so funktionieren # math if it is more than 180, because our polarvalues work this way if goal_angle < -180: goal_angle = 180 - (abs(goal_angle) - 180) elif goal_angle > 180: goal_angle = -180 + (abs(goal_angle) - 180) if goal_v > 0: goal_v -= 10000 else: goal_v += 10000 if False: # if goodangle if abs(goal_angle) < 30: # shoot connector.blackboard_capsule().set_finished_align() connector.blackboard_capsule().set_priorities(enemy_goal_priority=0, ball_priority=1000, own_goal_priority=0, align_priority=0) say("kick") return self.interrupt() # else (useful comment...) else: say("start") # feste zahl drehen # turn fixed value factor = 0.2 # chose direction if self.sign(goal_angle) == 1: return self.push(PlainWalkAction, [[WalkingCapsule.ZERO, WalkingCapsule.MEDIUM_ANGULAR_LEFT, WalkingCapsule.SLOW_SIDEWARDS_RIGHT, abs(goal_angle) * factor]]) else: return self.push(PlainWalkAction, [[WalkingCapsule.ZERO, WalkingCapsule.MEDIUM_ANGULAR_RIGHT, WalkingCapsule.SLOW_SIDEWARDS_LEFT, abs(goal_angle) * factor]]) # warten abbuf ende der drehung nicht nötig, # da wir dann einfach wieder oben anfangen und neu nach dem tor suchen if False: # should work if goal_angle < -30: say("Kick Ball right") return self.push(KickBall, init_data="SRK") elif goal_angle > 30: say("Kick Ball left") return self.push(KickBall, init_data="SLK") else: if connector.raw_vision_capsule().get_ball_info("v") <= 0: say("Kick Strong") return self.push(KickBall, init_data="RP") else: say("Kick Strong") return self.push(KickBall, init_data="LP") if True: if (goal_distance > 2000 and abs(goal_angle > 30)) or (goal_distance <= 2000 and abs(goal_v) > 1000): if goal_angle < 0: say("Kick Ball right") return self.push(KickBall, init_data="SRK") elif goal_angle >= 0: say("Kick Ball left") return self.push(KickBall, init_data="SLK") else: if connector.raw_vision_capsule().get_ball_info("v") <= 0: say("Kick Strong") return self.push(KickBall, init_data="RP") else: say("Kick Strong") return self.push(KickBall, init_data="LP") else: connector.blackboard_capsule().unset_cant_find_complete_goal() # shoot, to do at least anything connector.blackboard_capsule().set_finished_align() say("Cant find goal") connector.blackboard_capsule().set_priorities(enemy_goal_priority=0, ball_priority=1000, own_goal_priority=0, align_priority=0) if connector.raw_vision_capsule().get_ball_info("v") <= 0: say("Kick Strong") return self.push(KickBall, init_data="RP") else: say("Kick Strong") return self.push(KickBall, init_data="LP")
def update(self, data): if data[DATA_KEY_BUTTON_1] and not data.get(DATA_KEY_MANUAL_PENALTY, False): data[DATA_KEY_ANIMATION] = str(data[DATA_KEY_CONFIG]["animations"]["motion"]["walkready"]) say("Stand up")
def update(self, data): try: packet, addr = self.socket.recvfrom(50) except: # timeout and stuff return out = "" if packet[-1] == "\n": packet = packet[0:-1] if packet == "w": data[DATA_KEY_WALKING_ACTIVE] = True data[DATA_KEY_WALKING_FORWARD] += 1 elif packet == "s": data[DATA_KEY_WALKING_ACTIVE] = True data[DATA_KEY_WALKING_FORWARD] -= 1 elif packet == "a": data[DATA_KEY_WALKING_ACTIVE] = True data[DATA_KEY_WALKING_ANGULAR] += 1 elif packet == "d": data[DATA_KEY_WALKING_ACTIVE] = True data[DATA_KEY_WALKING_ANGULAR] -= 1 elif packet == "g": data[DATA_KEY_WALKING_ACTIVE] = True data[DATA_KEY_WALKING_SIDEWARD] -= 1 elif packet == "f": data[DATA_KEY_WALKING_ACTIVE] = True data[DATA_KEY_WALKING_SIDEWARD] += 1 elif packet == "q": data[DATA_KEY_WALKING_ACTIVE] = True elif packet == "o": data[DATA_KEY_WALKING_ARMS] = not data[DATA_KEY_WALKING_ARMS] elif packet == "i": pose = data[DATA_KEY_IPC].get_pose() try: pose.head_pan.goal = pose.head_pan.position - 10 data[DATA_KEY_IPC].update(pose) except ValueError: out += "Invalid Value for Head Position\n\r" elif packet == "u": pose = data[DATA_KEY_IPC].get_pose() try: pose.head_pan.goal = pose.head_pan.position + 10 data[DATA_KEY_IPC].update(pose) except ValueError: out += "Invalid Value for Head Position\n\r" elif packet == "j": pose = data[DATA_KEY_IPC].get_pose() try: pose.head_tilt.goal = pose.head_tilt.position + 10 data[DATA_KEY_IPC].update(pose) except ValueError: out += "Invalid Value for Head Position\n\r" elif packet == "8": pose = data[DATA_KEY_IPC].get_pose() try: pose.head_tilt.goal = pose.head_tilt.position - 10 data[DATA_KEY_IPC].update(pose) except: out += "Invalid Value for Head Position\n\r" elif packet == ".": data[DATA_KEY_WALKING_HIP_PITCH_IN] = data[DATA_KEY_WALKING_HIP_PITCH_OUT] + 1 out += "Hip Pitch is now " + str(data[DATA_KEY_WALKING_HIP_PITCH_IN]) + "\n\r" elif packet == ",": data[DATA_KEY_WALKING_HIP_PITCH_IN] = data[DATA_KEY_WALKING_HIP_PITCH_OUT] - 1 out += "Hip Pitch is now " + str(data[DATA_KEY_WALKING_HIP_PITCH_IN]) + "\n\r" elif packet == " ": out += "Walking Stopped\n\r" data[DATA_KEY_WALKING_ACTIVE] = False data[DATA_KEY_WALKING_FORWARD] = 0 data[DATA_KEY_WALKING_ANGULAR] = 0 data[DATA_KEY_WALKING_SIDEWARD] = 0 elif len(packet) > 4 and packet[0:4] == "play": data[DATA_KEY_WALKING_ACTIVE] = False data[DATA_KEY_WALKING_FORWARD] = 0 data[DATA_KEY_WALKING_ANGULAR] = 0 data[DATA_KEY_WALKING_SIDEWARD] = 0 data["Animation"] = packet[5:] out += "Play Animation " + packet[5:] + "\n\r" elif len(packet) > 3 and packet[0:3] == "say": say(packet[5:]) if data[DATA_KEY_WALKING_ACTIVE] is True: out += "Walking: True\n\r\tForward: %d\n\r\tSideward: %d\n\r\tAngular: %d\n\r" % ( data[DATA_KEY_WALKING_FORWARD_REAL], data[DATA_KEY_WALKING_SIDEWARD_REAL], data[DATA_KEY_WALKING_ANGULAR_REAL], ) if out != "": self.socket.sendto(out, addr)