def convert_maneuver_to_atomic(action, actor, catalogs):
"""
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('GlobalAction') is not None:
global_action = action.find('GlobalAction')
if global_action.find('InfrastructureAction') is not None:
infrastructure_action = global_action.find('InfrastructureAction').find('TrafficSignalAction')
if infrastructure_action.find('TrafficSignalStateAction') is not None:
traffic_light_id = None
traffic_light_action = infrastructure_action.find('TrafficSignalStateAction')
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 = carla_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 TrafficSignalStateAction")
elif global_action.find('EnvironmentAction') is not None:
weather_behavior = UpdateWeather(
OpenScenarioParser.get_weather_from_env_action(global_action, catalogs))
friction_behavior = UpdateRoadFriction(
OpenScenarioParser.get_friction_from_env_action(global_action, catalogs))
env_behavior = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name=maneuver_name)
env_behavior.add_child(
oneshot_behavior(variable_name=maneuver_name + ">WeatherUpdate", behaviour=weather_behavior))
env_behavior.add_child(
oneshot_behavior(variable_name=maneuver_name + ">FrictionUpdate", behaviour=friction_behavior))
return env_behavior
else:
raise NotImplementedError("Global actions are not yet supported")
elif action.find('UserDefinedAction') is not None:
user_defined_action = action.find('UserDefinedAction')
if user_defined_action.find('CustomCommandAction') is not None:
command = user_defined_action.find('CustomCommandAction').attrib.get('type')
atomic = RunScript(command, name=maneuver_name)
elif action.find('PrivateAction') is not None:
private_action = action.find('PrivateAction')
if private_action.find('LongitudinalAction') is not None:
private_action = private_action.find('LongitudinalAction')
if private_action.find('SpeedAction') is not None:
long_maneuver = private_action.find('SpeedAction')
# duration and distance
distance = float('inf')
duration = float('inf')
dimension = long_maneuver.find("SpeedActionDynamics").attrib.get('dynamicsDimension')
if dimension == "distance":
distance = float(long_maneuver.find("SpeedActionDynamics").attrib.get('value', float("inf")))
else:
duration = float(long_maneuver.find("SpeedActionDynamics").attrib.get('value', float("inf")))
# absolute velocity with given target speed
if long_maneuver.find("SpeedActionTarget").find("AbsoluteTargetSpeed") is not None:
target_speed = float(long_maneuver.find("SpeedActionTarget").find(
"AbsoluteTargetSpeed").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("SpeedActionTarget").find("RelativeTargetSpeed") is not None:
relative_speed = long_maneuver.find("SpeedActionTarget").find("RelativeTargetSpeed")
obj = relative_speed.attrib.get('entityRef')
value = float(relative_speed.attrib.get('value', 0))
value_type = relative_speed.attrib.get('speedTargetValueType')
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('LongitudinalDistanceAction') is not None:
raise NotImplementedError("Longitudinal distance actions are not yet supported")
else:
raise AttributeError("Unknown longitudinal action")
elif private_action.find('LateralAction') is not None:
private_action = private_action.find('LateralAction')
if private_action.find('LaneChangeAction') is not None:
# Note: LaneChangeActions are currently only supported for RelativeTargetLane
# with +1 or -1 referring to the action actor
lat_maneuver = private_action.find('LaneChangeAction')
target_lane_rel = float(lat_maneuver.find("LaneChangeTarget").find(
"RelativeTargetLane").attrib.get('value', 0))
# duration and distance
distance = float('inf')
duration = float('inf')
dimension = lat_maneuver.find("LaneChangeActionDynamics").attrib.get('dynamicsDimension')
if dimension == "distance":
distance = float(
lat_maneuver.find("LaneChangeActionDynamics").attrib.get('value', float("inf")))
else:
duration = float(
lat_maneuver.find("LaneChangeActionDynamics").attrib.get('value', 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('VisibilityAction') is not None:
raise NotImplementedError("Visibility actions are not yet supported")
elif private_action.find('SynchronizeAction') is not None:
raise NotImplementedError("Synchronization actions are not yet supported")
elif private_action.find('ActivateControllerAction') is not None:
private_action = private_action.find('ActivateControllerAction')
activate = strtobool(private_action.attrib.get('longitudinal'))
atomic = ChangeAutoPilot(actor, activate, name=maneuver_name)
elif private_action.find('ControllerAction') is not None:
raise NotImplementedError("Controller actions are not yet supported")
elif private_action.find('TeleportAction') is not None:
position = private_action.find('TeleportAction')
atomic = ActorTransformSetterToOSCPosition(actor, position, name=maneuver_name)
elif private_action.find('RoutingAction') is not None:
target_speed = 5.0
private_action = private_action.find('RoutingAction')
if private_action.find('AssignRouteAction') is not None:
private_action = private_action.find('AssignRouteAction')
if private_action.find('Route') is not None:
route = private_action.find('Route')
plan = []
if route.find('ParameterDeclarations') is not None:
if route.find('ParameterDeclarations').find('Parameter') is not None:
parameter = route.find('ParameterDeclarations').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)
plan.append(transform.location)
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('FollowTrajectoryAction') is not None:
raise NotImplementedError("Private FollowTrajectory actions are not yet supported")
elif private_action.find('AcquirePositionAction') 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:
if action:
raise AttributeError("Unknown action: {}".format(maneuver_name))
else:
return Idle(duration=0, name=maneuver_name)
return atomic
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
def convert_maneuver_to_atomic(action, actor, catalogs):
"""
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('GlobalAction') is not None:
global_action = action.find('GlobalAction')
if global_action.find('InfrastructureAction') is not None:
infrastructure_action = global_action.find('InfrastructureAction').find('TrafficSignalAction')
if infrastructure_action.find('TrafficSignalStateAction') is not None:
traffic_light_action = infrastructure_action.find('TrafficSignalStateAction')
name_condition = traffic_light_action.attrib.get('name')
traffic_light = OpenScenarioParser.get_traffic_light_from_osc_name(name_condition)
tl_state = traffic_light_action.attrib.get('state').upper()
if tl_state not in OpenScenarioParser.tl_states:
raise KeyError("CARLA only supports Green, Red, Yellow or Off")
traffic_light_state = OpenScenarioParser.tl_states[tl_state]
atomic = TrafficLightStateSetter(
traffic_light, traffic_light_state, name=maneuver_name + "_" + str(traffic_light.id))
else:
raise NotImplementedError("TrafficLights can only be influenced via TrafficSignalStateAction")
elif global_action.find('EnvironmentAction') is not None:
weather_behavior = ChangeWeather(
OpenScenarioParser.get_weather_from_env_action(global_action, catalogs))
friction_behavior = ChangeRoadFriction(
OpenScenarioParser.get_friction_from_env_action(global_action, catalogs))
env_behavior = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name=maneuver_name)
env_behavior.add_child(
oneshot_behavior(variable_name=maneuver_name + ">WeatherUpdate", behaviour=weather_behavior))
env_behavior.add_child(
oneshot_behavior(variable_name=maneuver_name + ">FrictionUpdate", behaviour=friction_behavior))
return env_behavior
else:
raise NotImplementedError("Global actions are not yet supported")
elif action.find('UserDefinedAction') is not None:
user_defined_action = action.find('UserDefinedAction')
if user_defined_action.find('CustomCommandAction') is not None:
command = user_defined_action.find('CustomCommandAction').attrib.get('type')
atomic = RunScript(command, base_path=OpenScenarioParser.osc_filepath, name=maneuver_name)
elif action.find('PrivateAction') is not None:
private_action = action.find('PrivateAction')
if private_action.find('LongitudinalAction') is not None:
private_action = private_action.find('LongitudinalAction')
if private_action.find('SpeedAction') is not None:
long_maneuver = private_action.find('SpeedAction')
# duration and distance
distance = float('inf')
duration = float('inf')
dimension = long_maneuver.find("SpeedActionDynamics").attrib.get('dynamicsDimension')
if dimension == "distance":
distance = float(long_maneuver.find("SpeedActionDynamics").attrib.get('value', float("inf")))
else:
duration = float(long_maneuver.find("SpeedActionDynamics").attrib.get('value', float("inf")))
# absolute velocity with given target speed
if long_maneuver.find("SpeedActionTarget").find("AbsoluteTargetSpeed") is not None:
target_speed = float(long_maneuver.find("SpeedActionTarget").find(
"AbsoluteTargetSpeed").attrib.get('value', 0))
atomic = ChangeActorTargetSpeed(
actor, target_speed, distance=distance, duration=duration, name=maneuver_name)
# relative velocity to given actor
if long_maneuver.find("SpeedActionTarget").find("RelativeTargetSpeed") is not None:
relative_speed = long_maneuver.find("SpeedActionTarget").find("RelativeTargetSpeed")
obj = relative_speed.attrib.get('entityRef')
value = float(relative_speed.attrib.get('value', 0))
value_type = relative_speed.attrib.get('speedTargetValueType')
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 = ChangeActorTargetSpeed(actor,
target_speed,
relative_actor=obj_actor,
value=value,
value_type=value_type,
continuous=continuous,
distance=distance,
duration=duration,
name=maneuver_name)
elif private_action.find('LongitudinalDistanceAction') is not None:
raise NotImplementedError("Longitudinal distance actions are not yet supported")
else:
raise AttributeError("Unknown longitudinal action")
elif private_action.find('LateralAction') is not None:
private_action = private_action.find('LateralAction')
if private_action.find('LaneChangeAction') is not None:
# Note: LaneChangeActions are currently only supported for RelativeTargetLane
# with +1 or -1 referring to the action actor
lat_maneuver = private_action.find('LaneChangeAction')
target_lane_rel = float(lat_maneuver.find("LaneChangeTarget").find(
"RelativeTargetLane").attrib.get('value', 0))
# duration and distance
distance = float('inf')
duration = float('inf')
dimension = lat_maneuver.find("LaneChangeActionDynamics").attrib.get('dynamicsDimension')
if dimension == "distance":
distance = float(
lat_maneuver.find("LaneChangeActionDynamics").attrib.get('value', float("inf")))
else:
duration = float(
lat_maneuver.find("LaneChangeActionDynamics").attrib.get('value', float("inf")))
atomic = ChangeActorLateralMotion(actor,
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('VisibilityAction') is not None:
raise NotImplementedError("Visibility actions are not yet supported")
elif private_action.find('SynchronizeAction') is not None:
raise NotImplementedError("Synchronization actions are not yet supported")
elif private_action.find('ActivateControllerAction') is not None:
private_action = private_action.find('ActivateControllerAction')
activate = strtobool(private_action.attrib.get('longitudinal'))
atomic = ChangeAutoPilot(actor, activate, name=maneuver_name)
elif private_action.find('ControllerAction') is not None:
controller_action = private_action.find('ControllerAction')
module, args = OpenScenarioParser.get_controller(controller_action, catalogs)
atomic = ChangeActorControl(actor, control_py_module=module, args=args)
elif private_action.find('TeleportAction') is not None:
position = private_action.find('TeleportAction')
atomic = ActorTransformSetterToOSCPosition(actor, position, name=maneuver_name)
elif private_action.find('RoutingAction') is not None:
private_action = private_action.find('RoutingAction')
if private_action.find('AssignRouteAction') is not None:
private_action = private_action.find('AssignRouteAction')
if private_action.find('Route') is not None:
route = private_action.find('Route')
waypoints = []
for waypoint in route.iter('Waypoint'):
position = waypoint.find('Position')
transform = OpenScenarioParser.convert_position_to_transform(position)
waypoints.append(transform)
# @TODO: How to handle relative positions here? This might chance at runtime?!
atomic = ChangeActorWaypoints(actor, waypoints=waypoints, 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('FollowTrajectoryAction') is not None:
raise NotImplementedError("Private FollowTrajectory actions are not yet supported")
elif private_action.find('AcquirePositionAction') 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:
if list(action):
raise AttributeError("Unknown action: {}".format(maneuver_name))
else:
return Idle(duration=0, name=maneuver_name)
return atomic