Automated multi-bf SITL creation

.gitignore

@@ -12,6 +12,9 @@ tmp/
 # Learning results
 experiments/learning/results/save-*
 
+# Betaflight
+betaflight_sitl/
+
 # macOS users
 .DS_Store
 

README.md

@@ -54,7 +54,7 @@ In another terminal, run the example
 ```sh
 conda activate drones
 cd gym_pybullet_drones/examples/
-python3 beta.py # also check the steps in the file's docstrings
+python3 beta.py --num_drones 1 # also check the steps in the file's docstrings to use multiple drones
 ```
 
 ## Troubleshooting

gym_pybullet_drones/assets/clone_bfs.sh

@@ -3,63 +3,72 @@
 # Clone, edit, build, configure, multiple Betaflight SITL executables
 
 # Check for the correct number of command-line arguments
-if [ "$#" -ne 2 ]; then
-    echo "Usage: $0 <gym_pybullet_drones full path> <num_iterations>"
+if [ "$#" -ne 1 ]; then
+    echo "Usage: $0 <desired_max_num_drones>"
     exit 1
 fi
 
 # Extract command-line arguments
-gpd_base_path="$1"
-num_iterations="$2"
+desired_max_num_drones="$1"
 
-# Create directory along gym-pybullet-donres
-cd $gpd_base_path
-cd ..
-mkdir betaflights/
-cd betaflights/
+# Create gitignored directory in gym-pybullet-donres
+SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
+cd $SCRIPT_DIR
+cd ../../
+mkdir betaflight_sitl/
+cd betaflight_sitl/
 
-pattern0="delayMicroseconds_real(50); // max rate 20kHz"
-pattern1="#define PORT_PWM_RAW    9001    // Out"
-pattern2="#define PORT_PWM        9002    // Out"
-pattern3="#define PORT_STATE      9003    // In"
-pattern4="#define PORT_RC         9004    // In"
-# pattern5="#define BASE_PORT 5760"
-pattern6="ret = udpInit(&stateLink, NULL, 9003, true);"
+# Step 1: Clone and open betaflight's source:
+git clone https://github.com/betaflight/betaflight temp/
 
-replacement0="// delayMicroseconds_real(50); // max rate 20kHz"
-replacement6="ret = udpInit(&stateLink, NULL, PORT_STATE, true);"
 
-for ((i = 1; i <= num_iterations; i++)); do
+# Step 2: Comment out line `delayMicroseconds_real(50); // max rate 20kHz` 
+# (https://github.com/betaflight/betaflight/blob/master/src/main/main.c#L52) 
+# from Betaflight's `SIMULATOR_BUILD`
+cd temp/
+sed -i "s/delayMicroseconds_real(50);/\/\/delayMicroseconds_real(50);/g" ./src/main/main.c
+sed -i "s/ret = udpInit(\&stateLink, NULL, 9003, true);/\/\/ret = udpInit(\&stateLink, NULL, PORT_STATE, true);/g" ./src/main/target/SITL/sitl.c
+sed -i "s/printf(\"start UDP server.../\/\/printf(\"start UDP server.../g" ./src/main/target/SITL/sitl.c
 
-    # Clone
-    git clone https://github.com/betaflight/betaflight "bf${i}"
-    cd "bf${i}/"
+# Prepare
+make arm_sdk_install 
 
-    # Edit
-    sed -i "s/$pattern0/$replacement0/g" ./src/main/target/SITL/sitl.c
+cd ..
 
-    replacement1="#define PORT_PWM_RAW    90${i}1    // Out"
-    sed -i "s/$pattern1/$replacement1/g" ./src/main/target/SITL/sitl.c
+pattern1="PORT_PWM_RAW    9001"
+pattern2="PORT_PWM        9002"
+pattern3="PORT_STATE      9003"
+pattern4="PORT_RC         9004"
+# pattern5="BASE_PORT 5760"
 
-    replacement2="#define PORT_PWM    90${i}2    // Out"
-    sed -i "s/$pattern2/$replacement2/g" ./src/main/target/SITL/sitl.c
+for ((i = 0; i < desired_max_num_drones; i++)); do
 
-    replacement3="#define PORT_STATE    90${i}3    // In"
-    sed -i "s/$pattern3/$replacement3/g" ./src/main/target/SITL/sitl.c
+    # Copy
+    cp -r temp/ "bf${i}/"
+    cd "bf${i}/"
 
-    replacement4="#define PORT_PWPORT_RCM_RAW    90${i}4    // In"
+    # Step 3: Change the UDP ports used by each Betaflight SITL instancet
+    replacement1="PORT_PWM_RAW    90${i}1"
+    sed -i "s/$pattern1/$replacement1/g" ./src/main/target/SITL/sitl.c
+    replacement2="PORT_PWM    90${i}2"
+    sed -i "s/$pattern2/$replacement2/g" ./src/main/target/SITL/sitl.c
+    replacement3="PORT_STATE    90${i}3"
+    sed -i "s/$pattern3/$replacement3/g" ./src/main/target/SITL/sitl.c
+    replacement4="PORT_RC    90${i}4"
     sed -i "s/$pattern4/$replacement4/g" ./src/main/target/SITL/sitl.c
-
-    # replacement5="#define BASE_PORT 57${i}0"
+    # replacement5="BASE_PORT 57${i}0"
     # sed -i "s/$pattern5/$replacement5/g" ./src/main/drivers/serial_tcp.c
 
-    sed -i "s/$pattern6/$replacement6/g" ./src/main/target/SITL/sitl.c
-
     # Build
-    make arm_sdk_install 
-    make TARGET=SITL 
+    make TARGET=SITL
+
+    cd ..
+
+done
+
+for ((i = 0; i < desired_max_num_drones; i++)); do
 
