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()
