def _update_route(self, world, config, debug_mode):
"""
Update the input route, i.e. refine waypoint list, and extract possible scenario locations
Parameters:
- world: CARLA world
- config: Scenario configuration (RouteConfiguration)
"""
# Transform the scenario file into a dictionary
world_annotations = RouteParser.parse_annotations_file(config.scenario_file)
# prepare route's trajectory (interpolate and add the GPS route)
gps_route, route = interpolate_trajectory(world, config.trajectory)
potential_scenarios_definitions, _ = RouteParser.scan_route_for_scenarios(
config.town, route, world_annotations)
self.route = route
CarlaDataProvider.set_ego_vehicle_route(convert_transform_to_location(self.route))
config.agent.set_global_plan(gps_route, self.route)
# Sample the scenarios to be used for this route instance.
self.sampled_scenarios_definitions = self._scenario_sampling(potential_scenarios_definitions)
# Timeout of scenario in seconds
self.timeout = self._estimate_route_timeout()
# Print route in debug mode
if debug_mode:
self._draw_waypoints(world, self.route, vertical_shift=1.0, persistency=50000.0)
def get_center_transform_of_interpolated_trajectory(world,
trajectory,
waypoint_ratio=0.5):
frame_rate = 10
settings = world.get_settings()
settings.fixed_delta_seconds = 1.0 / frame_rate
settings.synchronous_mode = True
world.apply_settings(settings)
# spectator = CarlaDataProvider.get_world().get_spectator()
# spectator.set_transform(carla.Transform(carla.Location(x=0, y=0,z=20), carla.Rotation(pitch=-90)))
# Wait for the world to be ready
if world.get_settings().synchronous_mode:
world.tick()
else:
world.wait_for_tick()
_, route = interpolate_trajectory(world, trajectory)
ind = np.min([int(len(route) * waypoint_ratio), len(route) - 1])
loc = route[ind][0].location
center_transform = create_transform(loc.x, loc.y, 0, 0, 0, 0)
return center_transform
def _update_route(self, world, config, debug_mode):
"""
Update the input route, i.e. refine waypoint list, and extract possible scenario locations
Parameters:
- world: CARLA world
- config: Scenario configuration (RouteConfiguration)
"""
# Transform the scenario file into a dictionary
world_annotations = RouteParser.parse_annotations_file(config.scenario_file)
# prepare route's trajectory (interpolate and add the GPS route)
gps_route, route = interpolate_trajectory(world, config.trajectory)
if self.customized_data:
sample_factor = self.customized_data['sample_factor']
config.agent.set_global_plan(gps_route, route, sample_factor)
else:
config.agent.set_global_plan(gps_route, route)
if self.customized_data and 'ego_car_waypoints_perturbation' in self.customized_data:
perturb_route(config.agent._global_plan_world_coord, self.customized_data['ego_car_waypoints_perturbation'])
# visualize_route(config.agent._global_plan_world_coord)
# recalculate gps to accomodate the perturbation
lat_ref, lon_ref = _get_latlon_ref(world)
config.agent._global_plan = location_route_to_gps(route, lat_ref, lon_ref)
self.route = config.agent._global_plan_world_coord
potential_scenarios_definitions, _ = RouteParser.scan_route_for_scenarios(config.town, route, world_annotations)
print('\n potential_scenarios_definitions[0] :', len(potential_scenarios_definitions[0]), '\n')
print('\n potential_scenarios_definitions.keys() :', len(potential_scenarios_definitions.keys()), '\n')
CarlaDataProvider.set_ego_vehicle_route(convert_transform_to_location(self.route))
# Sample the scenarios to be used for this route instance.
self.sampled_scenarios_definitions = self._scenario_sampling(potential_scenarios_definitions)
# Timeout of scenario in seconds
self.timeout = self._estimate_route_timeout()
# Print route in debug mode
if debug_mode:
self._draw_waypoints(world, self.route, vertical_shift=1.0, persistency=50000.0)
def _update_route(self, world, config, debug_mode):
"""
Update the input route, i.e. refine waypoint list, and extract possible scenario locations
Parameters:
- world: CARLA world
- config: Scenario configuration (RouteConfiguration)
"""
# retrieve worlds annotations
world_annotations = RouteParser.parse_annotations_file(
config.scenario_file)
_route_description = copy.copy(config.route_description)
# prepare route's trajectory
gps_route, _route_description['trajectory'] = interpolate_trajectory(
world, _route_description['trajectory'])
potential_scenarios_definitions, _ = RouteParser.scan_route_for_scenarios(
_route_description, world_annotations)
self.route = _route_description['trajectory']
self.target = self.route[-1][0]
CarlaDataProvider.set_ego_vehicle_route(
convert_transform_to_location(self.route))
config.agent.set_global_plan(gps_route, self.route)
#print(potential_scenarios_definitions)
# Sample the scenarios to be used for this route instance.
self.sampled_scenarios_definitions = self._scenario_sampling(
potential_scenarios_definitions)
# Timeout of scenario in seconds
self.timeout = self._estimate_route_timeout()
# Print route in debug mode
if debug_mode:
self._draw_waypoints(world,
self.route,
vertical_shift=1.0,
persistency=50000.0)
def _create_behavior(self):
"""
"""
# building the tree
scenario_sequence = py_trees.composites.Sequence()
waypoint_events = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
destroy_actors = py_trees.composites.Sequence()
reach_destination = InTriggerDistanceToLocation(
self.ego_vehicles[0], self.customized_data['destination'], 2)
scenario_sequence.add_child(waypoint_events)
scenario_sequence.add_child(reach_destination)
scenario_sequence.add_child(destroy_actors)
for i in range(len(self.pedestrian_list)):
pedestrian_actor, pedestrian_generated_transform = self.pedestrian_list[
i]
pedestrian_info = self.customized_data['pedestrian_list'][i]
trigger_distance = InTriggerDistanceToVehicle(
self.ego_vehicles[0], pedestrian_actor,
pedestrian_info.trigger_distance)
movement = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
actor_velocity = KeepVelocity(pedestrian_actor,
pedestrian_info.speed)
actor_traverse = DriveDistance(pedestrian_actor,
pedestrian_info.dist_to_travel)
movement.add_child(actor_velocity)
movement.add_child(actor_traverse)
if pedestrian_info.after_trigger_behavior == 'destroy':
after_trigger_behavior = ActorDestroy(pedestrian_actor)
elif pedestrian_info.after_trigger_behavior == 'stop':
after_trigger_behavior = StopVehicle(pedestrian_actor,
brake_value=0.5)
destroy_actor = ActorDestroy(pedestrian_actor)
destroy_actors.add_child(destroy_actor)
else:
raise
pedestrian_behaviors = py_trees.composites.Sequence()
pedestrian_behaviors.add_child(trigger_distance)
pedestrian_behaviors.add_child(movement)
pedestrian_behaviors.add_child(after_trigger_behavior)
waypoint_events.add_child(pedestrian_behaviors)
for i in range(len(self.vehicle_list)):
vehicle_actor, generated_transform = self.vehicle_list[i]
vehicle_info = self.customized_data['vehicle_list'][i]
keep_velocity = py_trees.composites.Parallel(
"Trigger condition for changing behavior",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
keep_velocity.add_child(
InTriggerDistanceToVehicle(self.ego_vehicles[0], vehicle_actor,
vehicle_info.trigger_distance))
keep_velocity.add_child(
WaypointFollower(vehicle_actor,
vehicle_info.initial_speed,
avoid_collision=vehicle_info.avoid_collision))
if vehicle_info.waypoint_follower:
# interpolate current location and destination to find a path
start_location = generated_transform.location
end_location = vehicle_info.targeted_waypoint.location
_, route = interpolate_trajectory(
self.world, [start_location, end_location])
ds_ids = downsample_route(
route, self.customized_data['sample_factor'])
route = [(route[x][0], route[x][1]) for x in ds_ids]
# print('route', len(route))
perturb_route(route, vehicle_info.waypoints_perturbation)
# visualize_route(route)
plan = []
for transform, cmd in route:
wp = self._wmap.get_waypoint(transform.location,
project_to_road=False,
lane_type=carla.LaneType.Any)
if not wp:
wp = self._wmap.get_waypoint(
transform.location,
project_to_road=True,
lane_type=carla.LaneType.Any)
print('(', transform.location.x, transform.location.y,
')', 'is replaced by', '(',
wp.transform.location.x, wp.transform.location.y,
')')
plan.append((wp, cmd))
movement = WaypointFollower(
actor=vehicle_actor,
target_speed=vehicle_info.targeted_speed,
plan=plan,
avoid_collision=vehicle_info.avoid_collision)
else:
movement = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
actor_velocity = KeepVelocity(
vehicle_actor,
vehicle_info.targeted_speed,
target_direction=vehicle_info.target_direction)
actor_traverse = DriveDistance(vehicle_actor,
vehicle_info.dist_to_travel)
movement.add_child(actor_velocity)
movement.add_child(actor_traverse)
if vehicle_info.after_trigger_behavior == 'destroy':
after_trigger_behavior = ActorDestroy(vehicle_actor)
elif vehicle_info.after_trigger_behavior == 'stop':
after_trigger_behavior = StopVehicle(vehicle_actor,
brake_value=0.5)
destroy_actor = ActorDestroy(vehicle_actor)
destroy_actors.add_child(destroy_actor)
else:
raise
vehicle_behaviors = py_trees.composites.Sequence()
vehicle_behaviors.add_child(keep_velocity)
vehicle_behaviors.add_child(movement)
vehicle_behaviors.add_child(after_trigger_behavior)
waypoint_events.add_child(vehicle_behaviors)
return scenario_sequence
def _load_and_run_scenario(self, args, config, customized_data):
"""
Load and run the scenario given by config
"""
# hack:
config.weather = WEATHERS[args.weather_index]
config.friction = customized_data['friction']
config.cur_server_port = customized_data['port']
if not self._load_and_wait_for_world(args, config.town,
config.ego_vehicles):
self._cleanup()
return
_, route = interpolate_trajectory(self.world, config.trajectory)
customized_data['center_transform'] = route[int(len(route) // 2)][0]
'''
customized non-default center transforms for actors
['waypoint_ratio', 'absolute_location']
'''
for k, v in customized_data['customized_center_transforms'].items():
if v[0] == 'waypoint_ratio':
ind = np.min([int(len(route) * v[1]), len(route) - 1])
loc = route[ind][0].location
customized_data[k] = create_transform(loc.x, loc.y, 0, 0, 0, 0)
elif v[0] == 'absolute_location':
customized_data[k] = create_transform(v[1], v[2], 0, 0, 0, 0)
else:
print('unknown key', k)
if 'weather_index' in customized_data:
print('-' * 100)
print('port :', customized_data['port'])
print('center_transform :', '(',
customized_data['center_transform'].location.x,
customized_data['center_transform'].location.y, ')')
print('friction :', customized_data['friction'])
print('weather_index :', customized_data['weather_index'])
print('num_of_static :', customized_data['num_of_static'])
print('num_of_pedestrians :',
customized_data['num_of_pedestrians'])
print('num_of_vehicles :', customized_data['num_of_vehicles'])
print('-' * 100)
agent_class_name = getattr(self.module_agent, 'get_entry_point')()
try:
self.agent_instance = getattr(self.module_agent,
agent_class_name)(args.agent_config)
# addition
# self.agent_instance.set_trajectory(config.trajectory)
self.agent_instance.set_deviations_path(args.deviations_folder)
config.agent = self.agent_instance
self.sensors = [
sensors_to_icons[sensor['type']]
for sensor in self.agent_instance.sensors()
]
except Exception as e:
print("Could not setup required agent due to {}".format(e))
traceback.print_exc()
self._cleanup()
return
# Prepare scenario
print("Preparing scenario: " + config.name)
try:
self._prepare_ego_vehicles(config.ego_vehicles, False)
# print('\n'*10, 'RouteScenario config.cur_server_port', config.cur_server_port, '\n'*10)
scenario = RouteScenario(world=self.world,
config=config,
debug_mode=args.debug,
customized_data=customized_data)
except Exception as exception:
print("The scenario cannot be loaded")
if args.debug:
traceback.print_exc()
print(exception)
self._cleanup()
return
# Set the appropriate weather conditions
weather = carla.WeatherParameters(
cloudiness=config.weather.cloudiness,
precipitation=config.weather.precipitation,
precipitation_deposits=config.weather.precipitation_deposits,
wind_intensity=config.weather.wind_intensity,
sun_azimuth_angle=config.weather.sun_azimuth_angle,
sun_altitude_angle=config.weather.sun_altitude_angle,
fog_density=config.weather.fog_density,
fog_distance=config.weather.fog_distance,
wetness=config.weather.wetness)
self.world.set_weather(weather)
# Set the appropriate road friction
if config.friction is not None:
friction_bp = self.world.get_blueprint_library().find(
'static.trigger.friction')
extent = carla.Location(1000000.0, 1000000.0, 1000000.0)
friction_bp.set_attribute('friction', str(config.friction))
friction_bp.set_attribute('extent_x', str(extent.x))
friction_bp.set_attribute('extent_y', str(extent.y))
friction_bp.set_attribute('extent_z', str(extent.z))
# Spawn Trigger Friction
transform = carla.Transform()
transform.location = carla.Location(-10000.0, -10000.0, 0.0)
self.world.spawn_actor(friction_bp, transform)
# night mode
if config.weather.sun_altitude_angle < 0.0:
for vehicle in scenario.ego_vehicles:
vehicle.set_light_state(
carla.VehicleLightState(self._vehicle_lights))
# addition: to turn on lights of
actor_list = self.world.get_actors()
vehicle_list = actor_list.filter('*vehicle*')
for vehicle in vehicle_list:
vehicle.set_light_state(
carla.VehicleLightState(self._vehicle_lights))
try:
# Load scenario and run it
if args.record:
self.client.start_recorder("{}/{}.log".format(
args.record, config.name))
self.manager.load_scenario(scenario, self.agent_instance)
self.statistics_manager.set_route(config.name, config.index,
scenario.scenario)
print('start to run scenario')
self.manager.run_scenario()
print('stop to run scanario')
# Stop scenario
self.manager.stop_scenario()
# register statistics
current_stats_record = self.statistics_manager.compute_route_statistics(
config, self.manager.scenario_duration_system,
self.manager.scenario_duration_game)
# save
# modification
self.statistics_manager.save_record(current_stats_record,
config.index, self.save_path)
# Remove all actors
scenario.remove_all_actors()
settings = self.world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
self.world.apply_settings(settings)
except SensorConfigurationInvalid as e:
self._cleanup(True)
sys.exit(-1)
except Exception as e:
if args.debug:
traceback.print_exc()
print(e)
self._cleanup()
def _load_and_run_scenario(self, args, config, customized_data):
"""
Load and run the scenario given by config
"""
# hack:
if args.weather_index == -1:
weather = customized_data["fine_grained_weather"]
else:
weather = WEATHERS[args.weather_index]
config.weather = weather
config.friction = customized_data["friction"]
config.cur_server_port = customized_data["port"]
if not self._load_and_wait_for_world(args, config.town,
config.ego_vehicles):
self._cleanup()
return
_, route = interpolate_trajectory(self.world, config.trajectory)
customized_data["center_transform"] = route[int(len(route) // 2)][0]
# from customized_utils import visualize_route
# visualize_route(route)
# transforms = []
# for x in route:
# transforms.append(x[0])
# print('len(transforms)', len(transforms))
if customized_data['using_customized_route_and_scenario']:
"""
customized non-default center transforms for actors
['waypoint_ratio', 'absolute_location']
"""
for k, v in customized_data["customized_center_transforms"].items(
):
if v[0] == "waypoint_ratio":
r = v[1] / 100
ind = np.min([int(len(route) * r), len(route) - 1])
loc = route[ind][0].location
customized_data[k] = create_transform(
loc.x, loc.y, 0, 0, 0, 0)
print("waypoint_ratio", loc.x, loc.y)
elif v[0] == "absolute_location":
customized_data[k] = create_transform(
v[1], v[2], 0, 0, 0, 0)
else:
print("unknown key", k)
print("-" * 100)
print("port :", customized_data["port"])
print(
"center_transform :",
"(",
customized_data["center_transform"].location.x,
customized_data["center_transform"].location.y,
")",
)
print("friction :", customized_data["friction"])
print("weather_index :", customized_data["weather_index"])
print("num_of_static :", customized_data["num_of_static"])
print("num_of_pedestrians :",
customized_data["num_of_pedestrians"])
print("num_of_vehicles :", customized_data["num_of_vehicles"])
print("-" * 100)
agent_class_name = getattr(self.module_agent, "get_entry_point")()
try:
self.agent_instance = getattr(self.module_agent,
agent_class_name)(args.agent_config)
# addition
# self.agent_instance.set_trajectory(config.trajectory)
self.agent_instance.set_args(args)
config.agent = self.agent_instance
self.sensors = [
sensors_to_icons[sensor["type"]]
for sensor in self.agent_instance.sensors()
]
except Exception as e:
print("Could not setup required agent due to {}".format(e))
traceback.print_exc()
self._cleanup()
return
# Prepare scenario
print("Preparing scenario: " + config.name)
try:
self._prepare_ego_vehicles(config.ego_vehicles, False)
# print('\n'*10, 'RouteScenario config.cur_server_port', config.cur_server_port, '\n'*10)
scenario = RouteScenario(
world=self.world,
config=config,
debug_mode=args.debug,
customized_data=customized_data,
)
except Exception as exception:
print("The scenario cannot be loaded")
if args.debug:
traceback.print_exc()
print(exception)
self._cleanup()
return
# Set the appropriate weather conditions
weather = carla.WeatherParameters(
cloudiness=config.weather.cloudiness,
precipitation=config.weather.precipitation,
precipitation_deposits=config.weather.precipitation_deposits,
wind_intensity=config.weather.wind_intensity,
sun_azimuth_angle=config.weather.sun_azimuth_angle,
sun_altitude_angle=config.weather.sun_altitude_angle,
fog_density=config.weather.fog_density,
fog_distance=config.weather.fog_distance,
wetness=config.weather.wetness,
)
self.world.set_weather(weather)
# Set the appropriate road friction
if config.friction is not None:
friction_bp = self.world.get_blueprint_library().find(
"static.trigger.friction")
extent = carla.Location(1000000.0, 1000000.0, 1000000.0)
friction_bp.set_attribute("friction", str(config.friction))
friction_bp.set_attribute("extent_x", str(extent.x))
friction_bp.set_attribute("extent_y", str(extent.y))
friction_bp.set_attribute("extent_z", str(extent.z))
# Spawn Trigger Friction
transform = carla.Transform()
transform.location = carla.Location(-10000.0, -10000.0, 0.0)
self.world.spawn_actor(friction_bp, transform)
# night mode
if config.weather.sun_altitude_angle < 0.0:
for vehicle in scenario.ego_vehicles:
vehicle.set_light_state(
carla.VehicleLightState(self._vehicle_lights))
# addition: to turn on lights of
actor_list = self.world.get_actors()
vehicle_list = actor_list.filter("*vehicle*")
for vehicle in vehicle_list:
vehicle.set_light_state(
carla.VehicleLightState(self._vehicle_lights))
try:
# Load scenario and run it
if args.record:
self.client.start_recorder("{}/{}.log".format(
args.record, config.name))
self.manager.load_scenario(scenario, self.agent_instance)
self.statistics_manager.set_route(config.name, config.index,
scenario.scenario)
print("start to run scenario")
self.manager.run_scenario()
print("stop to run scanario")
# Stop scenario
self.manager.stop_scenario()
# register statistics
current_stats_record = self.statistics_manager.compute_route_statistics(
config,
self.manager.scenario_duration_system,
self.manager.scenario_duration_game,
)
# save
# modification
self.statistics_manager.save_record(current_stats_record,
config.index, self.save_path)
# Remove all actors
scenario.remove_all_actors()
settings = self.world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
self.world.apply_settings(settings)
except SensorConfigurationInvalid as e:
self._cleanup(True)
sys.exit(-1)
except Exception as e:
if args.debug:
traceback.print_exc()
print(e)
self._cleanup()