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