diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index e76d84a04..d64be7318 100644 --- a/Docs/CHANGELOG.md +++ b/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 diff --git a/Docs/openscenario_support.md b/Docs/openscenario_support.md index 6094cfdfc..5c5e0608b 100755 --- a/Docs/openscenario_support.md +++ b/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`
`LaneChangeAction` | ❌ | ✅ | Currently all lane changes have a linear dynamicShape, the dynamicDimension is defined as the distance and are relative to the actor itself (RelativeTargetLane). | -| `LateralAction`
`LaneOffsetAction` | ❌ | ❌ | | +| `LateralAction`
`LaneOffsetAction` | ✅ | ❌ | 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`
`LateralDistanceAction` | ❌ | ❌ | | | `LongitudinalAction`
`LongitudinalDistanceAction` | ❌ | ❌ | | | `LongitudinalAction`
`SpeedAction` | ✅ | ✅ | | diff --git a/srunner/scenariomanager/actorcontrols/actor_control.py b/srunner/scenariomanager/actorcontrols/actor_control.py index 480e41347..333618d6c 100644 --- a/srunner/scenariomanager/actorcontrols/actor_control.py +++ b/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 diff --git a/srunner/scenariomanager/actorcontrols/basic_control.py b/srunner/scenariomanager/actorcontrols/basic_control.py index b2ecb9456..fa59fa51c 100644 --- a/srunner/scenariomanager/actorcontrols/basic_control.py +++ b/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 diff --git a/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py b/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py index 9969df57c..73481aad0 100644 --- a/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py +++ b/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py @@ -47,7 +47,6 @@ 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( @@ -55,6 +54,12 @@ def _update_plan(self): 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: diff --git a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py index 9358727f8..a152607b9 100644 --- a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py +++ b/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 diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 75f5b68b7..a46b35828 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/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): """ diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index 5da0fd222..8b6ee8a97 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -32,6 +32,7 @@ ChangeActorControl, ChangeActorWaypoints, ChangeActorLateralMotion, + ChangeActorLaneOffset, SyncArrivalOSC, Idle) # pylint: disable=unused-import @@ -1041,6 +1042,37 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): distance_other_lane=1000, lane_changes=lane_changes, name=maneuver_name) + elif private_action.find('LaneOffsetAction') is not None: + lat_maneuver = private_action.find('LaneOffsetAction') + continuous = strtobool(lat_maneuver.attrib.get('continuous', True)) + # Parsing of the different Dynamic shapes is missing + + lane_target_offset = lat_maneuver.find('LaneOffsetTarget') + if lane_target_offset.find('AbsoluteTargetLaneOffset') is not None: + absolute_offset = float( + lane_target_offset.find('AbsoluteTargetLaneOffset').attrib.get('value', 0)) + atomic = ChangeActorLaneOffset( + actor, absolute_offset, continuous=continuous, name=maneuver_name) + + elif lane_target_offset.find('RelativeTargetLaneOffset') is not None: + relative_target_offset = lane_target_offset.find('RelativeTargetLaneOffset') + relative_offset = float(relative_target_offset.attrib.get('value', 0)) + + relative_actor = None + for _actor in actor_list: + if relative_target_offset.attrib.get('entityRef', None) == _actor.attributes['role_name']: + relative_actor = _actor + break + + if relative_actor is None: + raise AttributeError("Cannot find actor '{}' for condition".format( + relative_target_offset.attrib.get('entityRef', None))) + + atomic = ChangeActorLaneOffset(actor, relative_offset, relative_actor, + continuous=continuous, name=maneuver_name) + + else: + raise AttributeError("Unknown target offset") else: raise AttributeError("Unknown lateral action") elif private_action.find('VisibilityAction') is not None: @@ -1048,6 +1080,7 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): elif private_action.find('SynchronizeAction') is not None: sync_action = private_action.find('SynchronizeAction') + master_actor = None for actor_ins in actor_list: if sync_action.attrib.get('masterEntityRef', None) == actor_ins.attributes['role_name']: master_actor = actor_ins