Added support for LaneOffsetAction

* Added support for LaneOffsetAction

Co-authored-by: Fabian Oboril <[email protected]>

Docs/CHANGELOG.md

@@ -16,6 +16,7 @@
 * Added a static obstacle evasion OpenSCENARIO scenario
 * Added support for OSC Routing options
 * Added support for OSC SynchronizeAction
+* Added support for OSC LaneOffsetAction
 * Added support to place OSC controller implementation alongside the OSC scenario
 * Updated *GameTime.restart()* at *srunner/scenariomanager/timer.py* to also reset the frame number
 ### :bug: Bug Fixes

Docs/openscenario_support.md

@@ -92,13 +92,12 @@ contains of submodules, which are not listed, the support status applies to all
 ###### PrivateAction
 
 
-
 | PrivateAction                          | Init  support                            | Story support                              | Notes                              |
 | ---------------------------------------- | ---------------------------------------- | ---------------------------------------- | ---------------------------------------- |
 | `ActivateControllerAction`                          | ❌             | ✅            | Can be used to activate/deactive the CARLA autopilot.                                             |
 | `ControllerAction`                                  | ✅            | ✅            | AssignControllerAction is supported, but a Python module has to be provided for the controller implementation, and in OverrideControllerValueAction all values need to be `False`. |
 | `LateralAction`<br> `LaneChangeAction`             | &#10060;             | &#9989;            | Currently all lane changes have a linear dynamicShape, the dynamicDimension is defined as the distance and are relative to the actor itself (RelativeTargetLane).                  |
-| `LateralAction`<br>`LaneOffsetAction`             | &#10060;             | &#10060;             |                  |
+| `LateralAction`<br>`LaneOffsetAction`             | &#9989;             | &#10060;             |  Currently all type of dynamicShapes are ignored and depend on the controller. This action might not work as intended if the offset is high enough to make the vehicle exit its lane  |
 | `LateralAction`<br> `LateralDistanceAction`        | &#10060;             | &#10060;             |                  |
 | `LongitudinalAction`<br> `LongitudinalDistanceAction`                                            | &#10060;             | &#10060;             |                  |
 | `LongitudinalAction`<br> `SpeedAction`             | &#9989;            | &#9989;            |                  |

srunner/scenariomanager/actorcontrols/actor_control.py

@@ -60,6 +60,7 @@ class ActorControl(object):
 
     _last_longitudinal_command = None
     _last_waypoint_command = None
+    _last_lane_offset_command = None
 
     def __init__(self, actor, control_py_module, args, scenario_file_path):
 
@@ -117,6 +118,19 @@ def update_waypoints(self, waypoints, start_time=None):
         if start_time:
             self._last_waypoint_command = start_time
 
+    def update_offset(self, offset, start_time=None):
+        """
+        Update the actor's offset
+
+        Args:
+            offset (float): Value of the new offset.
+            start_time (float): Start time of the new "maneuver" [s].
+        """
+        self.control_instance.update_offset(offset)
+        if start_time:
+            self._last_waypoint_command = start_time
+            self._last_lane_offset_command = start_time
+
     def check_reached_waypoint_goal(self):
         """
         Check if the actor reached the end of the waypoint list
@@ -144,6 +158,15 @@ def get_last_waypoint_command(self):
         """
         return self._last_waypoint_command
 
+    def get_last_lane_offset_command(self):
+        """
+        Get timestamp of the last issued lane offset control command
+
+        returns:
+            Timestamp of last lane offset control command
+        """
+        return self._last_lane_offset_command
+
     def set_init_speed(self):
         """
         Update the actor's initial speed setting

srunner/scenariomanager/actorcontrols/basic_control.py

@@ -42,6 +42,8 @@ class BasicControl(object):
     _actor = None
     _waypoints = []
     _waypoints_updated = False
+    _offset = 0
+    _offset_updated = False
     _target_speed = 0
     _reached_goal = False
     _init_speed = False
@@ -72,6 +74,16 @@ def update_waypoints(self, waypoints, start_time=None):
         self._waypoints = waypoints
         self._waypoints_updated = True
 
+    def update_offset(self, offset, start_time=None):
+        """
+        Update the actor's waypoints
+
+        Args:
+            waypoints (List of carla.Transform): List of new waypoints.
+        """
+        self._offset = offset
+        self._offset_updated = True
+
     def set_init_speed(self):
         """
         Set _init_speed to True

srunner/scenariomanager/actorcontrols/npc_vehicle_control.py

@@ -47,14 +47,19 @@ 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(
                 transform.location, project_to_road=True, lane_type=carla.LaneType.Any)
             plan.append((waypoint, RoadOption.LANEFOLLOW))
         self._local_planner.set_global_plan(plan)
 
