def apply_physics(self, phys_settings):
self.world.tick()
"""
Default wheel settings:
front_left_wheel = carla.WheelPhysicsControl(tire_friction=3.5, damping_rate=0.25, max_steer_angle=70, radius=36.7)
front_right_wheel = carla.WheelPhysicsControl(tire_friction=3.5, damping_rate=0.25, max_steer_angle=70, radius=36.7)
rear_left_wheel = carla.WheelPhysicsControl(tire_friction=3.5, damping_rate=0.25, max_steer_angle=0.0, radius=36.0)
rear_right_wheel = carla.WheelPhysicsControl(tire_friction=3.5, damping_rate=0.25, max_steer_angle=0.0, radius=36.0)
"""
front_left_wheel = carla.WheelPhysicsControl(
tire_friction=phys_settings['flwf'],
damping_rate=0.25,
max_steer_angle=phys_settings['flwmsa'],
radius=36.7)
front_right_wheel = carla.WheelPhysicsControl(
tire_friction=phys_settings['frwf'],
damping_rate=0.25,
max_steer_angle=phys_settings['frwmsa'],
radius=36.7)
rear_left_wheel = carla.WheelPhysicsControl(
tire_friction=phys_settings['rlwf'],
damping_rate=0.25,
max_steer_angle=0.0,
radius=36.0)
rear_right_wheel = carla.WheelPhysicsControl(
tire_friction=phys_settings['rrwf'],
damping_rate=0.25,
max_steer_angle=0.0,
radius=36.0)
wheels = [
front_left_wheel, front_right_wheel, rear_left_wheel,
rear_right_wheel
]
# Change Vehicle Physics Control parameters of the vehicle
physics_control = self.player.get_physics_control()
#physics_control.max_rpm = phys_settings['max_rpm']
physics_control.mass = phys_settings['mass']
#physics_control.drag_coefficient = phys_settings['drag_coefficient']
physics_control.wheels = wheels
physics_control.steering_curve = [
carla.Vector2D(0, 1),
carla.Vector2D(20, phys_settings['steer1']),
carla.Vector2D(60, phys_settings['steer2']),
carla.Vector2D(120, phys_settings['steer3'])
]
physics_control.torque_curve = [
carla.Vector2D(0, 400),
carla.Vector2D(890, phys_settings['torque1']),
carla.Vector2D(5729.577, 400)
]
self.speed = phys_settings['speed']
# Apply Vehicle Physics Control for the vehicle
self.player.apply_physics_control(physics_control)
def parse_wheels_physics(route):
"""
Returns a list of carla.WheelPhysicsControl objects.
The CARLA documentation does not state any units tire_friction and damping rate.
"""
route_wheel_physics = route.find("wheel_physics")
if route_wheel_physics is None:
wheel_physics = carla.WheelPhysicsControl()
else:
tire_friction = float(route_wheel_physics.attrib['tire_friction'])
damping_rate = float(route_wheel_physics.attrib['damping_rate'])
max_brake_torque = float(
route_wheel_physics.attrib['max_brake_torque'])
max_handbrake_torque = float(
route_wheel_physics.attrib['max_handbrake_torque'])
wheels_physics = []
for i in range(2):
wheel_physics = carla.WheelPhysicsControl(
tire_friction=tire_friction,
damping_rate=damping_rate,
max_steer_angle=70.,
radius=35.5,
max_brake_torque=max_brake_torque,
max_handbrake_torque=0,
)
wheels_physics.append(wheel_physics)
for i in range(2):
wheel_physics = carla.WheelPhysicsControl(
tire_friction=tire_friction,
damping_rate=damping_rate,
max_steer_angle=0.,
radius=35.5,
max_brake_torque=max_brake_torque,
max_handbrake_torque=max_handbrake_torque,
)
wheels_physics.append(wheel_physics)
return wheels_physics
def parse_wheels_control(info):
"""
Parses a list into a WheelsPhysicsControl
Args:
info (list): list corresponding to a row of the recorder
"""
gears_control = carla.WheelPhysicsControl(float(info[3]), float(info[5]),
float(info[7]), float(info[9]),
float(info[11]), float(info[13]),
carla.Vector3D())
return gears_control
def main():
# Connect to client
client = carla.Client('127.0.0.1', 2000)
client.set_timeout(2.0)
# Get World and Actors
world = client.get_world()
current_map = world.get_map()
actors = world.get_actors()
# Get a random vehicle from world (there should be one at least)
vehicle = random.choice([actor for actor in actors if 'vehicle' in actor.type_id])
# Create Wheels Physics Control
front_left_wheel = carla.WheelPhysicsControl(tire_friction=4.5, damping_rate=1.0, max_steer_angle=70.0, radius=30.0)
front_right_wheel = carla.WheelPhysicsControl(tire_friction=2.5, damping_rate=1.5, max_steer_angle=70.0, radius=25.0)
rear_left_wheel = carla.WheelPhysicsControl(tire_friction=1.0, damping_rate=0.2, max_steer_angle=0.0, radius=15.0)
rear_right_wheel = carla.WheelPhysicsControl(tire_friction=1.5, damping_rate=1.3, max_steer_angle=0.0, radius=20.0)
wheels = [front_left_wheel, front_right_wheel, rear_left_wheel, rear_right_wheel]
# Change Vehicle Physics Control parameters of the vehicle
physics_control = vehicle.get_physics_control()
physics_control.torque_curve = [carla.Vector2D(x=0, y=400), carla.Vector2D(x=1300, y=600)]
physics_control.max_rpm = 100000
physics_control.moi = 1.0
physics_control.damping_rate_full_throttle = 0.0
physics_control.use_gear_autobox = True
physics_control.gear_switch_time = 0.5
physics_control.clutch_strength = 10
physics_control.mass = 10000
physics_control.drag_coefficient = 0.25
physics_control.steering_curve = [carla.Vector2D(x=0, y=1), carla.Vector2D(x=100, y=1), carla.Vector2D(x=300, y=1)]
physics_control.wheels = wheels
physics_control.use_sweep_wheel_collision = True
# Apply Vehicle Physics Control for the vehicle
vehicle.apply_physics_control(physics_control)
def parse_wheels_control(info):
"""Parses a list into a WheelsPhysicsControl"""
wheels_control = carla.WheelPhysicsControl(
tire_friction=float(info[3]),
damping_rate=float(info[5]),
max_steer_angle=float(info[7]),
radius=float(info[9]),
max_brake_torque=float(info[11]),
max_handbrake_torque=float(info[13]),
position=carla.Vector3D(x=float(info[17][1:-1]) / 100,
y=float(info[17][:-1]) / 100,
z=float(info[17][:-1]) / 100))
return wheels_control
def use_sample(self, sample):
print('Sample:', sample)
init_conds = sample.init_conditions
self.target_speed = init_conds.target_speed[0]
# Ego vehicle physics parameters
com = carla.Vector3D(
init_conds.center_of_mass[0],
init_conds.center_of_mass[1],
init_conds.center_of_mass[2],
)
wheels = [
carla.WheelPhysicsControl(
tire_friction=init_conds.tire_friction[0]) for _ in range(4)
]
self.vehicle_mass = init_conds.mass[0]
physics = carla.VehiclePhysicsControl(
mass=self.vehicle_mass,
max_rpm=init_conds.max_rpm[0],
center_of_mass=com,
drag_coefficient=init_conds.drag_coefficient[0],
wheels=wheels)
# PID controller parameters
opt_dict = {'target_speed': self.target_speed}
# Deterministic blueprint, spawnpoint.
blueprint = self.world.world.get_blueprint_library().find(
'vehicle.tesla.model3')
spawn = self.world.map.get_spawn_points()[0]
self.world.add_vehicle(PIDAgent,
control_params=opt_dict,
blueprint=blueprint,
spawn=spawn,
physics=physics,
has_collision_sensor=True,
has_lane_sensor=True,
ego=True)
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()
def test_named_args(self):
torque_curve = [[0, 400], [24, 56], [24, 56], [1315.47, 654.445],
[5729, 400]]
steering_curve = [
carla.Vector2D(x=0, y=1),
carla.Vector2D(x=20.0, y=0.9),
carla.Vector2D(x=63.0868, y=0.703473),
carla.Vector2D(x=119.12, y=0.573047)
]
wheels = [
carla.WheelPhysicsControl(tire_friction=2,
damping_rate=0,
steer_angle=30,
disable_steering=1),
carla.WheelPhysicsControl(tire_friction=2,
damping_rate=0,
steer_angle=30,
disable_steering=1),
carla.WheelPhysicsControl(tire_friction=2,
damping_rate=0,
steer_angle=30,
disable_steering=1),
carla.WheelPhysicsControl(tire_friction=2,
damping_rate=0,
steer_angle=30,
disable_steering=1)
]
pc = carla.VehiclePhysicsControl(
torque_curve=torque_curve,
max_rpm=5729,
moi=1,
damping_rate_full_throttle=0.15,
damping_rate_zero_throttle_clutch_engaged=2,
damping_rate_zero_throttle_clutch_disengaged=0.35,
use_gear_autobox=1,
gear_switch_time=0.5,
clutch_strength=10,
mass=5500,
drag_coefficient=0.3,
center_of_mass=carla.Vector3D(x=0.5, y=1, z=1),
steering_curve=steering_curve,
wheels=wheels)
error = .001
for i in range(0, len(torque_curve)):
self.assertTrue(
abs(pc.torque_curve[i].x - torque_curve[i][0]) <= error)
self.assertTrue(
abs(pc.torque_curve[i].y - torque_curve[i][1]) <= error)
self.assertTrue(abs(pc.max_rpm - 5729) <= error)
self.assertTrue(abs(pc.moi - 1) <= error)
self.assertTrue(abs(pc.damping_rate_full_throttle - 0.15) <= error)
self.assertTrue(
abs(pc.damping_rate_zero_throttle_clutch_engaged - 2) <= error)
self.assertTrue(
abs(pc.damping_rate_zero_throttle_clutch_disengaged -
0.35) <= error)
self.assertTrue(abs(pc.use_gear_autobox - 1) <= error)
self.assertTrue(abs(pc.gear_switch_time - 0.5) <= error)
self.assertTrue(abs(pc.clutch_strength - 10) <= error)
self.assertTrue(abs(pc.mass - 5500) <= error)
self.assertTrue(abs(pc.drag_coefficient - 0.3) <= error)
self.assertTrue(abs(pc.center_of_mass.x - 0.5) <= error)
self.assertTrue(abs(pc.center_of_mass.y - 1) <= error)
self.assertTrue(abs(pc.center_of_mass.z - 1) <= error)
for i in range(0, len(steering_curve)):
self.assertTrue(
abs(pc.steering_curve[i].x - steering_curve[i].x) <= error)
self.assertTrue(
abs(pc.steering_curve[i].y - steering_curve[i].y) <= error)
for i in range(0, len(wheels)):
self.assertTrue(
abs(pc.wheels[i].tire_friction -
wheels[i].tire_friction) <= error)
self.assertTrue(
abs(pc.wheels[i].damping_rate -
wheels[i].damping_rate) <= error)
self.assertTrue(
abs(pc.wheels[i].steer_angle - wheels[i].steer_angle) <= error)
self.assertEqual(pc.wheels[i].disable_steering,
wheels[i].disable_steering)
