Adding Multidrone Deployment Feature with Zenoh-ROS2

.gitignore

@@ -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/

config/multi_level0.toml

@@ -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
@@ -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]

config/multi_level2.toml

@@ -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
@@ -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"

lsy_drone_racing/control/attitude_controller_multi.py

@@ -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)

lsy_drone_racing/control/attitude_mpc.py

@@ -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)
 

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_env_client:RealMultiDroneRaceEnvClient",
+    disable_env_checker=True,
+)

lsy_drone_racing/envs/real_race_env.py

@@ -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}"]