def _initialize_actors(self, config):
"""
Custom initialization
"""
# Get the waypoint left after the junction
waypoint = generate_target_waypoint(self._reference_waypoint, 1)
while True:
# Try to spawn the actor
try:
self._other_actor_transform = waypoint.transform
first_vehicle = CarlaDataProvider.request_new_actor(
'vehicle.*', self._other_actor_transform)
first_vehicle.set_simulate_physics(enabled=False)
break
# Move the spawning point a bit and try again
except RuntimeError as r:
# In the case there is an object just move a little bit and retry
waypoint = waypoint.next(1)[0]
self._spawn_attempted += 1
if self._spawn_attempted >= self._number_of_attempts:
raise r
# Set the transform to -500 z after we are able to spawn it
actor_transform = carla.Transform(
carla.Location(self._other_actor_transform.location.x,
self._other_actor_transform.location.y,
self._other_actor_transform.location.z - 500),
self._other_actor_transform.rotation)
first_vehicle.set_transform(actor_transform)
first_vehicle.set_simulate_physics(enabled=False)
self.other_actors.append(first_vehicle)
def generate_target_waypoint_in_route(waypoint, route):
"""
This method follow waypoints to a junction
@returns a waypoint list according to turn input
"""
wmap = CarlaDataProvider.get_map()
reached_junction = False
# Get the route location
shortest_distance = float('inf')
for index, route_pos in enumerate(route):
wp = route_pos[0]
trigger_location = waypoint.transform.location
dist_to_route = trigger_location.distance(wp)
if dist_to_route <= shortest_distance:
closest_index = index
shortest_distance = dist_to_route
route_location = route[closest_index][0]
index = closest_index
while True:
# Get the next route location
index = min(index + 1, len(route))
route_location = route[index][0]
road_option = route[index][1]
# Enter the junction
if not reached_junction and (road_option in (RoadOption.LEFT, RoadOption.RIGHT, RoadOption.STRAIGHT)):
reached_junction = True
# End condition for the behavior, at the end of the junction
if reached_junction and (road_option not in (RoadOption.LEFT, RoadOption.RIGHT, RoadOption.STRAIGHT)):
break
return wmap.get_waypoint(route_location)
def detect_lane_obstacle(actor, extension_factor=3, margin=1.02):
"""
This function identifies if an obstacle is present in front of the reference actor
"""
world = CarlaDataProvider.get_world()
world_actors = world.get_actors().filter('vehicle.*')
actor_bbox = actor.bounding_box
actor_transform = actor.get_transform()
actor_location = actor_transform.location
actor_vector = actor_transform.rotation.get_forward_vector()
actor_vector = np.array([actor_vector.x, actor_vector.y])
actor_vector = actor_vector / np.linalg.norm(actor_vector)
actor_vector = actor_vector * (extension_factor - 1) * actor_bbox.extent.x
actor_location = actor_location + carla.Location(actor_vector[0], actor_vector[1])
actor_yaw = actor_transform.rotation.yaw
is_hazard = False
for adversary in world_actors:
if adversary.id != actor.id and \
actor_transform.location.distance(adversary.get_location()) < 50:
adversary_bbox = adversary.bounding_box
adversary_transform = adversary.get_transform()
adversary_loc = adversary_transform.location
adversary_yaw = adversary_transform.rotation.yaw
overlap_adversary = RotatedRectangle(
adversary_loc.x, adversary_loc.y,
2 * margin * adversary_bbox.extent.x, 2 * margin * adversary_bbox.extent.y, adversary_yaw)
overlap_actor = RotatedRectangle(
actor_location.x, actor_location.y,
2 * margin * actor_bbox.extent.x * extension_factor, 2 * margin * actor_bbox.extent.y, actor_yaw)
overlap_area = overlap_adversary.intersection(overlap_actor).area
if overlap_area > 0:
is_hazard = True
break
return is_hazard
def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
timeout=60):
"""
Initialize all parameters required for NewScenario
"""
# ToDo
self._map = CarlaDataProvider.get_map()
self._first_vehicle_location = 25
self._first_vehicle_speed = 10
self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location)
self._other_actor_max_brake = 1.0
self._other_actor_stop_in_front_intersection = 20
self._other_actor_transform = None
# Timeout of scenario in seconds
self.timeout = timeout
# Call constructor of BasicScenario
super(NewScenario, self).__init__("NewScenario",
ego_vehicles,
config,
world,
debug_mode,
criteria_enable=criteria_enable)
def create_cones_side(
self,
start_transform,
forward_vector,
z_inc=0,
cone_length=0,
cone_offset=0):
"""
Creates One Side of the Cones
"""
_dist = 0
while _dist < (cone_length * cone_offset):
# Move forward
_dist += cone_offset
forward_dist = carla.Vector3D(0, 0, 0) + forward_vector * _dist
location = start_transform.location + forward_dist
location.z += z_inc
transform = carla.Transform(location, start_transform.rotation)
cone = CarlaDataProvider.request_new_actor(
'static.prop.constructioncone', transform)
cone.set_simulate_physics(True)
self.other_actors.append(cone)
def run_scenario(self):
"""
Trigger the start of the scenario and wait for it to finish/fail
"""
print("ScenarioManager: Running scenario {}".format(
self.scenario_tree.name))
self.start_system_time = time.time()
start_game_time = GameTime.get_time()
self._running = True
while self._running:
self._tick_scenario(
CarlaDataProvider.get_world().get_snapshot().timestamp)
self.end_system_time = time.time()
end_game_time = GameTime.get_time()
self.scenario_duration_system = self.end_system_time - \
self.start_system_time
self.scenario_duration_game = end_game_time - start_game_time
if self.scenario_tree.status == py_trees.common.Status.FAILURE:
print("ScenarioManager: Terminated due to failure")
def _initialize_actors(self, config):
"""
Set other_actors to the superset of all scenario actors
"""
# Create the background activity of the route
town_amount = {
'Town01': 120,
'Town02': 100,
'Town03': 120,
'Town04': 200,
'Town05': 120,
'Town06': 150,
'Town07': 110,
'Town08': 180,
'Town09': 300,
'Town10': 120,
}
amount = town_amount[config.town] if config.town in town_amount else 0
new_actors = CarlaDataProvider.request_new_batch_actors('vehicle.*',
amount,
carla.Transform(),
autopilot=True,
random_location=True,
rolename='background')
if new_actors is None:
raise Exception("Error: Unable to add the background activity, all spawn points were occupied")
for _actor in new_actors:
self.other_actors.append(_actor)
# Add all the actors of the specific scenarios to self.other_actors
for scenario in self.list_scenarios:
self.other_actors.extend(scenario.other_actors)
def _initialize_actors(self, config):
"""
Custom initialization
"""
_start_distance = 40
lane_width = self._reference_waypoint.lane_width
location, _ = get_location_in_distance_from_wp(
self._reference_waypoint, _start_distance)
waypoint = self._wmap.get_waypoint(location)
offset = {"orientation": 270, "position": 90, "z": 0.4, "k": 0.2}
position_yaw = waypoint.transform.rotation.yaw + offset['position']
orientation_yaw = waypoint.transform.rotation.yaw + offset[
'orientation']
offset_location = carla.Location(
offset['k'] * lane_width * math.cos(math.radians(position_yaw)),
offset['k'] * lane_width * math.sin(math.radians(position_yaw)))
location += offset_location
location.z += offset['z']
self.transform = carla.Transform(location,
carla.Rotation(yaw=orientation_yaw))
static = CarlaDataProvider.request_new_actor('static.prop.container',
self.transform)
static.set_simulate_physics(True)
self.other_actors.append(static)
def update(self):
"""
Check velocity
"""
new_status = py_trees.common.Status.RUNNING
if self.actor is None:
return new_status
velocity = CarlaDataProvider.get_velocity(self.actor)
self.actual_value = max(velocity, self.actual_value)
if velocity > self.expected_value_success:
self.test_status = "FAILURE"
else:
self.test_status = "SUCCESS"
if self._terminate_on_failure and (self.test_status == "FAILURE"):
new_status = py_trees.common.Status.FAILURE
self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
return new_status
def update(self):
"""
Check distance
"""
new_status = py_trees.common.Status.RUNNING
if self.actor is None:
return new_status
location = CarlaDataProvider.get_location(self.actor)
if location is None:
return new_status
if self._last_location is None:
self._last_location = location
return new_status
self.actual_value += location.distance(self._last_location)
self._last_location = location
if self.actual_value > self.expected_value_success:
self.test_status = "SUCCESS"
elif (self.expected_value_acceptable is not None
and self.actual_value > self.expected_value_acceptable):
self.test_status = "ACCEPTABLE"
else:
self.test_status = "RUNNING"
if self._terminate_on_failure and (self.test_status == "FAILURE"):
new_status = py_trees.common.Status.FAILURE
self.logger.debug("%s.update()[%s->%s]" %
(self.__class__.__name__, self.status, new_status))
return new_status
def _setup_scenario_trigger(self, config):
"""
This function creates a trigger maneuver, that has to be finished before the real scenario starts.
This implementation focuses on the first available ego vehicle.
The function can be overloaded by a user implementation inside the user-defined scenario class.
"""
start_location = None
if config.trigger_points and config.trigger_points[0]:
start_location = config.trigger_points[0].location # start location of the scenario
ego_vehicle_route = CarlaDataProvider.get_ego_vehicle_route()
if start_location:
if ego_vehicle_route:
return conditions.InTriggerDistanceToLocationAlongRoute(self.ego_vehicles[0],
ego_vehicle_route,
start_location,
5)
return conditions.InTimeToArrivalToLocation(self.ego_vehicles[0],
2.0,
start_location)
return None
def _build_scenario_instances(self,
world,
ego_vehicle,
scenario_definitions,
town,
scenarios_per_tick=5,
timeout=300,
debug_mode=False):
"""
Based on the parsed route and possible scenarios, build all the scenario classes.
"""
scenario_instance_vec = []
if debug_mode:
for scenario in scenario_definitions:
loc = carla.Location(
scenario['trigger_position']['x'],
scenario['trigger_position']['y'],
scenario['trigger_position']['z']) + carla.Location(z=2.0)
world.debug.draw_point(loc,
size=1.0,
color=carla.Color(255, 0, 0),
life_time=100000)
world.debug.draw_string(loc,
str(scenario['name']),
draw_shadow=False,
color=carla.Color(0, 0, 255),
life_time=100000,
persistent_lines=True)
for scenario_number, definition in enumerate(scenario_definitions):
# Get the class possibilities for this scenario number
scenario_class = NUMBER_CLASS_TRANSLATION[definition['name']]
# Create the other actors that are going to appear
if definition['other_actors'] is not None:
list_of_actor_conf_instances = self._get_actors_instances(
definition['other_actors'])
else:
list_of_actor_conf_instances = []
# Create an actor configuration for the ego-vehicle trigger position
egoactor_trigger_position = convert_json_to_transform(
definition['trigger_position'])
scenario_configuration = ScenarioConfiguration()
scenario_configuration.other_actors = list_of_actor_conf_instances
scenario_configuration.town = town
scenario_configuration.trigger_points = [egoactor_trigger_position]
scenario_configuration.subtype = definition['scenario_type']
scenario_configuration.ego_vehicles = [
ActorConfigurationData('vehicle.lincoln.mkz2017',
ego_vehicle.get_transform(), 'hero')
]
route_var_name = "ScenarioRouteNumber{}".format(scenario_number)
scenario_configuration.route_var_name = route_var_name
try:
scenario_instance = scenario_class(world, [ego_vehicle],
scenario_configuration,
criteria_enable=False,
timeout=timeout)
# Do a tick every once in a while to avoid spawning everything at the same time
if scenario_number % scenarios_per_tick == 0:
sync_mode = world.get_settings().synchronous_mode
if sync_mode:
CarlaDataProvider.perform_carla_tick()
else:
world.wait_for_tick()
scenario_number += 1
except Exception as e: # pylint: disable=broad-except
if debug_mode:
traceback.print_exc()
print("Skipping scenario '{}' due to setup error: {}".format(
definition['name'], e))
continue
scenario_instance_vec.append(scenario_instance)
return scenario_instance_vec
def __call__(self):
return {'opendrive': CarlaDataProvider.get_map().to_opendrive()}
def get_distance_along_route(route, target_location):
"""
Calculate the distance of the given location along the route
Note: If the location is not along the route, the route length will be returned
"""
wmap = CarlaDataProvider.get_map()
covered_distance = 0
prev_position = None
found = False
# Don't use the input location, use the corresponding wp as location
target_location_from_wp = wmap.get_waypoint(
target_location).transform.location
for position, _ in route:
location = target_location_from_wp
# Don't perform any calculations for the first route point
if not prev_position:
prev_position = position
continue
# Calculate distance between previous and current route point
interval_length_squared = ((prev_position.x - position.x)**2) + (
(prev_position.y - position.y)**2)
distance_squared = ((location.x - prev_position.x)**2) + (
(location.y - prev_position.y)**2)
# Close to the current position? Stop calculation
if distance_squared < 0.01:
break
if distance_squared < 400 and not distance_squared < interval_length_squared:
# Check if a neighbor lane is closer to the route
# Do this only in a close distance to correct route interval, otherwise the computation load is too high
starting_wp = wmap.get_waypoint(location)
wp = starting_wp.get_left_lane()
while wp is not None:
new_location = wp.transform.location
new_distance_squared = (
(new_location.x - prev_position.x)**2) + (
(new_location.y - prev_position.y)**2)
if np.sign(starting_wp.lane_id) != np.sign(wp.lane_id):
break
if new_distance_squared < distance_squared:
distance_squared = new_distance_squared
location = new_location
else:
break
wp = wp.get_left_lane()
wp = starting_wp.get_right_lane()
while wp is not None:
new_location = wp.transform.location
new_distance_squared = (
(new_location.x - prev_position.x)**2) + (
(new_location.y - prev_position.y)**2)
if np.sign(starting_wp.lane_id) != np.sign(wp.lane_id):
break
if new_distance_squared < distance_squared:
distance_squared = new_distance_squared
location = new_location
else:
break
wp = wp.get_right_lane()
if distance_squared < interval_length_squared:
# The location could be inside the current route interval, if route/lane ids match
# Note: This assumes a sufficiently small route interval
# An alternative is to compare orientations, however, this also does not work for
# long route intervals
curr_wp = wmap.get_waypoint(position)
prev_wp = wmap.get_waypoint(prev_position)
wp = wmap.get_waypoint(location)
if prev_wp and curr_wp and wp:
if wp.road_id == prev_wp.road_id or wp.road_id == curr_wp.road_id:
# Roads match, now compare the sign of the lane ids
if (np.sign(wp.lane_id) == np.sign(prev_wp.lane_id) or
np.sign(wp.lane_id) == np.sign(curr_wp.lane_id)):
# The location is within the current route interval
covered_distance += math.sqrt(distance_squared)
found = True
break
covered_distance += math.sqrt(interval_length_squared)
prev_position = position
return covered_distance, found
def _tick_scenario(self, timestamp):
"""
Run next tick of scenario and the agent and tick the world.
"""
if self._timestamp_last_run < timestamp.elapsed_seconds and self._running:
self._timestamp_last_run = timestamp.elapsed_seconds
self._watchdog.update()
# Update game time and actor information
GameTime.on_carla_tick(timestamp)
CarlaDataProvider.on_carla_tick()
game_time_ms = int(GameTime.get_time() * 1000)
try:
res = self._agent()
if self.timely_commands:
assert isinstance(res, tuple), 'Agent did not return the runtime'
ego_action, runtime = res
else:
if isinstance(res, tuple):
ego_action = res[0]
else:
ego_action = res
runtime = 0
heapq.heappush(self._delayed_cmds,
(game_time_ms + runtime, game_time_ms, ego_action))
# Special exception inside the agent that isn't caused by the agent
except SensorReceivedNoData as e:
raise RuntimeError(e)
except Exception as e:
raise AgentError(e)
while len(self._delayed_cmds) > 0 and game_time_ms >= self._delayed_cmds[0][0]:
self._last_cmd = self._delayed_cmds[0]
heapq.heappop(self._delayed_cmds)
print("Command at {} using {}".format(self._last_cmd[0], self._last_cmd[1]))
self.ego_vehicles[0].apply_control(self._last_cmd[2])
# Tick scenario
self.scenario_tree.tick_once()
if self._debug_mode:
print("\n")
py_trees.display.print_ascii_tree(
self.scenario_tree, show_status=True)
sys.stdout.flush()
if self.scenario_tree.status != py_trees.common.Status.RUNNING:
self._running = False
spectator = CarlaDataProvider.get_world().get_spectator()
ego_trans = self.ego_vehicles[0].get_transform()
spectator.set_transform(carla.Transform(ego_trans.location + carla.Location(z=50),
carla.Rotation(pitch=-90)))
if self._running and self.get_running_status():
CarlaDataProvider.get_world().tick(self._timeout)
def initialise(self):
self._location = CarlaDataProvider.get_location(self._actor)
super(DriveDistance, self).initialise()
def run_scenario(self,path,path1):
"""
Trigger the start of the scenario and wait for it to finish/fail
"""
print("ScenarioManager: Running scenario {}".format(self.scenario_tree.name))
self.start_system_time = time.time()
start_game_time = GameTime.get_time()
total_risk_score = []
self._watchdog.start()
self._running = True
x = 0
while self._running:
timestamp = None
world = CarlaDataProvider.get_world()
if world:
snapshot = world.get_snapshot()
if snapshot:
timestamp = snapshot.timestamp
if timestamp:
self._tick_scenario(timestamp)
x+=1
if(x%60==1):
res = nvidia_smi.nvmlDeviceGetUtilizationRates(self.handle)
mem_res = nvidia_smi.nvmlDeviceGetMemoryInfo(self.handle)
#print(f'mem: {mem_res.used / (1024**2)} (GiB)') # usage in GiB
print(f'mem: {100 * (mem_res.used / mem_res.total):.3f}%') # percentage usage
cpu = psutil.cpu_percent()#cpu utilization stats
mem = psutil.virtual_memory()#virtual memory stats
print(f'gpu: {res.gpu}%, gpu-mem: {res.memory}%')
fields = ['GPU Utilization',
'GPU Memory',
'CPU Utilization',
'CPU Memory'
]
dict = [{'GPU Utilization':res.gpu, 'GPU Memory':100 * (mem_res.used / mem_res.total), 'CPU Utilization':cpu, 'CPU Memory':100 * (mem.used/mem.total)}]
file_exists = os.path.isfile(path1)
with open(path1, 'a') as csvfile:
# creating a csv dict writer object
writer = csv.DictWriter(csvfile, fieldnames = fields)
if not file_exists:
writer.writeheader()
writer.writerows(dict)
#total_risk_score.append(risk_score)
#print("--------------------------------------------------------------------------")
#print("Average Risk Score:%f"%(float(sum(total_risk_score))/len(total_risk_score)))
#print("--------------------------------------------------------------------------")
self._watchdog.stop()
self.end_system_time = time.time()
end_game_time = GameTime.get_time()
self.scenario_duration_system = self.end_system_time - \
self.start_system_time
self.scenario_duration_game = end_game_time - start_game_time
fields = ['Route Completed',
'Collisions'
]
route_completed, collisions = self._console_message()
dict = [{'Route Completed':route_completed, 'Collisions':collisions}]
file_exists = os.path.isfile(path)
with open(path, 'a') as csvfile:
# creating a csv dict writer object
writer = csv.DictWriter(csvfile, fieldnames = fields)
if not file_exists:
writer.writeheader()
writer.writerows(dict)
def setup_sensors(self, vehicle, debug_mode=False):
"""
Create the sensors defined by the user and attach them to the ego-vehicle
:param vehicle: ego vehicle
:return:
"""
bp_library = CarlaDataProvider.get_world().get_blueprint_library()
for sensor_spec in self._agent.sensors():
# These are the pseudosensors (not spawned)
if sensor_spec['type'].startswith('sensor.opendrive_map'):
# The HDMap pseudo sensor is created directly here
sensor = OpenDriveMapReader(vehicle, sensor_spec['reading_frequency'])
elif sensor_spec['type'].startswith('sensor.speedometer'):
delta_time = CarlaDataProvider.get_world().get_settings().fixed_delta_seconds
frame_rate = 1 / delta_time
sensor = SpeedometerReader(vehicle, frame_rate)
# These are the sensors spawned on the carla world
else:
bp = bp_library.find(str(sensor_spec['type']))
if sensor_spec['type'].startswith('sensor.camera'):
bp.set_attribute('image_size_x', str(sensor_spec['width']))
bp.set_attribute('image_size_y', str(sensor_spec['height']))
bp.set_attribute('fov', str(sensor_spec['fov']))
bp.set_attribute('lens_circle_multiplier', str(3.0))
bp.set_attribute('lens_circle_falloff', str(3.0))
bp.set_attribute('chromatic_aberration_intensity', str(0.5))
bp.set_attribute('chromatic_aberration_offset', str(0))
# Change __call__ in autonomous agent if you change sensor_tick.
bp.set_attribute('sensor_tick', '0.05')
sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
elif sensor_spec['type'].startswith('sensor.lidar'):
bp.set_attribute('range', str(85))
bp.set_attribute('rotation_frequency', str(20))
bp.set_attribute('channels', str(64))
bp.set_attribute('upper_fov', str(10))
bp.set_attribute('lower_fov', str(-30))
bp.set_attribute('points_per_second', str(600000))
# bp.set_attribute('atmosphere_attenuation_rate', str(0.004))
# bp.set_attribute('dropoff_general_rate', str(0.45))
# bp.set_attribute('dropoff_intensity_limit', str(0.8))
# bp.set_attribute('dropoff_zero_intensity', str(0.4))
bp.set_attribute('sensor_tick', '0.05')
sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
elif sensor_spec['type'].startswith('sensor.other.radar'):
bp.set_attribute('horizontal_fov', str(sensor_spec['fov'])) # degrees
bp.set_attribute('vertical_fov', str(sensor_spec['fov'])) # degrees
bp.set_attribute('points_per_second', '1500')
bp.set_attribute('range', '100') # meters
sensor_location = carla.Location(x=sensor_spec['x'],
y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
elif sensor_spec['type'].startswith('sensor.other.gnss'):
bp.set_attribute('noise_alt_stddev', str(0.000005))
bp.set_attribute('noise_lat_stddev', str(0.000005))
bp.set_attribute('noise_lon_stddev', str(0.000005))
bp.set_attribute('noise_alt_bias', str(0.0))
bp.set_attribute('noise_lat_bias', str(0.0))
bp.set_attribute('noise_lon_bias', str(0.0))
sensor_location = carla.Location(x=sensor_spec['x'],
y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation()
elif sensor_spec['type'].startswith('sensor.other.imu'):
bp.set_attribute('noise_accel_stddev_x', str(0.001))
bp.set_attribute('noise_accel_stddev_y', str(0.001))
bp.set_attribute('noise_accel_stddev_z', str(0.015))
bp.set_attribute('noise_gyro_stddev_x', str(0.001))
bp.set_attribute('noise_gyro_stddev_y', str(0.001))
bp.set_attribute('noise_gyro_stddev_z', str(0.001))
sensor_location = carla.Location(x=sensor_spec['x'],
y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
# create sensor
sensor_transform = carla.Transform(sensor_location, sensor_rotation)
sensor = CarlaDataProvider.get_world().spawn_actor(bp, sensor_transform, vehicle)
# setup callback
sensor.listen(CallBack(sensor_spec['id'], sensor_spec['type'], sensor, self._agent.sensor_interface))
self._sensors_list.append(sensor)
# Tick once to spawn the sensors
CarlaDataProvider.get_world().tick()
def convert_position_to_transform(position, actor_list=None):
"""
Convert an OpenScenario position into a CARLA transform
Not supported: Road, RelativeRoad, Lane, RelativeLane as the PythonAPI currently
does not provide sufficient access to OpenDrive information
Also not supported is Route. This can be added by checking additional
route information
"""
if position.find('World') is not None:
world_pos = position.find('World')
x = float(world_pos.attrib.get('x', 0))
y = float(world_pos.attrib.get('y', 0))
z = float(world_pos.attrib.get('z', 0))
yaw = math.degrees(float(world_pos.attrib.get('h', 0)))
pitch = math.degrees(float(world_pos.attrib.get('p', 0)))
roll = math.degrees(float(world_pos.attrib.get('r', 0)))
if not OpenScenarioParser.use_carla_coordinate_system:
y = y * (-1.0)
yaw = yaw * (-1.0)
return carla.Transform(
carla.Location(x=x, y=y, z=z),
carla.Rotation(yaw=yaw, pitch=pitch, roll=roll))
elif ((position.find('RelativeWorld') is not None)
or (position.find('RelativeObject') is not None)
or (position.find('RelativeLane') is not None)):
rel_pos = position.find('RelativeWorld') or position.find(
'RelativeObject') or position.find('RelativeLane')
# get relative object and relative position
obj = rel_pos.attrib.get('object')
obj_actor = None
actor_transform = None
if actor_list is not None:
for actor in actor_list:
if actor.rolename == obj:
obj_actor = actor
actor_transform = actor.transform
else:
for actor in CarlaDataProvider.get_world().get_actors():
if 'role_name' in actor.attributes and actor.attributes[
'role_name'] == obj:
obj_actor = actor
actor_transform = obj_actor.get_transform()
break
if obj_actor is None:
raise AttributeError(
"Object '{}' provided as position reference is not known".
format(obj))
# calculate orientation h, p, r
is_absolute = False
if rel_pos.find('Orientation') is not None:
orientation = rel_pos.find('Orientation')
is_absolute = (orientation.attrib.get('type') == "absolute")
dyaw = math.degrees(float(orientation.attrib.get('h', 0)))
dpitch = math.degrees(float(orientation.attrib.get('p', 0)))
droll = math.degrees(float(orientation.attrib.get('r', 0)))
if not OpenScenarioParser.use_carla_coordinate_system:
dyaw = dyaw * (-1.0)
yaw = actor_transform.rotation.yaw
pitch = actor_transform.rotation.pitch
roll = actor_transform.rotation.roll
if not is_absolute:
yaw = yaw + dyaw
pitch = pitch + dpitch
roll = roll + droll
else:
yaw = dyaw
pitch = dpitch
roll = droll
# calculate location x, y, z
# dx, dy, dz
if (position.find('RelativeWorld')
is not None) or (position.find('RelativeObject')
is not None):
dx = float(rel_pos.attrib.get('dx', 0))
dy = float(rel_pos.attrib.get('dy', 0))
dz = float(rel_pos.attrib.get('dz', 0))
if not OpenScenarioParser.use_carla_coordinate_system:
dy = dy * (-1.0)
x = actor_transform.location.x + dx
y = actor_transform.location.y + dy
z = actor_transform.location.z + dz
# dLane, ds, offset
elif position.find('RelativeLane') is not None:
dlane = float(rel_pos.attrib.get('dLane'))
ds = float(rel_pos.attrib.get('ds'))
offset = float(rel_pos.attrib.get('offset'))
carla_map = CarlaDataProvider.get_map()
relative_waypoint = carla_map.get_waypoint(
actor_transform.location)
if dlane == 0:
wp = relative_waypoint
elif dlane == -1:
wp = relative_waypoint.get_left_lane()
elif dlane == 1:
wp = relative_waypoint.get_right_lane()
if wp is None:
raise AttributeError(
"Object '{}' position with dLane={} is not valid".
format(obj, dlane))
wp = wp.next(ds)[-1]
# offset
# Verschiebung von Mittelpunkt
h = math.radians(wp.transform.rotation.yaw)
x_offset = math.sin(h) * offset
y_offset = math.cos(h) * offset
if OpenScenarioParser.use_carla_coordinate_system:
x_offset = x_offset * (-1.0)
y_offset = y_offset * (-1.0)
x = wp.transform.location.x + x_offset
y = wp.transform.location.y + y_offset
z = wp.transform.location.z
return carla.Transform(
carla.Location(x=x, y=y, z=z),
carla.Rotation(yaw=yaw, pitch=pitch, roll=roll))
# Not implemented
elif position.find('Road') is not None:
raise NotImplementedError("Road positions are not yet supported")
elif position.find('RelativeRoad') is not None:
raise NotImplementedError(
"RelativeRoad positions are not yet supported")
elif position.find('Lane') is not None:
raise NotImplementedError("Lane positions are not yet supported")
elif position.find('Route') is not None:
raise NotImplementedError("Route positions are not yet supported")
else:
raise AttributeError("Unknown position")
def test_routes(args):
"""
Run all routes according to provided commandline args
"""
# Routes are always visible
args.route_visible = True
challenge = ChallengeEvaluator(args)
# retrieve worlds annotations
world_annotations = parser.parse_annotations_file(args.scenarios)
# retrieve routes
route_descriptions_list = parser.parse_routes_file(args.routes)
# find and filter potential scenarios for each of the evaluated routes
# For each of the routes and corresponding possible scenarios to be evaluated.
route_description = route_descriptions_list[args.route_id]
# setup world and client assuming that the CARLA server is up and running
client = carla.Client(args.host, int(args.port))
client.set_timeout(challenge.client_timeout)
# load the challenge.world variable to be used during the route
challenge.load_world(client, route_description['town_name'])
# Set the actor pool so the scenarios can prepare themselves when needed
CarlaActorPool.set_client(client)
CarlaActorPool.set_world(challenge.world)
# Also se the Data provider pool.
CarlaDataProvider.set_world(challenge.world)
# tick world so we can start.
challenge.world.tick()
# prepare route's trajectory
gps_route, route_description['trajectory'] = interpolate_trajectory(
challenge.world, route_description['trajectory'])
CarlaDataProvider.set_ego_vehicle_route(
convert_transform_to_location(route_description['trajectory']))
potential_scenarios_definitions, existent_triggers = parser.scan_route_for_scenarios(
route_description, world_annotations)
potential_scenarios_definitions = challenge.filter_scenarios(
potential_scenarios_definitions, args.removed_scenarios)
print("WE HAVE This number of posibilities : ",
len(potential_scenarios_definitions))
# Sample the scenarios to be used for this route instance.
sampled_scenarios_definitions = challenge.scenario_sampling(
potential_scenarios_definitions)
# create agent
challenge.agent_instance = getattr(
challenge.module_agent, challenge.module_agent.__name__)(args.config)
correct_sensors, error_message = challenge.valid_sensors_configuration(
challenge.agent_instance, challenge.track)
if not correct_sensors:
# the sensor configuration is illegal
challenge.report_fatal_error(args.filename, args.show_to_participant,
error_message)
return
challenge.agent_instance.set_global_plan(gps_route,
route_description['trajectory'])
# prepare the ego car to run the route.
# It starts on the first wp of the route
elevate_transform = route_description['trajectory'][0][0]
elevate_transform.location.z += 0.5
challenge.prepare_ego_car(elevate_transform)
# build the master scenario based on the route and the target.
challenge.master_scenario = challenge.build_master_scenario(
route_description['trajectory'], route_description['town_name'])
list_scenarios = [challenge.master_scenario]
if args.background:
background_scenario = challenge.build_background_scenario(
route_description['town_name'])
list_scenarios.append(background_scenario)
# build the instance based on the parsed definitions.
print("Definition of the scenarios present on the route ")
pprint(sampled_scenarios_definitions)
list_scenarios += challenge.build_scenario_instances(
sampled_scenarios_definitions, route_description['town_name'])
if args.debug > 0:
for scenario in sampled_scenarios_definitions:
loc = carla.Location(
scenario['trigger_position']['x'],
scenario['trigger_position']['y'],
scenario['trigger_position']['z']) + carla.Location(z=2.0)
challenge.world.debug.draw_point(loc,
size=1.0,
color=carla.Color(255, 0, 0),
life_time=100000)
challenge.world.debug.draw_string(loc,
scenario['name'],
draw_shadow=False,
color=carla.Color(0, 0, 255),
life_time=100000,
persistent_lines=True)
# Tick once to start the scenarios.
print(" Running these scenarios --- ", list_scenarios)
for scenario in list_scenarios:
scenario.scenario.scenario_tree.tick_once()
challenge.run_route(list_scenarios, route_description['trajectory'])
# statistics recording
challenge.record_route_statistics(route_description['id'])
# clean up
for scenario in list_scenarios:
scenario.remove_all_actors()
challenge.cleanup(ego=True)
challenge.agent_instance.destroy()
def _run_challenge(self):
"""
Run the challenge mode
"""
result = False
phase_codename = os.getenv('CHALLENGE_PHASE_CODENAME', 'dev_track_3')
phase = phase_codename.split("_")[0]
repetitions = self._args.repetitions
if self._args.challenge:
weather_profiles = CarlaDataProvider.find_weather_presets()
scenario_runner_root = os.getenv('ROOT_SCENARIO_RUNNER', "./")
if phase == 'dev':
routes = '{}/srunner/challenge/routes_devtest.xml'.format(
scenario_runner_root)
repetitions = 1
elif phase == 'validation':
routes = '{}/srunner/challenge/routes_testprep.xml'.format(
scenario_runner_root)
repetitions = 3
elif phase == 'test':
routes = '{}/srunner/challenge/routes_testchallenge.xml'.format(
scenario_runner_root)
repetitions = 3
else:
# debug mode
routes = '{}/srunner/challenge/routes_debug.xml'.format(
scenario_runner_root)
repetitions = 1
if self._args.route:
routes = self._args.route[0]
scenario_file = self._args.route[1]
single_route = None
if len(self._args.route) > 2:
single_route = self._args.route[2]
# retrieve routes
route_descriptions_list = RouteParser.parse_routes_file(
routes, single_route)
# find and filter potential scenarios for each of the evaluated routes
# For each of the routes and corresponding possible scenarios to be evaluated.
if self._args.challenge:
n_routes = len(route_descriptions_list) * repetitions
ChallengeStatisticsManager.set_number_of_scenarios(n_routes)
for _, route_description in enumerate(route_descriptions_list):
for repetition in range(repetitions):
if self._args.challenge and not self._within_available_time():
error_message = 'Not enough simulation time available to continue'
print(error_message)
ChallengeStatisticsManager.record_fatal_error(
error_message)
self._cleanup()
return False
config = RouteScenarioConfiguration(route_description,
scenario_file)
if self._args.challenge:
profile = weather_profiles[repetition %
len(weather_profiles)]
config.weather = profile[0]
config.weather.sun_azimuth_angle = -1
config.weather.sun_altitude_angle = -1
result = self._load_and_run_scenario(config)
self._cleanup()
return result
def run_step(self):
"""
Execute on tick of the controller's control loop
If _waypoints are provided, the vehicle moves towards the next waypoint
with the given _target_speed, until reaching the final waypoint. Upon reaching
the final waypoint, _reached_goal is set to True.
If _waypoints is empty, the vehicle moves in its current direction with
the given _target_speed.
For further details see :func:`_set_new_velocity`
"""
if self._reached_goal:
# Reached the goal, so stop
velocity = carla.Vector3D(0, 0, 0)
self._actor.set_target_velocity(velocity)
return
if self._visualizer:
self._visualizer.render()
self._reached_goal = False
if not self._waypoints:
# No waypoints are provided, so we have to create a list of waypoints internally
# get next waypoints from map, to avoid leaving the road
self._reached_goal = False
map_wp = None
if not self._generated_waypoint_list:
map_wp = CarlaDataProvider.get_map().get_waypoint(
CarlaDataProvider.get_location(self._actor))
else:
map_wp = CarlaDataProvider.get_map().get_waypoint(
self._generated_waypoint_list[-1].location)
while len(self._generated_waypoint_list) < 50:
map_wps = map_wp.next(2.0)
if map_wps:
self._generated_waypoint_list.append(map_wps[0].transform)
map_wp = map_wps[0]
else:
break
# Remove all waypoints that are too close to the vehicle
while (self._generated_waypoint_list
and self._generated_waypoint_list[0].location.distance(
self._actor.get_location()) < 0.5):
self._generated_waypoint_list = self._generated_waypoint_list[
1:]
direction_norm = self._set_new_velocity(
self._offset_waypoint(self._generated_waypoint_list[0]))
if direction_norm < 2.0:
self._generated_waypoint_list = self._generated_waypoint_list[
1:]
else:
# When changing from "free" driving without pre-defined waypoints to a defined route with waypoints
# it may happen that the first few waypoints are too close to the ego vehicle for obtaining a
# reasonable control command. Therefore, we drop these waypoints first.
while self._waypoints and self._waypoints[0].location.distance(
self._actor.get_location()) < 0.5:
self._waypoints = self._waypoints[1:]
self._reached_goal = False
if not self._waypoints:
self._reached_goal = True
else:
direction_norm = self._set_new_velocity(
self._offset_waypoint(self._waypoints[0]))
if direction_norm < 4.0:
self._waypoints = self._waypoints[1:]
if not self._waypoints:
self._reached_goal = True
def initialise(self):
self._last_location = CarlaDataProvider.get_location(self.actor)
super(AverageVelocityTest, self).initialise()
def initialise(self):
self._last_location = CarlaDataProvider.get_location(self.actor)
super(DrivenDistanceTest, self).initialise()
def _set_new_velocity(self, next_location):
"""
Calculate and set the new actor veloctiy given the current actor
location and the _next_location_
If _consider_obstacles is true, the speed is adapted according to the closest
obstacle in front of the actor, if it is within the _proximity_threshold distance.
If _consider_trafficlights is true, the vehicle will enforce a stop at a red
traffic light.
If _max_deceleration is set, the vehicle will reduce its speed according to the
given deceleration value.
If the vehicle reduces its speed, braking lights will be activated.
Args:
next_location (carla.Location): Next target location of the actor
returns:
direction (carla.Vector3D): Length of direction vector of the actor
"""
current_time = GameTime.get_time()
target_speed = self._target_speed
if not self._last_update:
self._last_update = current_time
current_speed = math.sqrt(self._actor.get_velocity().x**2 +
self._actor.get_velocity().y**2)
if self._consider_obstacles:
# If distance is less than the proximity threshold, adapt velocity
if self._obstacle_distance < self._proximity_threshold:
distance = max(self._obstacle_distance, 0)
if distance > 0:
current_speed_other = math.sqrt(
self._obstacle_actor.get_velocity().x**2 +
self._obstacle_actor.get_velocity().y**2)
if current_speed_other < current_speed:
acceleration = -0.5 * (
current_speed - current_speed_other)**2 / distance
target_speed = max(
acceleration * (current_time - self._last_update) +
current_speed, 0)
else:
target_speed = 0
if self._consider_traffic_lights:
if (self._actor.is_at_traffic_light()
and self._actor.get_traffic_light_state()
== carla.TrafficLightState.Red):
target_speed = 0
if target_speed < current_speed:
self._actor.set_light_state(carla.VehicleLightState.Brake)
if self._max_deceleration is not None:
target_speed = max(
target_speed,
current_speed - (current_time - self._last_update) *
self._max_deceleration)
else:
self._actor.set_light_state(carla.VehicleLightState.NONE)
if self._max_acceleration is not None:
target_speed = min(
target_speed,
current_speed + (current_time - self._last_update) *
self._max_acceleration)
# set new linear velocity
velocity = carla.Vector3D(0, 0, 0)
direction = next_location - CarlaDataProvider.get_location(self._actor)
direction_norm = math.sqrt(direction.x**2 + direction.y**2)
velocity.x = direction.x / direction_norm * target_speed
velocity.y = direction.y / direction_norm * target_speed
self._actor.set_target_velocity(velocity)
# set new angular velocity
current_yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw
# When we have a waypoint list, use the direction between the waypoints to calculate the heading (change)
# otherwise use the waypoint heading directly
if self._waypoints:
delta_yaw = math.degrees(math.atan2(direction.y,
direction.x)) - current_yaw
else:
new_yaw = CarlaDataProvider.get_map().get_waypoint(
next_location).transform.rotation.yaw
delta_yaw = new_yaw - current_yaw
if math.fabs(delta_yaw) > 360:
delta_yaw = delta_yaw % 360
if delta_yaw > 180:
delta_yaw = delta_yaw - 360
elif delta_yaw < -180:
delta_yaw = delta_yaw + 360
angular_velocity = carla.Vector3D(0, 0, 0)
if target_speed == 0:
angular_velocity.z = 0
else:
angular_velocity.z = delta_yaw / (direction_norm / target_speed)
self._actor.set_target_angular_velocity(angular_velocity)
self._last_update = current_time
return direction_norm
def update(self):
master_scenario_command = self.blackboard.get(
'master_scenario_command')
if master_scenario_command and master_scenario_command == 'scenarios_stop_request':
new_status = py_trees.common.Status.SUCCESS
return new_status
else:
new_status = py_trees.common.Status.RUNNING
# find a suitable target
if not self.target_traffic_light:
traffic_light = CarlaDataProvider.get_next_traffic_light(
self.ego_vehicle, use_cached_location=False)
if not traffic_light:
# nothing else to do in this iteration...
return new_status
base_transform = traffic_light.get_transform()
area_loc = carla.Location(
base_transform.transform(
traffic_light.trigger_volume.location))
distance_to_traffic_light = area_loc.distance(
self.ego_vehicle.get_location())
if self.debug:
print("[{}] distance={}".format(traffic_light.id,
distance_to_traffic_light))
if distance_to_traffic_light < self.MAX_DISTANCE_TRAFFIC_LIGHT:
self.target_traffic_light = traffic_light
self.intervention = random.random(
) > self.RANDOM_VALUE_INTERVENTION
if self.intervention:
if self.debug:
print(
"--- We are going to affect the following intersection"
)
loc = self.target_traffic_light.get_location()
CarlaDataProvider.get_world().debug.draw_point(
loc + carla.Location(z=1.0),
size=0.5,
color=carla.Color(255, 255, 0),
life_time=50000)
self.annotations = CarlaDataProvider.annotate_trafficlight_in_group(
self.target_traffic_light)
else:
if not self.reset_annotations:
if self.intervention:
# the light has not been manipulated yet
choice = random.choice(self.INTERSECTION_CONFIGURATIONS)
self.reset_annotations = CarlaDataProvider.update_light_states(
self.target_traffic_light,
self.annotations,
choice,
freeze=True)
else:
# the traffic light has been manipulated...
base_transform = self.target_traffic_light.get_transform()
area_loc = carla.Location(
base_transform.transform(
self.target_traffic_light.trigger_volume.location))
distance_to_traffic_light = area_loc.distance(
self.ego_vehicle.get_location())
if self.debug:
print("++ distance={}".format(distance_to_traffic_light))
if distance_to_traffic_light > self.MAX_DISTANCE_TRAFFIC_LIGHT:
if self.reset_annotations:
CarlaDataProvider.reset_lights(self.reset_annotations)
self.target_traffic_light = None
self.reset_annotations = None
self.annotations = None
self.intervention = False
return new_status
def _create_behavior(self):
"""
Scenario behavior:
The other vehicle waits until the ego vehicle is close enough to the
intersection and that its own traffic light is red. Then, it will start
driving and 'illegally' cross the intersection. After a short distance
it should stop again, outside of the intersection. The ego vehicle has
to avoid the crash, but continue driving after the intersection is clear.
If this does not happen within 120 seconds, a timeout stops the scenario
"""
crossing_point_dynamic = get_crossing_point(self.ego_vehicles[0])
# start condition
startcondition = InTriggerDistanceToLocation(
self.ego_vehicles[0],
crossing_point_dynamic,
self._ego_distance_to_traffic_light,
name="Waiting for start position")
sync_arrival_parallel = py_trees.composites.Parallel(
"Synchronize arrival times",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
location_of_collision_dynamic = get_geometric_linear_intersection(
self.ego_vehicles[0], self.other_actors[0])
sync_arrival = SyncArrival(self.other_actors[0], self.ego_vehicles[0],
location_of_collision_dynamic)
sync_arrival_stop = InTriggerDistanceToNextIntersection(
self.other_actors[0], 5)
sync_arrival_parallel.add_child(sync_arrival)
sync_arrival_parallel.add_child(sync_arrival_stop)
# Generate plan for WaypointFollower
turn = 0 # drive straight ahead
plan = []
# generating waypoints until intersection (target_waypoint)
plan, target_waypoint = generate_target_waypoint_list(
CarlaDataProvider.get_map().get_waypoint(
self.other_actors[0].get_location()), turn)
# Generating waypoint list till next intersection
wp_choice = target_waypoint.next(5.0)
while len(wp_choice) == 1:
target_waypoint = wp_choice[0]
plan.append((target_waypoint, RoadOption.LANEFOLLOW))
wp_choice = target_waypoint.next(5.0)
continue_driving = py_trees.composites.Parallel(
"ContinueDriving",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
continue_driving_waypoints = WaypointFollower(
self.other_actors[0],
self._other_actor_target_velocity,
plan=plan,
avoid_collision=False)
continue_driving_distance = DriveDistance(self.other_actors[0],
self._other_actor_distance,
name="Distance")
continue_driving_timeout = TimeOut(10)
continue_driving.add_child(continue_driving_waypoints)
continue_driving.add_child(continue_driving_distance)
continue_driving.add_child(continue_driving_timeout)
# finally wait that ego vehicle drove a specific distance
wait = DriveDistance(self.ego_vehicles[0],
self._ego_distance_to_drive,
name="DriveDistance")
# Build behavior tree
sequence = py_trees.composites.Sequence("Sequence Behavior")
sequence.add_child(
ActorTransformSetter(self.other_actors[0],
self._other_actor_transform))
sequence.add_child(startcondition)
sequence.add_child(sync_arrival_parallel)
sequence.add_child(continue_driving)
sequence.add_child(wait)
sequence.add_child(ActorDestroy(self.other_actors[0]))
return sequence
def initialise(self):
# get initial actor position
self._initial_actor_pos = CarlaDataProvider.get_location(self._actor)
def test_routes(args):
"""
Run all routes according to provided commandline args
"""
# instance a challenge
challenge = ChallengeEvaluator(args)
# retrieve worlds annotations
world_annotations = parser.parse_annotations_file(args.scenarios)
# retrieve routes
routes = []
route_descriptions_list = parser.parse_routes_file(args.routes)
for route_description in route_descriptions_list:
if route_description['town_name'] == args.debug_town:
routes.append(route_description)
# find and filter potential scenarios for each of the evaluated routes
# For each of the routes and corresponding possible scenarios to be evaluated.
list_scenarios_town = []
if args.debug_town in world_annotations.keys():
list_scenarios_town = world_annotations[args.debug_town]
scenarios_current_type = []
for scenario in list_scenarios_town:
if args.debug_scenario == scenario['scenario_type']:
scenarios_current_type = scenario
break
for scenario_configuration in scenarios_current_type[
'available_event_configurations']:
scenario_conf = create_configuration_scenario(scenario_configuration,
args.debug_scenario)
client = carla.Client(args.host, int(args.port))
client.set_timeout(challenge.client_timeout)
# load the challenge.world variable to be used during the route
challenge.load_world(client, args.debug_town)
# Set the actor pool so the scenarios can prepare themselves when needed
CarlaActorPool.set_world(challenge.world)
# Also se the Data provider pool.
CarlaDataProvider.set_world(challenge.world)
# tick world so we can start.
challenge.world.tick()
# create agent
challenge.agent_instance = getattr(challenge.module_agent,
challenge.module_agent.__name__)(
args.config)
correct_sensors, error_message = challenge.valid_sensors_configuration(
challenge.agent_instance, challenge.track)
if not correct_sensors:
# the sensor configuration is illegal
challenge.report_fatal_error(args.filename,
args.show_to_participant,
error_message)
return
waypoint = routes[0]['trajectory'][0]
location = carla.Location(x=float(waypoint.attrib['x']),
y=float(waypoint.attrib['y']),
z=float(waypoint.attrib['z']))
rotation = carla.Rotation(pitch=float(waypoint.attrib['pitch']),
yaw=float(waypoint.attrib['yaw']))
challenge.prepare_ego_car(carla.Transform(location, rotation))
CarlaDataProvider.register_actor(challenge.ego_vehicle)
list_scenarios = []
list_scenarios += challenge.build_scenario_instances([scenario_conf],
args.debug_town)
# Tick once to start the scenarios.
print(" Running these scenarios --- ", list_scenarios)
for scenario in list_scenarios:
scenario.scenario.scenario_tree.tick_once()
challenge.run_route(list_scenarios, None, True)
# clean up
for scenario in list_scenarios:
del scenario
challenge.cleanup(ego=True)
challenge.agent_instance.destroy()
break
# final measurements from the challenge
challenge.report_challenge_statistics(args.filename,
args.show_to_participant)
del challenge
def convert_maneuver_to_atomic(action, actor):
"""
Convert an OpenSCENARIO maneuver action into a Behavior atomic
Note not all OpenSCENARIO actions are currently supported
"""
maneuver_name = action.attrib.get('name', 'unknown')
if action.find('Global') is not None:
global_action = action.find('Global')
if global_action.find('Infrastructure') is not None:
infrastructure_action = global_action.find(
'Infrastructure').find('Signal')
if infrastructure_action.find('SetState') is not None:
traffic_light_id = None
traffic_light_action = infrastructure_action.find(
'SetState')
name = traffic_light_action.attrib.get('name')
if name.startswith("id="):
traffic_light_id = name[3:]
elif name.startswith("pos="):
position = name[4:]
pos = position.split(",")
for carla_actor in CarlaDataProvider.get_world(
).get_actors().filter('traffic.traffic_light'):
carla_actor_loc = carla_actor.get_transform(
).location
distance = carla_actor_loc.distance(
carla.Location(x=float(pos[0]),
y=float(pos[1]),
z=carla_actor_loc.z))
if distance < 2.0:
traffic_light_id = actor.id
break
if traffic_light_id is None:
raise AttributeError(
"Unknown traffic light {}".format(name))
traffic_light_state = traffic_light_action.attrib.get(
'state')
atomic = TrafficLightStateSetter(traffic_light_id,
traffic_light_state,
name=maneuver_name + "_" +
str(traffic_light_id))
else:
raise NotImplementedError(
"TrafficLights can only be influenced via SetState")
else:
raise NotImplementedError(
"Global actions are not yet supported")
elif action.find('UserDefined') is not None:
user_defined_action = action.find('UserDefined')
if user_defined_action.find('Command') is not None:
command = user_defined_action.find('Command').text.replace(
" ", "")
if command == 'Idle':
atomic = Idle()
else:
raise AttributeError(
"Unknown user command action: {}".format(command))
else:
raise NotImplementedError(
"UserDefined script actions are not yet supported")
elif action.find('Private') is not None:
private_action = action.find('Private')
if private_action.find('Longitudinal') is not None:
private_action = private_action.find('Longitudinal')
if private_action.find('Speed') is not None:
long_maneuver = private_action.find('Speed')
# duration and distance
duration = float(
long_maneuver.find("Dynamics").attrib.get(
'time', float("inf")))
distance = float(
long_maneuver.find("Dynamics").attrib.get(
'distance', float("inf")))
# absolute velocity with given target speed
if long_maneuver.find("Target").find(
"Absolute") is not None:
target_speed = float(
long_maneuver.find("Target").find(
"Absolute").attrib.get('value', 0))
atomic = KeepVelocity(actor,
target_speed,
distance=distance,
duration=duration,
name=maneuver_name)
# relative velocity to given actor
if long_maneuver.find("Target").find(
"Relative") is not None:
relative_speed = long_maneuver.find("Target").find(
"Relative")
obj = relative_speed.attrib.get('object')
value = float(relative_speed.attrib.get('value', 0))
value_type = relative_speed.attrib.get('valueType')
continuous = relative_speed.attrib.get('continuous')
for traffic_actor in CarlaDataProvider.get_world(
).get_actors():
if 'role_name' in traffic_actor.attributes and traffic_actor.attributes[
'role_name'] == obj:
obj_actor = traffic_actor
atomic = SetRelativeOSCVelocity(
actor, obj_actor, value, value_type, continuous,
duration, distance)
elif private_action.find('Distance') is not None:
raise NotImplementedError(
"Longitudinal distance actions are not yet supported")
else:
raise AttributeError("Unknown longitudinal action")
elif private_action.find('Lateral') is not None:
private_action = private_action.find('Lateral')
if private_action.find('LaneChange') is not None:
lat_maneuver = private_action.find('LaneChange')
target_lane_rel = float(
lat_maneuver.find("Target").find(
"Relative").attrib.get('value', 0))
distance = float(
lat_maneuver.find("Dynamics").attrib.get(
'distance', float("inf")))
atomic = LaneChange(
actor,
None,
direction="left" if target_lane_rel < 0 else "right",
distance_lane_change=distance,
name=maneuver_name)
else:
raise AttributeError("Unknown lateral action")
elif private_action.find('Visibility') is not None:
raise NotImplementedError(
"Visibility actions are not yet supported")
elif private_action.find('Meeting') is not None:
raise NotImplementedError(
"Meeting actions are not yet supported")
elif private_action.find('Autonomous') is not None:
private_action = private_action.find('Autonomous')
activate = strtobool(private_action.attrib.get('activate'))
atomic = ChangeAutoPilot(actor, activate, name=maneuver_name)
elif private_action.find('Controller') is not None:
raise NotImplementedError(
"Controller actions are not yet supported")
elif private_action.find('Position') is not None:
position = private_action.find('Position')
atomic = ActorTransformSetterToOSCPosition(actor,
position,
name=maneuver_name)
elif private_action.find('Routing') is not None:
target_speed = 5.0
private_action = private_action.find('Routing')
if private_action.find('FollowRoute') is not None:
private_action = private_action.find('FollowRoute')
if private_action.find('Route') is not None:
route = private_action.find('Route')
plan = []
if route.find('ParameterDeclaration') is not None:
if route.find('ParameterDeclaration').find(
'Parameter') is not None:
parameter = route.find(
'ParameterDeclaration').find('Parameter')
if parameter.attrib.get('name') == "Speed":
target_speed = float(
parameter.attrib.get('value', 5.0))
for waypoint in route.iter('Waypoint'):
position = waypoint.find('Position')
transform = OpenScenarioParser.convert_position_to_transform(
position)
waypoint = CarlaDataProvider.get_map(
).get_waypoint(transform.location)
plan.append((waypoint, RoadOption.LANEFOLLOW))
atomic = WaypointFollower(actor,
target_speed=target_speed,
plan=plan,
name=maneuver_name)
elif private_action.find('CatalogReference') is not None:
raise NotImplementedError(
"CatalogReference private actions are not yet supported"
)
else:
raise AttributeError(
"Unknown private FollowRoute action")
elif private_action.find('FollowTrajectory') is not None:
raise NotImplementedError(
"Private FollowTrajectory actions are not yet supported"
)
elif private_action.find('AcquirePosition') is not None:
raise NotImplementedError(
"Private AcquirePosition actions are not yet supported"
)
else:
raise AttributeError("Unknown private routing action")
else:
raise AttributeError("Unknown private action")
else:
raise AttributeError("Unknown action")
return atomic
