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)