+    def _update_offset(self):
+        """
+        Update the plan (waypoint list) of the LocalPlanner
+        """
+        self._local_planner._vehicle_controller._lat_controller._offset = self._offset   # pylint: disable=protected-access
+
     def reset(self):
         """
         Reset the controller
@@ -88,6 +93,10 @@ def run_step(self):
             self._waypoints_updated = False
             self._update_plan()
 
+        if self._offset_updated:
+            self._offset_updated = False
+            self._update_offset()
+
         target_speed = self._target_speed
         # If target speed is negavite, raise an exception
         if target_speed < 0:

srunner/scenariomanager/actorcontrols/simple_vehicle_control.py

@@ -191,7 +191,7 @@ def run_step(self):
                 else:
                     break
 
-            direction_norm = self._set_new_velocity(self._generated_waypoint_list[0].location)
+            direction_norm = self._set_new_velocity(self._offset_waypoint(self._generated_waypoint_list[0]))
             if direction_norm < 2.0:
                 self._generated_waypoint_list = self._generated_waypoint_list[1:]
         else:
@@ -202,12 +202,32 @@ def run_step(self):
                 self._waypoints = self._waypoints[1:]
 
             self._reached_goal = False
-            direction_norm = self._set_new_velocity(self._waypoints[0].location)
+            direction_norm = self._set_new_velocity(self._offset_waypoint(self._waypoints[0]))
             if direction_norm < 4.0:
                 self._waypoints = self._waypoints[1:]
                 if not self._waypoints:
                     self._reached_goal = True
 
+    def _offset_waypoint(self, transform):
+        """
+        Given a transform (which should be the position of a waypoint), displaces it to the side,
+        according to a given offset
+
+        Args:
+            transform (carla.Transform): Transform to be moved
+
+        returns:
+            offset_location (carla.Transform): Moved transform
+        """
+        if self._offset == 0:
+            offset_location = transform.location
+        else:
+            right_vector = transform.get_right_vector()
+            offset_location = transform.location + carla.Location(x=self._offset*right_vector.x,
+                                                                y=self._offset*right_vector.y)
+
+        return offset_location
+
     def _set_new_velocity(self, next_location):
         """
         Calculate and set the new actor veloctiy given the current actor

srunner/scenariomanager/scenarioatomics/atomic_behaviors.py

@@ -698,6 +698,7 @@ class ChangeActorWaypoints(AtomicBehavior):
     second waypoint related atomic for the same actor is triggered. These are:
     - ChangeActorWaypoints
     - ChangeActorLateralMotion
+    - ChangeActorLaneOffset
 
     Args:
         actor (carla.Actor): Controlled actor.
@@ -843,6 +844,7 @@ class ChangeActorLateralMotion(AtomicBehavior):
     second waypoint related atomic for the same actor is triggered. These are:
     - ChangeActorWaypoints
     - ChangeActorLateralMotion
+    - ChangeActorLaneOffset
 
     If an impossible lane change is asked for (due to the lack of lateral lanes,
     next waypoints, continuous line, etc) the atomic will return a plan with the
@@ -978,6 +980,178 @@ def update(self):
         return new_status
 
 
