def __init__(self, robot, walker, pose_handler, pose_switcher, cam,
localization):
self.fall_indicator_count = 3
self._robot = robot
self._walker = walker
self._cam = cam
self._localization = localization
self._pose_handler = pose_handler
self._pose_switcher = pose_switcher
self._stance_determinator = StanceDeterminator(robot)
self._iterrupt = False
self._behavior = UnknownBehavior()
self._lock = Lock()
self._worker = Thread(target=BehaviorHandler.__worker, args=(self, ))
self._worker.start()
self.odo = RobotPose(0.0, 0.0, 0.0)
def __init__(self, robot, walker, pose_handler, pose_switcher, cam, localization):
self.fall_indicator_count = 3
self._robot = robot
self._walker = walker
self._cam = cam
self._localization = localization
self._pose_handler = pose_handler
self._pose_switcher = pose_switcher
self._stance_determinator = StanceDeterminator(robot)
self._iterrupt = False
self._behavior = UnknownBehavior()
self._lock = Lock()
self._worker = Thread(target=BehaviorHandler.__worker, args=(self,))
self._worker.start()
self.odo = RobotPose(0.0, 0.0, 0.0)
class BehaviorHandler(OdoListener):
FINDING_SECTOR_ANGLE = math.radians(60.0)
HEAD_PITCH_STEP = math.radians(30.0)
HEAD_YAW_STEP = math.radians(15.0)
SLEEP_TIME = 0.01
def __init__(self, robot, walker, pose_handler, pose_switcher, cam, localization):
self.fall_indicator_count = 3
self._robot = robot
self._walker = walker
self._cam = cam
self._localization = localization
self._pose_handler = pose_handler
self._pose_switcher = pose_switcher
self._stance_determinator = StanceDeterminator(robot)
self._iterrupt = False
self._behavior = UnknownBehavior()
self._lock = Lock()
self._worker = Thread(target=BehaviorHandler.__worker, args=(self,))
self._worker.start()
self.odo = RobotPose(0.0, 0.0, 0.0)
def notify(self, frodo):
self.odo.odoTranslate(frodo[0], frodo[1], frodo[2])
def __worker(self):
self._lock.acquire()
try:
while not self._iterrupt:
behavior = self._behavior
self._lock.release()
try:
behavior.run()
time.sleep(self.SLEEP_TIME)
finally:
self._lock.acquire()
finally:
self._lock.release()
def __set_behavior(self, behavior, *args, **kwargs):
with self._lock:
if not isinstance(self._behavior, behavior):
self._behavior.stop()
self._behavior = behavior(*args, **kwargs)
def run(self):
counter = 0
start = time.time()
left_leg = True
self.__set_behavior(UnknownBehavior)
stance = self._stance_determinator.determinate()
if stance == Stance.STAND:
self._pose_handler.set_pose("walking_pose", 2.0)
timer = 0
ball_found = False
ball = {
"x": 0,
"y": 0,
"width": 0,
"height": 0,
}
timestamp = 0
pix = [0.0, 0.0]
reached = False
dode = 0
fff = False
initialized = False
tuc = None
print "Behavior was started"
while not self._iterrupt:
stance = self._stance_determinator.determinate()
counter = counter + 1 if stance != Stance.STAND else 0
if counter >= self.fall_indicator_count:
dode = 0
self.__set_behavior(StandingUpBehavior, self._robot, self._pose_handler,
self._pose_switcher, self._stance_determinator, self._walker)
if self._behavior.is_done():
self._behavior.reset()
self._behavior.start()
timer = timer + 1
time.sleep(self.SLEEP_TIME)
continue
if any(isinstance(self._behavior, behavior) for behavior in (StandingUpBehavior, KickBehavior,)):
if not self._behavior.is_done():
timer = timer + 1
time.sleep(self.SLEEP_TIME)
continue
else:
if isinstance(self._behavior, StandingUpBehavior):
self._localization.localization(True)
self.__set_behavior(UnknownBehavior)
if timestamp + 24 < timer or pix == [0.0, 0.0, 0.0]:
reached = False
self._robot.vision.updateFrame()
ball = self._robot.vision.ballDetect()
ball_found = (ball["width"] != 0.0)
if ball_found:
timestamp = timer
pix = self._cam.imagePixelToWorld(ball["x"] + ball["width"]/2, ball["y"], False)
# dode = False
# if not initialized:
# print "go to start"
# if tuc is None or math.hypot(tuc[0], tuc[1]) > 100:
# tuc = self._localization.global_to_local(self._localization.map.start_point.x, self._localization.map.start_point.y)
# self.__set_behavior(WalkBehavior, self._walker)
# self._behavior.smart_go_to(tuc[0], tuc[1], 100)
# self._walker.look_at(500.0, 0.0, 0.0)
# time.sleep(2.0)
# timer += 1
# continue
# else:
# initialized = True
if ball_found:
print "FOUND!!!!!!"
if reached or dode > 0:
fff = False
bdone = self._behavior.is_done()
if ball_found:
if pix[0] > 0.0:
self._walker.look_at(pix[0], pix[1])
if dode == 0:
enemy_point = self._localization.map.enemy_point
gates = self._localization.global_to_local(enemy_point.x, enemy_point.y)
self.__set_behavior(WalkBehavior, self._walker)
if gates[2] > math.radians(30):
self._behavior.go_around(gates[2])
dode = 1
bdone = False
time.sleep(0.2)
else:
dode = 1
bdone = True
if dode == 1 and bdone:
self.__set_behavior(KickBehavior, self._robot, self._pose_handler,
self._pose_switcher, self._stance_determinator, self._walker)
self._behavior.set_left_leg(pix[1] > 0)
aa = self._robot.locomotion.autoapply.enable(False)
self._behavior.start()
dode = 2
bdone = False
time.sleep(0.2)
if dode == 2 and bdone:
dode = 0
time.sleep(0.5)
if dode == 0:
if timestamp == 0 or timestamp + 54 < timer or pix == [0.0, 0.0, 0.0] or pix[0] < 0.0:
self.__set_behavior(WalkBehavior, self._walker)
if not ball_found:
if fff == False:
h_angle = 0.0
low = True
to_left = True
fff = True
print "h_angle", math.degrees(h_angle), to_left, low,
if to_left:
if h_angle == -self.FINDING_SECTOR_ANGLE / 2.0:
low = True
to_left = False
h_angle += self.HEAD_YAW_STEP
else:
h_angle -= self.HEAD_YAW_STEP
else:
if h_angle == self.FINDING_SECTOR_ANGLE / 2.0:
low = False
to_left = True
h_angle -= self.HEAD_YAW_STEP
else:
h_angle += self.HEAD_YAW_STEP
h_y = 0.0
h_x = 300.0 if low else 1000.0
c = math.cos(h_angle)
s = math.sin(h_angle)
h_x, h_y = h_x * c - h_y * s, h_x * s + h_y * c
print "h x, y", h_x, h_y
self._walker.look_at(h_x, h_y, 0.0)
else:
print "!!!"
if pix[0] > 0.0:
self._walker.look_at(pix[0], pix[1])
self._behavior.go_around(math.pi, 0.5)
elif math.hypot(pix[0], pix[1]) > 350.0:
fff = False
# print pix
if pix[0] > 0.0:
self._walker.look_at(pix[0], pix[1])
enemy_point = self._localization.map.enemy_point
gates = self._localization.global_to_local(enemy_point.x, enemy_point.y)
dx = pix[0] - gates[0]
dy = pix[1] - gates[1]
angle = math.atan2(dy, dx)
distance = math.hypot(dx, dy)
new_dist = distance + 180
target = (pix[0] + math.cos(angle) * (new_dist) - dx, pix[1] + math.sin(angle) * (new_dist) - dy)
print "target", target
self.__set_behavior(WalkBehavior, self._walker)
print repr(self._behavior)
self._behavior.smart_go_to(target[0], target[1], 100)
elif pix != [0.0, 0.0, 0,0] and pix[0] > 0.0:
fff = False
reached = True
timer = timer + 1
time.sleep(self.SLEEP_TIME)
# elif t == 20.0:
# self.__set_behavior(KickBehavior, self._robot, self._pose_handler,
# self._pose_switcher, self._stance_determinator, self._walker)
# self._behavior.set_left_leg(left_leg)
# left_leg = not left_leg
def stop(self):
self._iterrupt = True
self._worker.join()
class BehaviorHandler(OdoListener):
FINDING_SECTOR_ANGLE = math.radians(60.0)
HEAD_PITCH_STEP = math.radians(30.0)
HEAD_YAW_STEP = math.radians(15.0)
SLEEP_TIME = 0.01
def __init__(self, robot, walker, pose_handler, pose_switcher, cam,
localization):
self.fall_indicator_count = 3
self._robot = robot
self._walker = walker
self._cam = cam
self._localization = localization
self._pose_handler = pose_handler
self._pose_switcher = pose_switcher
self._stance_determinator = StanceDeterminator(robot)
self._iterrupt = False
self._behavior = UnknownBehavior()
self._lock = Lock()
self._worker = Thread(target=BehaviorHandler.__worker, args=(self, ))
self._worker.start()
self.odo = RobotPose(0.0, 0.0, 0.0)
def notify(self, frodo):
self.odo.odoTranslate(frodo[0], frodo[1], frodo[2])
def __worker(self):
self._lock.acquire()
try:
while not self._iterrupt:
behavior = self._behavior
self._lock.release()
try:
behavior.run()
time.sleep(self.SLEEP_TIME)
finally:
self._lock.acquire()
finally:
self._lock.release()
def __set_behavior(self, behavior, *args, **kwargs):
with self._lock:
if not isinstance(self._behavior, behavior):
self._behavior.stop()
self._behavior = behavior(*args, **kwargs)
def run(self):
counter = 0
start = time.time()
left_leg = True
self.__set_behavior(UnknownBehavior)
stance = self._stance_determinator.determinate()
if stance == Stance.STAND:
self._pose_handler.set_pose("walking_pose", 2.0)
timer = 0
ball_found = False
ball = {
"x": 0,
"y": 0,
"width": 0,
"height": 0,
}
timestamp = 0
pix = [0.0, 0.0]
reached = False
dode = 0
fff = False
initialized = False
tuc = None
print "Behavior was started"
while not self._iterrupt:
stance = self._stance_determinator.determinate()
counter = counter + 1 if stance != Stance.STAND else 0
if counter >= self.fall_indicator_count:
dode = 0
self.__set_behavior(StandingUpBehavior, self._robot,
self._pose_handler, self._pose_switcher,
self._stance_determinator, self._walker)
if self._behavior.is_done():
self._behavior.reset()
self._behavior.start()
timer = timer + 1
time.sleep(self.SLEEP_TIME)
continue
if any(
isinstance(self._behavior, behavior) for behavior in (
StandingUpBehavior,
KickBehavior,
)):
if not self._behavior.is_done():
timer = timer + 1
time.sleep(self.SLEEP_TIME)
continue
else:
if isinstance(self._behavior, StandingUpBehavior):
self._localization.localization(True)
self.__set_behavior(UnknownBehavior)
if timestamp + 24 < timer or pix == [0.0, 0.0, 0.0]:
reached = False
self._robot.vision.updateFrame()
ball = self._robot.vision.ballDetect()
ball_found = (ball["width"] != 0.0)
if ball_found:
timestamp = timer
pix = self._cam.imagePixelToWorld(
ball["x"] + ball["width"] / 2, ball["y"], False)
# dode = False
# if not initialized:
# print "go to start"
# if tuc is None or math.hypot(tuc[0], tuc[1]) > 100:
# tuc = self._localization.global_to_local(self._localization.map.start_point.x, self._localization.map.start_point.y)
# self.__set_behavior(WalkBehavior, self._walker)
# self._behavior.smart_go_to(tuc[0], tuc[1], 100)
# self._walker.look_at(500.0, 0.0, 0.0)
# time.sleep(2.0)
# timer += 1
# continue
# else:
# initialized = True
if ball_found:
print "FOUND!!!!!!"
if reached or dode > 0:
fff = False
bdone = self._behavior.is_done()
if ball_found:
if pix[0] > 0.0:
self._walker.look_at(pix[0], pix[1])
if dode == 0:
enemy_point = self._localization.map.enemy_point
gates = self._localization.global_to_local(
enemy_point.x, enemy_point.y)
self.__set_behavior(WalkBehavior, self._walker)
if gates[2] > math.radians(30):
self._behavior.go_around(gates[2])
dode = 1
bdone = False
time.sleep(0.2)
else:
dode = 1
bdone = True
if dode == 1 and bdone:
self.__set_behavior(KickBehavior, self._robot,
self._pose_handler,
self._pose_switcher,
self._stance_determinator,
self._walker)
self._behavior.set_left_leg(pix[1] > 0)
aa = self._robot.locomotion.autoapply.enable(False)
self._behavior.start()
dode = 2
bdone = False
time.sleep(0.2)
if dode == 2 and bdone:
dode = 0
time.sleep(0.5)
if dode == 0:
if timestamp == 0 or timestamp + 54 < timer or pix == [
0.0, 0.0, 0.0
] or pix[0] < 0.0:
self.__set_behavior(WalkBehavior, self._walker)
if not ball_found:
if fff == False:
h_angle = 0.0
low = True
to_left = True
fff = True
print "h_angle", math.degrees(h_angle), to_left, low,
if to_left:
if h_angle == -self.FINDING_SECTOR_ANGLE / 2.0:
low = True
to_left = False
h_angle += self.HEAD_YAW_STEP
else:
h_angle -= self.HEAD_YAW_STEP
else:
if h_angle == self.FINDING_SECTOR_ANGLE / 2.0:
low = False
to_left = True
h_angle -= self.HEAD_YAW_STEP
else:
h_angle += self.HEAD_YAW_STEP
h_y = 0.0
h_x = 300.0 if low else 1000.0
c = math.cos(h_angle)
s = math.sin(h_angle)
h_x, h_y = h_x * c - h_y * s, h_x * s + h_y * c
print "h x, y", h_x, h_y
self._walker.look_at(h_x, h_y, 0.0)
else:
print "!!!"
if pix[0] > 0.0:
self._walker.look_at(pix[0], pix[1])
self._behavior.go_around(math.pi, 0.5)
elif math.hypot(pix[0], pix[1]) > 350.0:
fff = False
# print pix
if pix[0] > 0.0:
self._walker.look_at(pix[0], pix[1])
enemy_point = self._localization.map.enemy_point
gates = self._localization.global_to_local(
enemy_point.x, enemy_point.y)
dx = pix[0] - gates[0]
dy = pix[1] - gates[1]
angle = math.atan2(dy, dx)
distance = math.hypot(dx, dy)
new_dist = distance + 180
target = (pix[0] + math.cos(angle) * (new_dist) - dx,
pix[1] + math.sin(angle) * (new_dist) - dy)
print "target", target
self.__set_behavior(WalkBehavior, self._walker)
print repr(self._behavior)
self._behavior.smart_go_to(target[0], target[1], 100)
elif pix != [0.0, 0.0, 0, 0] and pix[0] > 0.0:
fff = False
reached = True
timer = timer + 1
time.sleep(self.SLEEP_TIME)
# elif t == 20.0:
# self.__set_behavior(KickBehavior, self._robot, self._pose_handler,
# self._pose_switcher, self._stance_determinator, self._walker)
# self._behavior.set_left_leg(left_leg)
# left_leg = not left_leg
def stop(self):
self._iterrupt = True
self._worker.join()
