From 9bf3431c438e93389ff944dec011990fe8c54905 Mon Sep 17 00:00:00 2001 From: Guilherme Barros Date: Tue, 30 Sep 2025 17:55:48 +0100 Subject: [PATCH 1/4] Created test python file to connect and read info from Carla --- Dockerfile | 15 ++++++++++ python/carla-connection.py | 58 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 73 insertions(+) create mode 100644 Dockerfile create mode 100644 python/carla-connection.py diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..2847457 --- /dev/null +++ b/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"] \ No newline at end of file diff --git a/python/carla-connection.py b/python/carla-connection.py new file mode 100644 index 0000000..b00d96e --- /dev/null +++ b/python/carla-connection.py @@ -0,0 +1,58 @@ +import sys +import time +import math + +# Add CARLA egg to PYTHONPATH +# Replace with your actual path to carla-.egg if needed +try: + sys.path.append('path/to/carla-.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(): + 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") + + # Poll info every 0.5 seconds + while True: + transform = vehicle.get_transform() + velocity = vehicle.get_velocity() + + location = transform.location + rotation = transform.rotation + speed = get_speed(velocity) + + 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() From 00068ece236fa1bed9fb7fd9b09594159687e660 Mon Sep 17 00:00:00 2001 From: Guilherme Barros Date: Wed, 1 Oct 2025 09:29:11 +0100 Subject: [PATCH 2/4] Created requirements file and added connections to Zenoh --- .DS_Store | Bin 0 -> 6148 bytes .idea/aws.xml | 1 - .idea/misc.xml | 5 ++++- .idea/vcs.xml | 1 + python/carla-connection.py | 15 +++++++++++++++ python/listener.py | 38 +++++++++++++++++++++++++++++++++++++ requirements.txt | 7 +++++++ 7 files changed, 65 insertions(+), 2 deletions(-) create mode 100644 .DS_Store create mode 100644 python/listener.py diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..c15eb26dc919d6bbc3cb46313c54eb05fab1a915 GIT binary patch literal 6148 zcmeHKyG{c^3>-s>NNG}1?hjD#2dgN2L4F{R(47=SNPiXI#iucT2lzr_*qqsH_x_0#ZN921ictKq}yWUC3q;&i^hMLDb|DoOz< zFje3-w`=eJkMtkr|0zj3DIf*@l>)Zd?zS6Vse0?|a^7njeNXqAPr4h|L1Bn?OpJES gjkn{wD9XC#Yd-IVV`9*m4?0ml1Fnlq3jDPKU!V0An*aa+ literal 0 HcmV?d00001 diff --git a/.idea/aws.xml b/.idea/aws.xml index ec328d0..a851319 100644 --- a/.idea/aws.xml +++ b/.idea/aws.xml @@ -1,7 +1,6 @@ - \ No newline at end of file diff --git a/python/carla-connection.py b/python/carla-connection.py index b00d96e..748b81c 100644 --- a/python/carla-connection.py +++ b/python/carla-connection.py @@ -1,6 +1,7 @@ import sys import time import math +import zenoh # Add CARLA egg to PYTHONPATH # Replace with your actual path to carla-.egg if needed @@ -35,6 +36,14 @@ def main(): # Poll info every 0.5 seconds while True: + session = zenoh.open({}) + + # vehicle position topic + vehicle_pose_topic = 'vehicle/pose' + + # raw IMU info + vehicle_imu_raw_topic = 'vehicle/imu/raw' + transform = vehicle.get_transform() velocity = vehicle.get_velocity() @@ -42,6 +51,12 @@ def main(): rotation = transform.rotation speed = get_speed(velocity) + # 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") diff --git a/python/listener.py b/python/listener.py new file mode 100644 index 0000000..1376dd5 --- /dev/null +++ b/python/listener.py @@ -0,0 +1,38 @@ +import sys +import time +import math +import zenoh + +def position_listener(sample): + print() + +def imu_listener(sample): + # apply rule to check severity of pothole + print() + +def main(): + # Open Zenoh session + print("[Subscriber] Opening Zenoh session...") + session = zenoh.open({}) + + # Subscribe to a key expression + # vehicle position topic + vehicle_pose_topic = 'vehicle/pose' + # raw IMU info + vehicle_imu_raw_topic = 'vehicle/imu/raw' + print(f"[Subscriber] Subscribing to: {vehicle_imu_raw_topic} and {vehicle_pose_topic}") + session.declare_subscriber(vehicle_pose_topic, position_listener) + session.declare_subscriber(vehicle_imu_raw_topic, imu_listener) + + # Keep the subscriber running + try: + print("[Subscriber] Listening for data. Press Ctrl+C to stop.") + while True: + time.sleep(1) + except KeyboardInterrupt: + print("\n[Subscriber] Shutting down.") + finally: + session.close() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/requirements.txt b/requirements.txt index e69de29..2f2be28 100644 --- a/requirements.txt +++ b/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 From ae7cbc83fc183272dfb0ae099060afeab2e40fc7 Mon Sep 17 00:00:00 2001 From: Rodrigo Oliveira Date: Wed, 1 Oct 2025 11:00:29 +0100 Subject: [PATCH 3/4] changes to listener and carla connection --- python/carla-connection.py | 50 +++++++++++++++++++------ python/listener.py | 77 +++++++++++++++++++++++++++++++------- 2 files changed, 101 insertions(+), 26 deletions(-) diff --git a/python/carla-connection.py b/python/carla-connection.py index 748b81c..f940965 100644 --- a/python/carla-connection.py +++ b/python/carla-connection.py @@ -1,3 +1,4 @@ +import json import sys import time import math @@ -17,6 +18,12 @@ def get_speed(velocity): 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) @@ -34,28 +41,47 @@ def main(): 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: - session = zenoh.open({}) - # vehicle position topic - vehicle_pose_topic = 'vehicle/pose' - - # raw IMU info - vehicle_imu_raw_topic = 'vehicle/imu/raw' + t0 = time.time() transform = vehicle.get_transform() velocity = vehicle.get_velocity() + acc = vehicle.get_acceleration() - location = transform.location - rotation = transform.rotation + 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) + ## 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})") diff --git a/python/listener.py b/python/listener.py index 1376dd5..faa517c 100644 --- a/python/listener.py +++ b/python/listener.py @@ -1,36 +1,85 @@ +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): - print() + 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): - # apply rule to check severity of pothole - print() + 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("[Subscriber] Opening Zenoh session...") + print("[Listener] Opening Zenoh session...") session = zenoh.open({}) - # Subscribe to a key expression - # vehicle position topic - vehicle_pose_topic = 'vehicle/pose' - # raw IMU info - vehicle_imu_raw_topic = 'vehicle/imu/raw' - print(f"[Subscriber] Subscribing to: {vehicle_imu_raw_topic} and {vehicle_pose_topic}") - session.declare_subscriber(vehicle_pose_topic, position_listener) - session.declare_subscriber(vehicle_imu_raw_topic, imu_listener) + 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("[Subscriber] Listening for data. Press Ctrl+C to stop.") + print("[Listener] Listening for data. Press Ctrl+C to stop") while True: time.sleep(1) except KeyboardInterrupt: - print("\n[Subscriber] Shutting down.") + print("\n[Listener] Shutting down") finally: session.close() From 646dbb772e675181c7df66ef7ca0d3abd8d5ad00 Mon Sep 17 00:00:00 2001 From: Rodrigo Oliveira Date: Wed, 1 Oct 2025 11:05:59 +0100 Subject: [PATCH 4/4] minor changes to potholes.py --- python/potholes.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/python/potholes.py b/python/potholes.py index 9d011e5..ddfdeb0 100644 --- a/python/potholes.py +++ b/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)