carla-simulator / carla

Open-source simulator for autonomous driving research.
http://carla.org
MIT License
11.12k stars 3.59k forks source link

Autopilot wiggles and crashes after setting sync mode on #6352

Closed pAplakidis closed 1 year ago

pAplakidis commented 1 year ago

I've been trying to collect driving data for a project of mine using carla. My first problem was that the collected frames and poses were not synched properly. This was solved by turning sync mode on in my client script and matching delta_seconds (0.05) with the server's fps (20). However, after doing this the car accelerates a bit and then starts steering left and right until it crashes. My code is the following:


#!/usr/bin/env python3
import random
import math
import glob
import sys
import os
import time
import cv2
import numpy as np
from multiprocessing import Process, Queue
from threading import Thread

"""
try:
    sys.path.append(glob.glob('/opt/carla-simulator/PythonAPI/carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'linux-x86_64'))[0])
except IndexError as e:
  print("index error", e)
"""

import carla

# EXAMPLE RUN: OUT_PATH="../collected_data/8/" ./carla_collector.py

FRAMES = []
POSES = []
IMG_WIDTH = 1164
IMG_HEIGHT = 874
REC_TIME = 30 # recording length in seconds

#out_path = "../collected_data/8/"   # CHANGE THIS
out_path = os.getenv("OUT_PATH")
if out_path == None:
  print("Need to specify OUT_PATH")
  exit(1)
if not os.path.exists(out_path):
  os.mkdir(out_path)
plog_poses = out_path+"poses.npy"

actor_list = []
weather = {
  "ClearNoon": carla.WeatherParameters.ClearNoon,
  "CloudyNoon": carla.WeatherParameters.CloudyNoon,
  "WetNoon": carla.WeatherParameters.WetNoon,
  "WetNoon": carla.WeatherParameters.WetCloudyNoon,
  "WetNoon": carla.WeatherParameters.MidRainyNoon,
  "HardRainNoon": carla.WeatherParameters.HardRainNoon,
  "SoftRainNoon": carla.WeatherParameters.SoftRainNoon,
  "ClearSunset": carla.WeatherParameters.ClearSunset,
  "CloudySunset": carla.WeatherParameters.CloudySunset,
  "WetSunset": carla.WeatherParameters.WetSunset,
  "WetCloudySunset": carla.WeatherParameters.WetCloudySunset,
  "MidRainSunset": carla.WeatherParameters.MidRainSunset,
  "HardRainSunset": carla.WeatherParameters.HardRainSunset,
  "SoftRainSunset": carla.WeatherParameters.SoftRainSunset
  }
map = [
  "Town01",
  "Town02",
  "Town03",
  "Town04",
  "Town05",
  "Town06",
  "Town07",
  "Town08",
  "Town09",
  "Town10"
  ]

def render_img(img):
  cv2.imshow("DISPLAY", img)
  if cv2.waitKey(1) & 0xFF == 27: pass

class Car:
  def __init__(self):
    #self.front_camera = np.zeros((IMG_HEIGHT, IMG_WIDTH, 3))
    self.front_camera = None
    self.pose = None
    self.gyro = None

  def process_img(self, img, location, rotation):
    img = np.array(img.raw_data)
    img = img.reshape((IMG_HEIGHT, IMG_WIDTH, 4))
    img = img[:, :, :3]

    if location is not None and rotation is not None:
      FRAMES.append(img)
      POSES.append((location, rotation))
    self.front_camera = img

  def process_imu(self, imu):
    self.bearing_deg = math.degrees(imu.compass)
    self.acceleration = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z]
    self.gyro = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z]

  def process_gps(self, gps):
    self.gps_location = {
      "timestamp": int(time.time() * 1000),
      "accuracy": 1.0,
      "speed_accuracy": 0.1,
      "bearing_accuracy_deg": 0.1,
      "bearing_deg": self.bearing_deg,
      "latitude": gps.latitude,
      "longitude": gps.longitude,
      "altitude": gps.altitude,
      "speed": 0,
    }

