in CARLA_0.9.6/PythonAPI/carla/agents/navigation/carla_env.py [0:0]
def _simulator_step(self, action, dt=0.05):
if self.render_display:
if should_quit():
return
self.clock.tick()
if action is not None:
steer = float(action[0])
throttle_brake = float(action[1])
if throttle_brake >= 0.0:
throttle = throttle_brake
brake = 0.0
else:
throttle = 0.0
brake = -throttle_brake
assert 0.0 <= throttle <= 1.0
assert -1.0 <= steer <= 1.0
assert 0.0 <= brake <= 1.0
vehicle_control = carla.VehicleControl(
throttle=throttle,
steer=steer,
brake=brake,
hand_brake=False,
reverse=False,
manual_gear_shift=False
)
self.vehicle.apply_control(vehicle_control)
else:
throttle, steer, brake = 0., 0., 0.
# Advance the simulation and wait for the data.
if self.render_display:
snapshot, image_rgb, image_rl, image_rl_left, image_rl_lefter, image_rl_right, image_rl_righter = self.sync_mode.tick(timeout=2.0)
else:
snapshot, image_rl, image_rl_left, image_rl_lefter, image_rl_right, image_rl_righter = self.sync_mode.tick(timeout=2.0)
info = {}
info['reason_episode_ended'] = ''
dist_from_center, vel_s, speed, done, info = self.dist_from_center_lane(self.vehicle, info)
collision_intensities_during_last_time_step = sum(self._collision_intensities_during_last_time_step)
self._collision_intensities_during_last_time_step.clear() # clear it ready for next time step
assert collision_intensities_during_last_time_step >= 0.
collision_cost = 0.0001 * collision_intensities_during_last_time_step
vel_t = math.sqrt(speed**2 - vel_s**2)
reward = vel_s * dt - collision_cost - abs(steer) # doesn't work if 0.001 cost collisions
info['crash_intensity'] = collision_intensities_during_last_time_step
info['steer'] = steer
info['brake'] = brake
info['distance'] = vel_s * dt
self.dist_s += vel_s * dt
self.return_ += reward
self.weather.tick()
# Draw the display.
if self.render_display:
draw_image(self.display, image_rgb)
if self.display_text:
self.display.blit(self.font.render('frame %d' % self.count, True, (255, 255, 255)), (8, 10))
self.display.blit(self.font.render('highway progression %4.1f m/s (%5.1f m) (%5.2f speed)' % (vel_s, self.dist_s, speed), True, (255, 255, 255)), (8, 28))
self.display.blit(self.font.render('%5.2f meters off center' % dist_from_center, True, (255, 255, 255)), (8, 46))
self.display.blit(self.font.render('%5.2f reward (return %.2f)' % (reward, self.return_), True, (255, 255, 255)), (8, 64))
self.display.blit(self.font.render('%5.2f collision intensity ' % collision_intensities_during_last_time_step, True, (255, 255, 255)), (8, 82))
self.display.blit(self.font.render('%5.2f thottle, %3.2f steer, %3.2f brake' % (throttle, steer, brake), True, (255, 255, 255)), (8, 100))
self.display.blit(self.font.render(str(self.weather), True, (255, 255, 255)), (8, 118))
pygame.display.flip()
rgbs = []
if self.num_cameras == 1:
ims = [image_rl]
elif self.num_cameras == 3:
ims = [image_rl_left, image_rl, image_rl_right]
elif self.num_cameras == 5:
ims = [image_rl_lefter, image_rl_left, image_rl, image_rl_right, image_rl_righter]
else:
raise ValueError("num cameras must be 1 or 3 or 5")
for im in ims:
bgra = np.array(im.raw_data).reshape(self.rl_image_size, self.rl_image_size, 4) # BGRA format
bgr = bgra[:, :, :3] # BGR format (84 x 84 x 3)
rgb = np.flip(bgr, axis=2) # RGB format (84 x 84 x 3)
rgbs.append(rgb)
rgb = np.concatenate(rgbs, axis=1) # (84 x 252 x 3)
# Rowan added
if self.render_display and self.save_display_images:
image_name = os.path.join(self.image_dir, "display%08d.jpg" % self.count)
pygame.image.save(self.display, image_name)
# ffmpeg -r 20 -pattern_type glob -i 'display*.jpg' carla.mp4
if self.save_rl_images:
image_name = os.path.join(self.image_dir, "rl%08d.png" % self.count)
im = Image.fromarray(rgb)
metadata = PngInfo()
metadata.add_text("throttle", str(throttle))
metadata.add_text("steer", str(steer))
metadata.add_text("brake", str(brake))
im.save(image_name, "PNG", pnginfo=metadata)
# # Example usage:
# from PIL.PngImagePlugin import PngImageFile
# im = PngImageFile("rl00001234.png")
# # Actions are stored in the image's metadata:
# print("Actions: %s" % im.text)
# throttle = float(im.text['throttle']) # range [0, 1]
# steer = float(im.text['steer']) # range [-1, 1]
# brake = float(im.text['brake']) # range [0, 1]
self.count += 1
next_obs = rgb # (84 x 252 x 3) or (84 x 420 x 3)
# debugging - to inspect images:
# import matplotlib.pyplot as plt
# import pdb; pdb.set_trace()
# plt.imshow(next_obs)
# plt.show()
next_obs = np.transpose(next_obs, [2, 0, 1]) # 3 x 84 x 84/252/420
assert next_obs.shape == self.observation_space.shape
if self.count >= self._max_episode_steps:
print("Episode success: I've reached the episode horizon ({}).".format(self._max_episode_steps))
info['reason_episode_ended'] = 'success'
done = True
if speed < 0.02 and self.count >= 100 and self.count % 100 == 0: # a hack, instead of a counter
print("Episode fail: speed too small ({}), think I'm stuck! (frame {})".format(speed, self.count))
info['reason_episode_ended'] = 'stuck'
done = True
return next_obs, reward, done, info