-    # Copy configured memory
-    cp "${gpd_base_path}/gym_pybullet_drones/assets/eeprom.bin" .
+    # Step 4: Copy over the configured `eeprom.bin` file from folder 
+    cp "${SCRIPT_DIR}/eeprom.bin" "bf${i}/"
 
 done

gym_pybullet_drones/examples/beta.py

@@ -2,29 +2,23 @@
 
 Setup
 -----
-Step 1: Clone and open betaflight's source:
-    $ git clone https://github.com/betaflight/betaflight
-    $ cd betaflight/
-    $ code ./src/main/main.c 
-
-Step 2: Comment out line `delayMicroseconds_real(50); // max rate 20kHz`
-    (https://github.com/betaflight/betaflight/blob/master/src/main/main.c#L52)
-    from Betaflight's `SIMULATOR_BUILD` and compile:
-    $ cd betaflight/
-    $ make arm_sdk_install 
-    $ make TARGET=SITL 
-
-Step 3: Copy over the configured `eeprom.bin` file from folder 
-    `gym-pybullet-drones/gym_pybullet_drones/assets/` to BF's main folder `betaflight/`
-    $ cp ~/gym-pybullet-drones/gym_pybullet_drones/assets/eeprom.bin ~/betaflight/
+Use script `gym_pybullet_drones/assets/clone_bfs.sh` to create
+executables for as many drones as needed (e.g. 2):
+    $ ./gym_pybullet_drones/assets/clone_bfs.sh 2
 
 Example
 -------
-In one terminal run the SITL Betaflight:
+Run as many SITL Betaflight as drones in the simulation 
+in separate terminals (navigate the each `bf0`, `bf1`, etc. folder first):
 
-    $ cd betaflight/
+    $ cd betaflights/bf0/
     $ ./obj/main/betaflight_SITL.elf
 
+    $ cd betaflights/bf1/
+    $ ./obj/main/betaflight_SITL.elf
+
+    $ ..
+
 In a separate  terminal, run:
 
     $ cd gym-pybullet-drones/gym_pybullet_drones/examples/
@@ -54,12 +48,11 @@
 DEFAULT_CONTROL_FREQ_HZ = 500
 DEFAULT_DURATION_SEC = 20
 DEFAULT_OUTPUT_FOLDER = 'results'
-NUM_DRONES = 2
-INIT_XYZ = np.array([[.3*i, .3*i, .1] for i in range(1,NUM_DRONES+1)])
-INIT_RPY = np.array([[.0, .0, .0] for _ in range(NUM_DRONES)])
+DEFAULT_NUM_DRONES = 2
 
 def run(
         drone=DEFAULT_DRONES,
+        num_drones=DEFAULT_NUM_DRONES,
         physics=DEFAULT_PHYSICS,
         gui=DEFAULT_GUI,
         plot=DEFAULT_PLOT,
@@ -70,8 +63,10 @@ def run(
         output_folder=DEFAULT_OUTPUT_FOLDER,
         ):
     #### Create the environment with or without video capture ##
+    INIT_XYZ = np.array([[.3*i, .3*i, .1] for i in range(1,num_drones+1)])
+    INIT_RPY = np.array([[.0, .0, .0] for _ in range(num_drones)])
     env = BetaAviary(drone_model=drone,
-                        num_drones=NUM_DRONES,
+                        num_drones=num_drones,
                         initial_xyzs=INIT_XYZ,
                         initial_rpys=INIT_RPY,
                         physics=physics,
@@ -88,7 +83,7 @@ def run(
 
     #### Initialize the logger #################################
     logger = Logger(logging_freq_hz=control_freq_hz,
-                    num_drones=NUM_DRONES,
+                    num_drones=num_drones,
                     output_folder=output_folder,
                     )
 
@@ -121,7 +116,7 @@ def run(
                 float(row["v_z"]),
             ]),
         } for row in csv_reader]))
-    action = np.zeros((NUM_DRONES,4))
+    action = np.zeros((num_drones,4))
     ARM_TIME = 1.
     TRAJ_TIME = 1.5
     START = time.time()
@@ -131,7 +126,7 @@ def run(
         obs, reward, terminated, truncated, info = env.step(action, i)
 
         if t > env.TRAJ_TIME:
-            for j in range(NUM_DRONES):
+            for j in range(num_drones):
                 try:
                     target = next(trajectory1) if j%2 == 0 else next(trajectory2)
                     action[j,:] = ctrl.computeControlFromState(control_timestep=env.CTRL_TIMESTEP,
@@ -144,7 +139,7 @@ def run(
 
 
         #### Log the simulation ####################################
-        for j in range(NUM_DRONES):
+        for j in range(num_drones):
             logger.log(drone=j,
                         timestamp=i/env.CTRL_FREQ,
                         state=obs[j]
@@ -173,6 +168,7 @@ def run(
     #### Define and parse (optional) arguments for the script ##
     parser = argparse.ArgumentParser(description='Test flight script using SITL Betaflight')
     parser.add_argument('--drone',              default=DEFAULT_DRONES,     type=DroneModel,    help='Drone model (default: BETA)', metavar='', choices=DroneModel)
+    parser.add_argument('--num_drones',         default=DEFAULT_NUM_DRONES,          type=int,           help='Number of drones (default: 3)', metavar='')
     parser.add_argument('--physics',            default=DEFAULT_PHYSICS,      type=Physics,       help='Physics updates (default: PYB)', metavar='', choices=Physics)
     parser.add_argument('--gui',                default=DEFAULT_GUI,       type=str2bool,      help='Whether to use PyBullet GUI (default: True)', metavar='')
     parser.add_argument('--plot',               default=DEFAULT_PLOT,       type=str2bool,      help='Whether to plot the simulation results (default: True)', metavar='')