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 parse_recorder_info(self):
"""
Parses the recorder into readable information.
Args:
recorder_info (str): string given by the recorder
"""
# Divide it into frames
recorder_list = self.recorder_info.split("Frame")
# Get general information
header = recorder_list[0].split("\n")
sim_map = header[1][5:]
sim_date = header[2][6:]
annex = recorder_list[-1].split("\n")
sim_frames = int(annex[0][3:])
sim_duration = float(annex[1][10:-8])
recorder_list = recorder_list[1:-1]
simulation_info = {
"map": sim_map,
"date:": sim_date,
"total_frames": sim_frames,
"duration": sim_duration
}
actors_info = {}
frames_info = []
for frame in recorder_list:
# Divide the frame in lines
self.frame_list = frame.split("\n")
# Get the general frame information
frame_info = self.frame_list[0].split(" ")
frame_number = int(frame_info[1])
frame_time = float(frame_info[3])
try:
prev_frame = frames_info[frame_number - 2]
prev_time = prev_frame["frame"]["elapsed_time"]
delta_time = round(frame_time - prev_time, 6)
except IndexError:
delta_time = 0
# Variable to store all the information about the frame
frame_state = {
"frame": {
"elapsed_time": frame_time,
"delta_time": delta_time,
"platform_time": None
},
"actors": {},
"events": {
"scene_lights": {},
"physics_control": {},
"traffic_light_state_time": {},
"collisions": {}
}
}
# Loop through all the other rows.
self.i = 0
self.next_row()
while self.frame_row.startswith(
' Create') or self.frame_row.startswith(' '):
if self.frame_row.startswith(' Create'):
elements = self.get_row_elements(1, " ")
actor_id = int(elements[1][:-1])
actor = parse_actor(elements)
actors_info.update({actor_id: actor})
actors_info[actor_id].update({"created": frame_number})
else:
elements = self.get_row_elements(2, " = ")
actors_info[actor_id].update({elements[0]: elements[1]})
self.next_row()
while self.frame_row.startswith(' Destroy'):
elements = self.get_row_elements(1, " ")
actor_id = int(elements[1])
actors_info[actor_id].update({"destroyed": frame_number})
self.next_row()
while self.frame_row.startswith(' Collision'):
elements = self.get_row_elements(1, " ")
actor_id = int(elements[4])
other_id = int(elements[-1])
if actor_id not in frame_state["events"]["collisions"]:
frame_state["events"]["collisions"][actor_id] = [other_id]
else:
collisions = frame_state["events"]["collisions"][actor_id]
collisions.append(other_id)
frame_state["events"]["collisions"].update(
{actor_id: collisions})
self.next_row()
while self.frame_row.startswith(' Parenting'):
elements = self.get_row_elements(1, " ")
actor_id = int(elements[1])
parent_id = int(elements[3])
actors_info[actor_id].update({"parent": parent_id})
self.next_row()
if self.frame_row.startswith(' Positions'):
self.next_row()
while self.frame_row.startswith(' '):
elements = self.get_row_elements(2, " ")
actor_id = int(elements[1])
transform = parse_transform(elements)
frame_state["actors"].update(
{actor_id: {
"transform": transform
}})
self.next_row()
if self.frame_row.startswith(' State traffic lights'):
self.next_row()
while self.frame_row.startswith(' '):
elements = self.get_row_elements(2, " ")
actor_id = int(elements[1])
traffic_light = parse_traffic_light(elements)
frame_state["actors"].update({actor_id: traffic_light})
self.next_row()
if self.frame_row.startswith(' Vehicle animations'):
self.next_row()
while self.frame_row.startswith(' '):
elements = self.get_row_elements(2, " ")
actor_id = int(elements[1])
control = parse_control(elements)
frame_state["actors"][actor_id].update(
{"control": control})
self.next_row()
if self.frame_row.startswith(' Walker animations'):
self.next_row()
while self.frame_row.startswith(' '):
elements = self.get_row_elements(2, " ")
actor_id = int(elements[1])
frame_state["actors"][actor_id].update(
{"speed": elements[3]})
self.next_row()
if self.frame_row.startswith(' Vehicle light animations'):
self.next_row()
while self.frame_row.startswith(' '):
elements = self.get_row_elements(2, " ")
actor_id = int(elements[1])
lights = parse_vehicle_lights(elements)
frame_state["actors"][actor_id].update({"lights": lights})
self.next_row()
if self.frame_row.startswith(' Scene light changes'):
self.next_row()
while self.frame_row.startswith(' '):
elements = self.get_row_elements(2, " ")
actor_id = int(elements[1])
scene_light = parse_scene_lights(elements)
frame_state["events"]["scene_lights"].update(
{actor_id: scene_light})
self.next_row()
if self.frame_row.startswith(' Dynamic actors'):
self.next_row()
while self.frame_row.startswith(' '):
elements = self.get_row_elements(2, " ")
actor_id = int(elements[1])
velocity = parse_velocity(elements)
frame_state["actors"][actor_id].update(
{"velocity": velocity})
angular_v = parse_angular_velocity(elements)
frame_state["actors"][actor_id].update(
{"angular_velocity": angular_v})
if delta_time == 0:
acceleration = carla.Vector3D(0, 0, 0)
else:
prev_velocity = frame_state["actors"][actor_id][
"velocity"]
acceleration = (velocity - prev_velocity) / delta_time
frame_state["actors"][actor_id].update(
{"acceleration": acceleration})
self.next_row()
if self.frame_row.startswith(' Actor bounding boxes'):
self.next_row()
while self.frame_row.startswith(' '):
elements = self.get_row_elements(2, " ")
actor_id = int(elements[1])
bbox = parse_bounding_box(elements)
actors_info[actor_id].update({"bounding_box": bbox})
self.next_row()
if self.frame_row.startswith(' Actor trigger volumes'):
self.next_row()
while self.frame_row.startswith(' '):
elements = self.get_row_elements(2, " ")
actor_id = int(elements[1])
trigvol = parse_bounding_box(elements)
actors_info[actor_id].update({"trigger_volume": trigvol})
self.next_row()
if self.frame_row.startswith(' Current platform time'):
elements = self.get_row_elements(1, " ")
platform_time = float(elements[-1])
frame_state["frame"]["platform_time"] = platform_time
self.next_row()
if self.frame_row.startswith(' Physics Control'):
self.next_row()
actor_id = None
while self.frame_row.startswith(' '):
elements = self.get_row_elements(2, " ")
actor_id = int(elements[1])
physics_control = carla.VehiclePhysicsControl()
self.next_row()
forward_gears = []
wheels = []
while self.frame_row.startswith(' '):
if self.frame_row.startswith(' '):
elements = self.get_row_elements(4, " ")
if elements[0] == "gear":
forward_gears.append(
parse_gears_control(elements))
elif elements[0] == "wheel":
wheels.append(parse_wheels_control(elements))
else:
elements = self.get_row_elements(3, " = ")
name = elements[0]
if name == "center_of_mass":
values = elements[1].split(" ")
value = carla.Vector3D(
float(values[0][1:-1]),
float(values[1][:-1]),
float(values[2][:-1]),
)
setattr(physics_control, name, value)
elif name == "torque_curve" or name == "steering_curve":
values = elements[1].split(" ")
value = parse_vector_list(values)
setattr(physics_control, name, value)
elif name == "use_gear_auto_box":
name = "use_gear_autobox"
value = True if elements[1] == "true" else False
setattr(physics_control, name, value)
elif "forward_gears" in name or "wheels" in name:
pass
else:
name = name.lower()
value = float(elements[1])
setattr(physics_control, name, value)
self.next_row()
setattr(physics_control, "forward_gears", forward_gears)
setattr(physics_control, "wheels", wheels)
frame_state["events"]["physics_control"].update(
{actor_id: physics_control})
if self.frame_row.startswith(' Traffic Light time events'):
self.next_row()
while self.frame_row.startswith(' '):
elements = self.get_row_elements(2, " ")
actor_id = int(elements[1])
state_times = parse_state_times(elements)
frame_state["events"]["traffic_light_state_time"].update(
{actor_id: state_times})
self.next_row()
frames_info.append(frame_state)
return simulation_info, actors_info, frames_info
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)
topo = amap.get_topology()
global_route_plan = global_path_planner(world_map=amap, sampling_resolution=3) # 实例化全局规划器
# topology, graph, id_map, road_id_to_edge = global_route_plan.get_topology_and_graph_info()
All_spawn_points = amap.get_spawn_points() # 获取所有carla提供的actor产生位置
"""# 定义一个ego-vehicle"""
model3_bp = world.get_blueprint_library().find('vehicle.tesla.model3')
model3_bp.set_attribute('color', '255,88,0')
model3_spawn_point = All_spawn_points[259]
print(model3_spawn_point)
# model3_spawn_point.location = model3_spawn_point.location + carla.Location(x=-100, y=0, z=0)
model3_actor = world.spawn_actor(model3_bp, model3_spawn_point) # type: carla.Vehicle
# 定义轮胎特性
# wheel_f = carla.WheelPhysicsControl() # type: carla.WheelPhysicsControl
# 定义车辆特性，
physics_control = carla.VehiclePhysicsControl() # type: carla.VehiclePhysicsControl
physics_control.mass = 1412 # 质量kg
model3_actor.apply_physics_control(physics_control)
# 车辆特性和轮胎特性，这里暂时没有使用其他的具体参数，后面引入carsim的时候会用
# 规划路径
pathway = global_route_plan.search_path_way(origin=model3_spawn_point.location,
destination=All_spawn_points[48].location)
debug = world.debug # type: carla.DebugHelper
i = 0
for waypoint in pathway:
# print(waypoint)
debug.draw_point(waypoint[0].transform.location + carla.Location(0, 0, 2),
size=0.05, color=carla.Color(0, 255, 0), life_time=0)
mark = str((i,
round(waypoint[0].transform.location.x, 2),