コード例 #1
0
    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
コード例 #2
0
    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
コード例 #3
0
    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