Implementation of Traffic Scenario 01

Docs/list_of_scenarios.md

@@ -63,3 +63,8 @@ through a junction without signal.
 The ego vehicle is passing through a junction without traffic lights
 And encounters another vehicle passing across the junction. The ego vehicle has
 to avoid collision and navigate accross the junction to succeed.
+
+### ControlLoss
+In this scenario control loss of a vehicle is tested due to bad road conditions, etc
+and it checks whether the vehicle is regained its control and corrected its course.
+

ScenarioManager/atomic_scenario_behavior.py

@@ -635,3 +635,33 @@ def terminate(self, new_status):
         self._control.brake = 0.0
         self._vehicle.apply_control(self._control)
         super(SyncArrival, self).terminate(new_status)
+
+class SteerVehicle(AtomicBehavior):
+
+    """
+    This class contains an atomic steer behavior.
+    To set the steer value of the vehicle.
+    """
+
+    def __init__(self, vehicle, steer_value, name="Steering"):
+        """
+        Setup vehicle and maximum steer value
+        """
+        super(SteerVehicle, self).__init__(name)
+        self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+        self._control = carla.VehicleControl()
+        self._vehicle = vehicle
+        self._steer_value = steer_value
+
+    def update(self):
+        """
+        Set steer to steer_value until reaching full stop
+        """
+        self._control.steer = self._steer_value
+        new_status = py_trees.common.Status.SUCCESS
+
+        self.logger.debug("%s.update()[%s->%s]" %
+                          (self.__class__.__name__, self.status, new_status))
+        self._vehicle.apply_control(self._control)
+
+        return new_status

ScenarioManager/atomic_scenario_criteria.py

@@ -354,3 +354,52 @@ def _count_lane_invasion(weak_self, event):
         if not self:
             return
         self.actual_value += 1
+
+
+class ReachedRegionTest(Criterion):
+
+    """
+    This class contains the reached region test
+    The test is a success if the vehicle reaches a specified region
+    """
+
+    def __init__(self, vehicle, min_x, max_x, min_y,
+                 max_y, name="ReachedRegionTest"):
+        """
+        Setup trigger region (rectangle provided by
+        [min_x,min_y] and [max_x,max_y]
+        """
+        super(ReachedRegionTest, self).__init__(name, vehicle, 0)
+        self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+        self._vehicle = vehicle
+        self._min_x = min_x
+        self._max_x = max_x
+        self._min_y = min_y
+        self._max_y = max_y
+
+    def update(self):
+        """
+        Check if the vehicle location is within trigger region
+        """
+        new_status = py_trees.common.Status.RUNNING
+
+        location = CarlaDataProvider.get_location(self._vehicle)
+        if location is None:
+            return new_status
+
+        in_region = False
+        if self.test_status != "SUCCESS":
+            in_region = (location.x > self._min_x and location.x < self._max_x) and (
+                location.y > self._min_y and location.y < self._max_y)
+            if in_region:
+                self.test_status = "SUCCESS"
+            else:
+                self.test_status = "RUNNING"
+
+        if self.test_status == "SUCCESS":
+            new_status = py_trees.common.Status.SUCCESS
+
+        self.logger.debug("%s.update()[%s->%s]" %
+                          (self.__class__.__name__, self.status, new_status))
+
+        return new_status

Scenarios/control_loss.py

