Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
55 commits
Select commit Hold shift + click to select a range
c6daa85
Fix cam config and add pngs (#62)
ratheron Dec 19, 2025
fe35a54
Fix UnboundError when trying to close an uninitialized ROSConnector (…
N0OBSTUDENT Dec 22, 2025
ae6fedd
Merge remote-tracking branch 'origin/main' into dev
N0OBSTUDENT Jan 22, 2026
674f981
Protptype for multi drone racing using a Barrier object to sync start
N0OBSTUDENT Jan 27, 2026
21d7b1e
Fixed dimension problem when trying to intialize a controller with mu…
N0OBSTUDENT Jan 27, 2026
2d59f68
Vibe coded 1st version of multi drone racing using Zenoh
N0OBSTUDENT Mar 2, 2026
74e8d27
Added real race client
N0OBSTUDENT Mar 3, 2026
5079284
Fixed all bugs to make it run
N0OBSTUDENT Mar 3, 2026
5764cad
Added latency calibration
N0OBSTUDENT Mar 3, 2026
f12bbf1
Make unknown observation nan instead of zero
N0OBSTUDENT Mar 3, 2026
6f4c80d
Merge branch 'zenoh_multidrone' of https://github.com/N0OBSTUDENT/lsy…
N0OBSTUDENT Mar 3, 2026
1e373f6
Switched to time.time() from perf_counter()
N0OBSTUDENT Mar 3, 2026
756c5aa
Made it work
N0OBSTUDENT Mar 17, 2026
b87f41a
Added rank to other controllers
N0OBSTUDENT Mar 17, 2026
df93e96
Made a demo for different controller, different frequency in differen…
N0OBSTUDENT Mar 17, 2026
d7f6654
Clean up codes
N0OBSTUDENT Mar 17, 2026
22f5aa3
Removed raw-zenoh and takeoff barrier related code
N0OBSTUDENT Mar 27, 2026
7d30658
Edit host, client and comm node code to make it look neater
N0OBSTUDENT Mar 27, 2026
4355b63
Resolve naming issues
N0OBSTUDENT Mar 27, 2026
c869263
Split the multi and single controllers
N0OBSTUDENT Mar 27, 2026
b4b790e
Pass ruff checks
N0OBSTUDENT Mar 27, 2026
03222bc
Added ROS2 package
N0OBSTUDENT Mar 27, 2026
36b925b
Removed take-off barrier configuration
N0OBSTUDENT Mar 27, 2026
8e310d7
Switch power cycle timeout to 2 second
N0OBSTUDENT Mar 28, 2026
9fe98bd
Restored real_race_env to latest commit of branch dev
N0OBSTUDENT Mar 28, 2026
ddcc4e4
Restore the controllers
N0OBSTUDENT Mar 28, 2026
4f2ae07
Close ros connector upon closing the client
N0OBSTUDENT Mar 28, 2026
029b2c5
Restore utils.py
N0OBSTUDENT Mar 28, 2026
f16ba56
Fixed the default config name in deploy_client.py
N0OBSTUDENT Mar 28, 2026
c3ba63c
Merge branch 'dev' into zenoh_ros2_multidrone
N0OBSTUDENT Mar 28, 2026
3c30975
-Resolve the pixi lock file again to make it compatible with later co…
N0OBSTUDENT Mar 28, 2026
39d8c46
Added gate pose update to clients
N0OBSTUDENT Mar 28, 2026
e4757da
Remove duplicate entreis in level config
N0OBSTUDENT Mar 28, 2026
07f0b69
Reorganized the functions.
N0OBSTUDENT Mar 28, 2026
46212bc
Organize imports, docstring and output format to pass ruff check
N0OBSTUDENT Mar 28, 2026
86dfcbf
Restored attitude rl to latest commit in dev and introduce wrapped at…
N0OBSTUDENT Mar 28, 2026
fec0381
Pass ruff check
N0OBSTUDENT Mar 28, 2026
fc5fa94
Replace events with barrier.
N0OBSTUDENT Apr 21, 2026
767a622
keep only one example for multi-drone racing wrapping
N0OBSTUDENT Apr 26, 2026
0940e1c
Modified ros2 messages.
N0OBSTUDENT Apr 26, 2026
4a810db
Made a temporary repository to test the message
N0OBSTUDENT Apr 26, 2026
78c2965
Reused the EnvData from real_race_env.py
N0OBSTUDENT Apr 26, 2026
6fdd078
Merge the ROS connectors
N0OBSTUDENT Apr 26, 2026
c895bee
Fixed wrong docstring
N0OBSTUDENT Apr 26, 2026
eccd03c
Changed drone_rank check to assert, removed redundant deploy variable
N0OBSTUDENT Apr 26, 2026
9142d42
docstring in attitude_controller_multi.py is shorter now
N0OBSTUDENT Apr 26, 2026
287dab3
Fixed one ignored naming change
N0OBSTUDENT Apr 26, 2026
a3f5b94
Avoided the getter function naming
N0OBSTUDENT Apr 26, 2026
bf96661
Spreaded out the safety limit bound condition
N0OBSTUDENT Apr 26, 2026
a8dcf25
Removed the 0.1sec waiting and bare exception at shutdown of client
N0OBSTUDENT Apr 27, 2026
20919df
Split the ConfigDict argument in RealRaceHost
N0OBSTUDENT Apr 27, 2026
b671a74
Fixed bad docstring
N0OBSTUDENT Apr 27, 2026
bbd9c31
Replace the busy-wait loop with event blocking the thread
N0OBSTUDENT Apr 27, 2026
2ed0147
Remove the property and make the node externally accessible
N0OBSTUDENT Apr 27, 2026
2458860
Remove unnecessary imports
N0OBSTUDENT Apr 27, 2026
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 5 additions & 1 deletion .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -17,5 +17,9 @@ c_generated_code/
# pixi environments
.pixi/*
!.pixi/config.toml
ros_ws/
# ROS2 workspace build artifacts
ros_ws/build
ros_ws/install
ros_ws/log
ros_ws/src/motion_capture_tracking
acados/
9 changes: 5 additions & 4 deletions config/multi_level0.toml
Original file line number Diff line number Diff line change
Expand Up @@ -35,10 +35,11 @@ drone_model = "cf21B_500" # Model of the drone, i.e., cf2
# "so_rpy_rotor": Simplified identified model with rotor dynamics.
# "so_rpy_rotor_drag": Simplified identified model with rotor dynamics and drag.
physics = "first_principles"
drone_model = "cf21B_500" # Model of the drone, i.e., cf2x_L250, cf2x_P250, cf2x_T500, cf21B_500. For this course, we use cf21B_500
drone_model = "cf21B_500" # Model of the drone, i.e., cf2x_L250, cf2x_P250, cf2x_T500, cf21B_500. For this course, we use cf21B_500
camera_view = [5.0, 180.0, -25.0, 0.0, 0.0, 0.0] # Camera view [distance, azimuth, elevation, lookat_x, lookat_y, lookat_z]
attitude_freq = 500 # Controller frequency, in Hz. This frequency is used to simulate the onboard controller, NOT for the environment's step function
render = false # Enable/disable PyBullet's GUI
freq = 500 # Simulation frequency, in Hz
attitude_freq = 500 # Controller frequency, in Hz. This frequency is used to simulate the onboard controller, NOT for the environment's step function
render = true # Enable/disable Crazyflow's GUI
camera = -1 # camera id or name, i.e., drone cam ids start at 0, cam names are "fpv_cam:0" or "track_cam:0", where the number is the drone id
[[sim.cam_config]] # only used if camera == -1, i.e., world view is activated
distance = 5.0
Expand Down Expand Up @@ -97,7 +98,7 @@ rpy = [0, 0, 0]
vel = [0, 0, 0]
ang_vel = [0, 0, 0]
[[env.track.drones]]
pos = [-1.3, 0.75, 0.01]
pos = [-1.5, 0.5, 0.01]
rpy = [0, 0, 0]
vel = [0, 0, 0]
ang_vel = [0, 0, 0]
Expand Down
10 changes: 5 additions & 5 deletions config/multi_level2.toml
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ check_race_track = true
# Whether to check if the drone start position is within the limits specified down below.
check_drone_start_pos = true
# Lets you practice your controller without putting up gates & obstacles, assumes nominal positions given below.
real_track_objects = true
real_track_objects = false

[[deploy.drones]]
id = 10
Expand Down Expand Up @@ -92,20 +92,20 @@ pos = [-1.5, -0.25, 1.55]
pos = [-0.5, -0.75, 1.55]

[[env.track.drones]]
pos = [-1.5, 0.75, 0.01]
pos = [-1.5, 0.5, 0.01]
rpy = [0, 0, 0]
vel = [0, 0, 0]
ang_vel = [0, 0, 0]
[[env.track.drones]]
pos = [-1.3, 0.75, 0.01]
pos = [-1.5, 1.5, 0.01]
rpy = [0, 0, 0]
vel = [0, 0, 0]
ang_vel = [0, 0, 0]

# If the drones exceed those bounds in real, the run will be stopped and the drone will safely be returned to the starting position
[env.track.safety_limits]
pos_limit_low = [-2.5, -1.5, -1e-3]
pos_limit_high = [2.5, 1.5, 2.0]
pos_limit_low = [-2.5, -1.5, -1.0]
pos_limit_high = [2.5, 2.0, 2.0]

[env.disturbances.action]
fn = "normal"
Expand Down
59 changes: 59 additions & 0 deletions lsy_drone_racing/control/attitude_controller_multi.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
"""This module implements a wrapper for the attitude controller for a multi-agent environment.

It extracts the relevant information for the current agent from the observation,
and passes it to the single-agent attitude controller.
"""

from __future__ import annotations # Python 3.10 type hints

from typing import TYPE_CHECKING

from lsy_drone_racing.control.attitude_controller import (
AttitudeController as SingleAttitudeController,
)

if TYPE_CHECKING:
import numpy as np
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.
"""
super().__init__(obs, info, config)
self.rank = info["rank"]

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.
"""
assert obs["pos"].ndim == 2, (
f"Observation should have 2 dimensions but now it has {obs['pos'].ndim} dimensions. "
"Are you sure you are running the multi-agent environment?"
)
obs = {
"pos": obs["pos"][self.rank],
"vel": obs["vel"][self.rank],
"quat": obs["quat"][self.rank],
"ang_vel": obs["ang_vel"][self.rank],
}
return super().compute_control(obs, info)
6 changes: 3 additions & 3 deletions lsy_drone_racing/control/attitude_mpc.py
Original file line number Diff line number Diff line change
Expand Up @@ -243,9 +243,9 @@ def compute_control(
self._finished = True

# Setting initial state
obs["rpy"] = R.from_quat(obs["quat"]).as_euler("xyz")
obs["drpy"] = ang_vel2rpy_rates(obs["quat"], obs["ang_vel"])
x0 = np.concatenate((obs["pos"], obs["rpy"], obs["vel"], obs["drpy"]))
rpy = R.from_quat(obs["quat"]).as_euler("xyz")
drpy = ang_vel2rpy_rates(obs["quat"], obs["ang_vel"])
x0 = np.concatenate((obs["pos"], rpy, obs["vel"], drpy))
self._acados_ocp_solver.set(0, "lbx", x0)
self._acados_ocp_solver.set(0, "ubx", x0)

Expand Down
6 changes: 6 additions & 0 deletions lsy_drone_racing/envs/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -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_env_client:RealMultiDroneRaceEnvClient",
disable_env_checker=True,
)
4 changes: 3 additions & 1 deletion lsy_drone_racing/envs/real_race_env.py
Original file line number Diff line number Diff line change
Expand Up @@ -297,11 +297,13 @@ def _update_track_poses(self):
"""Update the track poses from the motion capture system."""
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 = None
try:
ros_connector = ROSConnector(tf_names=tf_names, timeout=10.0)
pos, quat = ros_connector.pos, ros_connector.quat
finally:
ros_connector.close()
if ros_connector is not None:
ros_connector.close()
try:
for i in range(self.n_gates):
self.gates.pos[i, ...] = pos[f"gate{i + 1}"]
Expand Down
Loading