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