+class ChangeActorLaneOffset(AtomicBehavior):
+
+    """
+    OpenSCENARIO atomic.
+    Atomic to change the offset of the controller.
+
+    The behavior is in RUNNING state until the offset os reached (if 'continuous' is set to False)
+    or forever (if 'continuous' is True). This behavior will automatically stop if a second waypoint
+    related atomic for the same actor is triggered. These are:
+    - ChangeActorWaypoints
+    - ChangeActorLateralMotion
+    - ChangeActorLaneOffset
+
+    Args:
+        actor (carla.Actor): Controlled actor.
+        offset (float): Float determined the distance to the center of the lane. Positive distance imply a
+            displacement to the right, while negative displacements are to the left.
+        relative_actor (carla.Actor): The actor from which the offset is taken from. Defaults to None
+        continuous (bool): If True, the behaviour never ends. If False, the behaviour ends when the lane
+            offset is reached. Defaults to True.
+
+    Attributes:
+        _offset (float): lane offset.
+        _relative_actor (carla.Actor): relative actor.
+        _continuous (bool): stored the value of the 'continuous' argument.
+        _start_time (float): Start time of the atomic [s].
+            Defaults to None.
+        _overwritten (bool): flag to check whether or not this behavior was overwritten by another. Helps
+            to avoid the missinteraction between two ChangeActorLaneOffsets.
+        _current_target_offset (float): stores the value of the offset when dealing with relative distances
+        _map (carla.Map): instance of the CARLA map.
+    """
+
+    OFFSET_THRESHOLD = 0.1
+
+    def __init__(self, actor, offset, relative_actor=None, continuous=True, name="ChangeActorWaypoints"):
+        """
+        Setup parameters
+        """
+        super(ChangeActorLaneOffset, self).__init__(name, actor)
+
+        self._offset = offset
+        self._relative_actor = relative_actor
+        self._continuous = continuous
+        self._start_time = None
+        self._current_target_offset = 0
+
+        self._overwritten = False
+        self._map = CarlaDataProvider.get_map()
+
+    def initialise(self):
+        """
+        Set _start_time and get (actor, controller) pair from Blackboard.
+
+        Set offset for actor controller.
+
+        May throw if actor is not available as key for the ActorsWithController
+        dictionary from Blackboard.
+        """
+        actor_dict = {}
+
+        try:
+            check_actors = operator.attrgetter("ActorsWithController")
+            actor_dict = check_actors(py_trees.blackboard.Blackboard())
+        except AttributeError:
+            pass
+
+        if not actor_dict or not self._actor.id in actor_dict:
+            raise RuntimeError("Actor not found in ActorsWithController BlackBoard")
+
+        self._start_time = GameTime.get_time()
+
+        actor_dict[self._actor.id].update_offset(self._offset, start_time=self._start_time)
+
+        super(ChangeActorLaneOffset, self).initialise()
+
+    def update(self):
+        """
+        Check the actor's state along the waypoint route.
+
+        returns:
+            py_trees.common.Status.SUCCESS, if the lane offset was reached (and 'continuous' was False), or
+                                            if another waypoint atomic for the same actor was triggered
+            py_trees.common.Status.FAILURE, if the actor is not found in ActorsWithController Blackboard dictionary.
+            py_trees.common.Status.RUNNING, else.
+        """
+        try:
+            check_actors = operator.attrgetter("ActorsWithController")
+            actor_dict = check_actors(py_trees.blackboard.Blackboard())
+        except AttributeError:
+            pass
+
+        if not actor_dict or not self._actor.id in actor_dict:
+            return py_trees.common.Status.FAILURE
+
+        if actor_dict[self._actor.id].get_last_lane_offset_command() != self._start_time:
+            # Differentiate between lane offset and other lateral commands
+            self._overwritten = True
+            return py_trees.common.Status.SUCCESS
+
+        if actor_dict[self._actor.id].get_last_waypoint_command() != self._start_time:
+            return py_trees.common.Status.SUCCESS
+
+        if self._relative_actor:
+            # Calculate new offset
+            relative_actor_loc = CarlaDataProvider.get_location(self._relative_actor)
+            relative_center_wp = self._map.get_waypoint(relative_actor_loc)
+
+            # Value
+            relative_center_loc = relative_center_wp.transform.location
+            relative_actor_offset = relative_actor_loc.distance(relative_center_loc)
+
+            # Sign
+            f_vec = relative_center_wp.transform.get_forward_vector()
+            d_vec = relative_actor_loc - relative_center_loc
+            cross = f_vec.x * d_vec.y - f_vec.y * d_vec.x
+
+            if cross < 0:
+                relative_actor_offset *= -1.0
+
+            self._current_target_offset = relative_actor_offset + self._offset
+            # Set the new offset
+            actor_dict[self._actor.id].update_offset(self._current_target_offset)
+
+        if not self._continuous:
+            # Calculate new offset
+            actor_loc = CarlaDataProvider.get_location(self._actor)
+            center_wp = self._map.get_waypoint(actor_loc)
+
+            # Value
+            center_loc = center_wp.transform.location
+            actor_offset = actor_loc.distance(center_loc)
+
+            # Sign
+            f_vec = center_wp.transform.get_forward_vector()
+            d_vec = actor_loc - center_loc
+            cross = f_vec.x * d_vec.y - f_vec.y * d_vec.x
+
+            if cross < 0:
+                actor_offset *= -1.0
+
+            # Check if the offset has been reached
+            if abs(actor_offset - self._current_target_offset) < self.OFFSET_THRESHOLD:
+                return py_trees.common.Status.SUCCESS
+
+        # TODO: As their is no way to check the distance to a specific lane, both checks will fail if the
+        # actors are outside its 'route lane' or at an intersection
+
+        new_status = py_trees.common.Status.RUNNING
+
+        return new_status
+
+    def terminate(self, new_status):
+        """
+        On termination of this behavior, the offset is set back to zero
+        """
+
+        if not self._overwritten:
+            try:
+                check_actors = operator.attrgetter("ActorsWithController")
+                actor_dict = check_actors(py_trees.blackboard.Blackboard())
+            except AttributeError:
+                pass
+
+            if actor_dict and self._actor.id in actor_dict:
+                actor_dict[self._actor.id].update_offset(0)
+
+            self._overwritten = True
+
+        super(ChangeActorLaneOffset, self).terminate(new_status)
+
+
 class ActorTransformSetterToOSCPosition(AtomicBehavior):
 
     """