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