Feature/carla connection

Dockerfile

@@ -0,0 +1,15 @@
+FROM ubuntu:22.04
+
+RUN apt-get update && apt-get install -y \
+    curl \
+    bash \
+    ca-certificates \
+    && rm -rf /var/lib/apt/lists/*
+
+RUN curl -sfL https://github.com/eclipse-ankaios/ankaios/releases/download/v0.6.0/install.sh | INSTALL_ANK_SERVER_RUST_LOG=debug INSTALL_ANK_AGENT_RUST_LOG=info bash -s -- -t both
+
+ENV PATH="/root/.local/bin:$PATH"
+
+EXPOSE 25551
+
+CMD ["ank-server"]

python/carla-connection.py

@@ -0,0 +1,99 @@
+import json
+import sys
+import time
+import math
+import zenoh
+
+# Add CARLA egg to PYTHONPATH
+# Replace with your actual path to carla-<version>.egg if needed
+try:
+    sys.path.append('path/to/carla-<version>.egg')  # Only if not installed via pip
+    import carla
+except ImportError:
+    print("CARLA egg not found. Please update the sys.path line.")
+    sys.exit(1)
+
+def get_speed(velocity):
+    """Calculate speed in m/s from velocity vector"""
+    return math.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2)
+
+def main():
+    #--------- CONFIG ----------
+    POSE_TOPIC = 'vehicle/pose' #Vehicle Position info
+    IMU_TOPIC = 'vehicle/imu/raw' #Raw IMU
+    RATE_HZ = 20
+    ZENOH_CONNECT = {}
+    # -------------------------
+    try:
+        # Connect to CARLA server
+        client = carla.Client("localhost", 2000)
+        client.set_timeout(5.0)
+
+        world = client.get_world()
+
+        # Get all vehicles
+        vehicles = world.get_actors().filter('vehicle.*')
+        if not vehicles:
+            print("No vehicles found in the simulation.")
+            return
+
+        # Pick the first vehicle
+        vehicle = vehicles[0]
+        print(f"Tracking vehicle: ID {vehicle.id}, Type {vehicle.type_id}\n")
+
+        session = zenoh.open(ZENOH_CONNECT)
+        pub_pose = session.declare_publisher(POSE_TOPIC)
+        pub_imu = session.publish(IMU_TOPIC)
+        # Poll info every 0.5 seconds
+        while True:
+
+            t0 = time.time()
+
+            transform = vehicle.get_transform()
+            velocity = vehicle.get_velocity()
+            acc = vehicle.get_acceleration()
+
+            location, rotation = transform.location, transform.rotation
+            speed = get_speed(velocity)
+
+            pose_msg = {
+                "ts": t0,
+                "x": location.x,
+                "y": location.y,
+                "yaw": math.radians(rotation.yaw), # CARLA gives in degrees
+                "speed": speed,
+            }
+
+            pub_pose.put(json.dumps(pose_msg).encode("utf-8"))
+
+            '----------IMU----------'
+            imu_msg = {
+                "ts": t0,
+                "src": "carla",
+                "hz": RATE_HZ,
+                "acc": {"x": acc.x, "y": acc.y, "z": acc.z}
+            }
+
+            pub_imu.put(json.dumps(imu_msg).encode("utf-8"))
+
+
+            # Validate this if
+           ## if (rotation.yaw < -2.0):
+           ##     # sends data to topics
+           ##     session.put(vehicle_pose_topic, location)
+           ##     session.put(vehicle_imu_raw_topic, rotation)
+
+            print(f"Location: (X={location.x:.2f}, Y={location.y:.2f}, Z={location.z:.2f})")
+            print(f"Rotation: (Pitch={rotation.pitch:.2f}, Yaw={rotation.yaw:.2f}, Roll={rotation.roll:.2f})")
+            print(f"Speed: {speed:.2f} m/s")
+            print("-" * 40)
+
+            time.sleep(0.5)
+
+    except KeyboardInterrupt:
+        print("\nExiting gracefully.")
+    except Exception as e:
+        print(f"Error: {e}")
+
+if __name__ == "__main__":
+    main()

python/listener.py

@@ -0,0 +1,87 @@
+import json
+import sys
+import time
+import math
+import zenoh
+
+
+# tiny state
+state = {
+    "last_imu_ts": 0.0,
+    "prev_acc_z": None,
+    "last_event_ts": 0.0,
+    "last_pose": {"speed_mps": 0.0, "x": 0.0, "y": 0.0}
+}
+
+
+#changable variables
+MIN_SPEED = 4.0          # m/s (~14 km/h)
+DELTA_Z_TH = 2.5         # m/s^2 change between consecutive samples
+DEBOUNCE_S = 1.5         # seconds between events
+#-------------------
+
+def severity_from_delta(dz):
+    if dz > 4.0: return "HIGH"
+    if dz > 3.0: return "MED"
+    return "LOW"
+
+def position_listener(sample):
+    try:
+        data = json.loads(sample.payload.decode("utf-8"))
+        state["last_pose"] = data
+    except Exception as e:
+        print(f"[listener] pose parse err: {e}")
+
+def imu_listener(sample):
+    try:
+        data = json.loads(sample.payload.decode("utf-8"))
+        ts = float(data.get("ts", time.time()))
+        acc = data.get("acc", {})
+        acc_z = float(acc.get("z", 0.0))
+        speed = float(state["last_pose"].get("speed_mps", 0.0))
+
+        prev = state["prev_acc_z"]
+        state["prev_acc_z"] = acc_z
+
+        ## On the first IMU sample theres no previous acc_z to compare against
+        if prev is None:
+            return
+
+        ## In theory potholes will cause a sharp change so a big dz (Delta-z acceleration, calculates the absolute change in vertical acceleration)
+        dz = abs(acc_z - prev)
+        now = ts
+
+        ''' Little help from our 5th member ChatGPT on doing these calculations '''
+        if speed >= MIN_SPEED and dz >= DELTA_Z_TH and (now - state["last_event_ts"]) >= DEBOUNCE_S:
+            sev = severity_from_delta(dz)
+            x = state["last_pose"].get("x", 0.0)
+            y = state["last_pose"].get("y", 0.0)
+            print(f"[POTHOLE] {sev} at ({x:.1f},{y:.1f}) | variation_acc_z={dz:.2f} m/s2 speed={speed:.1f} m/s")
+            state["last_event_ts"] = now
+
+    except Exception as e:
+        print(f"[listener] imu parse err: {e}")
+
+def main():
+    # Open Zenoh session
+    print("[Listener] Opening Zenoh session...")
+    session = zenoh.open({})
+
+    POSE_TOPIC = 'vehicle/pose' #Vehicle Position info
+    IMU_TOPIC = 'vehicle/imu/raw' #Raw IMU
+    print(f"[Subscriber] Subscribing to: {IMU_TOPIC} and {POSE_TOPIC}")
+    session.declare_subscriber(POSE_TOPIC, position_listener)
+    session.declare_subscriber(IMU_TOPIC, imu_listener)
+
+    # Keep the subscriber running
+    try:
+        print("[Listener] Listening for data. Press Ctrl+C to stop")
+        while True:
+            time.sleep(1)
+    except KeyboardInterrupt:
+        print("\n[Listener] Shutting down")
+    finally:
+        session.close()
+
+if __name__ == '__main__':
+   main()

python/potholes.py

@@ -3,7 +3,7 @@
 import random
 import time
 
-def spawn_pothole(world, bp_lib, location, scale=(2.0, 2.0, 0.2)):
+def spawn_pothole(world, bp_lib, location, scale=(2.0, 2.0, 0.08)):
     """
     Spawn a thin cube collider to simulate a pothole.
     """
@@ -13,7 +13,11 @@ def spawn_pothole(world, bp_lib, location, scale=(2.0, 2.0, 0.2)):
     transform = carla.Transform(location, carla.Rotation())
     pothole = world.try_spawn_actor(cube_bp, transform)
     if pothole:
-        pothole.set_simulate_physics(True)
+        try:
+            pothole.set_simulate_physics(True)
+            pothole.set_enable_gravity(True)
+        except:
+            pass
         print(f"Spawned pothole at {location}")
     else:
         print("Failed to spawn pothole")
@@ -58,7 +62,7 @@ def main():
 
     # Put vehicle on autopilot
     vehicle.set_autopilot(True)
-
+    print("Simulation running for 20 seconds..")
     # Let simulation run
     time.sleep(20)
 

requirements.txt

@@ -0,0 +1,7 @@
+certifi==2025.4.26
+cffi==1.17.1
+cryptography==44.0.3
+pycparser==2.22
+wheel==0.45.1
+carla==0.9.16
+eclipse-zenoh==1.5.1