route_parser.py 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325
  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. Module used to parse all the route and scenario configuration parameters.
  6. """
  7. import json
  8. import math
  9. import xml.etree.ElementTree as ET
  10. import carla
  11. from agents.navigation.local_planner import RoadOption
  12. from srunner.scenarioconfigs.route_scenario_configuration import RouteScenarioConfiguration
  13. # TODO check this threshold, it could be a bit larger but not so large that we cluster scenarios.
  14. TRIGGER_THRESHOLD = 2.0 # Threshold to say if a trigger position is new or repeated, works for matching positions
  15. TRIGGER_ANGLE_THRESHOLD = 10 # Threshold to say if two angles can be considering matching when matching transforms.
  16. class RouteParser(object):
  17. """
  18. Pure static class used to parse all the route and scenario configuration parameters.
  19. """
  20. @staticmethod
  21. def parse_annotations_file(annotation_filename):
  22. """
  23. Return the annotations of which positions where the scenarios are going to happen.
  24. :param annotation_filename: the filename for the anotations file
  25. :return:
  26. """
  27. with open(annotation_filename, 'r', encoding='utf-8') as f:
  28. annotation_dict = json.loads(f.read())
  29. final_dict = {}
  30. for town_dict in annotation_dict['available_scenarios']:
  31. final_dict.update(town_dict)
  32. return final_dict # the file has a current maps name that is an one element vec
  33. @staticmethod
  34. def parse_routes_file(route_filename, scenario_file, single_route=None):
  35. """
  36. Returns a list of route elements.
  37. :param route_filename: the path to a set of routes.
  38. :param single_route: If set, only this route shall be returned
  39. :return: List of dicts containing the waypoints, id and town of the routes
  40. """
  41. list_route_descriptions = []
  42. tree = ET.parse(route_filename)
  43. for route in tree.iter("route"):
  44. route_id = route.attrib['id']
  45. if single_route and route_id != single_route:
  46. continue
  47. new_config = RouteScenarioConfiguration()
  48. new_config.town = route.attrib['town']
  49. new_config.name = "RouteScenario_{}".format(route_id)
  50. new_config.weather = RouteParser.parse_weather(route)
  51. new_config.scenario_file = scenario_file
  52. waypoint_list = [] # the list of waypoints that can be found on this route
  53. for waypoint in route.iter('waypoint'):
  54. waypoint_list.append(carla.Location(x=float(waypoint.attrib['x']),
  55. y=float(waypoint.attrib['y']),
  56. z=float(waypoint.attrib['z'])))
  57. new_config.trajectory = waypoint_list
  58. list_route_descriptions.append(new_config)
  59. return list_route_descriptions
  60. @staticmethod
  61. def parse_weather(route):
  62. """
  63. Returns a carla.WeatherParameters with the corresponding weather for that route. If the route
  64. has no weather attribute, the default one is triggered.
  65. """
  66. route_weather = route.find("weather")
  67. if route_weather is None:
  68. weather = carla.WeatherParameters(sun_altitude_angle=70)
  69. else:
  70. weather = carla.WeatherParameters()
  71. for weather_attrib in route.iter("weather"):
  72. if 'cloudiness' in weather_attrib.attrib:
  73. weather.cloudiness = float(weather_attrib.attrib['cloudiness'])
  74. if 'precipitation' in weather_attrib.attrib:
  75. weather.precipitation = float(weather_attrib.attrib['precipitation'])
  76. if 'precipitation_deposits' in weather_attrib.attrib:
  77. weather.precipitation_deposits = float(weather_attrib.attrib['precipitation_deposits'])
  78. if 'wind_intensity' in weather_attrib.attrib:
  79. weather.wind_intensity = float(weather_attrib.attrib['wind_intensity'])
  80. if 'sun_azimuth_angle' in weather_attrib.attrib:
  81. weather.sun_azimuth_angle = float(weather_attrib.attrib['sun_azimuth_angle'])
  82. if 'sun_altitude_angle' in weather_attrib.attrib:
  83. weather.sun_altitude_angle = float(weather_attrib.attrib['sun_altitude_angle'])
  84. if 'wetness' in weather_attrib.attrib:
  85. weather.wetness = float(weather_attrib.attrib['wetness'])
  86. if 'fog_distance' in weather_attrib.attrib:
  87. weather.fog_distance = float(weather_attrib.attrib['fog_distance'])
  88. if 'fog_density' in weather_attrib.attrib:
  89. weather.fog_density = float(weather_attrib.attrib['fog_density'])
  90. return weather
  91. @staticmethod
  92. def check_trigger_position(new_trigger, existing_triggers):
  93. """
  94. Check if this trigger position already exists or if it is a new one.
  95. :param new_trigger:
  96. :param existing_triggers:
  97. :return:
  98. """
  99. for trigger_id in existing_triggers.keys():
  100. trigger = existing_triggers[trigger_id]
  101. dx = trigger['x'] - new_trigger['x']
  102. dy = trigger['y'] - new_trigger['y']
  103. distance = math.sqrt(dx * dx + dy * dy)
  104. dyaw = (trigger['yaw'] - new_trigger['yaw']) % 360
  105. if distance < TRIGGER_THRESHOLD \
  106. and (dyaw < TRIGGER_ANGLE_THRESHOLD or dyaw > (360 - TRIGGER_ANGLE_THRESHOLD)):
  107. return trigger_id
  108. return None
  109. @staticmethod
  110. def convert_waypoint_float(waypoint):
  111. """
  112. Convert waypoint values to float
  113. """
  114. waypoint['x'] = float(waypoint['x'])
  115. waypoint['y'] = float(waypoint['y'])
  116. waypoint['z'] = float(waypoint['z'])
  117. waypoint['yaw'] = float(waypoint['yaw'])
  118. @staticmethod
  119. def match_world_location_to_route(world_location, route_description):
  120. """
  121. We match this location to a given route.
  122. world_location:
  123. route_description:
  124. """
  125. def match_waypoints(waypoint1, wtransform):
  126. """
  127. Check if waypoint1 and wtransform are similar
  128. """
  129. dx = float(waypoint1['x']) - wtransform.location.x
  130. dy = float(waypoint1['y']) - wtransform.location.y
  131. dz = float(waypoint1['z']) - wtransform.location.z
  132. dpos = math.sqrt(dx * dx + dy * dy + dz * dz)
  133. dyaw = (float(waypoint1['yaw']) - wtransform.rotation.yaw) % 360
  134. return dpos < TRIGGER_THRESHOLD \
  135. and (dyaw < TRIGGER_ANGLE_THRESHOLD or dyaw > (360 - TRIGGER_ANGLE_THRESHOLD))
  136. match_position = 0
  137. # TODO this function can be optimized to run on Log(N) time
  138. for route_waypoint in route_description:
  139. if match_waypoints(world_location, route_waypoint[0]):
  140. return match_position
  141. match_position += 1
  142. return None
  143. @staticmethod
  144. def get_scenario_type(scenario, match_position, trajectory):
  145. """
  146. Some scenarios have different types depending on the route.
  147. :param scenario: the scenario name
  148. :param match_position: the matching position for the scenarion
  149. :param trajectory: the route trajectory the ego is following
  150. :return: tag representing this subtype
  151. Also used to check which are not viable (Such as an scenario
  152. that triggers when turning but the route doesnt')
  153. WARNING: These tags are used at:
  154. - VehicleTurningRoute
  155. - SignalJunctionCrossingRoute
  156. and changes to these tags will affect them
  157. """
  158. def check_this_waypoint(tuple_wp_turn):
  159. """
  160. Decides whether or not the waypoint will define the scenario behavior
  161. """
  162. if RoadOption.LANEFOLLOW == tuple_wp_turn[1]:
  163. return False
  164. elif RoadOption.CHANGELANELEFT == tuple_wp_turn[1]:
  165. return False
  166. elif RoadOption.CHANGELANERIGHT == tuple_wp_turn[1]:
  167. return False
  168. return True
  169. # Unused tag for the rest of scenarios,
  170. # can't be None as they are still valid scenarios
  171. subtype = 'valid'
  172. if scenario == 'Scenario4':
  173. for tuple_wp_turn in trajectory[match_position:]:
  174. if check_this_waypoint(tuple_wp_turn):
  175. if RoadOption.LEFT == tuple_wp_turn[1]:
  176. subtype = 'S4left'
  177. elif RoadOption.RIGHT == tuple_wp_turn[1]:
  178. subtype = 'S4right'
  179. else:
  180. subtype = None
  181. break # Avoid checking all of them
  182. subtype = None
  183. if scenario == 'Scenario7':
  184. for tuple_wp_turn in trajectory[match_position:]:
  185. if check_this_waypoint(tuple_wp_turn):
  186. if RoadOption.LEFT == tuple_wp_turn[1]:
  187. subtype = 'S7left'
  188. elif RoadOption.RIGHT == tuple_wp_turn[1]:
  189. subtype = 'S7right'
  190. elif RoadOption.STRAIGHT == tuple_wp_turn[1]:
  191. subtype = 'S7opposite'
  192. else:
  193. subtype = None
  194. break # Avoid checking all of them
  195. subtype = None
  196. if scenario == 'Scenario8':
  197. for tuple_wp_turn in trajectory[match_position:]:
  198. if check_this_waypoint(tuple_wp_turn):
  199. if RoadOption.LEFT == tuple_wp_turn[1]:
  200. subtype = 'S8left'
  201. else:
  202. subtype = None
  203. break # Avoid checking all of them
  204. subtype = None
  205. if scenario == 'Scenario9':
  206. for tuple_wp_turn in trajectory[match_position:]:
  207. if check_this_waypoint(tuple_wp_turn):
  208. if RoadOption.RIGHT == tuple_wp_turn[1]:
  209. subtype = 'S9right'
  210. else:
  211. subtype = None
  212. break # Avoid checking all of them
  213. subtype = None
  214. return subtype
  215. @staticmethod
  216. def scan_route_for_scenarios(route_name, trajectory, world_annotations):
  217. """
  218. Just returns a plain list of possible scenarios that can happen in this route by matching
  219. the locations from the scenario into the route description
  220. :return: A list of scenario definitions with their correspondent parameters
  221. """
  222. # the triggers dictionaries:
  223. existent_triggers = {}
  224. # We have a table of IDs and trigger positions associated
  225. possible_scenarios = {}
  226. # Keep track of the trigger ids being added
  227. latest_trigger_id = 0
  228. for town_name in world_annotations.keys():
  229. if town_name != route_name:
  230. continue
  231. scenarios = world_annotations[town_name]
  232. for scenario in scenarios: # For each existent scenario
  233. if "scenario_type" not in scenario:
  234. break
  235. scenario_name = scenario["scenario_type"]
  236. for event in scenario["available_event_configurations"]:
  237. waypoint = event['transform'] # trigger point of this scenario
  238. RouteParser.convert_waypoint_float(waypoint)
  239. # We match trigger point to the route, now we need to check if the route affects
  240. match_position = RouteParser.match_world_location_to_route(
  241. waypoint, trajectory)
  242. if match_position is not None:
  243. # We match a location for this scenario, create a scenario object so this scenario
  244. # can be instantiated later
  245. if 'other_actors' in event:
  246. other_vehicles = event['other_actors']
  247. else:
  248. other_vehicles = None
  249. scenario_subtype = RouteParser.get_scenario_type(scenario_name, match_position,
  250. trajectory)
  251. if scenario_subtype is None:
  252. continue
  253. scenario_description = {
  254. 'name': scenario_name,
  255. 'other_actors': other_vehicles,
  256. 'trigger_position': waypoint,
  257. 'scenario_type': scenario_subtype, # some scenarios have route dependent configs
  258. }
  259. trigger_id = RouteParser.check_trigger_position(waypoint, existent_triggers)
  260. if trigger_id is None:
  261. # This trigger does not exist create a new reference on existent triggers
  262. existent_triggers.update({latest_trigger_id: waypoint})
  263. # Update a reference for this trigger on the possible scenarios
  264. possible_scenarios.update({latest_trigger_id: []})
  265. trigger_id = latest_trigger_id
  266. # Increment the latest trigger
  267. latest_trigger_id += 1
  268. possible_scenarios[trigger_id].append(scenario_description)
  269. return possible_scenarios, existent_triggers