def on_received_get_commands(self, context: Context, data: GetCommands):
# context.info(f'on_received_get_commands')
if not self.agent.initialized:
pwm_left, pwm_right = [0, 0]
else:
# TODO: let's use a queue here. Performance suffers otherwise.
# What you should do is: *get the last command*, if available
# otherwise, wait for one command.
t0 = time.time()
while not self.agent.updated:
dt = time.time() - t0
if dt > 2.0:
context.info(f"agent not ready since {dt:.1f} s")
time.sleep(0.5)
if dt > 60:
msg = "I have been waiting for commands from the ROS part" f" since {int(dt)} s"
context.error(msg)
raise Exception(msg)
time.sleep(0.02)
dt = time.time() - t0
if dt > 2.0:
context.info(f"obtained agent commands after {dt:.1f} s")
time.sleep(0.2)
pwm_left, pwm_right = self.agent.action
self.agent.updated = False
grey = RGB(0.5, 0.5, 0.5)
led_commands = LEDSCommands(grey, grey, grey, grey, grey)
pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right)
commands = DB20Commands(pwm_commands, led_commands)
context.write("commands", commands)
def on_received_get_commands(self, context: Context, data: GetCommands):
self.n += 1
# behavior = 0 # random trajectory
behavior = 1 # primary motions
if behavior == 0:
pwm_left = np.random.uniform(0.5, 1.0)
pwm_right = np.random.uniform(0.5, 1.0)
col = RGB(0.0, 1.0, 1.0)
elif behavior == 1:
t = data.at_time
d = 1.0
phases = [
(+1, -1, RGB(1.0, 0.0, 0.0)),
(-1, +1, RGB(0.0, 1.0, 0.0)),
(+1, +1, RGB(0.0, 0.0, 1.0)),
(-1, -1, RGB(1.0, 1.0, 0.0)),
]
phase = int(t / d) % len(phases)
pwm_right, pwm_left, col = phases[phase]
else:
raise ValueError(behavior)
led_commands = LEDSCommands(col, col, col, col, col)
pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right)
commands = DB20Commands(pwm_commands, led_commands)
context.write("commands", commands)
def on_received_get_commands(self, context: Context, data: GetCommands):
if self.n == 0:
pwm_left = 0.0
pwm_right = 0.0
else:
pwm_left = 0.1
pwm_right = 0.1
self.n += 1
t = data.at_time
d = 1.0
phase = int(t / d) % 4
# if phase == 0:
# pwm_right = +1
# pwm_left = -1
# elif phase == 1:
# pwm_right = +1
# pwm_left = +1
# elif phase == 2:
# pwm_right = -1
# pwm_left = +1
# elif phase == 3:
# pwm_right = -1
# pwm_left = -1
# pwm_left = 1.0
# pwm_right = 1.0
col = RGB(0.0, 0.0, 1.0)
led_commands = LEDSCommands(col, col, col, col, col)
pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right)
commands = DB20Commands(pwm_commands, led_commands)
context.write("commands", commands)
def on_received_get_commands(self, context: Context):
pwm_left, pwm_right = self.compute_action(self.current_image)
pwm_left = float(np.clip(pwm_left, -1, +1))
pwm_right = float(np.clip(pwm_right, -1, +1))
grey = RGB(0.0, 0.0, 0.0)
led_commands = LEDSCommands(grey, grey, grey, grey, grey)
pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right)
commands = DB20Commands(pwm_commands, led_commands)
context.write('commands', commands)
def on_received_get_commands(self, context: Context):
velocity, omega = self.compute_action(self.current_image)
# multiplying steering angle by a gain
omega *= 7
pwm_left, pwm_right = self.wrapper.convert(velocity, omega)
grey = RGB(0.0, 0.0, 0.0)
led_commands = LEDSCommands(grey, grey, grey, grey, grey)
pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right)
commands = DB20Commands(pwm_commands, led_commands)
context.write('commands', commands)
def on_received_get_commands(self, context: Context, data: GetCommands):
pwm_left, pwm_right = self.compute_commands()
col = RGB(0.0, 0.0, 1.0)
col_left = RGB(pwm_left, pwm_left, 0.0)
col_right = RGB(pwm_right, pwm_right, 0.0)
led_commands = LEDSCommands(col, col_left, col_right, col_left,
col_right)
pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right)
commands = DB20Commands(pwm_commands, led_commands)
context.write("commands", commands)
def on_received_get_commands(self, context: Context):
linear, angular = self.compute_action(self.to_predictor)
# ! Inverse Kinematics
pwm_left, pwm_right = self.convertion_wrapper.convert(linear, angular)
pwm_left = float(np.clip(pwm_left, -1, +1))
pwm_right = float(np.clip(pwm_right, -1, +1))
# ! LED Commands Sherrif Duck
grey = RGB(0.0, 0.0, 0.0)
red = RGB(1.0, 0.0, 0.0)
blue = RGB(0.0, 0.0, 1.0)
led_commands = LEDSCommands(red, grey, blue, red, blue)
# ! Send PWM Command
pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right)
commands = DB20Commands(pwm_commands, led_commands)
context.write('commands', commands)
def on_received_get_commands(self, context: Context, data: GetCommands):
if not self.agent.initialized:
pwm_left, pwm_right = [0, 0]
else:
# TODO: let's use a queue here. Performance suffers otherwise.
# What you should do is: *get the last command*, if available
# otherwise, wait for one command.
while not self.agent.updated:
time.sleep(0.01)
pwm_left, pwm_right = self.agent.action
self.agent.updated = False
grey = RGB(0.5, 0.5, 0.5)
led_commands = LEDSCommands(grey, grey, grey, grey, grey)
pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right)
commands = DB20Commands(pwm_commands, led_commands)
context.write("commands", commands)
def on_received_get_commands(self, context: Context):
"""
Converts requested linear/angular velocities to control commands
and issues them to the robot.
Don't change this, standard stuff from the submission template.
"""
linear, angular = self.compute_action(self.to_predictor)
# Inverse kinematics
pwm_left, pwm_right = self.convertion_wrapper.convert(linear, angular)
pwm_left = float(np.clip(pwm_left, -1, +1))
pwm_right = float(np.clip(pwm_right, -1, +1))
# LED commands
grey = RGB(0.0, 0.0, 0.0)
red = RGB(1.0, 0.0, 0.0)
blue = RGB(0.0, 0.0, 1.0)
led_commands = LEDSCommands(red, grey, blue, red, blue)
# Send PWM command
pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right)
commands = DB20Commands(pwm_commands, led_commands)
context.write('commands', commands)