Create follow trajectory behavior for osc2 (#1149)

* Create follow trajectory modifier and adapt ChangeActorWaypoints for osc2

* Added follow trajectory prototype beahvior

* Added control types

* Minor improvmenet to velocity control

* Moved csv files to examples folder

* code cleanup

* Removed update_speed

* Merge branch 'master' into pablo/osc2_trajectory

Co-Authored-By: glopezdiest <[email protected]>
Co-Authored-By: glopezdiest <[email protected]>

Author: 3 people

srunner/examples/basic.osc

@@ -28,6 +28,14 @@ unit mmph                of velocity is SI(m: 1, s: -1, factor: 0.000000278)
 unit millimeter_per_hour of velocity is SI(m: 1, s: -1, factor: 0.000000278)
 # end::library-physical-velocity[]
 
+# tag::library-physical-angle[]
+type angle is SI(rad: 1)
+unit degree of angle is SI(rad: 1, factor: 57.295779513)
+unit deg    of angle is SI(rad: 1, factor: 57.295779513)
+unit radian of angle is SI(rad: 1, factor: 1)
+unit rad    of angle is SI(rad: 1, factor: 1)
+# end::library-physical-angle[]
+
 actor dut
 actor Path
 modifier Path.set_map

srunner/examples/follow_trajectory.osc

@@ -0,0 +1,21 @@
+import basic.osc
+
+scenario top:
+    path: Path                          # A path in the map
+    path.set_map("Town10HD_Opt")        # Map to employ
+    path.path_min_driving_lanes(2)      # Path should have at least two lanes
+
+    ego_vehicle: Model3                	# Ego car
+    npc1: Rubicon                       # Another car
+    npc2: Rubicon                       # Anotherother car
+
+    do parallel(duration: 18s):         # Execute the following lines in parallel
+
+        ego_vehicle.drive(path) with:
+            follow_trajectory(input_file: "srunner/examples/resources/ego_logs.csv", control:"physics")
+
+        npc1.drive(path) with:
+            follow_trajectory(input_file: "srunner/examples/resources/npc1_logs.csv", control:"physics")
+
+        npc2.drive(path) with:
+            follow_trajectory(input_file: "srunner/examples/resources/npc2_logs.csv", control:"physics")

srunner/examples/resources/ego_logs.csv

@@ -156,3 +156,29 @@ def __init__(self, actor_name: str, name: str) -> None:
 
     def get_accelerate(self):
         return self.args["acceleration"]
+
+
+class FollowTrajectoryModifier(Modifier):
+    def __init__(self, actor_name: str, name: str) -> None:
+        super().__init__(actor_name, name)
+
+    def get_points(self):
+        if self.args.get("points"):
+            return self.args.get("points")
+        else:
+            return None
+
+    def get_times(self):
+        if self.args.get("times"):
+            return self.args.get("times")
+        else:
+            return None
+
+    def get_control(self):
+        if self.args.get("control"):
+            return self.args.get("control")
+        else:
+            return None
+
+    def get_trigger_point(self) -> str:
+        return self.args.get("at", "all")

srunner/examples/resources/npc1_logs.csv

@@ -105,7 +105,6 @@ def path_explicit(cls, start_point, end_point, tolerance):
         start_point = start_point.split(",")
         end_point = end_point.split(",")
         cls.is_explicit = PathExplicit(start_point, end_point, tolerance)
-        print(cls.is_explicit)
 
     @classmethod
     def path_over_speed_limit_change(cls, first_speed, sec_speed):

srunner/examples/resources/npc2_logs.csv

@@ -106,15 +106,15 @@ def update_target_speed(self, target_speed, start_time=None):
         if start_time:
             self._last_longitudinal_command = start_time
 
-    def update_waypoints(self, waypoints, start_time=None):
+    def update_waypoints(self, waypoints, times=None, start_time=None):
         """
         Update the actor's waypoints
 
         Args:
             waypoints (List of carla.Transform): List of new waypoints.
             start_time (float): Start time of the new "maneuver" [s].
         """
-        self.control_instance.update_waypoints(waypoints)
+        self.control_instance.update_waypoints(waypoints, times)
         if start_time:
             self._last_waypoint_command = start_time
 

srunner/osc2_stdlib/modifier.py

@@ -33,6 +33,8 @@ class BasicControl(object):
         _waypoints (list of carla.Transform): List of target waypoints the actor
             should travel along. A waypoint here is of type carla.Transform!
             Defaults to [].
+        _times (list of float): List of target times for each waypoint. Optional.
+            Defaults to False.
         _waypoints_updated (boolean):
             Defaults to False.
         _reached_goal (boolean):
@@ -42,11 +44,13 @@ class BasicControl(object):
     _actor = None
     _waypoints = []
     _waypoints_updated = False
+    _times = []
     _offset = 0
     _offset_updated = False
     _target_speed = 0
     _reached_goal = False
     _init_speed = False
+    _start_time = None
 
     def __init__(self, actor):
         """
@@ -64,15 +68,18 @@ def update_target_speed(self, speed):
         self._target_speed = speed
         self._init_speed = False
 
-    def update_waypoints(self, waypoints, start_time=None):
+    def update_waypoints(self, waypoints, times=None, start_time=None):
         """
         Update the actor's waypoints
 
         Args:
             waypoints (List of carla.Transform): List of new waypoints.
+            times (List of float): List of new times. Optional.
         """
         self._waypoints = waypoints
+        self._times = times
         self._waypoints_updated = True
+        self._start_time = start_time
 
     def update_offset(self, offset, start_time=None):
         """
@@ -83,6 +90,7 @@ def update_offset(self, offset, start_time=None):
         """
         self._offset = offset
         self._offset_updated = True
+        self._start_time = start_time
 
     def set_init_speed(self):
         """

srunner/osc2_stdlib/path.py

@@ -17,6 +17,7 @@
 
 from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
 from srunner.scenariomanager.actorcontrols.basic_control import BasicControl
+from srunner.scenariomanager.timer import GameTime
 
 
 class NpcVehicleControl(BasicControl):
@@ -99,6 +100,18 @@ def run_step(self):
             self._offset_updated = False
             self._update_offset()
 
+        # Times have been specified, modify the speed accordingly
+        if self._times:
+            if self._start_time is None:
+                self._start_time = GameTime.get_time()
+            plan_len = len(self._local_planner.get_plan())
+            current_index = len(self._waypoints) - plan_len
+            target_time = self._times[current_index]
+            delta_time = target_time - (GameTime.get_time() - self._start_time)
+            target_location = self._local_planner.get_plan()[0][0].transform.location
+            target_distance = self._actor.get_location().distance(target_location)
+            self._target_speed = target_distance / max(delta_time, 0.001)
+
         target_speed = self._target_speed
         # If target speed is negavite, raise an exception
         if target_speed < 0:
@@ -126,16 +139,3 @@ def run_step(self):
                 vx = math.cos(yaw) * target_speed
                 vy = math.sin(yaw) * target_speed
                 self._actor.set_target_velocity(carla.Vector3D(vx, vy, 0))
-
-        # Change Brake light state
-        if (current_speed > target_speed or target_speed < 0.2) and not self._brake_lights_active:
-            light_state = self._actor.get_light_state()
-            light_state |= carla.VehicleLightState.Brake
-            self._actor.set_light_state(carla.VehicleLightState(light_state))
-            self._brake_lights_active = True
-
-        if self._brake_lights_active and current_speed < target_speed:
-            self._brake_lights_active = False
-            light_state = self._actor.get_light_state()
-            light_state &= ~carla.VehicleLightState.Brake
-            self._actor.set_light_state(carla.VehicleLightState(light_state))