Multi-drone real race support via host-client ROS2 architecture

.gitignore

@@ -20,5 +20,9 @@ site/
 # pixi environments
 .pixi/*
 !.pixi/config.toml
-ros_ws/
-acados/
+# ROS2 workspace build artifacts
+ros_ws/src/motion_capture_tracking
+ros_ws/build
+ros_ws/install
+ros_ws/log
+acados/**

config/multi_level0.toml

@@ -1,8 +1,8 @@
 # Level 0
 
-# | Evaluation Scenario | Rand. Inertial Properties | Randomized Obstacles, Gates | Rand. Between Episodes | Notes             |
-# | :-----------------: | :-----------------------: | :-------------------------: | :--------------------: | :---------------: |
-# | `level0.toml`       | *No*                      | *No*                        | *No*                   | Perfect knowledge |
+# | Evaluation Scenario | Randomized Inertial Properties | Randomized Obstacles, Gates | Randomized Tracks | Notes             |
+# | :-----------------: | :----------------------------: | :-------------------------: | :---------------: | :---------------: |
+# | `level0.toml`       | *No*                           | *No*                        | *No*              | Perfect knowledge |
 [[controller]]
 file = "attitude_controller_multi.py" # Put your controller file name here. Specifying a controller as argument to scripts will override this setting. Controllers are located in `lsy_drone_racing/control/`
 [[controller]]  # Add the controller files for each drone
@@ -18,11 +18,13 @@ check_drone_start_pos = true
 real_track_objects = true
 
 [[deploy.drones]]
+radio = 0
 id = 10
 channel = 100
 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
 
 [[deploy.drones]]
+radio = 0
 id = 20
 channel = 100
 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

config/multi_level2.toml

@@ -1,8 +1,8 @@
-# Level 0
+# Level 2
 
-# | Evaluation Scenario | Rand. Inertial Properties | Randomized Obstacles, Gates | Rand. Between Episodes | Notes             |
-# | :-----------------: | :-----------------------: | :-------------------------: | :--------------------: | :---------------: |
-# | `level0.toml`       | *No*                      | *No*                        | *No*                   | Perfect knowledge |
+# | Evaluation Scenario | Randomized Inertial Properties | Randomized Obstacles, Gates | Randomized Tracks | Notes                 |
+# | :-----------------: | :----------------------------: | :-------------------------: | :---------------: | :-------------------: |
+# | `level2.toml`       | *Yes*                          | *Yes*                       | *No*              | Learning, re-planning |
 [[controller]]
 file = "attitude_controller_multi.py" # Put your controller file name here. Specifying a controller as argument to scripts will override this setting. Controllers are located in `lsy_drone_racing/control/`
 [[controller]]
@@ -18,11 +18,13 @@ check_drone_start_pos = true
 real_track_objects = true
 
 [[deploy.drones]]
+radio = 0
 id = 10
 channel = 100
 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
 
 [[deploy.drones]]
+radio = 0
 id = 20
 channel = 100
 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
@@ -83,7 +85,7 @@ rpy = [0.0, 0.0, 0.0]
 # Obstacle height: 1.52m + reflective marker on top. Height is measured from the ground to the top of the obstacle.
 # Obstacles are cylinders with a diameter of 0.03m.
 [[env.track.obstacles]]
-pos = [-1.5, 0.55, 0.01]
+pos = [0.0, 0.75, 1.55]
 [[env.track.obstacles]]
 pos = [1.0, 0.25, 1.55]
 [[env.track.obstacles]]
@@ -92,12 +94,12 @@ pos = [-1.5, -0.25, 1.55]
 pos = [-0.5, -0.75, 1.55]
 
 [[env.track.drones]]
-pos = [-1.5, 0.95, 0.01]
+pos = [-1.5, 0.55, 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, 0.95, 0.01]
 rpy = [0, 0, 0]
 vel = [0, 0, 0]
 ang_vel = [0, 0, 0]

config/multi_test.toml

@@ -0,0 +1,168 @@
+# Level 2
+
+# | Evaluation Scenario | Randomized Inertial Properties | Randomized Obstacles, Gates | Randomized Tracks | Notes                 |
+# | :-----------------: | :----------------------------: | :-------------------------: | :---------------: | :-------------------: |
+# | `level2.toml`       | *Yes*                          | *Yes*                       | *No*              | Learning, re-planning |
+[[controller]]
+file = "testing_race_slow.py" # Put your controller file name here. Specifying a controller as argument to scripts will override this setting. Controllers are located in `lsy_drone_racing/control/`
+[[controller]]
+file = "attitude_mpc_multi.py"
+
+[deploy]
+### Settings only relevant for deployment
+# Whether to check if gate and obstacle positions observed by vicon are within the limits defined down below.
+check_race_track = false
+# Whether to check if the drone start position is within the limits specified down below.
+check_drone_start_pos = false
+# Lets you practice your controller without putting up gates & obstacles, assumes nominal positions given below.
+real_track_objects = false
+
+# Specify the minimal and maximal height for the return controller 
+return_height_min = 1.5
+return_height_max = 1.75
+
+[[deploy.drones]]
+radio = 0
+id = 52
+channel = 80
+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
+
+[[deploy.drones]]
+radio = 1
+id = 20
+channel = 100
+drone_model = "cf21B_500"  
+
+
+[sim]
+# Physics options:
+# "first_principles": first_principles (physics based) model
+# "so_rpy": Simplified identified model.
+# "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
+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
+azimuth = 180.0
+elevation = -25.0
+lookat = [0.0, 0.0, 0.0]
+
+
+[env]
+id = "MultiDroneRacing-v0"
+seed = -1               # -1 for random seeds or a non-zero integer for reproducibility
+[[env.kwargs]]
+freq = 50               # Frequency of the environment's step function, in Hz
+sensor_range = 0.7      # Range at which the exact location of gates and obstacles become visible to the drone. Objects that are not in the drone's sensor range report their nominal position.
+control_mode = "attitude"  # Control mode of the environment. Can be either "state" or "attitude"
+[[env.kwargs]]
+freq = 50               # Frequency of the environment's step function, in Hz
+sensor_range = 0.7      # Range at which the exact location of gates and obstacles become visible to the drone. Objects that are not in the drone's sensor range report their nominal position.
+control_mode = "attitude"  # Control mode of the environment. Can be either "state" or "attitude"
+
+
+[env.track]
+# Full track radomization. When activated, the position (x & y) and orientation (yaw) 
+# of the below given gates, obstacles and drones is randomized inside the safety limits. 
+# Note: The order and therefore also height is not randomized!
+randomize = false
+
+# Tall gates: 1.195m height. Short gates: 0.695m height. Height is measured from the ground to the center of the gate.
+# Gates are square. Gates are 0.72m wide (outer dimensions of the frame) with a 0.4m wide opening.
+[[env.track.gates]]
+pos = [0.5 , 0.25, 0.7]
+rpy = [0.0, 0.0, -0.78]
+[[env.track.gates]]
+pos = [1.05, 0.75, 1.2]
+rpy = [0.0, 0.0, 2.35]
+[[env.track.gates]]
+pos = [-1.0, -0.25, 0.7]
+rpy = [0.0, 0.0, 3.14]
+[[env.track.gates]]
+pos = [0.0, -0.75, 1.2]
+rpy = [0.0, 0.0, 0.0]
+
+# Obstacle height: 1.52m + reflective marker on top. Height is measured from the ground to the top of the obstacle.
+# Obstacles are cylinders with a diameter of 0.03m.
+[[env.track.obstacles]]
+pos = [0.0, 0.75, 1.55]
+[[env.track.obstacles]]
+pos = [1.0, 0.25, 1.55]
+[[env.track.obstacles]]
+pos = [-1.5, -0.25, 1.55]
+[[env.track.obstacles]]
+pos = [-0.5, -0.75, 1.55]
+
+[[env.track.drones]]
+pos = [-1.5, 0.50, 0.01]
+rpy = [0, 0, 0]
+vel = [0, 0, 0]
+ang_vel = [0, 0, 0]
+[[env.track.drones]]
+pos = [-1.5, 1.0, 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]
+
+# We intentionally reduce the disturbances for the multi-drone scenario, so students can focus on the multi-drone aspect for now.
+[env.disturbances.action]
+fn = "normal"
+scale = 0.001
+
+[env.disturbances.dynamics]
+fn = "uniform"
+[env.disturbances.dynamics.kwargs]
+minval = [-0.02, -0.02, -0.02]
+maxval = [0.02, 0.02, 0.02]
+
+[env.randomizations.drone_pos]
+fn = "uniform"
+[env.randomizations.drone_pos.kwargs]
+minval = [-0.02, -0.02, 0.0]
+maxval = [0.02, 0.02, 0.02]
+
+[env.randomizations.drone_rpy]
+fn = "uniform"
+[env.randomizations.drone_rpy.kwargs]
+minval = [-0.1, -0.1, -0.1]
+maxval = [0.1, 0.1, 0.1]
+
+[env.randomizations.drone_mass]
+fn = "uniform"
+[env.randomizations.drone_mass.kwargs]
+minval = -0.002
+maxval = 0.002
+
+[env.randomizations.drone_inertia]
+fn = "uniform"
+[env.randomizations.drone_inertia.kwargs]
+minval = [-0.000001, -0.000001, -0.000001]
+maxval = [0.000001, 0.000001, 0.000001]
+
+[env.randomizations.gate_pos]
+fn = "uniform"
+[env.randomizations.gate_pos.kwargs]
+minval = [-0.02, -0.02, -0.01]
+maxval = [0.02, 0.02, 0.01]
+
+[env.randomizations.gate_rpy]
+fn = "uniform"
+[env.randomizations.gate_rpy.kwargs]
+minval = [-0.01, -0.01, -0.01]
+maxval = [0.01, 0.01, 0.01]
+
+[env.randomizations.obstacle_pos]
+fn = "uniform"
+[env.randomizations.obstacle_pos.kwargs]
+minval = [-0.02, -0.02, -0.01]
+maxval = [0.02, 0.02, 0.01]