def carla_main():
  #fourcc = cv2.CV_FOURCC(*'MP4V')
  out = cv2.VideoWriter(out_path+"video.mp4", cv2.VideoWriter_fourcc('M','J','P','G'), 20.0, (IMG_WIDTH, IMG_HEIGHT))
  location, rotation = None, None

  # setup
  client = carla.Client('localhost', 2000)
  client.set_timeout(2.0)  # seconds
  #world = client.get_world()
  world = client.load_world(map[2])
  world.set_weather(weather["CloudySunset"])
  bp_lib = world.get_blueprint_library()
  car = Car()

  # spawn traffic
  """
  traffic_manager = client.get_trafficmanager(2000)
  traffic_manager.set_global_distance_to_leading_vehicle(2.5)
  traffic_manager.set_respawn_dormant_vehicles(True)
  print("Spawned Traffic Manager")
  """

  # spawn a car
  vehicle_bp = bp_lib.filter('vehicle.tesla.*')[1]
  spawn_point = random.choice(world.get_map().get_spawn_points())
  vehicle = world.spawn_actor(vehicle_bp, spawn_point)
  print("Car Spawned")

  # make tires less slippery
  wheel_control = carla.WheelPhysicsControl(tire_friction=5)
  physics_control = vehicle.get_physics_control()
  physics_control.mass = 2326
  physics_control.wheels = [wheel_control]*4
  physics_control.torque_curve = [[20.0, 500.0], [5000.0, 500.0]]
  physics_control.gear_switch_time = 0.0
  vehicle.apply_physics_control(physics_control)

  # temp controls
  #vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0))
  vehicle.set_autopilot(True)

  actor_list.append(vehicle)

  # spawn camera
  camera_bp = bp_lib.find('sensor.camera.rgb')
  camera_bp.set_attribute('image_size_x', f'{IMG_WIDTH}')
  camera_bp.set_attribute('image_size_y', f'{IMG_HEIGHT}')
  camera_bp.set_attribute('fov', '70')
  camera_bp.set_attribute('sensor_tick', '0.05')
  spawn_point  = carla.Transform(carla.Location(x=0.8, z=1.13))
  camera = world.spawn_actor(camera_bp, spawn_point, attach_to=vehicle)
  actor_list.append(camera_bp)
  camera.listen(lambda img: car.process_img(img, location, rotation))
  print("Camera Spawned")

  # spawn IMU
  imu_bp = bp_lib.find("sensor.other.imu")
  imu = world.spawn_actor(imu_bp, spawn_point, attach_to=vehicle)
  imu.listen(lambda imu: car.process_imu(imu))
  print("IMU Spawned")

  # spawn GPS
  gps_bp = bp_lib.find("sensor.other.gnss")
  gps = world.spawn_actor(gps_bp, spawn_point, attach_to=vehicle)
  gps.listen(lambda gps: car.process_gps(gps))
  print("GPS Spawned")

  # TODO: autopilot crashes due to this, but frame-poses synchronization problem is gone
  # Enable synchronous mode
  settings = world.get_settings()
  settings.synchronous_mode = True 
  settings.no_rendering_mode = False
  settings.fixed_delta_seconds = 0.05
  world.apply_settings(settings)
  #traffic_manager.set_synchronous_mode(True)

  world.tick()

  # mainloop
  frame_id = 0
  start_time = time.time()
  try:
    print("Starting mainloop ...")
    while True:
      lx,ly,lz = vehicle.get_location().x, vehicle.get_location().y ,vehicle.get_location().z
      #rot = vehicle.get_transform().get_forward_vector()
      #rx, ry, rz = rot.x, rot.y, rot.z
      location = [lx, ly, lz]
      if car.gyro is not None:
        rotation = [car.gyro[0], car.gyro[1], car.gyro[2]]  # NOTE: IMU data could be noisy

      if car.front_camera is not None:
        render_img(car.front_camera)
        print("[+] Frame: ", frame_id, "=>", car.front_camera.shape)

        print("[+] Car Location: (x y z)=(", location, ")")
        print("[+] Car Rotation: (x y z)=(", rotation, ")")
        print("[->] IMU DATA => acceleration", car.acceleration, " : gyroscope", car.gyro)
        print("[->] GNSS DATA => latitude", car.gps_location['latitude'],
              " : longtitude", car.gps_location['longitude'],
              " : altitude", car.gps_location['altitude'])
        frame_id += 1
        curr_time = time.time() - start_time
        print("Current Time: %.2fs"%curr_time)
        print()
        if curr_time >= REC_TIME:
          break

      world.tick()
  except KeyboardInterrupt:
    print("[~] Stopped recording")

  print("[+] Time recorded: %.2f"%(time.time() - start_time))
  for f in FRAMES:
    out.write(f)
  print("[+] Camera recordings saved at: ", out_path+"video.mp4")
  poses = np.array(POSES)
  print(poses)
  print(poses.shape)
  np.save(plog_poses, poses)
  print("[+] Poses saved at:", plog_poses)
  out.release()

if __name__ == '__main__':
  print("Hello")
  try:
    carla_main()
  except RuntimeError:
    print("Restarting ...")
  finally:
    print("destroying all actors")
    for a in actor_list:
      a.destroy()
    cv2.destroyAllWindows()
    print('Done')

And the server-start script start_carla.sh:

!/bin/bash

/opt/carla-simulator/CarlaUE4.sh -opengl -nosound -RenderOffScreen -benchmark -fps=20 -quality -level=High

pAplakidis commented 1 year ago

Nevermind, I solved it by uncommenting the traffic manager lines:

traffic_manager = client.get_trafficmanager()
traffic_manager.set_global_distance_to_leading_vehicle(2.5)
traffic_manager.set_respawn_dormant_vehicles(True)

...

settings = world.get_settings()
settings.synchronous_mode = True 
settings.no_rendering_mode = False
settings.fixed_delta_seconds = 0.05
world.apply_settings(settings)
traffic_manager.set_synchronous_mode(True)