* Added new scenario in control_loss.py

* Added new atomic SteerVehicle
* Added new criteria ReachedLocationTest
* Added new scenario to list in scenario_runner.py

ScenarioManager/atomic_scenario_behavior.py

@@ -201,9 +201,14 @@ def update(self):
         """
         new_status = py_trees.common.Status.RUNNING
 
+<<<<<<< 211b9fbc35138cf2a3f18736fb53036ed40447f0
         delta_velocity = CarlaDataProvider.get_velocity(
             self._vehicle) - self._target_velocity
         if delta_velocity < EPSILON:
+=======
+        if (CarlaDataProvider.get_velocity(
+                self.vehicle) - self.target_velocity) < EPSILON:
+>>>>>>> * Added new scenario in control_loss.py
             new_status = py_trees.common.Status.SUCCESS
 
         self.logger.debug("%s.update()[%s->%s]" %
@@ -635,3 +640,39 @@ 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 control loss behavior.
+    The vehicle looses control with steering angle.
+    """
+
+    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
+        """
+        new_status = py_trees.common.Status.RUNNING
+
+        if CarlaDataProvider.get_velocity(self.vehicle) > EPSILON:
+            self.control.steer = self.steer_value
+            new_status = py_trees.common.Status.SUCCESS
+        else:
+            new_status = py_trees.common.Status.FAILURE
+            self.control.steer = 0
+
+        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,45 @@ 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
+    """
+
+    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
+
+        not_in_region = (location.x < self.min_x or location.x > self.max_x) or (
+            location.y < self.min_y or location.y > self.max_y)
+        if not not_in_region:
+            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,124 @@
+#!/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.
+"""
+
+import random
+import sys
+
+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):
+
+    """
+    This class holds everything required for a simple "Control Loss Vehicle"
+    to perform jitter sequence with steering angle.
+    """
+
+    timeout = 60            # Timeout of scenario in seconds
+
+    # ego vehicle parameters
+    ego_vehicle_model = 'vehicle.carlamotors.carlacola'
+    ego_vehicle_start = carla.Transform(
+        carla.Location(x=60, y=109.5, z=2.0), carla.Rotation(yaw=0))
+    ego_vehicle_max_steer = 0.1
+    no_of_jitter_actions = 5
+    ego_vehicle_driven_distance = 35
+
+    # other vehicle parameter
+    other_vehicles = []
+
+    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",
+                                          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
+        startcondition = InTriggerRegion(self.ego_vehicle, 75, 80, 100, 110)
+
+        # jitter sequence
+        jitterSequence = py_trees.composites.Sequence(
+            "Jitter Sequence Behavior")
+        jitterTimeout = TimeOut(timeout=1.0, name="Timeout for next jitter")
+
+        for i in range(self.no_of_jitter_actions):
+            jitter_steer = self.ego_vehicle_max_steer
+            if i % 2 != 0:
+                jitter_steer = self.ego_vehicle_max_steer * -1.0
+            # turn vehicle
+            turn = SteerVehicle(
+                self.ego_vehicle,
+                jitter_steer,
+                name='Steering ' + str(i))
+            jitterAction = py_trees.composites.Parallel("Jitter Actions with Timeouts",
+                                                        policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
+            jitterAction.add_child(turn)
+            if i == 0:
+                jitterAction.add_child(TimeOut(0.5))
+            else:
+                jitterAction.add_child(jitterTimeout)
+            jitterSequence.add_child(jitterAction)
+
+        # Build behavior tree
+        sequence = py_trees.composites.Sequence("Sequence Behavior")
+        sequence.add_child(startcondition)
+        sequence.add_child(jitterSequence)
+        sequence.add_child(TimeOut(20))
+        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)
+        driven_distance_criterion = DrivenDistanceTest(
+            self.ego_vehicle,
+            self.ego_vehicle_driven_distance)
+        reached_region_criterion = ReachedRegionTest(
+            self.ego_vehicle,
+            115, 120,
+            104, 110)
+        keep_lane_criterion = KeepLaneTest(self.ego_vehicle)
+
+        criteria.append(collision_criterion)
+        criteria.append(driven_distance_criterion)
+        criteria.append(reached_region_criterion)
+        criteria.append(keep_lane_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!")