#!/usr/bin/env python

from six.moves.queue import Queue  # pylint: disable=relative-import

import math
import py_trees
import carla

from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter,
                                                                      ActorDestroy,
                                                                      ActorSource,
                                                                      ActorSink,
                                                                      WaypointFollower,
                                                                      StopVehicle,
                                                                      ChangeActorTargetSpeed,
                                                                      KeepVelocity,
                                                                      Idle)
from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, DrivenDistanceTest, MaxVelocityTest
from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance
from srunner.scenarios.basic_scenario import BasicScenario
from srunner.tools.scenario_helper import get_waypoint_in_distance


class RangeTest(BasicScenario):

    def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
                 timeout=120):
        self._world = world
        self._map = CarlaDataProvider.get_map()
        self._speed = 5  # m/s
        self._drivenDistanceM = 2000

        # Timeout of scenario in seconds
        self.timeout = timeout

        super(RangeTest, self).__init__(
            "RangeTest",
            ego_vehicles,
            config,
            world,
            debug_mode,
            criteria_enable=criteria_enable)

    def _initialize_actors(self, config):
        # add actors from xml file
        for actor in config.other_actors:
            vehicle = CarlaDataProvider.request_new_actor(actor.model, actor.transform)
            self.other_actors.append(vehicle)
            vehicle.set_simulate_physics(enabled=True)

    def _create_behavior(self):
        sequence = py_trees.composites.Sequence("Sequence Behavior")

        parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
        parallel.add_child(StopVehicle(self.ego_vehicles[0], 1))
        parallel.add_child(StopVehicle(self.ego_vehicles[0], 1))
        sequence.add_child(parallel)

        # match all cars speed
        parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
        parallel.add_child(KeepVelocity(self.ego_vehicles[0], 2, distance=7))
        parallel.add_child(KeepVelocity(self.other_actors[0], 2, distance=7))
        sequence.add_child(parallel)

        # turn target speed autopilot for all cars
        parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
        parallel.add_child(WaypointFollower(self.ego_vehicles[0], self._speed, avoid_collision=False))
        parallel.add_child(WaypointFollower(self.other_actors[0], self._speed, avoid_collision=False))
        sequence.add_child(parallel)

        driveDistance = DriveDistance(
            self.ego_vehicles[0],
            self._drivenDistanceM,
            name="DriveDistance")
        sequence.add_child(driveDistance)

        return sequence

    def _create_test_criteria(self):
        criteria = []

        collision_criterion = CollisionTest(self.ego_vehicles[0])
        driven_distance_criterion = DrivenDistanceTest(self.ego_vehicles[0], self._drivenDistanceM)

        criteria.append(collision_criterion)
        criteria.append(driven_distance_criterion)

        return criteria

    def __del__(self):
        """
        Remove all actors upon deletion
        """
        self.remove_all_actors()

#!/usr/bin/env python

from six.moves.queue import Queue  # pylint: disable=relative-import

import math
import py_trees
import carla

from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter,
                                                                      ActorDestroy,
                                                                      ActorSource,
                                                                      ActorSink,
                                                                      WaypointFollower,
                                                                      StopVehicle,
                                                                      ChangeActorTargetSpeed,
                                                                      KeepVelocity,
                                                                      Idle)
from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, DrivenDistanceTest, MaxVelocityTest
from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance
from srunner.scenarios.basic_scenario import BasicScenario
from srunner.tools.scenario_helper import get_waypoint_in_distance


class RangeTest(BasicScenario):

    def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
                 timeout=120):
        self._world = world
        self._map = CarlaDataProvider.get_map()
        self._speed = 5  # m/s
        self._drivenDistanceM = 2000

        # Timeout of scenario in seconds
        self.timeout = timeout

        super(RangeTest, self).__init__(
            "RangeTest",
            ego_vehicles,
            config,
            world,
            debug_mode,
            criteria_enable=criteria_enable)

    def _initialize_actors(self, config):
        # add actors from xml file
        for actor in config.other_actors:
            vehicle = CarlaDataProvider.request_new_actor(actor.model, actor.transform)
            self.other_actors.append(vehicle)
            vehicle.set_simulate_physics(enabled=True)

    def _create_behavior(self):
        sequence = py_trees.composites.Sequence("Sequence Behavior")

        parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
        parallel.add_child(StopVehicle(self.ego_vehicles[0], 1))
        parallel.add_child(StopVehicle(self.ego_vehicles[0], 1))
        sequence.add_child(parallel)

        # match all cars speed
        parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
        parallel.add_child(KeepVelocity(self.ego_vehicles[0], 2, distance=7))
        parallel.add_child(KeepVelocity(self.other_actors[0], 2, distance=7))
        sequence.add_child(parallel)

        # turn target speed autopilot for all cars
        parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
        parallel.add_child(WaypointFollower(self.ego_vehicles[0], self._speed, avoid_collision=False))
        parallel.add_child(WaypointFollower(self.other_actors[0], self._speed, avoid_collision=False))
        sequence.add_child(parallel)

        driveDistance = DriveDistance(
            self.ego_vehicles[0],
            self._drivenDistanceM,
            name="DriveDistance")
        sequence.add_child(driveDistance)

        return sequence

    def _create_test_criteria(self):
        criteria = []

        collision_criterion = CollisionTest(self.ego_vehicles[0])
        driven_distance_criterion = DrivenDistanceTest(self.ego_vehicles[0], self._drivenDistanceM)

        criteria.append(collision_criterion)
        criteria.append(driven_distance_criterion)

        return criteria

    def __del__(self):
        """
        Remove all actors upon deletion
        """
        self.remove_all_actors()

#!/usr/bin/env python # pylint: disable=relative-import # m/s # Timeout of scenario in seconds # add actors from xml file # match all cars speed # turn target speed autopilot for all cars Remove all actors upon deletion