def on_received_get_robot_state(self, context: Context, data: RobotName):
env = self.env
speed = env.speed
omega = 0.0 # XXX
if data == self.robot_name:
q = env.cartesian_from_weird(env.cur_pos, env.cur_angle)
v = geometry.se2_from_linear_angular([speed, 0], omega)
state = MyRobotInfo(pose=q,
velocity=v,
last_action=env.last_action,
wheels_velocities=env.wheelVels)
rs = MyRobotState(robot_name=data,
t_effective=self.current_time,
state=state)
else:
obj: DuckiebotObj = self.npcs[data]
q = env.cartesian_from_weird(obj.pos, obj.angle)
# FIXME: how to get velocity?
v = geometry.se2_from_linear_angular([0, 0], 0)
state = MyRobotInfo(pose=q,
velocity=v,
last_action=np.array([0, 0]),
wheels_velocities=np.array([0, 0]))
rs = MyRobotState(robot_name=data,
t_effective=self.current_time,
state=state)
# timing information
t = timestamp_from_seconds(self.current_time)
ts = TimeSpec(time=t,
frame=self.episode_name,
clock=context.get_hostname())
timing = TimingInfo(acquired={'state': ts})
context.write('robot_state', rs, timing=timing) #, with_schema=True)
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_sim_state(self, context: Context):
d = self.env._compute_done_reward()
done = d.done
done_why = d.done_why
done_code = d.done_code
sim_state = SimulationState(done, done_why, done_code)
context.write('sim_state', sim_state)
def on_received_get_commands(self, context: Context):
linear, angular = self.compute_action(
self.to_predictor) # * Changed to custom size
#0.6 1.5
linear = linear
angular = angular
#! Inverse Kinematics
pwm_left, pwm_right = 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(255.0, 0.0, 0.0)
blue = RGB(0.0, 0.0, 255.0)
led_commands = LEDSCommands(red, grey, blue, red, blue)
# if (self.led_counter < 30):
# led_commands = LEDSCommands(grey, red, blue, red, blue)
# self.led_counter += 1
# elif (self.led_counter >= 60):
# self.led_counter = 0
# led_commands = LEDSCommands(grey, red, blue, red, blue)
# elif(self.led_counter > 30):
# led_commands = LEDSCommands(blue, red, grey, blue, red)
# self.led_counter += 1
#! Do not modify here!
pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right)
commands = Duckiebot1Commands(pwm_commands, led_commands)
context.write('commands', commands)
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):
pwm_left, pwm_right = self.compute_action(self.current_image)
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 = Duckiebot1Commands(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 _start_episode(self, context: Context):
if self.state.episode >= self.config.num_episodes:
context.write("no_more_episodes", None)
return
self.state.episode += 1
self.state.nimages = 0
self.state.episode_name = f"episode{self.state.episode}"
context.write("episode_start", EpisodeStart(self.state.episode_name))
def on_received_get_commands(self, context: Context):
l, u = self.config.pwm_left_interval
pwm_left = np.random.uniform(l, u)
l, u = self.config.pwm_right_interval
pwm_right = np.random.uniform(l, u)
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 = Duckiebot1Commands(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_robot_observations(self, context: Context):
# timing information
t = timestamp_from_seconds(self.last_observations_time)
ts = TimeSpec(time=t,
frame=self.episode_name,
clock=context.get_hostname())
timing = TimingInfo(acquired={'image': ts})
context.write('robot_observations',
self.ro,
with_schema=True,
timing=timing)
def on_received_get_commands(self, context: Context):
l, u = self.config.pwm_left_interval
# pwm_left = np.random.uniform(l, u)
l, u = self.config.pwm_right_interval
# pwm_right = np.random.uniform(l, u)
pwm_left, pwm_right = self.compute_action(self.config.current_image)
grey = RGB(0.0, 0.0, 0.0)
led_commands = LEDSCommands(grey, grey, grey, grey, grey)
pwm_commands = PWMCommands(motor_left=float(pwm_left),
motor_right=float(pwm_right))
commands = Duckiebot1Commands(pwm_commands, led_commands)
context.write('commands', commands)
def on_received_get_robot_performance(self, context: Context,
data: RobotName):
metrics = {}
metrics['survival_time'] = Metric(higher_is_better=True,
cumulative_value=self.current_time,
description="Survival time.")
metrics['reward'] = Metric(higher_is_better=True,
cumulative_value=self.reward_cumulative,
description="Cumulative reward.")
pm = PerformanceMetrics(metrics)
rid = RobotPerformance(robot_name=data,
t_effective=self.current_time,
performance=pm)
context.write('robot_performance', rid)
def on_received_get_robot_performance(self, context: Context,
data: RobotName):
context.info(f"get_robot_interface_description()")
metrics = {}
metrics["reward"] = Metric(
higher_is_better=True,
cumulative_value=self.current_time,
description="Dummy reward equal to survival time.",
)
pm = PerformanceMetrics(metrics)
rid = RobotPerformance(robot_name=data,
t_effective=self.current_time,
performance=pm)
context.write("robot_performance", rid)
def on_received_query(self, context: Context, data: PlanningQuery):
# A planning query is a pair of initial and goal poses
start: FriendlyPose = data.start
goal: FriendlyPose = data.target
# You start at the start pose. You must reach the goal with a tolerance given by
# tolerance_xy_m and tolerance_theta_deg.
# You need to declare if it is feasible or not
feasible = True
if not feasible:
# If it's not feasible, just return this.
result: PlanningResult = PlanningResult(False, None)
context.write("response", result)
return
# If it is feasible you need to provide a plan.
# A plan is a list of PlanStep
plan: List[PlanStep] = []
# A plan step consists in a duration, a linear and angular velocity.
# Let's trace a square of side L at maximum velocity.
L = 1.0
duration_straight_m_s = L / self.params.max_linear_velocity_m_s
duration_turn_deg_s = 90.0 / self.params.max_angular_velocity_deg_s
# The plan will be: straight, turn, straight, turn, straight, turn, straight, turn
straight = PlanStep(duration=duration_straight_m_s,
angular_velocity_deg_s=0.0,
velocity_x_m_s=self.params.max_linear_velocity_m_s)
turn = PlanStep(
duration=duration_turn_deg_s,
angular_velocity_deg_s=self.params.max_angular_velocity_deg_s,
velocity_x_m_s=0.0)
plan.append(straight)
plan.append(turn)
plan.append(straight)
plan.append(turn)
plan.append(straight)
plan.append(turn)
plan.append(straight)
plan.append(turn)
result: PlanningResult = PlanningResult(feasible, plan)
context.write("response", result)
def on_received_get_commands(self, context: Context):
if self.n == 0:
pwm_left = 0.0
pwm_right = 0.0
else:
pwm_left = np.random.uniform(0.5, 1.0)
pwm_right = np.random.uniform(0.5, 1.0)
self.n += 1
# pwm_left = 1.0
# pwm_right = 1.0
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 = Duckiebot1Commands(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_next_image(self, context: Context):
if self.state.nimages >= self.config.images_per_episode:
context.write("no_more_images", None)
return
H, W = self.config.shape
values = (128 + np.random.randn(H, W, 3) * 60).astype("uint8")
jpg_data = bgr2jpg(values)
image = JPGImage(jpg_data)
delta = 0.15
t = self.state.nimages * delta
time = timestamp_from_seconds(t)
ts = TimeSpec(time=time,
frame=self.state.episode_name,
clock=context.get_hostname())
acquired = {"image": ts}
timing = TimingInfo(acquired=acquired)
context.write(topic="image", data=image, timing=timing)
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 = Duckiebot1Commands(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)
def on_received_next_scenario(self, context: Context):
if self.state.scenarios_to_go:
scenario = self.state.scenarios_to_go.pop(0)
context.write('scenario', scenario)
else:
context.write('finished', None)
def on_received_get_robot_state(self, context: Context, data: RobotName):
context.info(f"get_robot_state({data!r})")
rs = RobotState(robot_name=data,
t_effective=self.current_time,
state=None)
context.write("robot_state", rs)
def on_received_dump_state(self, context: Context):
context.write('dump_state', StateDump(None))
def on_received_episode_start(self, context: Context, data: EpisodeStart):
context.write("episode_start", data=data)
def on_received_get_robot_interface_description(self, context: Context,
data: RobotName):
rid = RobotInterfaceDescription(robot_name=data,
observations=JPGImage,
commands=PWMCommands)
context.write('robot_interface_description', rid)
def on_received_get_robot_interface_description(self, context: Context,
data: RobotName):
rid = RobotInterfaceDescription(robot_name=data,
observations=Duckiebot1Observations,
commands=Duckiebot1Commands)
context.write("robot_interface_description", rid)
def on_received_query(self, context: Context, data: CollisionCheckQuery):
collided = check_collision(
Wcoll=self.params.environment, robot_body=self.params.body, robot_pose=data.pose
)
result = CollisionCheckResult(collided)
context.write("response", result)
def on_received_image(self, context: Context, data: JPGImage):
transformed = data
context.write("image", transformed)
