def can_function_runner(vs): i = 0 while 1: can_function(pm, vs.speed, vs.angle, i, vs.cruise_button, vs.is_engaged) time.sleep(0.01) i += 1
def can_function_runner(vs: VehicleState, exit_event: threading.Event): i = 0 while not exit_event.is_set(): can_function(pm, vs.speed, vs.angle, i, vs.cruise_button, vs.is_engaged) time.sleep(0.01) i += 1
def go(): import carla client = carla.Client("127.0.0.1", 2000) client.set_timeout(5.0) world = client.load_world('Town03') settings = world.get_settings() settings.fixed_delta_seconds = 0.05 world.apply_settings(settings) weather = carla.WeatherParameters(cloudyness=0.0, precipitation=0.0, precipitation_deposits=0.0, wind_intensity=0.0, sun_azimuth_angle=0.0, sun_altitude_angle=0.0) world.set_weather(weather) blueprint_library = world.get_blueprint_library() """ for blueprint in blueprint_library.filter('sensor.*'): print(blueprint.id) exit(0) """ world_map = world.get_map() vehicle_bp = random.choice(blueprint_library.filter('vehicle.bmw.*')) vehicle = world.spawn_actor(vehicle_bp, random.choice(world_map.get_spawn_points())) if args.autopilot: vehicle.set_autopilot(True) blueprint = blueprint_library.find('sensor.camera.rgb') blueprint.set_attribute('image_size_x', str(W)) blueprint.set_attribute('image_size_y', str(H)) blueprint.set_attribute('fov', '70') blueprint.set_attribute('sensor_tick', '0.05') transform = carla.Transform(carla.Location(x=0.8, z=1.45)) camera = world.spawn_actor(blueprint, transform, attach_to=vehicle) camera.listen(cam_callback) # TODO: wait for carla 0.9.7 imu_bp = blueprint_library.find('sensor.other.imu') imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle) imu.listen(imu_callback) def destroy(): print("clean exit") imu.destroy() camera.destroy() vehicle.destroy() print("done") atexit.register(destroy) # can loop sendcan = messaging.sub_sock('sendcan') rk = Ratekeeper(100) steer_angle = 0 while 1: vel = vehicle.get_velocity() speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) can_function(pm, speed, steer_angle, rk.frame, rk.frame % 500 == 499) if rk.frame % 5 == 0: throttle, brake, steer = sendcan_function(sendcan) steer_angle += steer / 10000.0 # torque vc = carla.VehicleControl(throttle=throttle, steer=steer_angle, brake=brake) vehicle.apply_control(vc) print(speed, steer_angle, vc) rk.keep_time()
def go(q): threading.Thread(target=health_function).start() threading.Thread(target=fake_driver_monitoring).start() import carla client = carla.Client("127.0.0.1", 2000) client.set_timeout(5.0) world = client.load_world('Town04') settings = world.get_settings() settings.fixed_delta_seconds = 0.05 world.apply_settings(settings) weather = carla.WeatherParameters(cloudyness=0.1, precipitation=0.0, precipitation_deposits=0.0, wind_intensity=0.0, sun_azimuth_angle=15.0, sun_altitude_angle=75.0) world.set_weather(weather) blueprint_library = world.get_blueprint_library() """ for blueprint in blueprint_library.filter('sensor.*'): print(blueprint.id) exit(0) """ world_map = world.get_map() vehicle_bp = random.choice(blueprint_library.filter('vehicle.tesla.*')) vehicle = world.spawn_actor(vehicle_bp, world_map.get_spawn_points()[16]) # make tires less slippery wheel_control = carla.WheelPhysicsControl(tire_friction=5) physics_control = vehicle.get_physics_control() physics_control.mass = 1326 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) if args.autopilot: vehicle.set_autopilot(True) # print(vehicle.get_speed_limit()) blueprint = blueprint_library.find('sensor.camera.rgb') blueprint.set_attribute('image_size_x', str(W)) blueprint.set_attribute('image_size_y', str(H)) blueprint.set_attribute('fov', '70') blueprint.set_attribute('sensor_tick', '0.05') transform = carla.Transform(carla.Location(x=0.8, z=1.45)) camera = world.spawn_actor(blueprint, transform, attach_to=vehicle) camera.listen(cam_callback) # reenable IMU imu_bp = blueprint_library.find('sensor.other.imu') imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle) imu.listen(imu_callback) def destroy(): print("clean exit") imu.destroy() camera.destroy() vehicle.destroy() print("done") atexit.register(destroy) # can loop sendcan = messaging.sub_sock('sendcan') rk = Ratekeeper(100, print_delay_threshold=0.05) # init A_throttle = 2. A_brake = 2. A_steer_torque = 1. fake_wheel = FakeSteeringWheel() is_openpilot_engaged = False in_reverse = False throttle_out = 0 brake_out = 0 steer_angle_out = 0 while 1: cruise_button = 0 # check for a input message, this will not block if not q.empty(): print("here") message = q.get() m = message.split('_') if m[0] == "steer": steer_angle_out = float(m[1]) fake_wheel.set_angle( steer_angle_out ) # touching the wheel overrides fake wheel angle # print(" === steering overriden === ") if m[0] == "throttle": throttle_out = float(m[1]) / 100. if throttle_out > 0.3: cruise_button = CruiseButtons.CANCEL is_openpilot_engaged = False if m[0] == "brake": brake_out = float(m[1]) / 100. if brake_out > 0.3: cruise_button = CruiseButtons.CANCEL is_openpilot_engaged = False if m[0] == "reverse": in_reverse = not in_reverse cruise_button = CruiseButtons.CANCEL is_openpilot_engaged = False if m[0] == "cruise": if m[1] == "down": cruise_button = CruiseButtons.DECEL_SET is_openpilot_engaged = True if m[1] == "up": cruise_button = CruiseButtons.RES_ACCEL is_openpilot_engaged = True if m[1] == "cancel": cruise_button = CruiseButtons.CANCEL is_openpilot_engaged = False vel = vehicle.get_velocity() speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) * 3.6 can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=cruise_button, is_engaged=is_openpilot_engaged) if rk.frame % 1 == 0: # 20Hz? throttle_op, brake_op, steer_torque_op = sendcan_function(sendcan) # print(" === torq, ",steer_torque_op, " ===") if is_openpilot_engaged: fake_wheel.response(steer_torque_op * A_steer_torque, speed) throttle_out = throttle_op * A_throttle brake_out = brake_op * A_brake steer_angle_out = fake_wheel.angle # print(steer_torque_op) # print(steer_angle_out) vc = carla.VehicleControl(throttle=throttle_out, steer=steer_angle_out / 3.14, brake=brake_out, reverse=in_reverse) vehicle.apply_control(vc) rk.keep_time()