autonomous_agent.py 3.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108
  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. This module provides the base class for all autonomous agents
  6. """
  7. from __future__ import print_function
  8. import carla
  9. from srunner.autoagents.sensor_interface import SensorInterface
  10. from srunner.scenariomanager.timer import GameTime
  11. from srunner.tools.route_manipulation import downsample_route
  12. class AutonomousAgent(object):
  13. """
  14. Autonomous agent base class. All user agents have to be derived from this class
  15. """
  16. def __init__(self, path_to_conf_file):
  17. # current global plans to reach a destination
  18. self._global_plan = None
  19. self._global_plan_world_coord = None
  20. # this data structure will contain all sensor data
  21. self.sensor_interface = SensorInterface()
  22. # agent's initialization
  23. self.setup(path_to_conf_file)
  24. def setup(self, path_to_conf_file):
  25. """
  26. Initialize everything needed by your agent and set the track attribute to the right type:
  27. """
  28. pass
  29. def sensors(self): # pylint: disable=no-self-use
  30. """
  31. Define the sensor suite required by the agent
  32. :return: a list containing the required sensors in the following format:
  33. [
  34. {'type': 'sensor.camera.rgb', 'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0,
  35. 'width': 300, 'height': 200, 'fov': 100, 'id': 'Left'},
  36. {'type': 'sensor.camera.rgb', 'x': 0.7, 'y': 0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0,
  37. 'width': 300, 'height': 200, 'fov': 100, 'id': 'Right'},
  38. {'type': 'sensor.lidar.ray_cast', 'x': 0.7, 'y': 0.0, 'z': 1.60, 'yaw': 0.0, 'pitch': 0.0, 'roll': 0.0,
  39. 'id': 'LIDAR'}
  40. ]
  41. """
  42. sensors = []
  43. return sensors
  44. def run_step(self, input_data, timestamp):
  45. """
  46. Execute one step of navigation.
  47. :return: control
  48. """
  49. control = carla.VehicleControl()
  50. control.steer = 0.0
  51. control.throttle = 0.0
  52. control.brake = 0.0
  53. control.hand_brake = False
  54. return control
  55. def destroy(self):
  56. """
  57. Destroy (clean-up) the agent
  58. :return:
  59. """
  60. pass
  61. def __call__(self):
  62. """
  63. Execute the agent call, e.g. agent()
  64. Returns the next vehicle controls
  65. """
  66. input_data = self.sensor_interface.get_data()
  67. timestamp = GameTime.get_time()
  68. wallclock = GameTime.get_wallclocktime()
  69. print('======[Agent] Wallclock_time = {} / Sim_time = {}'.format(wallclock, timestamp))
  70. control = self.run_step(input_data, timestamp)
  71. control.manual_gear_shift = False
  72. return control
  73. def set_global_plan(self, global_plan_gps, global_plan_world_coord):
  74. """
  75. Set the plan (route) for the agent
  76. """
  77. ds_ids = downsample_route(global_plan_world_coord, 1)
  78. self._global_plan_world_coord = [(global_plan_world_coord[x][0], global_plan_world_coord[x][1])
  79. for x in ds_ids]
  80. self._global_plan = [global_plan_gps[x] for x in ds_ids]