Petro

competition_code/competition_runner.py

@@ -120,33 +120,34 @@ async def evaluate_solution(
 
     # Spawn vehicle and sensors to receive data
     waypoints = world.maneuverable_waypoints
+    yaw_vector = waypoints[1].location - waypoints[0].location
+    initial_yaw = np.arctan2(yaw_vector[1], yaw_vector[0])
+    initial_rpy = np.array([0, 0, initial_yaw])
+
     vehicle = world.spawn_vehicle(
-        "vehicle.tesla.model3",
-        waypoints[0].location + np.array([0,0,1]),
-        waypoints[0].roll_pitch_yaw,
-        True,
+        "vehicle.audi.tt",
+        waypoints[0].location + np.array([0, 0, 2.5]),
+        initial_rpy,  # ← aligned with path
+        True
     )
+
+
     assert vehicle is not None
+
+    print(f" Vehicle spawned: {vehicle}")
+
     camera = vehicle.attach_camera_sensor(
         roar_py_interface.RoarPyCameraSensorDataRGB,
-        np.array([-2.0 * vehicle.bounding_box.extent[0], 0.0, 3.0 * vehicle.bounding_box.extent[2]]), # relative position
-        np.array([0, 10/180.0*np.pi, 0]), # relative rotation
+        np.array([-2.0 * vehicle.bounding_box.extent[0], 0.0, 3.0 * vehicle.bounding_box.extent[2]]),
+        np.array([0, 10/180.0*np.pi, 0]),
         image_width=1024,
         image_height=768
     )
     location_sensor = vehicle.attach_location_in_world_sensor()
     velocity_sensor = vehicle.attach_velocimeter_sensor()
     rpy_sensor = vehicle.attach_roll_pitch_yaw_sensor()
-    occupancy_map_sensor = vehicle.attach_occupancy_map_sensor(
-        50,
-        50,
-        2.0,
-        2.0
-    )
-    collision_sensor = vehicle.attach_collision_sensor(
-        np.zeros(3),
-        np.zeros(3)
-    )
+    occupancy_map_sensor = vehicle.attach_occupancy_map_sensor(50, 50, 2.0, 2.0)
+    collision_sensor = vehicle.attach_collision_sensor(np.zeros(3), np.zeros(3))
 
     assert camera is not None
     assert location_sensor is not None
@@ -156,7 +157,6 @@ async def evaluate_solution(
     assert collision_sensor is not None
 
 
-    # Start to run solution 
     solution : RoarCompetitionSolution = solution_constructor(
         waypoints,
         RoarCompetitionAgentWrapper(vehicle),
@@ -167,40 +167,31 @@ async def evaluate_solution(
         occupancy_map_sensor,
         collision_sensor
     )
-    rule = RoarCompetitionRule(waypoints * 3,vehicle,world) # 3 laps
+
+    rule = RoarCompetitionRule(waypoints * 3, vehicle, world) # 3 laps
 
     for _ in range(20):
         await world.step()
 
     rule.initialize_race()
-    # vehicle.close()
-    # exit()
 
-    # Timer starts here 
     start_time = world.last_tick_elapsed_seconds
     current_time = start_time
     await vehicle.receive_observation()
     await solution.initialize()
 
-
     while True:
-        # terminate if time out
         current_time = world.last_tick_elapsed_seconds
         if current_time - start_time > max_seconds:
             vehicle.close()
             return None
 
-        # receive sensors' data
         await vehicle.receive_observation()
-
         await rule.tick()
 
-        # terminate if there is major collision
         collision_impulse_norm = np.linalg.norm(collision_sensor.get_last_observation().impulse_normal)
         if collision_impulse_norm > 100.0:
-            # vehicle.close()
-            print(f"major collision of tensity {collision_impulse_norm}")
-            # return None
+            print(f"Major collision! Intensity: {collision_impulse_norm:.2f}")
             await rule.respawn()
 
         if rule.lap_finished():
@@ -214,7 +205,7 @@ async def evaluate_solution(
         await solution.step()
         await world.step()
 
-    print("end of the loop")
+    print("🏁 End of the loop")
     end_time = world.last_tick_elapsed_seconds
     vehicle.close()
     if enable_visualization:
@@ -224,6 +215,7 @@ async def evaluate_solution(
         "elapsed_time" : end_time - start_time,
     }
 
+
 async def main():
     carla_client = carla.Client('127.0.0.1', 2000)
     carla_client.set_timeout(5.0)

competition_code/submission.py

@@ -2,35 +2,37 @@
 Competition instructions:
 Please do not change anything else but fill out the to-do sections.
 """
+print("submission.py loaded")
 
-from typing import List, Tuple, Dict, Optional
+from typing import List
 import roar_py_interface
 import numpy as np
 
-def normalize_rad(rad : float):
+
+def normalize_rad(rad: float):
     return (rad + np.pi) % (2 * np.pi) - np.pi
 
-def filter_waypoints(location : np.ndarray, current_idx: int, waypoints : List[roar_py_interface.RoarPyWaypoint]) -> int:
-    def dist_to_waypoint(waypoint : roar_py_interface.RoarPyWaypoint):
-        return np.linalg.norm(
-            location[:2] - waypoint.location[:2]
-        )
+
+def filter_waypoints(location: np.ndarray, current_idx: int, waypoints: List[roar_py_interface.RoarPyWaypoint]) -> int:
+    def dist_to_waypoint(waypoint: roar_py_interface.RoarPyWaypoint):
+        return np.linalg.norm(location[:2] - waypoint.location[:2])
     for i in range(current_idx, len(waypoints) + current_idx):
-        if dist_to_waypoint(waypoints[i%len(waypoints)]) < 3:
+        if dist_to_waypoint(waypoints[i % len(waypoints)]) < 3:
             return i % len(waypoints)
     return current_idx
 
+
 class RoarCompetitionSolution:
     def __init__(
         self,
         maneuverable_waypoints: List[roar_py_interface.RoarPyWaypoint],
-        vehicle : roar_py_interface.RoarPyActor,
-        camera_sensor : roar_py_interface.RoarPyCameraSensor = None,
-        location_sensor : roar_py_interface.RoarPyLocationInWorldSensor = None,
-        velocity_sensor : roar_py_interface.RoarPyVelocimeterSensor = None,
-        rpy_sensor : roar_py_interface.RoarPyRollPitchYawSensor = None,
-        occupancy_map_sensor : roar_py_interface.RoarPyOccupancyMapSensor = None,
-        collision_sensor : roar_py_interface.RoarPyCollisionSensor = None,
+        vehicle: roar_py_interface.RoarPyActor,
+        camera_sensor: roar_py_interface.RoarPyCameraSensor = None,
+        location_sensor: roar_py_interface.RoarPyLocationInWorldSensor = None,
+        velocity_sensor: roar_py_interface.RoarPyVelocimeterSensor = None,
+        rpy_sensor: roar_py_interface.RoarPyRollPitchYawSensor = None,
+        occupancy_map_sensor: roar_py_interface.RoarPyOccupancyMapSensor = None,
+        collision_sensor: roar_py_interface.RoarPyCollisionSensor = None,
     ) -> None:
         self.maneuverable_waypoints = maneuverable_waypoints
         self.vehicle = vehicle
@@ -40,69 +42,117 @@ def __init__(
         self.rpy_sensor = rpy_sensor
         self.occupancy_map_sensor = occupancy_map_sensor
         self.collision_sensor = collision_sensor
-
+
+        self._prev_steer = 0.0
+        self._steer_alpha = 0.5   # stronger smoothing
+        self._base_speed = 20.0   # normal cruising speed
+
     async def initialize(self) -> None:
-        # TODO: You can do some initial computation here if you want to.
-        # For example, you can compute the path to the first waypoint.
+        self._prev_steer = 0.0
+        self._last_yaw = None
+        self._last_dt = 0.05
 
-        # Receive location, rotation and velocity data 
         vehicle_location = self.location_sensor.get_last_gym_observation()
         vehicle_rotation = self.rpy_sensor.get_last_gym_observation()
-        vehicle_velocity = self.velocity_sensor.get_last_gym_observation()
+        vehicle_heading = np.array([np.cos(vehicle_rotation[2]), np.sin(vehicle_rotation[2])])
 
-        self.current_waypoint_idx = 10
-        self.current_waypoint_idx = filter_waypoints(
-            vehicle_location,
-            self.current_waypoint_idx,
-            self.maneuverable_waypoints
-        )
+        # Pick a waypoint ahead of the car
+        min_cost = np.inf
+        best_idx = 0
+        for i, wp in enumerate(self.maneuverable_waypoints):
+            vec_to_wp = wp.location[:2] - vehicle_location[:2]
+            if np.dot(vec_to_wp, vehicle_heading) > 0:  # must be in front
+                cost = np.linalg.norm(vec_to_wp)
+                if cost < min_cost:
+                    min_cost = cost
+                    best_idx = i
 
+        # 🔹 Skip ahead to avoid clipping first corner
+        self.current_waypoint_idx = (best_idx + 10) % len(self.maneuverable_waypoints)
+        self._startup_counter = 50  # ~50 steps (~2–3 seconds) of cautious driving
 
-    async def step(
-        self
-    ) -> None:
-        """
-        This function is called every world step.
-        Note: You should not call receive_observation() on any sensor here, instead use get_last_observation() to get the last received observation.
-        You can do whatever you want here, including apply_action() to the vehicle.
-        """
-        # TODO: Implement your solution here.
-
-        # Receive location, rotation and velocity data 
+    async def step(self) -> None:
         vehicle_location = self.location_sensor.get_last_gym_observation()
         vehicle_rotation = self.rpy_sensor.get_last_gym_observation()
         vehicle_velocity = self.velocity_sensor.get_last_gym_observation()
-        vehicle_velocity_norm = np.linalg.norm(vehicle_velocity)
-
-        # Find the waypoint closest to the vehicle
+        v = float(np.linalg.norm(vehicle_velocity))
+        yaw = float(vehicle_rotation[2])
+        pos_xy = vehicle_location[:2]
+
+        if self._last_yaw is None:
+            self._last_yaw = yaw
+
+        # Update waypoint index
         self.current_waypoint_idx = filter_waypoints(
             vehicle_location,
             self.current_waypoint_idx,
             self.maneuverable_waypoints
         )
-         # We use the 3rd waypoint ahead of the current waypoint as the target waypoint
-        waypoint_to_follow = self.maneuverable_waypoints[(self.current_waypoint_idx + 3) % len(self.maneuverable_waypoints)]
 
-        # Calculate delta vector towards the target waypoint
-        vector_to_waypoint = (waypoint_to_follow.location - vehicle_location)[:2]
-        heading_to_waypoint = np.arctan2(vector_to_waypoint[1],vector_to_waypoint[0])
+        # --- PARAMETERS FOR SAFE DRIVING ---
+        num_fit_points = 15
+        lookahead_distance = 12.0
+        max_speed = 20.0
+        min_speed = 5.0
+
+        # Gather waypoints ahead
+        wp_xy = np.array([
+            self.maneuverable_waypoints[(self.current_waypoint_idx + i) % len(self.maneuverable_waypoints)].location[:2]
+            for i in range(num_fit_points)
+        ])
+
+        rot = np.array([[np.cos(-yaw), -np.sin(-yaw)], [np.sin(-yaw), np.cos(-yaw)]])
+        local_wp = (wp_xy - pos_xy) @ rot.T
+
+        # Ensure most waypoints are in front
+        forward_points = [pt for pt in local_wp if pt[0] > 1.0]
+        if len(forward_points) < len(local_wp) // 2:
+            print("Too many waypoints behind car — braking to re-center")
+            await self.vehicle.apply_action({
+                "throttle": 0.0, "steer": 0.0, "brake": 1.0,
+                "hand_brake": 0.0, "reverse": 0, "target_gear": 0
+            })
+            return
+
+        # Fit curve
+        x = local_wp[:, 0]
+        y = local_wp[:, 1]
+        coeffs = np.polyfit(x, y, 2)
+
+        # Target farther point
+        x_target = lookahead_distance
+        y_target = np.polyval(coeffs, x_target)
+
+        # Steering
+        steer_angle = np.arctan2(2.0 * y_target, lookahead_distance)
+        raw_steer = float(np.clip(steer_angle / 0.7, -1.0, 1.0))
+        steer_control = (1 - self._steer_alpha) * self._prev_steer + self._steer_alpha * raw_steer
+        self._prev_steer = steer_control
+
+        # --- SPEED CONTROL ---
+        curvature = abs(coeffs[0])
+        target_speed = max(min_speed, max_speed * (1.0 - 12.0 * curvature))
+        target_speed = np.clip(target_speed, min_speed, max_speed)
 
-        # Calculate delta angle towards the target waypoint
-        delta_heading = normalize_rad(heading_to_waypoint - vehicle_rotation[2])
+        # During startup, keep speed capped
+        if self._startup_counter > 0:
+            self._startup_counter -= 1
+            target_speed = min(target_speed, 8.0)
 
-        # Proportional controller to steer the vehicle towards the target waypoint
-        steer_control = (
-            -8.0 / np.sqrt(vehicle_velocity_norm) * delta_heading / np.pi
-        ) if vehicle_velocity_norm > 1e-2 else -np.sign(delta_heading)
-        steer_control = np.clip(steer_control, -1.0, 1.0)
+        speed_error = target_speed - v
+        accel = 0.1 * speed_error
+        throttle = np.clip(accel, 0.0, 1.0)
+        brake = np.clip(-accel, 0.0, 1.0)
 
-        # Proportional controller to control the vehicle's speed towards 40 m/s
-        throttle_control = 0.05 * (20 - vehicle_velocity_norm)
+        # --- EMERGENCY COLLISION STOP ---
+        if self.collision_sensor and self.collision_sensor.get_last_gym_observation():
+            print("Collision detected — emergency stop")
+            throttle, brake = 0.0, 1.0
 
         control = {
-            "throttle": np.clip(throttle_control, 0.0, 1.0),
+            "throttle": throttle,
             "steer": steer_control,
-            "brake": np.clip(-throttle_control, 0.0, 1.0),
+            "brake": brake,
             "hand_brake": 0.0,
             "reverse": 0,
             "target_gear": 0