Fixed AcquirePositionAction in OSC

srunner/scenariomanager/scenarioatomics/atomic_behaviors.py

@@ -537,7 +537,6 @@ class ChangeActorWaypoints(AtomicBehavior):
     The behavior is in RUNNING state until the last waypoint is reached, or if a
     second waypoint related atomic for the same actor is triggered. These are:
     - ChangeActorWaypoints
-    - ChangeActorWaypointsToReachPosition
     - ChangeActorLateralMotion
 
     Args:
@@ -606,7 +605,14 @@ def initialise(self):
                 route.append(carla_route_elements[i][0])
             else:
                 if i == 0:
-                    waypoint = CarlaDataProvider.get_location(self._actor)
+                    mmap = CarlaDataProvider.get_map()
+                    ego_location = CarlaDataProvider.get_location(self._actor)
+                    ego_waypoint = mmap.get_waypoint(ego_location)
+                    try:
+                        ego_next_wp = ego_waypoint.next(1)[0]
+                    except IndexError:
+                        ego_next_wp = ego_waypoint
+                    waypoint = ego_next_wp.transform.location
                 else:
                     waypoint = carla_route_elements[i - 1][0].location
                 waypoint_next = carla_route_elements[i][0].location
@@ -625,7 +631,7 @@ def initialise(self):
                         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_vec = route[-1].location - ego_next_wp.transform.location
                         last_heading = np.arctan2(last_heading_vec.y, last_heading_vec.x)
 
                         heading_delta = math.fabs(new_heading - last_heading)
@@ -668,67 +674,6 @@ def update(self):
         return new_status
 
 
-class ChangeActorWaypointsToReachPosition(ChangeActorWaypoints):
-
-    """
-    Atomic to change the waypoints for an actor controller in order to reach
-    a given position.
-
-    The behavior is in RUNNING state until the last waypoint is reached, or if a
-    second waypoint related atomic for the same actor is triggered. These are:
-    - ChangeActorWaypoints
-    - ChangeActorWaypointsToReachPosition
-    - ChangeActorLateralMotion
-
-    Args:
-        actor (carla.Actor): Controlled actor.
-        position (carla.Transform): CARLA transform to be reached by the actor.
-        name (string): Name of the behavior.
-            Defaults to 'ChangeActorWaypointsToReachPosition'.
-
-    Attributes:
-        _end_transform (carla.Transform): Final position (CARLA transform).
-        _start_time (float): Start time of the atomic [s].
-            Defaults to None.
-        _grp (GlobalPlanner): global planner instance of the town
-    """
-
-    def __init__(self, actor, position, name="ChangeActorWaypointsToReachPosition"):
-        """
-        Setup parameters
-        """
-        super(ChangeActorWaypointsToReachPosition, self).__init__(actor, [])
-
-        self._end_transform = position
-
-        town_map = CarlaDataProvider.get_map()
-        dao = GlobalRoutePlannerDAO(town_map, 2)
-        self._grp = GlobalRoutePlanner(dao)
-        self._grp.setup()
-
-    def initialise(self):
-        """
-        Set _start_time and get (actor, controller) pair from Blackboard.
-
-        Generate a waypoint list (route) which representes the route. Set
-        this waypoint list for the actor controller.
-
-        May throw if actor is not available as key for the ActorsWithController
-        dictionary from Blackboard.
-        """
-
-        # get start position
-        position_actor = CarlaDataProvider.get_location(self._actor)
-
-        # calculate plan with global_route_planner function
-        plan = self._grp.trace_route(position_actor, self._end_transform.location)
-
-        for elem in plan:
-            self._waypoints.append(elem[0].transform)
-
-        super(ChangeActorWaypointsToReachPosition, self).initialise()
-
-
 class ChangeActorLateralMotion(AtomicBehavior):
 
     """
@@ -737,7 +682,6 @@ class ChangeActorLateralMotion(AtomicBehavior):
     The behavior is in RUNNING state until the last waypoint is reached, or if a
     second waypoint related atomic for the same actor is triggered. These are:
     - ChangeActorWaypoints
-    - ChangeActorWaypointsToReachPosition
     - ChangeActorLateralMotion
 
     If an impossible lane change is asked for (due to the lack of lateral lanes,

srunner/tools/openscenario_parser.py

@@ -31,7 +31,6 @@
                                                                       ChangeActorTargetSpeed,
                                                                       ChangeActorControl,
                                                                       ChangeActorWaypoints,
-                                                                      ChangeActorWaypointsToReachPosition,
                                                                       ChangeActorLateralMotion,
                                                                       Idle)
 # pylint: disable=unused-import
@@ -1071,8 +1070,8 @@ def convert_maneuver_to_atomic(action, actor, catalogs):
                 elif private_action.find('AcquirePositionAction') is not None:
                     route_action = private_action.find('AcquirePositionAction')
                     osc_position = route_action.find('Position')
-                    position = OpenScenarioParser.convert_position_to_transform(osc_position)
-                    atomic = ChangeActorWaypointsToReachPosition(actor, position=position, name=maneuver_name)
+                    waypoints = [(osc_position, 'fastest')]
+                    atomic = ChangeActorWaypoints(actor, waypoints=waypoints, name=maneuver_name)
                 else:
                     raise AttributeError("Unknown private routing action")
             else: