def _create_environment_behavior(self):
# Set the appropriate weather conditions
env_behavior = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="EnvironmentBehavior")
weather_update = ChangeWeather(
OpenScenarioParser.get_weather_from_env_action(self.config.init, self.config.catalogs))
road_friction = ChangeRoadFriction(
OpenScenarioParser.get_friction_from_env_action(self.config.init, self.config.catalogs))
env_behavior.add_child(oneshot_with_check(variable_name="InitialWeather", behaviour=weather_update))
env_behavior.add_child(oneshot_with_check(variable_name="InitRoadFriction", behaviour=road_friction))
return env_behavior
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