construction_crash_vehicle.py 5.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165
  1. #!/usr/bin/env python
  2. # This work is licensed under the terms of the MIT license.
  3. # For a copy, see <https://opensource.org/licenses/MIT>.
  4. """
  5. Object crash without prior vehicle action scenario:
  6. The scenario realizes the user controlled ego vehicle
  7. moving along the road and encountering a construction setup.
  8. """
  9. from __future__ import print_function
  10. import py_trees
  11. import carla
  12. from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
  13. from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy
  14. from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance
  15. from srunner.scenariomanager.scenarioatomics.atomic_behaviors import Idle
  16. from srunner.tools.scenario_helper import get_location_in_distance_from_wp
  17. from srunner.scenarios.object_crash_vehicle import StationaryObjectCrossing
  18. class ConstructionSetupCrossing(StationaryObjectCrossing):
  19. """
  20. This class holds everything required for a construction scenario
  21. The ego vehicle is passing through a road and encounters
  22. a stationary rectangular construction cones setup and traffic warning.
  23. This is a single ego vehicle scenario
  24. """
  25. def __init__(
  26. self,
  27. world,
  28. ego_vehicles,
  29. config,
  30. randomize=False,
  31. debug_mode=False,
  32. criteria_enable=True,
  33. timeout=60):
  34. """
  35. Setup all relevant parameters and create scenario
  36. """
  37. super(
  38. ConstructionSetupCrossing,
  39. self).__init__(
  40. world,
  41. ego_vehicles=ego_vehicles,
  42. config=config,
  43. randomize=randomize,
  44. debug_mode=debug_mode,
  45. criteria_enable=criteria_enable)
  46. def _initialize_actors(self, config):
  47. """
  48. Custom initialization
  49. """
  50. _start_distance = 40
  51. lane_width = self._reference_waypoint.lane_width
  52. location, _ = get_location_in_distance_from_wp(
  53. self._reference_waypoint, _start_distance)
  54. waypoint = self._wmap.get_waypoint(location)
  55. self._create_construction_setup(waypoint.transform, lane_width)
  56. def create_cones_side(
  57. self,
  58. start_transform,
  59. forward_vector,
  60. z_inc=0,
  61. cone_length=0,
  62. cone_offset=0):
  63. """
  64. Creates One Side of the Cones
  65. """
  66. _dist = 0
  67. while _dist < (cone_length * cone_offset):
  68. # Move forward
  69. _dist += cone_offset
  70. forward_dist = carla.Vector3D(0, 0, 0) + forward_vector * _dist
  71. location = start_transform.location + forward_dist
  72. location.z += z_inc
  73. transform = carla.Transform(location, start_transform.rotation)
  74. cone = CarlaDataProvider.request_new_actor(
  75. 'static.prop.constructioncone', transform)
  76. cone.set_simulate_physics(True)
  77. self.other_actors.append(cone)
  78. def _create_construction_setup(self, start_transform, lane_width):
  79. """
  80. Create Construction Setup
  81. """
  82. _initial_offset = {'cones': {'yaw': 180, 'k': lane_width / 2.0},
  83. 'warning_sign': {'yaw': 180, 'k': 5, 'z': 0},
  84. 'debris': {'yaw': 0, 'k': 2, 'z': 1}}
  85. _prop_names = {'warning_sign': 'static.prop.trafficwarning',
  86. 'debris': 'static.prop.dirtdebris02'}
  87. _perp_angle = 90
  88. _setup = {'lengths': [0, 6, 3], 'offsets': [0, 2, 1]}
  89. _z_increment = 0.1
  90. ############################# Traffic Warning and Debris ##############
  91. for key, value in _initial_offset.items():
  92. if key == 'cones':
  93. continue
  94. transform = carla.Transform(
  95. start_transform.location,
  96. start_transform.rotation)
  97. transform.rotation.yaw += value['yaw']
  98. transform.location += value['k'] * \
  99. transform.rotation.get_forward_vector()
  100. transform.location.z += value['z']
  101. transform.rotation.yaw += _perp_angle
  102. static = CarlaDataProvider.request_new_actor(
  103. _prop_names[key], transform)
  104. static.set_simulate_physics(True)
  105. self.other_actors.append(static)
  106. ############################# Cones ###################################
  107. side_transform = carla.Transform(
  108. start_transform.location,
  109. start_transform.rotation)
  110. side_transform.rotation.yaw += _perp_angle
  111. side_transform.location -= _initial_offset['cones']['k'] * \
  112. side_transform.rotation.get_forward_vector()
  113. side_transform.rotation.yaw += _initial_offset['cones']['yaw']
  114. for i in range(len(_setup['lengths'])):
  115. self.create_cones_side(
  116. side_transform,
  117. forward_vector=side_transform.rotation.get_forward_vector(),
  118. z_inc=_z_increment,
  119. cone_length=_setup['lengths'][i],
  120. cone_offset=_setup['offsets'][i])
  121. side_transform.location += side_transform.get_forward_vector() * \
  122. _setup['lengths'][i] * _setup['offsets'][i]
  123. side_transform.rotation.yaw += _perp_angle
  124. def _create_behavior(self):
  125. """
  126. Only behavior here is to wait
  127. """
  128. # leaf nodes
  129. actor_stand = Idle(15)
  130. end_condition = DriveDistance(
  131. self.ego_vehicles[0],
  132. self._ego_vehicle_distance_driven)
  133. # non leaf nodes
  134. scenario_sequence = py_trees.composites.Sequence()
  135. # building tree
  136. scenario_sequence.add_child(actor_stand)
  137. for i, _ in enumerate(self.other_actors):
  138. scenario_sequence.add_child(ActorDestroy(self.other_actors[i]))
  139. scenario_sequence.add_child(end_condition)
  140. return scenario_sequence