def on_received_set_params(self, context: Context, data: PlanningSetup):
context.info("initialized")
self.params = data
# This is the interval of allowed linear velocity
# Note that min_velocity_x_m_s and max_velocity_x_m_s might be different.
# Note that min_velocity_x_m_s may be 0 in advanced exercises (cannot go backward)
max_velocity_x_m_s: float = self.params.max_linear_velocity_m_s
min_velocity_x_m_s: float = self.params.min_linear_velocity_m_s
# This is the max curvature. In earlier exercises, this is +inf: you can turn in place.
# In advanced exercises, this is less than infinity: you cannot turn in place.
max_curvature: float = self.params.max_curvature
# these have the same meaning as the collision exercises
body: List[PlacedPrimitive] = self.params.body
environment: List[PlacedPrimitive] = self.params.environment
# these are the final tolerances - the precision at which you need to arrive at the goal
tolerance_theta_deg: float = self.params.tolerance_theta_deg
tolerance_xy_m: float = self.params.tolerance_xy_m
# For convenience, this is the rectangle that contains all the available environment,
# so you don't need to compute it
bounds: Rectangle = self.params.bounds
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 init(self, context: Context):
context.info("Check GPU...")
self.check_gpu_available(context)
# Model predicts linear and angular velocity but we need to issue
# wheel velocity commands to robot, this wrapper converts to the latter.
self.convertion_wrapper = SteeringToWheelVelWrapper()
context.info('init()')
model = Modelv1()
model.load_state_dict(
torch.load(
"modelv1_maserati_bill_simulated_amadobot_base_epoch_200.pth",
# "modelv1_maserati_bill_simulated_amadobot_base.pth",
# 'modelv1_maserati_plus_simulated.pth',
map_location=self.device,
))
model.eval()
self.model = model.to(self.device)
self.current_image = np.zeros(self.expect_shape)
self.input_image = np.zeros((150, 200, 3))
self.to_predictor = np.expand_dims(self.input_image, axis=0)
def update_observations(self, blur_time: float, context: Context):
context.info(f'update_observations() at {self.current_time}')
assert self.render_observations
to_average = []
n = len(self.render_observations)
# context.info(str(self.render_timestamps))
# context.info(f'now {self.current_time}')
for i in range(n):
ti = self.render_timestamps[i]
if math.fabs(self.current_time - ti) < blur_time:
to_average.append(self.render_observations[i])
obs0 = to_average[0].astype('float32')
context.info(str(obs0.shape))
for obs in to_average[1:]:
obs0 += obs
obs = obs0 / len(to_average)
obs = obs.astype('uint8')
# context.info(f'update {obs.shape} {obs.dtype}')
jpg_data = rgb2jpg(obs)
camera = JPGImage(jpg_data)
obs = Duckiebot1Observations(camera)
self.ro = DB18RobotObservations(self.robot_name, self.current_time,
obs)
self.last_observations_time = self.current_time
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_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 init(self, context: Context):
context.info("Check GPU...")
self.check_gpu_available(context)
# Model predicts linear and angular velocity but we need to issue
# wheel velocity commands to robot, this wrapper converts to the latter.
self.convertion_wrapper = SteeringToWheelVelWrapper()
context.info('init()')
model_lin = NvidiaModel()
model_lin.load_state_dict(
torch.load(
'saved_nvidia_model_lin.pth',
map_location=self.device,
))
model_lin.eval()
self.model_lin = model_lin.to(self.device)
model_ang = NvidiaModel()
model_ang.load_state_dict(
torch.load(
'saved_nvidia_model_ang.pth',
map_location=self.device,
))
model_ang.eval()
self.model_ang = model_ang.to(self.device)
self.current_image = np.zeros(self.expect_shape)
self.input_image = np.zeros((150, 200, 3))
self.to_predictor = np.expand_dims(self.input_image, axis=0)
def check_tensorflow_gpu(self, context: Context):
import tensorflow as tf
name = tf.test.gpu_device_name()
context.info(f"gpu_device_name: {name!r} ")
if not name: # None or ''
no_hardware_GPU_available(context)
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 init(self, context: Context):
print(time.time(), 'init')
context.info('init()')
frozen_model_filename = "frozen_graph.pb"
# We use our "load_graph" function to load the graph
self.graph = load_graph(frozen_model_filename)
self.session = tf.Session(graph=self.graph)
def init(self, context: Context):
context.info("init()")
self.rgb = None
self.l_max = -math.inf
self.r_max = -math.inf
self.l_min = math.inf
self.r_min = math.inf
self.left = None
self.right = None
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_observations(self, context: Context,
data: DB20ObservationsWithTimestamp):
profiler = context.get_profiler()
camera: JPGImageWithTimestamp = data.camera
odometry: DB20OdometryWithTimestamp = data.odometry
context.info(f"camera timestamp: {camera.timestamp}")
context.info(f"odometry timestamp: {odometry.timestamp}")
with profiler.prof("jpg2rgb"):
_rgb = jpg2rgb(camera.jpg_data)
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_set_robot_commands(self, data: DB18SetRobotCommands,
context: Context):
l, r = data.commands.wheels.motor_left, data.commands.wheels.motor_right
if max(math.fabs(l), math.fabs(r)) > 1:
msg = f'Received invalid PWM commands. They should be between -1 and +1.' \
f' Received left = {l!r}, right = {r!r}.'
context.error(msg)
raise Exception(msg)
self.last_commands = data.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_step(self, context: Context, data: Step):
delta_time = data.until - self.current_time
if delta_time > 0:
self.update_physics_and_observations(until=data.until,
context=context)
else:
context.warning(f'Already at time {data.until}')
d = self.env._compute_done_reward()
self.reward_cumulative += d.reward * delta_time
self.current_time = data.until
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)
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 init(self, context: Context):
context.info('init()')
self.image_processor = DTPytorchWrapper()
self.action_processor = ActionWrapper(FakeWrap())
from model import DDPG
self.check_gpu_available(context)
self.model = DDPG(state_dim=self.image_processor.shape, action_dim=2, max_action=1, net_type="cnn")
self.current_image = np.zeros((640, 480, 3))
self.model.load("model", directory="./models")
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_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_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_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_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 init(self, context: Context):
context.info("Check GPU...")
limit_gpu_memory()
test_payload()
self.check_tensorflow_gpu()
from cbcNet import cbcNet
from helperFncs import SteeringToWheelVelWrapper
self.convertion_wrapper = SteeringToWheelVelWrapper()
context.info('init()')
self.model = cbcNet.get_inference("cbcNet.h5")
self.current_image = np.zeros(self.expect_shape)
self.input_image = np.zeros((150, 200, 3))
self.to_predictor = np.expand_dims(self.input_image, axis=0)
def init(self, context: Context):
context.info("Check GPU...")
limit_gpu_memory()
self.check_tensorflow_gpu()
from frankModel import FrankNet
from helperFncs import SteeringToWheelVelWrapper
self.convertion_wrapper = SteeringToWheelVelWrapper()
context.info('init()')
self.model = FrankNet.build(200, 150)
self.model.load_weights("FrankNet.h5")
self.current_image = np.zeros(self.expect_shape)
self.input_image = np.zeros((150, 200, 3))
self.to_predictor = np.expand_dims(self.input_image, axis=0)
