change_lane.py 7.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181
  1. #!/usr/bin/env python
  2. # Copyright (c) 2019-2020 Intel Corporation
  3. #
  4. # This work is licensed under the terms of the MIT license.
  5. # For a copy, see <https://opensource.org/licenses/MIT>.
  6. """
  7. Change lane scenario:
  8. The scenario realizes a driving behavior, in which the user-controlled ego vehicle
  9. follows a fast driving car on the highway. There's a slow car driving in great distance to the fast vehicle.
  10. At one point the fast vehicle is changing the lane to overtake a slow car, which is driving on the same lane.
  11. The ego vehicle doesn't "see" the slow car before the lane change of the fast car, therefore it hast to react
  12. fast to avoid an collision. There are two options to avoid an accident:
  13. The ego vehicle adjusts its velocity or changes the lane as well.
  14. """
  15. import random
  16. import py_trees
  17. import carla
  18. from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
  19. from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter,
  20. StopVehicle,
  21. LaneChange,
  22. WaypointFollower,
  23. Idle)
  24. from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest
  25. from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerDistanceToVehicle, StandStill
  26. from srunner.scenarios.basic_scenario import BasicScenario
  27. from srunner.tools.scenario_helper import get_waypoint_in_distance
  28. class ChangeLane(BasicScenario):
  29. """
  30. This class holds everything required for a "change lane" scenario involving three vehicles.
  31. There are two vehicles driving in the same direction on the highway: A fast car and a slow car in front.
  32. The fast car will change the lane, when it is close to the slow car.
  33. The ego vehicle is driving right behind the fast car.
  34. This is a single ego vehicle scenario
  35. """
  36. timeout = 1200
  37. def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
  38. timeout=600):
  39. """
  40. Setup all relevant parameters and create scenario
  41. If randomize is True, the scenario parameters are randomized
  42. """
  43. self.timeout = timeout
  44. self._map = CarlaDataProvider.get_map()
  45. self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location)
  46. self._fast_vehicle_velocity = 70
  47. self._slow_vehicle_velocity = 0
  48. self._change_lane_velocity = 15
  49. self._slow_vehicle_distance = 100
  50. self._fast_vehicle_distance = 20
  51. self._trigger_distance = 30
  52. self._max_brake = 1
  53. self.direction = 'left' # direction of lane change
  54. self.lane_check = 'true' # check whether a lane change is possible
  55. super(ChangeLane, self).__init__("ChangeLane",
  56. ego_vehicles,
  57. config,
  58. world,
  59. debug_mode,
  60. criteria_enable=criteria_enable)
  61. if randomize:
  62. self._fast_vehicle_distance = random.randint(10, 51)
  63. self._fast_vehicle_velocity = random.randint(100, 201)
  64. self._slow_vehicle_velocity = random.randint(1, 6)
  65. def _initialize_actors(self, config):
  66. # add actors from xml file
  67. for actor in config.other_actors:
  68. vehicle = CarlaDataProvider.request_new_actor(actor.model, actor.transform)
  69. self.other_actors.append(vehicle)
  70. vehicle.set_simulate_physics(enabled=False)
  71. # fast vehicle, tesla
  72. # transform visible
  73. fast_car_waypoint, _ = get_waypoint_in_distance(self._reference_waypoint, self._fast_vehicle_distance)
  74. self.fast_car_visible = carla.Transform(
  75. carla.Location(fast_car_waypoint.transform.location.x,
  76. fast_car_waypoint.transform.location.y,
  77. fast_car_waypoint.transform.location.z + 1),
  78. fast_car_waypoint.transform.rotation)
  79. # slow vehicle, vw
  80. # transform visible
  81. slow_car_waypoint, _ = get_waypoint_in_distance(self._reference_waypoint, self._slow_vehicle_distance)
  82. self.slow_car_visible = carla.Transform(
  83. carla.Location(slow_car_waypoint.transform.location.x,
  84. slow_car_waypoint.transform.location.y,
  85. slow_car_waypoint.transform.location.z),
  86. slow_car_waypoint.transform.rotation)
  87. def _create_behavior(self):
  88. # sequence vw
  89. # make visible
  90. sequence_vw = py_trees.composites.Sequence("VW T2")
  91. vw_visible = ActorTransformSetter(self.other_actors[1], self.slow_car_visible)
  92. sequence_vw.add_child(vw_visible)
  93. # brake, avoid rolling backwarts
  94. brake = StopVehicle(self.other_actors[1], self._max_brake)
  95. sequence_vw.add_child(brake)
  96. sequence_vw.add_child(Idle())
  97. # sequence tesla
  98. # make visible
  99. sequence_tesla = py_trees.composites.Sequence("Tesla")
  100. tesla_visible = ActorTransformSetter(self.other_actors[0], self.fast_car_visible)
  101. sequence_tesla.add_child(tesla_visible)
  102. # drive fast towards slow vehicle
  103. just_drive = py_trees.composites.Parallel("DrivingTowardsSlowVehicle",
  104. policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
  105. tesla_driving_fast = WaypointFollower(self.other_actors[0], self._fast_vehicle_velocity)
  106. just_drive.add_child(tesla_driving_fast)
  107. distance_to_vehicle = InTriggerDistanceToVehicle(
  108. self.other_actors[1], self.other_actors[0], self._trigger_distance)
  109. just_drive.add_child(distance_to_vehicle)
  110. sequence_tesla.add_child(just_drive)
  111. # change lane
  112. lane_change_atomic = LaneChange(self.other_actors[0], distance_other_lane=200)
  113. sequence_tesla.add_child(lane_change_atomic)
  114. sequence_tesla.add_child(Idle())
  115. # ego vehicle
  116. # end condition
  117. endcondition = py_trees.composites.Parallel("Waiting for end position of ego vehicle",
  118. policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
  119. endcondition_part1 = InTriggerDistanceToVehicle(self.other_actors[1],
  120. self.ego_vehicles[0],
  121. distance=20,
  122. name="FinalDistance")
  123. endcondition_part2 = StandStill(self.ego_vehicles[0], name="FinalSpeed", duration=1)
  124. endcondition.add_child(endcondition_part1)
  125. endcondition.add_child(endcondition_part2)
  126. # build tree
  127. root = py_trees.composites.Parallel("Parallel Behavior", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
  128. root.add_child(sequence_vw)
  129. root.add_child(sequence_tesla)
  130. root.add_child(endcondition)
  131. return root
  132. def _create_test_criteria(self):
  133. """
  134. A list of all test criteria will be created that is later used
  135. in parallel behavior tree.
  136. """
  137. criteria = []
  138. collision_criterion = CollisionTest(self.ego_vehicles[0])
  139. criteria.append(collision_criterion)
  140. return criteria
  141. def __del__(self):
  142. """
  143. Remove all actors upon deletion
  144. """
  145. self.remove_all_actors()