+ 
+
+
+## Startup Sequence
+
+The startup order matters:
+
+1. Start `zenoh-router`.
+2. Start `mocap`.
+3. Start all estimator processes.
+4. Start the host script.
+5. Start all client scripts.
+
+Once the host is running, it repeatedly publishes a ready signal. Each client waits for that
+signal, calibrates its clock against the host, marks itself as ready, and blocks until the host
+releases the synchronized race start. During the race, clients send actions to the host and the
+host-side worker processes forward those actions to the physical drones.
diff --git a/lsy_drone_racing/control/testing_race_fast.py b/lsy_drone_racing/control/testing_race_fast.py
new file mode 100644
index 000000000..ed91bf9bd
--- /dev/null
+++ b/lsy_drone_racing/control/testing_race_fast.py
@@ -0,0 +1,57 @@
+"""This module wraps the AttitudeController to handle batched multi-agent environments.
+
+In multi-agent simulations, observations are batched across all drones.
+The rank index is used to select the state of the current drone.
+"""
+
+from __future__ import annotations # Python 3.10 type hints
+
+from typing import TYPE_CHECKING
+
+import numpy as np
+from scipy.interpolate import CubicSpline
+
+from lsy_drone_racing.control.attitude_controller import (
+ AttitudeController as SingleAttitudeController,
+)
+
+if TYPE_CHECKING:
+ from numpy.typing import NDArray
+
+
+class AttitudeController(SingleAttitudeController):
+ """Example of a controller using the collective thrust and attitude interface."""
+
+ def __init__(self, obs: dict[str, NDArray[np.floating]], info: dict, config: dict):
+ """Initialize the attitude controller.
+
+ Args:
+ obs: The initial observation of the environment's state. See the environment's
+ observation space for details.
+ info: Additional environment information from the reset.
+ config: The configuration of the environment.
+ """
+ self.rank = info["rank"]
+ super().__init__({k: v[self.rank] for k, v in obs.items()}, info, config)
+ # We don't want the example controllers to crash, so we speed up this one to get ahead
+ self._t_total = 6
+ waypoints = self._des_pos_spline._c[-1]
+ t = np.linspace(0, self._t_total, len(waypoints))
+ self._des_pos_spline = CubicSpline(t, waypoints)
+ self._des_vel_spline = self._des_pos_spline.derivative()
+
+ def compute_control(
+ self, obs: dict[str, NDArray[np.floating]], info: dict | None = None
+ ) -> NDArray[np.floating]:
+ """Compute the next desired collective thrust and roll/pitch/yaw of the drone.
+
+ Args:
+ obs: The current observation of the environment. See the environment's observation space
+ for details.
+ info: Optional additional information as a dictionary.
+
+ Returns:
+ The orientation as roll, pitch, yaw angles, and the collective thrust
+ [r_des, p_des, y_des, t_des] as a numpy array.
+ """
+ return super().compute_control({k: v[self.rank] for k, v in obs.items()}, info)
diff --git a/lsy_drone_racing/control/testing_race_slow.py b/lsy_drone_racing/control/testing_race_slow.py
new file mode 100644
index 000000000..7b507cb7e
--- /dev/null
+++ b/lsy_drone_racing/control/testing_race_slow.py
@@ -0,0 +1,58 @@
+"""This module wraps the AttitudeController to handle batched multi-agent environments.
+
+In multi-agent simulations, observations are batched across all drones.
+The rank index is used to select the state of the current drone.
+"""
+
+from __future__ import annotations # Python 3.10 type hints
+
+from typing import TYPE_CHECKING
+
+import numpy as np
+from scipy.interpolate import CubicSpline
+
+from lsy_drone_racing.control.attitude_controller import (
+ AttitudeController as SingleAttitudeController,
+)
+import time
+if TYPE_CHECKING:
+ from numpy.typing import NDArray
+
+
+class AttitudeController(SingleAttitudeController):
+ """Example of a controller using the collective thrust and attitude interface."""
+
+ def __init__(self, obs: dict[str, NDArray[np.floating]], info: dict, config: dict):
+ """Initialize the attitude controller.
+
+ Args:
+ obs: The initial observation of the environment's state. See the environment's
+ observation space for details.
+ info: Additional environment information from the reset.
+ config: The configuration of the environment.
+ """
+ self.rank = info["rank"]
+ super().__init__({k: v[self.rank] for k, v in obs.items()}, info, config)
+ # We don't want the example controllers to crash, so we speed up this one to get ahead
+ self._t_total = 10
+ time.sleep(20)
+ waypoints = self._des_pos_spline._c[-1]
+ t = np.linspace(0, self._t_total, len(waypoints))
+ self._des_pos_spline = CubicSpline(t, waypoints)
+ self._des_vel_spline = self._des_pos_spline.derivative()
+
+ def compute_control(
+ self, obs: dict[str, NDArray[np.floating]], info: dict | None = None
+ ) -> NDArray[np.floating]:
+ """Compute the next desired collective thrust and roll/pitch/yaw of the drone.
+
+ Args:
+ obs: The current observation of the environment. See the environment's observation space
+ for details.
+ info: Optional additional information as a dictionary.
+
+ Returns:
+ The orientation as roll, pitch, yaw angles, and the collective thrust
+ [r_des, p_des, y_des, t_des] as a numpy array.
+ """
+ return super().compute_control({k: v[self.rank] for k, v in obs.items()}, info)
diff --git a/lsy_drone_racing/envs/__init__.py b/lsy_drone_racing/envs/__init__.py
index 220daadf1..e2f4f2161 100644
--- a/lsy_drone_racing/envs/__init__.py
+++ b/lsy_drone_racing/envs/__init__.py
@@ -46,3 +46,9 @@
entry_point="lsy_drone_racing.envs.real_race_env:RealMultiDroneRaceEnv",
disable_env_checker=True,
)
+
+register(
+ id="RealMultiDroneRaceEnvClient-v0",
+ entry_point="lsy_drone_racing.envs.real_race_client_env:RealMultiDroneRaceEnvClient",
+ disable_env_checker=True,
+)
diff --git a/lsy_drone_racing/envs/real_race_client_env.py b/lsy_drone_racing/envs/real_race_client_env.py
new file mode 100644
index 000000000..efb70968b
--- /dev/null
+++ b/lsy_drone_racing/envs/real_race_client_env.py
@@ -0,0 +1,350 @@
+"""Client-side environment for multi-drone racing with host-client architecture.
+
+The RealMultiDroneRaceEnvClient operates as a client in a host-client system:
+- Receives coordination messages from the host via ROS2
+- Manages a single drone's state and control
+- Sends control actions and state updates to the host for supervision
+- Handles local observation and gate tracking
+"""
+
+from __future__ import annotations
+
+import logging
+import threading
+import time
+from typing import TYPE_CHECKING, Any, Literal
+
+import jax
+import numpy as np
+from drone_estimators.ros_nodes.ros2_connector import ROSConnector
+from drone_racing_msgs.msg import RealClientAction, RealHostState # type: ignore[import-untyped]
+from drone_racing_msgs.srv import RealCalibrateClock # type: ignore[import-untyped]
+from gymnasium import Env
+
+from lsy_drone_racing.envs.real_race_env import EnvData
+from lsy_drone_racing.envs.utils import gate_passed, load_track
+from lsy_drone_racing.utils.ros import track_poses
+from lsy_drone_racing.utils.ros_race_comm import RaceCommNode, calibrate_clock
+
+if TYPE_CHECKING:
+ from ml_collections import ConfigDict
+ from numpy.typing import NDArray
+
+logger = logging.getLogger(__name__)
+
+
+class RealMultiDroneRaceEnvClient(Env):
+ """Client-side Gymnasium environment for multi-drone racing.
+
+ Runs on each drone's computing unit. Receives host coordination messages via ROS2,
+ computes observations and gate tracking locally, and forwards actions to the host
+ which relays them to the physical drone.
+
+ Observation space:
+ A dictionary containing the state of all drones in the race, mirroring
+ :class:`lsy_drone_racing.envs.multi_drone_race.MultiDroneRaceEnv`.
+
+ Action space:
+ A single action vector for the drone identified by ``rank``. See
+ :class:`~lsy_drone_racing.envs.real_race_host.CrazyflieWorker` for format details.
+
+ Note:
+ rclpy must be initialized before creating this environment.
+ """
+
+ def __init__(
+ self,
+ drones: list[dict[str, int]],
+ rank: int,
+ freq: int,
+ track: ConfigDict,
+ randomizations: ConfigDict,
+ sensor_range: float = 0.5,
+ control_mode: Literal["state", "attitude"] = "state",
+ ):
+ """Initialize the client-side multi-drone environment.
+
+ Args:
+ drones: List of all drones in the race, each with ``id``, ``channel``, and
+ ``drone_model`` keys.
+ rank: Index of this drone among all drones in the race.
+ freq: Control frequency in Hz.
+ track: Track configuration (see :func:`~lsy_drone_racing.envs.utils.load_track`).
+ randomizations: Randomization configuration (unused on the client side).
+ sensor_range: Distance in metres at which gate/obstacle true poses are revealed.
+ control_mode: Either ``"state"`` or ``"attitude"``.
+ """
+ self.n_drones = len(drones)
+ self.rank = rank
+ self.freq = freq
+ self.sensor_range = sensor_range
+ self.control_mode = control_mode
+ self.drone_names = [f"cf{drone['id']}" for drone in drones]
+ self.drone_name = self.drone_names[rank]
+
+ self.gates, self.obstacles, self.drones_track = load_track(track)
+ self.n_gates = len(self.gates.pos)
+ self.n_obstacles = len(self.obstacles.pos)
+ self.pos_limit_low = np.array(track.safety_limits["pos_limit_low"])
+ self.pos_limit_high = np.array(track.safety_limits["pos_limit_high"])
+
+ self.device = jax.devices("cpu")[0]
+ self._ros_connector: ROSConnector | None = None
+ self.data = EnvData.create(self.n_drones, self.n_gates, self.n_obstacles)
+
+ self._comm: RaceCommNode | None = None
+ self._client_action_pub: Any = None
+ self._clock_calib_client: Any = None
+
+ self._host_ready_event = threading.Event()
+ self._race_started = False
+ self._race_start_time = 0.0
+ self._clock_offset = 0.0
+ self._host_finished = False
+ self._client_ready = False
+
+ def reset(self, *, seed: int | None = None, options: dict | None = None) -> tuple[dict, dict]:
+ """Reset the environment and wait for the host to signal readiness.
+
+ Args:
+ seed: Unused in real environments.
+ options: Deploy options to determine whether to load real track object poses
+
+ Returns:
+ Initial observation and info dictionaries.
+
+ Raises:
+ TimeoutError: If the host does not respond within 120 seconds.
+ """
+ if options and options.get("real_track_objects", False):
+ self.gates.pos, self.gates.quat, self.obstacles.pos = track_poses(
+ self.n_gates, self.n_obstacles
+ )
+
+ if self._ros_connector is None:
+ self._init_ros_connectors()
+ if self._comm is None:
+ self._init_comm()
+
+ current_pos, _, _, _ = self._all_drone_states()
+ self.data.reset(current_pos)
+
+ logger.debug("Environment reset complete")
+ return self.obs(), self.info()
+
+ def lock_until_race_start(self, timeout: float = 60.0):
+ """Sends dummy messages at the control frequency (``self.freq`` Hz) until the race starts.
+
+ After receiving host ready message, the client will calibrate the clock offset and
+ until the host broadcasts :class:`RaceStartMessage`.
+
+ Args:
+ timeout: Maximum time in seconds to wait for calibration and race start.
+
+ Raises:
+ TimeoutError: If calibration or race start exceeds ``timeout`` seconds.
+ """
+ logger.info("Waiting for host ready message...")
+ stop_sending = threading.Event()
+
+ def send_action_messages():
+ while not stop_sending.is_set():
+ if self.control_mode == "attitude":
+ dummy_action = np.zeros(4, dtype=np.float32)
+ else:
+ dummy_action = np.zeros(13, dtype=np.float32)
+ dummy_action[:3] = self._ros_connector.pos[self.drone_name]
+ self._send_action_update(dummy_action, stopped=False)
+ time.sleep(1 / self.freq)
+
+ threading.Thread(target=send_action_messages, daemon=True).start()
+
+ if not self._host_ready_event.wait(timeout=timeout):
+ stop_sending.set()
+ raise TimeoutError(
+ "Timeout waiting for host ready. "
+ "Host may not be running or network connection failed."
+ )
+
+ logger.info("Received host ready message.")
+ self._clock_offset = calibrate_clock(self._clock_calib_client, n=5, timeout=timeout)
+ logger.info(f"Clock offset = {self._clock_offset * 1000:.2f}ms")
+ logger.info("Waiting for race start")
+
+ t_start = time.time()
+ while not self._race_started:
+ if time.time() - t_start > timeout:
+ raise TimeoutError(f"Timeout waiting for race start after {timeout}s.")
+ time.sleep(0.001)
+ stop_sending.set()
+ logger.info("Race starts!")
+
+ def step(self, action: NDArray) -> tuple[dict, float, bool, bool, dict]:
+ """Perform a control step: update gate tracking, check bounds, and send the action.
+
+ Args:
+ action: Control action for this drone.
+
+ Returns:
+ Observation, reward (always 0.0), terminated, truncated (always False), info.
+ """
+ drone_pos, _, _, _ = self._all_drone_states()
+
+ dpos = drone_pos[:, None, :2] - self.gates.pos[None, :, :2]
+ self.data.gates_visited |= np.linalg.norm(dpos, axis=-1) < self.sensor_range
+ dpos = drone_pos[:, None, :2] - self.obstacles.pos[None, :, :2]
+ self.data.obstacles_visited |= np.linalg.norm(dpos, axis=-1) < self.sensor_range
+
+ gate_pos = self.gates.pos[self.data.target_gate]
+ gate_quat = self.gates.quat[self.data.target_gate]
+ with jax.default_device(self.device):
+ passed = gate_passed(
+ drone_pos, self.data.last_drone_pos, gate_pos, gate_quat, (0.45, 0.45)
+ )
+ self.data.target_gate += np.asarray(passed)
+ self.data.target_gate[self.data.target_gate >= self.n_gates] = -1
+ self.data.last_drone_pos[...] = drone_pos
+ self.data.taken_off |= drone_pos[self.rank, 2] > 0.1
+
+ terminated = bool(self.data.target_gate[self.rank] == -1)
+
+ within_bound = np.all(
+ (drone_pos[self.rank] >= self.pos_limit_low)
+ & (drone_pos[self.rank] <= self.pos_limit_high)
+ )
+ if not within_bound:
+ logger.warning("Drone exceeded safety bounds")
+ terminated = True
+
+ if self.control_mode == "attitude" and self._ros_connector:
+ self._ros_connector.publish_cmd(action)
+
+ self._send_action_update(action, terminated)
+
+ return self.obs(), 0.0, terminated, False, self.info()
+
+ def obs(self) -> dict[str, NDArray]:
+ """Return the current observation dictionary."""
+ mask = self.data.gates_visited[..., None]
+ gates_pos = np.where(mask, self.gates.pos, self.gates.nominal_pos).astype(np.float32)
+ gates_quat = np.where(mask, self.gates.quat, self.gates.nominal_quat).astype(np.float32)
+ mask = self.data.obstacles_visited[..., None]
+ obstacles_pos = np.where(mask, self.obstacles.pos, self.obstacles.nominal_pos).astype(
+ np.float32
+ )
+ drone_pos, drone_quat, drone_vel, drone_ang_vel = self._all_drone_states()
+ return {
+ "pos": drone_pos,
+ "quat": drone_quat,
+ "vel": drone_vel,
+ "ang_vel": drone_ang_vel,
+ "target_gate": self.data.target_gate,
+ "gates_pos": gates_pos,
+ "gates_quat": gates_quat,
+ "gates_visited": self.data.gates_visited,
+ "obstacles_pos": obstacles_pos,
+ "obstacles_visited": self.data.obstacles_visited,
+ }
+
+ def info(self) -> dict:
+ """Return the info dictionary."""
+ return {"rank": self.rank}
+
+ def close(self):
+ """Send a final stop message and close all ROS connections."""
+ logger.info("Closing environment...")
+ if self._client_action_pub:
+ self._send_action_update(
+ np.zeros(4 if self.control_mode == "attitude" else 13), stopped=True
+ )
+ if self._comm:
+ self._comm.close()
+ if self._ros_connector:
+ self._ros_connector.close()
+ logger.debug("Environment closed")
+
+ def set_client_ready(self):
+ """Mark the client as fully initialized and ready for race start."""
+ self._client_ready = True
+
+ def _send_action_update(self, action: NDArray, stopped: bool):
+ """Publish a :class:`ClientActionMessage` to the host.
+
+ The timestamp is adjusted by the calibrated clock offset so the host can
+ measure accurate latency without clock skew.
+
+ Args:
+ action: Current control action.
+ stopped: Whether this client has finished or crashed.
+ """
+ elapsed_time = time.time() - self._race_start_time if self._race_started else 0.0
+ msg = RealClientAction()
+ msg.drone_rank = self.rank
+ msg.action = action.tolist() if isinstance(action, np.ndarray) else list(action)
+ msg.elapsed_time = elapsed_time
+ msg.timestamp = time.time() + self._clock_offset
+ msg.client_ready = self._client_ready
+ msg.controller_stopped = stopped
+ self._client_action_pub.publish(msg)
+
+ def _init_ros_connectors(self):
+ """Open ROS connector for own drone (estimator) and others (TF)."""
+ other_names = [n for i, n in enumerate(self.drone_names) if i != self.rank]
+ self._ros_connector = ROSConnector(
+ estimator_names=[self.drone_name],
+ cmd_topic=f"/drones/{self.drone_name}/command",
+ tf_names=other_names,
+ timeout=10.0,
+ )
+
+ def _init_comm(self):
+ """Set up the ROS2 communication node with all publishers and subscribers."""
+ self._comm = RaceCommNode(f"lsy_race_client_{self.rank}")
+ node = self._comm.node
+
+ def on_host_state(msg: RealHostState):
+ latency_ms = (time.time() - msg.timestamp) * 1000
+ if msg.host_ready and not self._host_ready_event.is_set():
+ self._host_ready_event.set()
+ logger.debug(f"Host ready (latency: {latency_ms:.2f}ms)")
+ if msg.race_started and not self._race_started:
+ self._race_started = True
+ self._race_start_time = time.time() - msg.elapsed_time
+ logger.debug(f"Race started (latency: {latency_ms:.2f}ms)")
+ self._host_finished = bool(msg.race_finished)
+
+ # TODO Why do I need to save this if unused?
+ self._sub = node.create_subscription(
+ RealHostState, "lsy_drone_racing/host_state", on_host_state, 10
+ )
+ self._client_action_pub = node.create_publisher(
+ RealClientAction, f"lsy_drone_racing/client/drone_{self.rank}/action", 10
+ )
+ self._clock_calib_client = node.create_client(
+ RealCalibrateClock, "lsy_drone_racing/calibrate_clock"
+ )
+ logger.debug("ROS2 communication initialized")
+
+ def _all_drone_states(self) -> tuple[NDArray, NDArray, NDArray, NDArray]:
+ """Read positions, quaternions, velocities, and angular velocities for all drones.
+
+ Own drone state comes from the high-precision estimator; other drones from TF.
+ Fields for unreachable drones are filled with NaN.
+
+ Returns:
+ Tuple of ``(pos, quat, vel, ang_vel)``, each of shape ``(n_drones, ...)``.
+ """
+ pos = np.full((self.n_drones, 3), np.nan, dtype=np.float32)
+ quat = np.full((self.n_drones, 4), np.nan, dtype=np.float32)
+ vel = np.full((self.n_drones, 3), np.nan, dtype=np.float32)
+ ang_vel = np.full((self.n_drones, 3), np.nan, dtype=np.float32)
+ for i, name in enumerate(self.drone_names):
+ if i == self.rank:
+ pos[i] = self._ros_connector.pos[name]
+ quat[i] = self._ros_connector.quat[name]
+ vel[i] = self._ros_connector.vel[name]
+ ang_vel[i] = self._ros_connector.ang_vel[name]
+ else:
+ pos[i] = self._ros_connector.pos.get(name, np.nan)
+ quat[i] = self._ros_connector.quat.get(name, np.nan)
+ return pos, quat, vel, ang_vel
diff --git a/lsy_drone_racing/envs/real_race_host_env.py b/lsy_drone_racing/envs/real_race_host_env.py
new file mode 100644
index 000000000..51ddb270d
--- /dev/null
+++ b/lsy_drone_racing/envs/real_race_host_env.py
@@ -0,0 +1,672 @@
+"""Real-world drone racing host for multi-drone coordination.
+
+The RealRaceHost manages the central coordination of multi-drone racing:
+track validation, drone connection, client synchronization, race operation,
+and graceful shutdown. Communication with clients is handled via ROS2.
+"""
+
+from __future__ import annotations
+
+import logging
+import multiprocessing as mp
+import signal
+import threading
+import time
+from typing import TYPE_CHECKING, Literal
+
+import numpy as np
+import rclpy
+from drone_estimators.ros_nodes.ros2_connector import ROSConnector
+from drone_models.core import load_params
+from drone_racing_msgs.msg import RealClientAction, RealHostState
+from drone_racing_msgs.srv import RealCalibrateClock
+
+from lsy_drone_racing.envs.utils import load_track
+from lsy_drone_racing.utils.checks import check_drone_start_pos, check_race_track
+from lsy_drone_racing.utils.crazyflie import Crazyflie
+from lsy_drone_racing.utils.ros_race_comm import RaceCommNode
+
+if TYPE_CHECKING:
+ from ml_collections import ConfigDict
+ from numpy.typing import NDArray
+
+logger = logging.getLogger(__name__)
+
+# region Worker
+
+
+class CrazyflieWorker:
+ """Manages a single Crazyflie drone in a dedicated subprocess.
+
+ Connects to the drone via radio, resets it to its initial state, and runs
+ a control loop that forwards actions received from the client over ROS2.
+ External position updates from the ROS estimator are forwarded to the drone's
+ Kalman filter at a fixed rate.
+ """
+
+ POS_UPDATE_FREQ = 30 # Hz at which MoCap poses are forwarded to the drone's Kalman filter
+
+ def __init__(
+ self,
+ rank: int,
+ radio_id: int,
+ drone_id: int,
+ drone_channel: int,
+ drone_model: str,
+ start_pos: NDArray[np.floating],
+ return_to_start_event: mp.synchronize.Event,
+ return_height_ready_event: mp.synchronize.Event,
+ assigned_return_heights: mp.Array,
+ stop_event: mp.synchronize.Event,
+ init_barrier: mp.synchronize.Barrier,
+ control_mode: Literal["attitude", "state"],
+ control_freq: float = 50.0,
+ ):
+ """Initialize the Crazyflie worker.
+
+ Args:
+ rank: Index of this drone among all drones in the race.
+ radio_id: USB radio ID (used to build the radio URI).
+ drone_id: Crazyflie hardware ID (used to build the radio URI).
+ drone_channel: Radio channel to connect on.
+ drone_model: Drone model name for loading thrust/PWM parameters.
+ start_pos: Start position the drone should return to after the race.
+ return_to_start_event: Set while the worker is executing the return maneuver.
+ return_height_ready_event: Set by the host once this drone's return height is ready.
+ assigned_return_heights: Shared host-assigned return heights for all drones.
+ stop_event: Set by the host to request a shutdown.
+ init_barrier: Shared barrier; all workers and the host call :meth:`wait` once
+ initialized so that all drones start simultaneously. Any worker that fails
+ calls :meth:`abort` to propagate the failure to everyone.
+ init_pose: Initial pose used to seed the drone's Kalman filter.
+ control_mode: Either ``"attitude"`` or ``"state"``.
+ control_freq: Frequency in Hz at which actions are forwarded to the drone.
+ """
+ self.rank = rank
+ self.radio_id = radio_id
+ self.drone_id = drone_id
+ self.drone_channel = drone_channel
+ self.drone_model = drone_model
+ self.start_pos = np.array(start_pos, dtype=np.float32)
+ self.return_to_start_event = return_to_start_event
+ self.return_height_ready_event = return_height_ready_event
+ self.assigned_return_heights = assigned_return_heights
+ self.stop_event = stop_event
+ self.init_barrier = init_barrier
+ self.connection_failed = (
+ False # Set to True if connection fails during the initial connection phase
+ )
+ self.connection_lost = False # Set to True if connection is lost after being established
+
+ assert control_mode in ["attitude", "state"], "control_mode must be 'attitude' or 'state'"
+ self._control_mode: Literal["attitude", "state"] = control_mode
+ self._control_freq = control_freq
+
+ logging.basicConfig(level=logging.INFO, format=f"[Drone {rank}] %(levelname)s: %(message)s")
+ logging.getLogger("cflib").setLevel(logging.WARNING)
+ self.logger = logging.getLogger(__name__)
+
+ self.drone_name = f"cf{drone_id}"
+ self.drone: Crazyflie | None = None
+ self.drone_params: dict = load_params(
+ physics="first_principles", drone_model=self.drone_model
+ )
+ self.last_msg: RealClientAction | None = None
+ self.action_lock = threading.Lock()
+ self._comm: RaceCommNode | None = None
+ self._ros_connector: ROSConnector | None = None
+ self._last_drone_pos_update: float = 0.0
+
+ def _on_client_action(self, msg: RealClientAction):
+ """Store the latest action received from the client."""
+ with self.action_lock:
+ self.last_msg = msg
+
+ latency_ms = (time.time() - msg.timestamp) * 1000
+ self.logger.debug(f"Action received (latency={latency_ms:.2f}ms)")
+
+ def _init_cf(self):
+ """Connect to the Crazyflie, reset it, and initialize its Kalman filter."""
+ self.logger.info(f"Connecting to Crazyflie with ID {self.drone_id}...")
+ self.drone = Crazyflie.from_radio(self.radio_id, self.drone_channel, self.drone_id)
+ self.drone.connect()
+ self.drone.reset(arm=True)
+ self.logger.info("Connected to Crazyflie")
+
+ def _init_ros_comm(self):
+ """Subscribe to client action messages for this drone via ROS2."""
+ self._comm = RaceCommNode(f"lsy_race_worker_{self.rank}")
+ self._sub = self._comm.node.create_subscription(
+ RealClientAction,
+ f"lsy_drone_racing/client/drone_{self.rank}/action",
+ self._on_client_action,
+ 10,
+ )
+
+ def _init_ros_connector(self):
+ """Open a ROS connector for reading this drone's pose from the estimator."""
+ self.logger.info(f"Initializing ROS connector for {self.drone_name}...")
+ self._ros_connector = ROSConnector(
+ estimator_names=[self.drone_name],
+ cmd_topic=f"/drones/{self.drone_name}/command",
+ timeout=10.0,
+ )
+
+ def _control_loop(self):
+ """Send actions to the drone at the configured control frequency."""
+ with self.action_lock:
+ self.last_msg = None # Clear any stale message received during initialization
+
+ dt = 1.0 / self._control_freq
+
+ self._last_drone_pos_update = time.perf_counter()
+ while not self.stop_event.is_set() and not self.connection_lost:
+ t_start = time.time()
+
+ with self.action_lock:
+ if self.last_msg and (t_start - self.last_msg.timestamp) > 10 * dt:
+ self.logger.error(
+ f"No command received for 10 * {dt:.2f}s, handover control to host..."
+ )
+ break
+ if self.last_msg and self.last_msg.controller_stopped:
+ self.logger.info(
+ "Received stop signal from client, waiting for return height..."
+ )
+ self._return_to_start()
+ break
+ action = list(self.last_msg.action) if self.last_msg else None
+
+ if action is not None:
+ action_array = (
+ np.array(action) if isinstance(action, (list, tuple)) else np.array([action])
+ )
+ if self._control_mode == "attitude":
+ self.drone.send_action_attitude(
+ action_array[:3], action_array[3], self.drone_params
+ )
+ else:
+ self.drone.send_action_state(
+ action_array[0:3],
+ action_array[3:6],
+ action_array[6:9],
+ action_array[9],
+ action_array[10:12],
+ )
+
+ if (t := time.perf_counter()) - self._last_drone_pos_update > 1 / self.POS_UPDATE_FREQ:
+ self.drone.send_external_pose()
+ self._last_drone_pos_update = t
+ elapsed = time.time() - t_start
+
+ time.sleep(max(0.0, dt - elapsed))
+
+ def _cleanup(self):
+ """Send an emergency stop, close all connections, and shut down ROS."""
+ if self._ros_connector:
+ self._ros_connector.close()
+ if self._comm:
+ self._comm.close()
+ if self.drone:
+ self.drone.close(emergency_stop=True)
+ rclpy.shutdown()
+ self.logger.info("Drone process finished")
+
+ def _return_to_start(self):
+ """Return the drone to its configured start position if it has taken off."""
+ pos = self._ros_connector.pos[self.drone_name]
+ if pos[2] <= 0.1:
+ return
+ obs = {
+ "pos": pos,
+ "quat": self._ros_connector.quat[self.drone_name],
+ "vel": self._ros_connector.vel[self.drone_name],
+ "ang_vel": self._ros_connector.ang_vel[self.drone_name],
+ }
+ self.return_to_start_event.set()
+ try:
+ return_height = self._wait_for_return_height()
+ self.logger.info(f"Returning to start at height {return_height:.2f}m")
+ self.drone.return_to_start(
+ self.start_pos, obs, check_ok=rclpy.ok, return_height=return_height
+ )
+ finally:
+ self.return_to_start_event.clear()
+
+ def _wait_for_return_height(self) -> float:
+ """Wait until the host assigns a return height for this drone."""
+ while rclpy.ok() and not self.stop_event.is_set():
+ if self.return_height_ready_event.wait(timeout=0.1):
+ with self.assigned_return_heights.get_lock():
+ return float(self.assigned_return_heights[self.rank])
+ raise RuntimeError("Return height assignment was interrupted")
+
+ def run(self):
+ """Run the worker: connect to the drone, initialize, and enter the control loop.
+
+ Exceptions during initialization abort the shared barrier to notify the host and
+ all other workers. A BrokenBarrierError means another worker already failed.
+ """
+ rclpy.init()
+ try:
+ assert self._control_mode in ["attitude", "state"]
+ tasks = [self._init_cf, self._init_ros_connector, self._init_ros_comm]
+ for task in tasks:
+ if self.stop_event.is_set():
+ return
+ task()
+ self.logger.info("Waiting for start signal...")
+ self.init_barrier.wait(timeout=None)
+ self._control_loop()
+ except threading.BrokenBarrierError:
+ # This will ONLY trigger during initilization phase,
+ # since no further wait() will be called here
+ pass
+ except TimeoutError:
+ self.logger.error("Initialization timed out, aborting...")
+ self.init_barrier.abort()
+ except Exception:
+ self.init_barrier.abort()
+ raise
+ finally:
+ self._cleanup()
+
+ @staticmethod
+ def crazyflie_process_worker(
+ rank: int,
+ radio_id: int,
+ drone_id: int,
+ drone_channel: int,
+ drone_model: str,
+ start_pos: NDArray[np.floating],
+ return_to_start_event: mp.synchronize.Event,
+ return_height_ready_event: mp.synchronize.Event,
+ assigned_return_heights: mp.Array,
+ stop_event: mp.synchronize.Event,
+ control_mode: Literal["attitude", "state"],
+ init_barrier: mp.synchronize.Barrier,
+ control_freq: float = 50.0,
+ ):
+ """Multiprocessing entry point that creates and runs a :class:`CrazyflieWorker`.
+
+ SIGINT is ignored so that only the host process handles keyboard interrupts.
+ """
+ signal.signal(signal.SIGINT, signal.SIG_IGN) # Ignore SIGINT in worker processes
+ CrazyflieWorker(
+ rank=rank,
+ radio_id=radio_id,
+ drone_id=drone_id,
+ drone_channel=drone_channel,
+ drone_model=drone_model,
+ start_pos=start_pos,
+ return_to_start_event=return_to_start_event,
+ return_height_ready_event=return_height_ready_event,
+ assigned_return_heights=assigned_return_heights,
+ stop_event=stop_event,
+ control_mode=control_mode,
+ control_freq=control_freq,
+ init_barrier=init_barrier,
+ ).run()
+
+
+# region Host
+
+
+class CrazyflieRealRaceHost:
+ """Race host implementation for multi-drone racing with Crazyflie drones.
+
+ Each drone runs in its own subprocess (:class:`CrazyflieWorker`) that handles
+ radio communication independently. The host coordinates the race lifecycle via
+ ROS2 messages to the client processes.
+ """
+
+ gates: ConfigDict
+ obstacles: ConfigDict
+ drones_pose: ConfigDict
+ n_gates: int
+ pos_limit_high: NDArray[np.float32]
+ pos_limit_low: NDArray[np.float32]
+
+ _num_drones: int
+ _drone_names: list[str]
+ _drone_ids: list[int]
+ _drone_channels: list[int]
+ _radio_ids: list[str]
+ _drone_models: list[str]
+ _processes: list[mp.Process]
+ _drone_control_freq: list[float]
+ _drone_control_mode: list[str]
+ _stop_event: mp.synchronize.Event | None
+ _init_barrier: mp.synchronize.Barrier | None
+ _mp_ctx: mp.context.BaseContext
+
+ def __init__(self, track: ConfigDict, deploy_args: list[dict], control_args: list[dict]):
+ """Initialize the host.
+
+ Args:
+ track: Track configuration (see :func:`~lsy_drone_racing.envs.utils.load_track`).
+ deploy_args: List of drone configs, each with
+ ``id``, ``channel``, ``radio``, and ``drone_model``.
+ control_args: Per-drone kwargs, each with ``freq`` and ``control_mode``.
+ """
+ self.gates, self.obstacles, self.drones_pose = load_track(track)
+ self.n_gates = len(self.gates.pos)
+ self.n_obstacles = len(self.obstacles.pos)
+ self.pos_limit_low = np.array(track.safety_limits["pos_limit_low"])
+ self.pos_limit_high = np.array(track.safety_limits["pos_limit_high"])
+ self._drone_names = [f"cf{drone['id']}" for drone in deploy_args.drones]
+ self._drone_ids = [drone["id"] for drone in deploy_args.drones]
+ self._drone_channels = [drone["channel"] for drone in deploy_args.drones]
+ self._radio_ids = [drone["radio"] for drone in deploy_args.drones]
+ self._drone_models = [drone["drone_model"] for drone in deploy_args.drones]
+ self._drone_control_freq = [kwargs["freq"] for kwargs in control_args]
+ self._drone_control_mode = [kwargs["control_mode"] for kwargs in control_args]
+ self._num_drones = len(deploy_args.drones)
+ self._mp_ctx = mp.get_context("spawn")
+ self._processes = []
+ self._return_to_start_events = []
+ self._return_height_ready_events = []
+ self._assigned_return_heights = None
+ self._stop_event = None
+ self._init_barrier = None
+ self._shutdown_event = threading.Event()
+ self._clients_ready: dict[int, bool] = {rank: False for rank in range(self._num_drones)}
+ self._clients_stopped: dict[int, bool] = {rank: False for rank in range(self._num_drones)}
+ self._finish_order: list[int] = []
+ self._return_height_min = float(deploy_args.return_height_min)
+ self._return_height_max = float(deploy_args.return_height_max)
+ if self._return_height_min > self._return_height_max:
+ raise ValueError("return_height_min must be less than or equal to return_height_max")
+ self._start_time = time.time()
+ self._comm = None
+ self._host_state_pub = None
+ self.init_comm()
+
+ def init_comm(self):
+ """Set up the ROS2 communication node with all publishers and subscribers."""
+ self._comm = RaceCommNode("lsy_race_host")
+ node = self._comm.node
+ self._host_state_pub = node.create_publisher(
+ RealHostState, "lsy_drone_racing/host_state", 10
+ )
+ self._subs = []
+ for rank in range(self._num_drones):
+
+ def on_client_action(msg: RealClientAction, rank: int = rank):
+ if msg.client_ready and not self._clients_ready[rank]:
+ logger.debug(f"Client {rank} ready")
+ self._clients_ready[rank] = True
+ if msg.controller_stopped and not self._clients_stopped[rank]:
+ finish_order = len(self._finish_order)
+ return_height = self._return_height_for_finish_order(finish_order)
+ logger.info(
+ f"Client {rank} stopped, assigning return height {return_height:.2f}m"
+ )
+ with self._assigned_return_heights.get_lock():
+ self._assigned_return_heights[rank] = return_height
+ self._return_height_ready_events[rank].set()
+ self._finish_order.append(rank)
+ self._clients_stopped[rank] = True
+
+ self._subs.append(
+ node.create_subscription(
+ RealClientAction,
+ f"lsy_drone_racing/client/drone_{rank}/action",
+ on_client_action,
+ 10,
+ )
+ )
+
+ logger.debug("ROS2 communication initialized")
+
+ def check_track(
+ self, rng_config: ConfigDict, check_objects: bool = True, check_drones: bool = True
+ ) -> None:
+ """Verify that gates, obstacles, and drones are within their allowed tolerances.
+
+ Args:
+ rng_config: Randomization config used to determine position tolerances.
+ check_objects: Whether to validate gate and obstacle positions.
+ check_drones: Whether to validate drone start positions.
+ """
+ if not check_objects and not check_drones:
+ return
+ logger.debug("Checking track configuration...")
+ if check_objects:
+ check_race_track(
+ gates_pos=self.gates.pos,
+ nominal_gates_pos=self.gates.nominal_pos,
+ gates_quat=self.gates.quat,
+ nominal_gates_quat=self.gates.nominal_quat,
+ obstacles_pos=self.obstacles.pos,
+ nominal_obstacles_pos=self.obstacles.nominal_pos,
+ rng_config=rng_config,
+ )
+ logger.debug("Track object check passed")
+ if check_drones:
+ for rank, drone_name in enumerate(self._drone_names):
+ check_drone_start_pos(
+ nominal_pos=self.drones_pose.nominal_pos[rank],
+ real_pos=self.drones_pose.pos[rank],
+ rng_config=rng_config,
+ drone_name=drone_name,
+ )
+ logger.debug("Drone start position check passed")
+
+ def connect_drones(self):
+ """Spawn one subprocess per drone and wait until all workers finish initialization.
+
+ Returns once all workers are waiting at the init barrier, or immediately if the
+ barrier is broken by a worker failure. The race start is triggered in
+ :meth:`host_main_loop` when the host calls :meth:`~mp.Barrier.wait` on the barrier.
+ """
+ logger.debug(f"Spawning processes for {self._num_drones} Crazyflie drones...")
+ self._processes = []
+ self._return_to_start_events = [self._mp_ctx.Event() for _ in range(self._num_drones)]
+ self._return_height_ready_events = [
+ self._mp_ctx.Event() for _ in range(self._num_drones)
+ ]
+ self._assigned_return_heights = self._mp_ctx.Array(
+ "d", [float("nan")] * self._num_drones, lock=True
+ )
+ self._stop_event = self._mp_ctx.Event()
+ self._init_barrier = self._mp_ctx.Barrier(self._num_drones + 1)
+
+ for rank in range(self._num_drones):
+ process = self._mp_ctx.Process(
+ target=CrazyflieWorker.crazyflie_process_worker,
+ args=(
+ rank,
+ self._radio_ids[rank],
+ self._drone_ids[rank],
+ self._drone_channels[rank],
+ self._drone_models[rank],
+ self.drones_pose.pos[rank],
+ self._return_to_start_events[rank],
+ self._return_height_ready_events[rank],
+ self._assigned_return_heights,
+ self._stop_event,
+ self._drone_control_mode[rank],
+ self._init_barrier,
+ self._drone_control_freq[rank],
+ ),
+ name=f"CrazyflieProcess-{rank}",
+ )
+ process.start()
+ self._processes.append(process)
+ logger.debug(f"Spawned process for drone {rank} (PID: {process.pid})")
+
+ logger.debug("Waiting for drones to connect...")
+ while not self._init_barrier.broken:
+ if self._init_barrier.n_waiting == self._num_drones:
+ break
+ time.sleep(0.05)
+
+ def update_poses(self, track_obj: bool = False, drones: bool = False) -> None:
+ """Update gate, obstacle, and/or drone poses from the motion capture system.
+
+ Initializes temporary ROS connectors, reads the current TF poses, writes them
+ into ``self.gates``, ``self.obstacles``, and ``self.drones_pose``, then closes
+ the connectors.
+
+ Args:
+ track_obj: Whether to update gate and obstacle poses.
+ drones: Whether to update drone start poses.
+ """
+ if not track_obj and not drones:
+ return
+
+ ros_connector: ROSConnector | None = None
+ if track_obj:
+ try:
+ tf_names = [f"gate{i}" for i in range(1, self.n_gates + 1)]
+ tf_names += [f"obstacle{i}" for i in range(1, self.n_obstacles + 1)]
+ ros_connector = ROSConnector(tf_names=tf_names, timeout=5.0)
+ for i in range(self.n_gates):
+ self.gates.pos[i] = ros_connector.pos[f"gate{i + 1}"]
+ self.gates.quat[i] = ros_connector.quat[f"gate{i + 1}"]
+ for i in range(self.n_obstacles):
+ self.obstacles.pos[i] = ros_connector.pos[f"obstacle{i + 1}"]
+ self.obstacles.quat[i] = ros_connector.quat[f"obstacle{i + 1}"]
+ finally:
+ if ros_connector:
+ ros_connector.close()
+
+ if drones:
+ ros_connector = None
+ try:
+ ros_connector = ROSConnector(tf_names=self._drone_names, timeout=5.0)
+ for rank, drone_name in enumerate(self._drone_names):
+ self.drones_pose.pos[rank] = ros_connector.pos[drone_name]
+ self.drones_pose.quat[rank] = ros_connector.quat[drone_name]
+ finally:
+ if ros_connector:
+ ros_connector.close()
+
+ def close(self):
+ """Stop all drone subprocesses and close ROS communication."""
+ if self._host_state_pub:
+ self._publish_host_state(host_ready=True, race_started=True, race_finished=True)
+ if self._init_barrier is not None:
+ self._init_barrier.abort()
+ for process, return_event in zip(
+ self._processes, self._return_to_start_events, strict=False
+ ):
+ if process.is_alive() and return_event.is_set():
+ process.join(timeout=15)
+
+ if self._stop_event is not None:
+ self._stop_event.set()
+
+ for process in self._processes:
+ if process.is_alive():
+ process.join(timeout=5)
+ if process.is_alive():
+ process.terminate()
+ process.join(timeout=5)
+ if process.is_alive():
+ process.kill()
+ process.join()
+
+ if self._comm:
+ self._comm.close()
+ logger.info("Host shutdown complete")
+
+ def _return_height_for_finish_order(self, finish_order: int) -> float:
+ """Map finish order to a return height between configured max and min."""
+ if self._num_drones <= 1:
+ return self._return_height_max
+ alpha = finish_order / (self._num_drones - 1)
+ return self._return_height_max + alpha * (self._return_height_min - self._return_height_max)
+
+ def _calibrate_client_clocks(self):
+ """Expose the clock calibration service and wait for all clients to calibrate.
+
+ Creates a ``lsy_drone_racing/calibrate_clock`` service. Clients discover it via
+ ``wait_for_service`` and call it N times to estimate the clock offset using the
+ midpoint method.
+ """
+ logger.info("Starting clock calibration service...")
+
+ def _handler(
+ _: RealCalibrateClock.Request, response: RealCalibrateClock.Response
+ ) -> RealCalibrateClock.Response:
+ response.host_timestamp = time.time()
+ return response
+
+ self._calib_srv = self._comm.node.create_service(
+ RealCalibrateClock, "lsy_drone_racing/calibrate_clock", _handler
+ )
+ time.sleep(1.0)
+ logger.info("Clock calibration complete")
+
+ def _publish_host_state(
+ self,
+ *,
+ host_ready: bool,
+ race_started: bool,
+ race_finished: bool,
+ elapsed_time: float = 0.0,
+ ) -> None:
+ """Publish the current host lifecycle state."""
+ self._host_state_pub.publish(
+ RealHostState(
+ elapsed_time=elapsed_time,
+ timestamp=time.time(),
+ host_ready=host_ready,
+ race_started=race_started,
+ race_finished=race_finished,
+ )
+ )
+
+ def host_main_loop(self, race_update_freq: float = 50.0):
+ """Run the host coordination loop.
+
+ Broadcasts :class:`RealHostState` until all clients signal readiness, then performs
+ clock calibration and releases the drone workers via the init barrier. Enters the
+ race loop broadcasting host state until all clients report stopping.
+ Returns early without error if the init barrier was already broken by a worker failure.
+
+ Args:
+ race_update_freq: Frequency in Hz at which host state is broadcast.
+
+ Raises:
+ TimeoutError: If clients do not become ready within 300 seconds.
+ """
+ if self._init_barrier.broken:
+ return
+ logger.info("Drones connected, waiting for clients...")
+ t_start = time.time()
+ while time.time() - t_start < 300.0:
+ self._publish_host_state(host_ready=True, race_started=False, race_finished=False)
+ if all(self._clients_ready.values()):
+ logger.info("All clients ready")
+ break
+ time.sleep(0.1)
+
+ if not all(self._clients_ready.values()):
+ raise TimeoutError("Timeout waiting for all clients to become ready")
+
+ self._calibrate_client_clocks()
+
+ try:
+ self._init_barrier.wait(timeout=None)
+ except threading.BrokenBarrierError:
+ return
+
+ logger.info("Race started")
+ self._start_time = time.time()
+
+ while True:
+ elapsed_time = time.time() - self._start_time
+ finished = all(self._clients_stopped.values())
+ self._publish_host_state(
+ host_ready=True,
+ race_started=True,
+ race_finished=finished,
+ elapsed_time=elapsed_time,
+ )
+ if finished:
+ logger.info("All clients stopped")
+ break
+ time.sleep(1.0 / race_update_freq)
diff --git a/lsy_drone_racing/utils/__init__.py b/lsy_drone_racing/utils/__init__.py
index 04a4e5ba5..5cb90e2cd 100644
--- a/lsy_drone_racing/utils/__init__.py
+++ b/lsy_drone_racing/utils/__init__.py
@@ -4,6 +4,6 @@
dependency for sim-only scripts.
"""
-from lsy_drone_racing.utils.utils import load_config, load_controller
+from lsy_drone_racing.utils.utils import extract_config_for_rank, load_config, load_controller
-__all__ = ["load_config", "load_controller"]
+__all__ = ["load_config", "load_controller", "extract_config_for_rank"]
diff --git a/lsy_drone_racing/utils/ros_race_comm.py b/lsy_drone_racing/utils/ros_race_comm.py
new file mode 100644
index 000000000..2aaf8ae38
--- /dev/null
+++ b/lsy_drone_racing/utils/ros_race_comm.py
@@ -0,0 +1,131 @@
+"""ROS2-based communication for multi-drone racing coordination.
+
+Uses custom ``lsy_race_msgs`` message types for type-safe pub/sub and the
+``CalibrateClock`` service for clock offset estimation. Each participant
+creates a :class:`RaceCommNode` which spins a
+:class:`~rclpy.executors.SingleThreadedExecutor` in a background daemon thread.
+
+Clock offset estimation uses the midpoint method over N round-trips::
+
+ offset = host_timestamp - (t_send + t_recv) / 2
+
+Clients apply this offset when timestamping every ``ClientState`` message so
+the host observes accurate one-way latency without clock skew.
+"""
+
+from __future__ import annotations
+
+import logging
+import threading
+import time
+from typing import TYPE_CHECKING
+
+import rclpy
+from drone_racing_msgs.srv import RealCalibrateClock
+from rclpy.executors import ExternalShutdownException, SingleThreadedExecutor
+
+if TYPE_CHECKING:
+ from rclpy.client import Client
+
+logger = logging.getLogger(__name__)
+
+
+def _suppress_shutdown_thread_errors():
+ """Install a threading.excepthook that silences expected ROS2 shutdown exceptions.
+
+ Replaces the noisy default traceback for :class:`~rclpy.executors.ExternalShutdownException`
+ and :class:`KeyboardInterrupt` in background spin threads with a single DEBUG log line.
+ Any other uncaught thread exception still goes through the default handler.
+ """
+ _original = threading.excepthook
+
+ def _hook(args: threading.ExceptHookArgs) -> None:
+ if args.exc_type in (ExternalShutdownException, KeyboardInterrupt) or (
+ args.exc_type.__name__ == "RCLError"
+ ):
+ logger.debug(f"Thread '{args.thread.name}' stopped (shutdown)")
+ else:
+ _original(args)
+
+ threading.excepthook = _hook
+
+
+def calibrate_clock(client: Client, n: int = 5, timeout: float = 60.0) -> float:
+ """Estimate clock offset (host_time - client_time) in seconds via N round-trips.
+
+ Blocks until the :class:`~lsy_race_msgs.srv.CalibrateClock` service becomes
+ available or ``timeout`` is reached. Each call records the send and receive
+ times; the offset is estimated as::
+
+ offset = host_timestamp - (t_send + t_recv) / 2
+
+ and averaged over all ``n`` calls.
+
+ Args:
+ client: rclpy service client for the ``CalibrateClock`` service.
+ n: Number of round-trips to average.
+ timeout: Maximum time in seconds to wait for the service to become available.
+
+ Returns:
+ Estimated clock offset in seconds. Add this to ``time.time()`` on the client
+ to get the equivalent host-clock time.
+
+ Raises:
+ TimeoutError: If the service is not available within ``timeout`` seconds.
+ """
+ if not client.wait_for_service(timeout_sec=timeout):
+ raise TimeoutError(f"Calibration service not available after {timeout}s")
+ offsets = []
+ for _ in range(n):
+ t_send = time.time()
+ future = client.call_async(RealCalibrateClock.Request())
+ ready = threading.Event()
+ future.add_done_callback(lambda _: ready.set())
+ if not ready.wait(timeout=timeout):
+ raise TimeoutError("Clock calibration call timed out")
+ t_recv = time.time()
+ offsets.append(future.result().host_timestamp - (t_send + t_recv) / 2)
+ return sum(offsets) / len(offsets)
+
+
+class RaceCommNode:
+ """ROS2 node for race coordination, spinning in a background daemon thread.
+
+ Access the underlying rclpy node via :attr:`node` to create publishers,
+ subscriptions, and services directly. All cleanup is handled by :meth:`close`.
+
+ Args:
+ name: ROS2 node name (must be unique within the process).
+ """
+
+ def __init__(self, name: str):
+ """Initialize and spin the ROS2 node in a background thread."""
+ _suppress_shutdown_thread_errors()
+ self.node = rclpy.create_node(name)
+ self._executor = SingleThreadedExecutor()
+ self._executor.add_node(self.node)
+
+ def _spin():
+ try:
+ self._executor.spin()
+ except (ExternalShutdownException, KeyboardInterrupt):
+ logger.debug(f"RaceCommNode '{name}' spin thread stopped")
+ except Exception as e:
+ if type(e).__name__ == "RCLError":
+ # TODO: RCLError can not be imported
+ # but is raised when the context is already shutdown
+ # while the thread is still spinning
+ # this is stupid workaround
+ logger.debug(f"RaceCommNode '{name}' spin thread stopped (context invalid)")
+ else:
+ raise
+
+ self._thread = threading.Thread(target=_spin, daemon=True, name=f"spin-{name}")
+ self._thread.start()
+ logger.debug(f"RaceCommNode '{name}' started")
+
+ def close(self):
+ """Shut down the executor and destroy the node."""
+ self._executor.shutdown(timeout_sec=1.0)
+ self.node.destroy_node()
+ logger.debug("RaceCommNode closed")
diff --git a/lsy_drone_racing/utils/utils.py b/lsy_drone_racing/utils/utils.py
index 7da405dd6..e11cbf2c4 100644
--- a/lsy_drone_racing/utils/utils.py
+++ b/lsy_drone_racing/utils/utils.py
@@ -2,6 +2,7 @@
from __future__ import annotations
+import copy
import importlib.util
import inspect
import logging
@@ -20,6 +21,23 @@
logger = logging.getLogger(__name__)
+def extract_config_for_rank(config: ConfigDict, rank: int) -> ConfigDict:
+ """Get the configuration for a specific drone rank in a multi-drone setup.
+
+ Args:
+ config: The full configuration dictionary.
+ rank: The rank of the drone.
+
+ Returns:
+ The configuration specific to the given drone rank.
+ """
+ cfg = copy.deepcopy(config)
+ if "kwargs" in config.env:
+ for key, value in config.env.kwargs[rank].items():
+ cfg.env[key] = value
+ return cfg
+
+
def load_controller(path: Path) -> Type[Controller]:
"""Load the controller module from the given path and return the Controller class.
diff --git a/pixi.lock b/pixi.lock
index 8215f60c5..3c4f59e81 100644
--- a/pixi.lock
+++ b/pixi.lock
@@ -73,9 +73,10 @@ environments:
- conda: https://conda.anaconda.org/conda-forge/linux-64/zstd-1.5.7-hb78ec9c_6.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/ca-certificates-2026.5.20-hbd8a1cb_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/kernel-headers_linux-64-4.18.0-he073ed8_9.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/lark-1.3.1-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libgcc-devel_linux-64-13.4.0-hd1d28cc_119.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libstdcxx-devel_linux-64-13.4.0-h6963c3b_119.conda
- - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-26.2-pyhc364b38_0.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-25.0-pyh29332c3_1.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pip-25.3-pyh145f28c_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pygments-2.20.0-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/python_abi-3.13-8_cp313.conda
@@ -537,6 +538,7 @@ environments:
- conda: https://conda.anaconda.org/conda-forge/noarch/importlib_resources-7.1.0-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/iniconfig-2.3.0-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/kernel-headers_linux-64-4.18.0-he073ed8_9.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/lark-1.3.1-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/lark-parser-0.12.0-pyhd8ed1ab_1.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libgcc-devel_linux-64-13.4.0-hd1d28cc_119.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libstdcxx-devel_linux-64-13.4.0-h6963c3b_119.conda
@@ -888,7 +890,8 @@ environments:
- conda: https://prefix.dev/robostack-kilted/linux-64/ros-kilted-zstd-vendor-0.32.0-np2py312h31f38b3_14.conda
- conda: https://prefix.dev/robostack-kilted/linux-64/ros2-distro-mutex-0.12.0-kilted_14.conda
- pypi: .
- - pypi: git+https://github.com/ratheron/crazyflie-lib-python-v2.git?rev=bccd34092c22392b941bbef9bc5a3718b9d27da0#bccd34092c22392b941bbef9bc5a3718b9d27da0
+ - pypi: git+https://github.com/learnsyslab/drone-estimators#4cdf25b6980bdd682b5bdf1a9bb66a9ba9150b01
+ - pypi: git+https://github.com/ratheron/crazyflie-lib-python-v2?rev=bccd34092c22392b941bbef9bc5a3718b9d27da0#bccd34092c22392b941bbef9bc5a3718b9d27da0
- pypi: https://files.pythonhosted.org/packages/01/8e/1e35281b8ab6d5d72ebe9911edcdffa3f36b04ed9d51dec6dd140396e220/scipy-1.17.1-cp312-cp312-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl
- pypi: https://files.pythonhosted.org/packages/05/98/716a473cfb24750858ddd5d14e6527539dd206583a46408d08eeb2844a75/trimesh-4.12.2-py3-none-any.whl
- pypi: https://files.pythonhosted.org/packages/0c/b6/156a8de1e1b47694f0e7de6675866936608d45dc68388fd017d36f8693be/simplejson-4.1.1-cp312-cp312-manylinux1_x86_64.manylinux_2_28_x86_64.manylinux_2_5_x86_64.whl
@@ -949,7 +952,6 @@ environments:
- pypi: https://files.pythonhosted.org/packages/d5/0c/043d5e551459da400957a1395e0febbf771446ff34291afcbe3d8be2a279/fsspec-2026.4.0-py3-none-any.whl
- pypi: https://files.pythonhosted.org/packages/de/e4/1ba6f44e491c4eece978685230dde56b14d51a0365bc1b774ddaa94d14cd/pyopengl-3.1.10-py3-none-any.whl
- pypi: https://files.pythonhosted.org/packages/e5/4c/93d0f85318da65923e4b91c1c2ff03d8a458cbefebe3bc612a6693c7906d/fire-0.7.1-py3-none-any.whl
- - pypi: https://files.pythonhosted.org/packages/e8/60/d660a283ed138cdd5037abf461889ec117b9195d24322c958af187bb7163/drone_estimators-0.1.0b0-py3-none-any.whl
- pypi: https://files.pythonhosted.org/packages/e9/73/fda6a25f3beeb5e49d74330b44092b9e5a547395ccd478d1103ddcbff1fc/gymnasium-1.3.0-py3-none-any.whl
- pypi: https://files.pythonhosted.org/packages/f7/a1/47c08a81760cae84c4a4aa720f3fc1ce3bac6f7aafa5ab82c302d7946f07/jax_cuda12_pjrt-0.10.1-py3-none-manylinux_2_27_x86_64.whl
docs:
@@ -1042,6 +1044,7 @@ environments:
- conda: https://conda.anaconda.org/conda-forge/noarch/importlib-metadata-9.0.0-pyhcf101f3_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/jinja2-3.1.6-pyhcf101f3_1.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/kernel-headers_linux-64-4.18.0-he073ed8_9.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/lark-1.3.1-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libgcc-devel_linux-64-13.4.0-hd1d28cc_119.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libstdcxx-devel_linux-64-13.4.0-h6963c3b_119.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/markdown-3.10.2-pyhcf101f3_0.conda
@@ -1050,7 +1053,7 @@ environments:
- conda: https://conda.anaconda.org/conda-forge/noarch/mkdocs-get-deps-0.2.2-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/mkdocs-material-9.7.6-pyhcf101f3_1.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/mkdocs-material-extensions-1.3.1-pyhd8ed1ab_1.conda
- - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-26.2-pyhc364b38_0.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-25.0-pyh29332c3_1.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/paginate-0.5.7-pyhd8ed1ab_1.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pathspec-1.1.1-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pip-25.3-pyh145f28c_0.conda
@@ -1152,9 +1155,10 @@ environments:
- conda: https://conda.anaconda.org/conda-forge/linux-64/zstd-1.5.7-hb78ec9c_6.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/ca-certificates-2026.5.20-hbd8a1cb_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/kernel-headers_linux-64-4.18.0-he073ed8_9.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/lark-1.3.1-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libgcc-devel_linux-64-13.4.0-hd1d28cc_119.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libstdcxx-devel_linux-64-13.4.0-h6963c3b_119.conda
- - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-26.2-pyhc364b38_0.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-25.0-pyh29332c3_1.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pip-25.3-pyh145f28c_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pygments-2.20.0-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/python_abi-3.13-8_cp313.conda
@@ -1283,9 +1287,10 @@ environments:
- conda: https://conda.anaconda.org/conda-forge/linux-64/zstd-1.5.7-hb78ec9c_6.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/ca-certificates-2026.5.20-hbd8a1cb_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/kernel-headers_linux-64-4.18.0-he073ed8_9.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/lark-1.3.1-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libgcc-devel_linux-64-13.4.0-hd1d28cc_119.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libstdcxx-devel_linux-64-13.4.0-h6963c3b_119.conda
- - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-26.2-pyhc364b38_0.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-25.0-pyh29332c3_1.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pip-25.3-pyh145f28c_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pygments-2.20.0-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/python_abi-3.13-8_cp313.conda
@@ -1431,9 +1436,10 @@ environments:
- conda: https://conda.anaconda.org/conda-forge/noarch/exceptiongroup-1.3.1-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/iniconfig-2.3.0-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/kernel-headers_linux-64-4.18.0-he073ed8_9.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/lark-1.3.1-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libgcc-devel_linux-64-13.4.0-hd1d28cc_119.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libstdcxx-devel_linux-64-13.4.0-h6963c3b_119.conda
- - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-26.2-pyhc364b38_0.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-25.0-pyh29332c3_1.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pip-25.3-pyh145f28c_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pluggy-1.6.0-pyhf9edf01_1.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pygments-2.20.0-pyhd8ed1ab_0.conda
@@ -1580,9 +1586,10 @@ environments:
- conda: https://conda.anaconda.org/conda-forge/linux-64/zstd-1.5.7-hb78ec9c_6.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/ca-certificates-2026.5.20-hbd8a1cb_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/kernel-headers_linux-64-4.18.0-he073ed8_9.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/lark-1.3.1-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libgcc-devel_linux-64-13.4.0-hd1d28cc_119.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libstdcxx-devel_linux-64-13.4.0-h6963c3b_119.conda
- - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-26.2-pyhc364b38_0.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-25.0-pyh29332c3_1.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pip-25.3-pyh145f28c_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pygments-2.20.0-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/python_abi-3.13-8_cp313.conda
@@ -1728,9 +1735,10 @@ environments:
- conda: https://conda.anaconda.org/conda-forge/noarch/exceptiongroup-1.3.1-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/iniconfig-2.3.0-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/kernel-headers_linux-64-4.18.0-he073ed8_9.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/lark-1.3.1-pyhd8ed1ab_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libgcc-devel_linux-64-13.4.0-hd1d28cc_119.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/libstdcxx-devel_linux-64-13.4.0-h6963c3b_119.conda
- - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-26.2-pyhc364b38_0.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-25.0-pyh29332c3_1.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pip-25.3-pyh145f28c_0.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pluggy-1.6.0-pyhf9edf01_1.conda
- conda: https://conda.anaconda.org/conda-forge/noarch/pygments-2.20.0-pyhd8ed1ab_0.conda
@@ -8050,6 +8058,18 @@ packages:
run_exports: {}
size: 1278712
timestamp: 1765578681495
+- conda: https://conda.anaconda.org/conda-forge/noarch/lark-1.3.1-pyhd8ed1ab_0.conda
+ sha256: 49570840fb15f5df5d4b4464db8ee43a6d643031a2bc70ef52120a52e3809699
+ md5: 9b965c999135d43a3d0f7bd7d024e26a
+ depends:
+ - python >=3.10
+ license: MIT
+ license_family: MIT
+ purls:
+ - pkg:pypi/lark?source=hash-mapping
+ run_exports: {}
+ size: 94312
+ timestamp: 1761596921009
- conda: https://conda.anaconda.org/conda-forge/noarch/lark-parser-0.12.0-pyhd8ed1ab_1.conda
sha256: 7f1ad9630a87005a90099ad3ff883ac7a3fe5e85b9eb232d1f8ad0a670059cca
md5: 222dd97cb2d5da1638de5077da60712f
@@ -8235,21 +8255,9 @@ packages:
license_family: APACHE
purls:
- pkg:pypi/packaging?source=hash-mapping
+ run_exports: {}
size: 62477
timestamp: 1745345660407
-- conda: https://conda.anaconda.org/conda-forge/noarch/packaging-26.2-pyhc364b38_0.conda
- sha256: 3906abfb6511a3bb309e39b9b1b7bc38f50a723971de2395489fd1f379255890
- md5: 4c06a92e74452cfa53623a81592e8934
- depends:
- - python >=3.8
- - python
- license: Apache-2.0
- license_family: APACHE
- purls:
- - pkg:pypi/packaging?source=hash-mapping
- run_exports: {}
- size: 91574
- timestamp: 1777103621679
- conda: https://conda.anaconda.org/conda-forge/noarch/paginate-0.5.7-pyhd8ed1ab_1.conda
sha256: f6fef1b43b0d3d92476e1870c08d7b9c229aebab9a0556b073a5e1641cf453bd
md5: c3f35453097faf911fd3f6023fc2ab24
@@ -15874,20 +15882,33 @@ packages:
- toml>=0.10.2
- gymnasium[array-api]>=1.2.0 ; extra == 'sim'
- ml-collections>=1.0 ; extra == 'sim'
- - packaging>=24.0 ; extra == 'sim'
+ - packaging>=24.0,<26 ; extra == 'sim'
- drone-models ; extra == 'sim'
- drone-controllers ; extra == 'sim'
- crazyflow>=0.1.0 ; extra == 'sim'
- jax>=0.7 ; extra == 'sim'
- warp-lang ; extra == 'sim'
- jax[cuda12] ; extra == 'gpu'
- - cflib2 @ git+https://github.com/ratheron/crazyflie-lib-python-v2.git@bccd34092c22392b941bbef9bc5a3718b9d27da0 ; extra == 'deploy'
- - drone-estimators ; extra == 'deploy'
+ - cflib2 @ git+https://github.com/ratheron/crazyflie-lib-python-v2@bccd34092c22392b941bbef9bc5a3718b9d27da0 ; extra == 'deploy'
+ - drone-estimators @ git+https://github.com/learnsyslab/drone-estimators ; extra == 'deploy'
- torch==2.8.0 ; extra == 'rl'
- wandb ; extra == 'rl'
- pygame ; extra == 'gamepad'
requires_python: '>=3.10'
-- pypi: git+https://github.com/ratheron/crazyflie-lib-python-v2.git?rev=bccd34092c22392b941bbef9bc5a3718b9d27da0#bccd34092c22392b941bbef9bc5a3718b9d27da0
+- pypi: git+https://github.com/learnsyslab/drone-estimators#4cdf25b6980bdd682b5bdf1a9bb66a9ba9150b01
+ name: drone-estimators
+ version: 0.1.0b1
+ requires_dist:
+ - jax
+ - numpy>=2.0
+ - scipy>=1.17.0rc1
+ - drone-models>=0.1.0b0
+ - munch
+ - transforms3d
+ - flax
+ - array-api-compat
+ - array-api-extra
+- pypi: git+https://github.com/ratheron/crazyflie-lib-python-v2?rev=bccd34092c22392b941bbef9bc5a3718b9d27da0#bccd34092c22392b941bbef9bc5a3718b9d27da0
name: cflib2
version: 0.1.0
requires_dist:
@@ -17715,25 +17736,6 @@ packages:
- hypothesis<6.136.0 ; extra == 'test'
- levenshtein<=0.27.1 ; extra == 'test'
requires_python: '>=3.7'
-- pypi: https://files.pythonhosted.org/packages/e8/60/d660a283ed138cdd5037abf461889ec117b9195d24322c958af187bb7163/drone_estimators-0.1.0b0-py3-none-any.whl
- name: drone-estimators
- version: 0.1.0b0
- sha256: a1bce4ffb4dc04f6d20876271abe1719972bb7feb16cfed0c0ec5aff2468bcde
- requires_dist:
- - jax
- - numpy>=2.0
- - scipy>=1.17.0rc1
- - drone-models>=0.1.0b0
- - munch
- - transforms3d
- - flax
- - array-api-compat
- - array-api-extra
- - drone-models ; extra == 'dev'
- - pytest>=8.0.0 ; extra == 'test'
- - pytest-cov ; extra == 'test'
- - pytest-timeout ; extra == 'test'
- - torch ; extra == 'test'
- pypi: https://files.pythonhosted.org/packages/e9/73/fda6a25f3beeb5e49d74330b44092b9e5a547395ccd478d1103ddcbff1fc/gymnasium-1.3.0-py3-none-any.whl
name: gymnasium
version: 1.3.0
diff --git a/properdocs.yml b/properdocs.yml
index 19c3967cf..079457ba9 100644
--- a/properdocs.yml
+++ b/properdocs.yml
@@ -63,6 +63,8 @@ nav:
- utils/index.md
- utils.checks: utils/checks.md
- utils.utils: utils/utils.md
+ - Multi Agent:
+ - multi_agent/index.md
plugins:
- search
diff --git a/pyproject.toml b/pyproject.toml
index f0b73189f..cf5d02549 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -18,7 +18,6 @@ classifiers = [
"Intended Audience :: Science/Research",
]
-
########################
# Pip Dependencies
########################
@@ -29,7 +28,7 @@ dependencies = ["fire >= 0.6.0", "numpy", "toml >= 0.10.2"]
sim = [
"gymnasium[array-api] >= 1.2.0",
"ml-collections >= 1.0",
- "packaging >= 24.0",
+ "packaging >= 24.0, <26",
"drone-models",
"drone-controllers",
"crazyflow>=0.1.0",
@@ -41,7 +40,7 @@ gpu = ["jax[cuda12]"]
# option[deploy]: deploy to real drones
deploy = [
"cflib2 @ git+https://github.com/ratheron/crazyflie-lib-python-v2.git@bccd34092c22392b941bbef9bc5a3718b9d27da0",
- "drone-estimators",
+ "drone-estimators @ git+https://github.com/learnsyslab/drone-estimators.git",
]
# option[rl]: train rl policy
rl = ["torch == 2.8.0", "wandb"]
@@ -67,12 +66,14 @@ platforms = ["linux-64"]
[tool.pixi.dependencies]
python = ">=3.11.12,<3.14"
pip = ">=25.0.1,<26"
+packaging = ">=24,<26"
ruff = "*"
# Compilers needed for acados
compilers = ">=1.9.0, <1.11.0"
cmake = "==3.26.0" # Pinned for motion_capture_tracking
make = "==4.4.1" # Pinned for motion_capture_tracking
openblas = ">=0.3.30,<0.4"
+lark = ">=1.3.1" # Pinned to suppress deprecation warning
[tool.pixi.pypi-dependencies]
lsy_drone_racing = { path = ".", editable = true }
@@ -102,15 +103,38 @@ channels = ["https://prefix.dev/robostack-kilted"]
# Build tools
pkg-config = "*"
ninja = "*"
+libboost-devel = ">=1.86,<1.87"
# ROS specific tools
rosdep = "*"
colcon-common-extensions = "*"
# Kilted specific tools
ros-kilted-desktop = "*"
ros2-distro-mutex = "0.12.*" # Pinned for motion_capture_tracking
+ros-kilted-rmw-zenoh-cpp = "*"
[tool.pixi.feature.kilted.activation]
scripts = ["tools/setup_mocap.sh"]
+env = { RMW_IMPLEMENTATION = "rmw_zenoh_cpp", ZENOH_ROUTER_CHECK_ATTEMPTS = "-1" }
+
+[tool.pixi.feature.kilted.tasks.zenoh-router]
+# To use ROS with Zenoh backend, a router is mandatory
+cmd = "ros2 run rmw_zenoh_cpp rmw_zenohd"
+description = "Run the Zenoh router on the host machine"
+
+## feature [deploy]
+[tool.pixi.feature.deploy.tasks.mocap]
+cmd = "ros2 launch motion_capture_tracking launch.py"
+
+[tool.pixi.feature.deploy.tasks.estimator]
+cmd = [
+ "python",
+ "-m",
+ "drone_estimators.ros_nodes.ros2_node",
+ "--drone_name",
+ "{{drone_name}}",
+ "--legacy"
+]
+args = [{ "arg" = "drone_name", "default" = "cf52" }]
## feature [deploy]
[tool.pixi.feature.deploy.dependencies]
@@ -163,6 +187,7 @@ exclude = [
".vscode",
"__pypackages__",
"_build",
+ "acados",
"build",
"dist",
"site-packages",
diff --git a/ros_ws/src/drone_racing_msgs/CMakeLists.txt b/ros_ws/src/drone_racing_msgs/CMakeLists.txt
new file mode 100644
index 000000000..eab8b130f
--- /dev/null
+++ b/ros_ws/src/drone_racing_msgs/CMakeLists.txt
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.11)
+project(drone_racing_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(std_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "msg/Action.msg"
+ "msg/EpisodeEnd.msg"
+ "msg/EpisodeReset.msg"
+ "msg/Observations.msg"
+ "msg/StepResult.msg"
+ "msg/RaceEnd.msg"
+ "msg/RealHostState.msg"
+ "msg/RealClientAction.msg"
+ "srv/RealCalibrateClock.srv"
+ DEPENDENCIES builtin_interfaces std_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ set(ament_cmake_copyright_FOUND TRUE)
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/ros_ws/src/drone_racing_msgs/msg/Action.msg b/ros_ws/src/drone_racing_msgs/msg/Action.msg
new file mode 100644
index 000000000..6aba7f467
--- /dev/null
+++ b/ros_ws/src/drone_racing_msgs/msg/Action.msg
@@ -0,0 +1,4 @@
+int32 episode_id
+int32 step_id
+int32 drone_id
+float32[] action
diff --git a/ros_ws/src/drone_racing_msgs/msg/EpisodeEnd.msg b/ros_ws/src/drone_racing_msgs/msg/EpisodeEnd.msg
new file mode 100644
index 000000000..2eb5c4170
--- /dev/null
+++ b/ros_ws/src/drone_racing_msgs/msg/EpisodeEnd.msg
@@ -0,0 +1,3 @@
+int32 episode_id
+float32 curr_time
+bool finished
diff --git a/ros_ws/src/drone_racing_msgs/msg/EpisodeReset.msg b/ros_ws/src/drone_racing_msgs/msg/EpisodeReset.msg
new file mode 100644
index 000000000..4941677cf
--- /dev/null
+++ b/ros_ws/src/drone_racing_msgs/msg/EpisodeReset.msg
@@ -0,0 +1,12 @@
+int32 episode_id
+
+float32[] pos
+float32[] quat
+float32[] vel
+float32[] ang_vel
+int32[] target_gate
+float32[] gates_pos
+float32[] gates_quat
+bool[] gates_visited
+float32[] obstacles_pos
+bool[] obstacles_visited
diff --git a/ros_ws/src/drone_racing_msgs/msg/Observations.msg b/ros_ws/src/drone_racing_msgs/msg/Observations.msg
new file mode 100644
index 000000000..76bbacccd
--- /dev/null
+++ b/ros_ws/src/drone_racing_msgs/msg/Observations.msg
@@ -0,0 +1,15 @@
+int32 episode_id
+int32 step_id
+
+float32[] pos
+float32[] quat
+float32[] vel
+float32[] ang_vel
+int32[] target_gate
+float32[] gates_pos
+float32[] gates_quat
+bool[] gates_visited
+float32[] obstacles_pos
+bool[] obstacles_visited
+
+
diff --git a/ros_ws/src/drone_racing_msgs/msg/RaceEnd.msg b/ros_ws/src/drone_racing_msgs/msg/RaceEnd.msg
new file mode 100644
index 000000000..fdfa68b73
--- /dev/null
+++ b/ros_ws/src/drone_racing_msgs/msg/RaceEnd.msg
@@ -0,0 +1 @@
+bool race_finished
\ No newline at end of file
diff --git a/ros_ws/src/drone_racing_msgs/msg/RealClientAction.msg b/ros_ws/src/drone_racing_msgs/msg/RealClientAction.msg
new file mode 100644
index 000000000..ddff7fbeb
--- /dev/null
+++ b/ros_ws/src/drone_racing_msgs/msg/RealClientAction.msg
@@ -0,0 +1,6 @@
+int32 drone_rank
+float64[] action
+float64 elapsed_time
+float64 timestamp
+bool client_ready
+bool controller_stopped
diff --git a/ros_ws/src/drone_racing_msgs/msg/RealHostState.msg b/ros_ws/src/drone_racing_msgs/msg/RealHostState.msg
new file mode 100644
index 000000000..ba058f79b
--- /dev/null
+++ b/ros_ws/src/drone_racing_msgs/msg/RealHostState.msg
@@ -0,0 +1,5 @@
+float64 elapsed_time
+float64 timestamp
+bool host_ready
+bool race_started
+bool race_finished
diff --git a/ros_ws/src/drone_racing_msgs/msg/StepResult.msg b/ros_ws/src/drone_racing_msgs/msg/StepResult.msg
new file mode 100644
index 000000000..e4145dc97
--- /dev/null
+++ b/ros_ws/src/drone_racing_msgs/msg/StepResult.msg
@@ -0,0 +1,19 @@
+int32 episode_id
+int32 step_id
+
+float32[] action
+
+float32[] pos
+float32[] quat
+float32[] vel
+float32[] ang_vel
+int32[] target_gate
+float32[] gates_pos
+float32[] gates_quat
+bool[] gates_visited
+float32[] obstacles_pos
+bool[] obstacles_visited
+
+float32[] reward
+bool[] terminated
+bool[] truncated
diff --git a/ros_ws/src/drone_racing_msgs/package.xml b/ros_ws/src/drone_racing_msgs/package.xml
new file mode 100644
index 000000000..82632dea1
--- /dev/null
+++ b/ros_ws/src/drone_racing_msgs/package.xml
@@ -0,0 +1,25 @@
+
+
+
+