[OSC2.0] Trajectory fixes for slow speeds (#1150)

* Fixes for slow speeds

* misspelling

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

Author: glopezdiest

srunner/examples/follow_trajectory.osc

@@ -12,10 +12,10 @@ scenario top:
     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")
+            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")
+            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")
+            follow_trajectory(input_file: "srunner/examples/resources/npc2_logs.csv", control: "physics")

srunner/osc2_stdlib/path.py

@@ -139,13 +139,15 @@ def check(cls, pos) -> bool:
             len_ok = carla_data.CarlaDataProvider.check_road_length(wp, cls._length)
             if not len_ok:
                 return False
+
         # Check if the test road is signposted
         if cls.sign_type:
             is_sign = PathTrafficSign.path_has_traffic_sign(
                 wp, cls.sign_type, cls._length
             )
             if not is_sign:
                 return False
+
         # Restricted test roads cannot be signposted
         if cls.sign_types:
             no_sign = PathTrafficSign.path_has_no_traffic_signs(

srunner/scenariomanager/actorcontrols/vehicle_teleport_control.py

@@ -30,6 +30,8 @@ class VehicleTeleportControl(BasicControl):
 
 
     def __init__(self, actor, args=None):
+        self._prev_yaw = None
+        self._distance_threshold = 0.01
         super(VehicleTeleportControl, self).__init__(actor)
 
     def reset(self):
@@ -49,9 +51,17 @@ def run_step(self):
             return
         if not self._start_time:
             self._start_time = GameTime.get_time()
+        if not self._prev_yaw:
+            self._prev_yaw = self._actor.get_transform().rotation.yaw
 
         transform = self._waypoints.pop(0)
         vec = self._waypoints[0].location - transform.location
-        yaw = math.degrees(math.atan2(vec.y, vec.x))
+
+        if abs(vec.x) < self._distance_threshold and abs(vec.y) < self._distance_threshold:
+            yaw = self._prev_yaw  # Vehicle is stopped, avoid changing the yaw
+        else:
+            yaw = math.degrees(math.atan2(vec.y, vec.x))
 
         self._actor.set_transform(carla.Transform(transform.location, carla.Rotation(yaw=yaw)))
+
+        self._prev_yaw = yaw

srunner/scenariomanager/actorcontrols/vehicle_velocity_control.py

@@ -48,6 +48,7 @@ def __init__(self, actor, args=None):
         # This is the maximum amount of time used to aim the vehicle towards the next point.
         # Used to avoid weird lateral slides if the time between point is high.
         self._max_yaw_time = 1
+        self._distance_threshold = 1
 
     def reset(self):
         """
@@ -62,6 +63,7 @@ def run_step(self):
         Teleports the vehicle to a given waypoint on each tick. Use the location from the waypoints
         but calculate the transform so that it is always pointing towards the next waypoint.
         """
+
         if not self._waypoints:
             return
         if not self._start_time:
@@ -88,17 +90,22 @@ def run_step(self):
         linear_speed = (target_transform.location.distance(current_transform.location)) / delta_time
         linear_velocity = linear_speed * target_vec
 
-        delta_yaw = math.degrees(math.atan2(target_vec.y, target_vec.x)) - current_transform.rotation.yaw
+        self._actor.set_target_velocity(linear_velocity)
 
-        if math.fabs(delta_yaw) > 360:
-            delta_yaw = delta_yaw % 360
+
+        for i in range(len(self._waypoints)):
+            target_transform = self._waypoints[i]
+            target_time = self._times[i]
+            if current_transform.location.distance(target_transform.location) > self._distance_threshold:
+                break
+
+        delta_time = target_time + self._start_time - current_time
+        delta_yaw = math.degrees(math.atan2(target_vec.y, target_vec.x)) - current_transform.rotation.yaw
+        delta_yaw = delta_yaw % 360
         if delta_yaw > 180:
             delta_yaw = delta_yaw - 360
-        elif delta_yaw < -180:
-            delta_yaw = delta_yaw + 360
         angular_speed = delta_yaw / min(delta_time, self._max_yaw_time)
         angular_velocity = carla.Vector3D(0, 0, angular_speed)
 
-        self._actor.set_target_velocity(linear_velocity)
         self._actor.set_target_angular_velocity(angular_velocity)
         self._actor.apply_control(carla.VehicleControl(throttle=0.3))  # Make the wheels turn