Added support for OSC routing options

When using routing options (routeStrategy) for route waypoints in OSC,
shortest and fastest will be treated differently.

Shortest = direct connection between A and B
Others = shortest path along the road network from A to B

Docs/CHANGELOG.md

@@ -14,6 +14,7 @@
 * Added a sensor barrier for the agents to ensure that the simulation waits for them to render their data.
 * Added an option to produce a machine-readable JSON version of the scenario report.
 * Added a static obstacle evasion OpenSCENARIO scenario
+* Added support for OSC Routing options
 ### :bug: Bug Fixes
 * Fixed exception when using OSC scenarios without EnvironmentAction inside Storyboard-Init
 * Fixed bug causing the TrafficManager to not be correctly updated at asynchronous simualtions

Docs/openscenario_support.md

@@ -252,7 +252,7 @@ contains of submodules, which are not listed, the support status applies to all
 <td><small><code>RoutingAction</code></small><br><code>AssignRouteAction</code></td>
 <td>&#10060;</td>
 <td>&#9989;</td>
-<td></td>
+<td>Route Options (shortest/fastest/etc) are supported. Shortests means direct path between A and B, all other will use the shortest path along the road network between A and B</td>
 <tr>
 <td><small><code>RoutingAction</code></small><br><code>FollowTrajectoryAction</code></td>
 <td>&#10060;</td>

srunner/scenariomanager/actorcontrols/npc_vehicle_control.py

@@ -47,6 +47,7 @@ def _update_plan(self):
         """
         Update the plan (waypoint list) of the LocalPlanner
         """
+        self._local_planner._waypoint_buffer.clear()    # pylint: disable=protected-access
         plan = []
         for transform in self._waypoints:
             waypoint = CarlaDataProvider.get_map().get_waypoint(

srunner/scenariomanager/scenarioatomics/atomic_behaviors.py

@@ -26,6 +26,7 @@
 import numpy as np
 import py_trees
 from py_trees.blackboard import Blackboard
+import networkx
 
 import carla
 from agents.navigation.basic_agent import BasicAgent, LocalPlanner
@@ -541,15 +542,19 @@ class ChangeActorWaypoints(AtomicBehavior):
 
     Args:
         actor (carla.Actor): Controlled actor.
-        waypoints (List of OSC elements): List of 'Position' OpenScenario XML elements.
-            waypoints will be converted to Carla transforms.
+        waypoints (List of (OSC position, OSC route option)): List of (Position, Route Option) as OpenScenario elements.
+            position will be converted to Carla transforms, considering the corresponding
+            route option (e.g. shortest, fastest)
         name (string): Name of the behavior.
             Defaults to 'ChangeActorWaypoints'.
 
     Attributes:
-        _waypoints (List of carla.Transform): List of waypoints (CARLA transforms).
+        _waypoints (List of (OSC position, OSC route option)): List of (Position, Route Option) as OpenScenario elements
         _start_time (float): Start time of the atomic [s].
             Defaults to None.
+
+    '''Note: When using routing options such as fastest or shortest, it is advisable to run
+             in synchronous mode
     """
 
     def __init__(self, actor, waypoints, name="ChangeActorWaypoints"):
@@ -584,13 +589,52 @@ def initialise(self):
         self._start_time = GameTime.get_time()
 
         # Transforming OSC waypoints to Carla waypoints
-        carla_waypoints = []
-        for point in self._waypoints:
-            carla_transforms = srunner.tools.openscenario_parser.OpenScenarioParser.convert_position_to_transform(point)
-            carla_waypoints.append(carla_transforms)
-        self._waypoints = carla_waypoints
-
-        actor_dict[self._actor.id].update_waypoints(self._waypoints, start_time=self._start_time)
+        carla_route_elements = []
+        for (osc_point, routing_option) in self._waypoints:
+            carla_transforms = srunner.tools.openscenario_parser.OpenScenarioParser.convert_position_to_transform(
+                osc_point)
+            carla_route_elements.append((carla_transforms, routing_option))
+
+        # Obtain final route, considering the routing option
+        # At the moment everything besides "shortest" will use the CARLA GlobalPlanner
+        dao = GlobalRoutePlannerDAO(CarlaDataProvider.get_world().get_map(), 2.0)
+        grp = GlobalRoutePlanner(dao)
+        grp.setup()
+        route = []
+        for i, _ in enumerate(carla_route_elements):
+            if carla_route_elements[i][1] == "shortest":
+                route.append(carla_route_elements[i][0])
+            else:
+                if i == 0:
+                    waypoint = CarlaDataProvider.get_location(self._actor)
+                else:
+                    waypoint = carla_route_elements[i - 1][0].location
+                waypoint_next = carla_route_elements[i][0].location
+                try:
+                    interpolated_trace = grp.trace_route(waypoint, waypoint_next)
+                except networkx.NetworkXNoPath:
+                    print("WARNING: No route from {} to {} - Using direct path instead".format(waypoint, waypoint_next))
+                    route.append(carla_route_elements[i][0])
+                    continue
+                for wp_tuple in interpolated_trace:
+                    # The router sometimes produces points that go backward, or are almost identical
+                    # We have to filter these, to avoid problems
+                    if route and wp_tuple[0].transform.location.distance(route[-1].location) > 1.0:
+                        new_heading_vec = wp_tuple[0].transform.location - route[-1].location
+                        new_heading = np.arctan2(new_heading_vec.y, new_heading_vec.x)
+                        if len(route) > 1:
+                            last_heading_vec = route[-1].location - route[-2].location
+                        else:
+                            last_heading_vec = route[-1].location - CarlaDataProvider.get_location(self._actor)
+                        last_heading = np.arctan2(last_heading_vec.y, last_heading_vec.x)
+
+                        heading_delta = math.fabs(new_heading - last_heading)
+                        if math.fabs(heading_delta) < 0.5 or math.fabs(heading_delta) > 5.5:
+                            route.append(wp_tuple[0].transform)
+                    elif not route:
+                        route.append(wp_tuple[0].transform)
+
+        actor_dict[self._actor.id].update_waypoints(route, start_time=self._start_time)
 
         super(ChangeActorWaypoints, self).initialise()
 
@@ -643,7 +687,6 @@ class ChangeActorWaypointsToReachPosition(ChangeActorWaypoints):
             Defaults to 'ChangeActorWaypointsToReachPosition'.
 
     Attributes:
-        _waypoints (List of carla.Transform): List of waypoints (CARLA transforms).
         _end_transform (carla.Transform): Final position (CARLA transform).
         _start_time (float): Start time of the atomic [s].
             Defaults to None.

srunner/tools/openscenario_parser.py

@@ -411,7 +411,9 @@ def get_route(xml_tree, catalogs):
             catalogs: XML Catalogs that could contain the Route
 
         returns:
-           waypoints: List of route waypoints
+           waypoints: List of route waypoints = (waypoint, routing strategy)
+                      where the strategy is a string indicating if the fastest/shortest/etc.
+                      route should be used.
         """
         route = None
 
@@ -427,7 +429,8 @@ def get_route(xml_tree, catalogs):
         if route is not None:
             for waypoint in route.iter('Waypoint'):
                 position = waypoint.find('Position')
-                waypoints.append(position)
+                routing_option = str(waypoint.attrib.get('routeStrategy'))
+                waypoints.append((position, routing_option))
         else:
             raise AttributeError("No waypoints has been set")