@@ -0,0 +1,128 @@
+#!/usr/bin/env python
+
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see <https://opensource.org/licenses/MIT>.
+
+"""
+Control Loss Vehicle scenario:
+
+The scenario realizes that the vehicle looses control due to
+bad road conditions, etc. and checks to see if the vehicle
+regains control and corrects it's course.
+"""
+
+import random
+
+import py_trees
+import carla
+
+from ScenarioManager.atomic_scenario_behavior import *
+from ScenarioManager.atomic_scenario_criteria import *
+from ScenarioManager.scenario_manager import Scenario
+from ScenarioManager.timer import TimeOut
+from Scenarios.basic_scenario import *
+
+
+class ControlLoss(BasicScenario):
+
+    """
+    Implementation of "Control Loss Vehicle" (Traffic Scenario 01)
+
+    Location : Town03
+    """
+
+    timeout = 60            # Timeout of scenario in seconds
+
+    # ego vehicle parameters
+    _ego_vehicle_model = 'vehicle.carlamotors.carlacola'
+    _ego_vehicle_start = carla.Transform(
+        carla.Location(x=15, y=207.2, z=2.0), carla.Rotation(yaw=0))
+    _no_of_jitter_actions = 20
+    _noise_mean = 0     # Mean value of steering noise
+    _noise_std = 0.02    # Std. deviation of steerning noise
+    _dynamic_mean = 0.05
+
+    def __init__(self, world, debug_mode=False):
+        """
+        Setup all relevant parameters and create scenario
+        and instantiate scenario manager
+        """
+        self.ego_vehicle = setup_vehicle(world,
+                                         self._ego_vehicle_model,
+                                         self._ego_vehicle_start,
+                                         hero=True)
+
+        super(ControlLoss, self).__init__(
+            name="ControlLoss",
+            town="Town03",
+            world=world,
+            debug_mode=debug_mode)
+
+    def _create_behavior(self):
+        """
+        The scenario defined after is a "control loss vehicle" scenario. After
+        invoking this scenario, it will wait for the user controlled vehicle to
+        enter the start region, then it performs a jitter action. Finally,
+        the user-controlled vehicle has to reach a target region.
+        If this does not happen within 60 seconds, a timeout stops the scenario
+        """
+
+        # start condition
+        start_condition = InTriggerRegion(self.ego_vehicle, 43, 49, 190, 210)
+
+        # jitter sequence
+        jitter_sequence = py_trees.composites.Sequence(
+            "Jitter Sequence Behavior")
+        jitter_timeout = TimeOut(timeout=0.2, name="Timeout for next jitter")
+
+        for i in range(self._no_of_jitter_actions):
+            ego_vehicle_max_steer = random.gauss(
+                self._noise_mean, self._noise_std)
+            if ego_vehicle_max_steer > 0:
+                ego_vehicle_max_steer += self._dynamic_mean
+            elif ego_vehicle_max_steer < 0:
+                ego_vehicle_max_steer -= self._dynamic_mean
+
+            # turn vehicle
+            turn = SteerVehicle(
+                self.ego_vehicle,
+                ego_vehicle_max_steer,
+                name='Steering ' + str(i))
+
+            jitter_action = py_trees.composites.Parallel(
+                "Jitter Actions with Timeouts",
+                policy=py_trees.common.
+                ParallelPolicy.SUCCESS_ON_ALL)
+            jitter_action.add_child(turn)
+            jitter_action.add_child(jitter_timeout)
+            jitter_sequence.add_child(jitter_action)
+
+        # endcondition
+        end_condition = InTriggerRegion(self.ego_vehicle, 145, 150, 190, 210)
+
+        # Build behavior tree
+        sequence = py_trees.composites.Sequence("Sequence Behavior")
+        sequence.add_child(start_condition)
+        sequence.add_child(jitter_sequence)
+        sequence.add_child(end_condition)
+        return sequence
+
+    def _create_test_criteria(self):
+        """
+        A list of all test criteria will be created that is later used
+        in parallel behavior tree.
+        """
+        criteria = []
+
+        collision_criterion = CollisionTest(self.ego_vehicle)
+        # Region check to verify if the vehicle reached correct lane
+        reached_region_criterion = ReachedRegionTest(
+            self.ego_vehicle,
+            113, 119,
+            204.2, 210.2)
+
+        criteria.append(collision_criterion)
+        criteria.append(reached_region_criterion)
+
+        return criteria

scenario_runner.py

@@ -24,6 +24,7 @@
 from Scenarios.object_crash_vehicle import *
 from Scenarios.no_signal_junction_crossing import NoSignalJunctionCrossing
 from Scenarios.object_crash_intersection import *
+from Scenarios.control_loss import *
 from ScenarioManager.scenario_manager import ScenarioManager
 
 
@@ -40,7 +41,8 @@
     "OppositeVehicleRunningRedLight",
     "NoSignalJunctionCrossing",
     "VehicleTurningRight",
-    "VehicleTurningLeft"
+    "VehicleTurningLeft",
+    "ControlLoss"
 }
 
 
@@ -100,7 +102,8 @@ def main(args):
             if args.junit is not None:
                 junit_filename = args.junit.split(".")[0] + "_{}.xml".format(i)
 
-            if not manager.analyze_scenario(args.output, args.filename, junit_filename):
+            if not manager.analyze_scenario(
+                    args.output, args.filename, junit_filename):
                 print("Success!")
             else:
                 print("Failure!")