From b281a2dd6b3c3594c209a1abd48cd3e6aaab319c Mon Sep 17 00:00:00 2001 From: Fabian Oboril Date: Wed, 18 Nov 2020 11:06:49 +0100 Subject: [PATCH] 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 | 1 + Docs/openscenario_support.md | 2 +- .../actorcontrols/npc_vehicle_control.py | 1 + .../scenarioatomics/atomic_behaviors.py | 65 +++++++++++++++---- srunner/tools/openscenario_parser.py | 7 +- 5 files changed, 62 insertions(+), 14 deletions(-) diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 2b3fafc21..26675d234 100644 --- a/Docs/CHANGELOG.md +++ 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 diff --git a/Docs/openscenario_support.md b/Docs/openscenario_support.md index c2537f5b3..8eca0336d 100755 --- a/Docs/openscenario_support.md +++ b/Docs/openscenario_support.md @@ -252,7 +252,7 @@ contains of submodules, which are not listed, the support status applies to all RoutingAction
AssignRouteAction ❌ ✅ - +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 RoutingAction
FollowTrajectoryAction ❌ diff --git a/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py b/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py index d45cf3d30..9969df57c 100644 --- a/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py +++ b/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( diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 0bbf57614..70edca5ef 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/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. diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index 7e8cdd489..78105d695 100644 --- a/srunner/tools/openscenario_parser.py +++ b/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")