Merge branch 'main' into search-map-allocation

.vscode/c_cpp_properties.json

@@ -1,15 +1,37 @@
 {
-    "configurations": [
-      {
-        "name": "Linux",
-        "includePath": ["${workspaceFolder}/ros_ws/**", "/opt/ros/humble/include/**"],
-        "defines": [],
-        "compilerPath": "/usr/bin/gcc",
-        "cStandard": "c11",
-        "cppStandard": "c++17",
-        "intelliSenseMode": "clang-x64"
-      }
-    ],
-    "version": 4
-  }
-
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/root/AirStack/ros_ws/install/vdb_mapping_interfaces/include/**",
+        "/root/AirStack/ros_ws/install/trajectory_library/include/**",
+        "/root/AirStack/ros_ws/install/mavros_interface/include/**",
+        "/root/AirStack/ros_ws/install/robot_interface/include/**",
+        "/root/AirStack/ros_ws/install/px4_msgs/include/**",
+        "/root/AirStack/ros_ws/install/mav_system_msgs/include/**",
+        "/root/AirStack/ros_ws/install/mav_state_machine_msgs/include/**",
+        "/root/AirStack/ros_ws/install/mav_planning_msgs/include/**",
+        "/root/AirStack/ros_ws/install/mav_msgs/include/**",
+        "/root/AirStack/ros_ws/install/disparity_map_representation/include/**",
+        "/root/AirStack/ros_ws/install/map_representation_interface/include/**",
+        "/root/AirStack/ros_ws/install/disparity_graph/include/**",
+        "/root/AirStack/ros_ws/install/behavior_tree/include/**",
+        "/root/AirStack/ros_ws/install/behavior_tree_msgs/include/**",
+        "/root/AirStack/ros_ws/install/airstack_common/include/**",
+        "/root/AirStack/ros_ws/install/airstack_msgs/include/**",
+        "/opt/ros/humble/include/**",
+        "/usr/include/**",
+        "/root/AirStack/ros_ws/src/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

.vscode/extensions.json

@@ -1,5 +1,9 @@
 {
     "recommendations": [
         "ms-python.black-formatter",
+        "ms-iot.vscode-ros",
+        "ms-vscode-remote.remote-ssh",
+        "ms-python.python",
+        "ms-vscode-remote.remote-containers"
     ]
 }

.vscode/launch.json

@@ -0,0 +1,11 @@
+{
+  "version": "0.2.0",
+  "configurations": [
+    {
+      "name": "ROS2: Launch robot.launch.py",
+      "type": "ros",
+      "request": "launch",
+      "target": "${workspaceFolder}/ros_ws/install/robot_bringup/share/robot_bringup/launch/robot.launch.xml",
+    }
+  ]
+}

.vscode/tasks.json

@@ -2,20 +2,32 @@
   "version": "2.0.0",
   "tasks": [
     {
-      "label": "build",
+      "label": "build ros_ws",
       "type": "shell",
       "options": {
         "cwd": "${workspaceFolder}/ros_ws"
       },
-      "command": "colcon build --cmake-args '-DCMAKE_BUILD_TYPE=Debug'"
+      "command": [
+        "source /root/.bashrc &&",
+        "bws --cmake-args '-DCMAKE_BUILD_TYPE=Debug'"
+      ],
+      "problemMatcher": [],
+      "group": {
+        "kind": "build",
+        "isDefault": true
+      }
     },
     {
-      "label": "test",
+      "label": "test ros_ws",
       "type": "shell",
       "options": {
         "cwd": "${workspaceFolder}/ros_ws"
       },
-      "command": "colcon test && colcon test-result"
+      "command": [
+        "source /opt/ros/humble/setup.bash;",
+        "source ${workspaceFolder}/ros_ws/install/setup.bash;",
+        "colcon test && colcon test-result"
+      ],
     }
   ]
-}
+}

docker/Dockerfile.airstack-dev

@@ -9,7 +9,8 @@ RUN apt install -y \
     vim nano wget curl tree \
     cmake build-essential \
     less htop jq \
-    python3-pip
+    python3-pip \
+    gdb
 
 # Package dependencies
 # perform ROS dependency installation for our workspace.

docker/docker-compose.yaml

@@ -2,12 +2,12 @@
 services:
   # ==============================================================================
   isaac-sim:
+    image: &isaac-sim-image airlab-storage.andrew.cmu.edu:5001/shared/isaac-sim_ros-humble:v1.0.0
     build:
       context: ../
       dockerfile: docker/Dockerfile.isaac-ros
       tags:
-        - airlab-storage.andrew.cmu.edu:5001/shared/isaac-sim_ros-humble:v1.0.0
-    image: airlab-storage.andrew.cmu.edu:5001/shared/isaac-sim_ros-humble:v1.0.0
+        - *isaac-sim-image
     container_name: isaac-sim
     entrypoint: bash
     # Interactive shell
@@ -58,16 +58,21 @@ services:
       - ./extras/config:/root/.nvidia-omniverse/config:rw
 
   # ==============================================================================
-  ground-control-station:
-    image: airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:latest
-    container_name: ground-control-station
+  robot:
+    image: &airstack-dev-image airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:v0.5.1
+    build:
+      context: ../
+      dockerfile: docker/Dockerfile.airstack-dev
+      tags:
+        - *airstack-dev-image
     entrypoint: bash -c "service ssh restart && bash"
     # Interactive shell
     stdin_open: true # docker run -i
     tty: true # docker run -t
     # Needed to display graphical applications
-    ipc: host
     privileged: true
+    networks:
+      - airstack_network
     environment:
       - DISPLAY
       - QT_X11_NO_MITSHM=1
@@ -80,37 +85,30 @@ services:
               count: 1
               capabilities: [ gpu ]
     ports:
-      # for ssh
-      - 2222:22
+      # for ssh, starting from 2223-2243 on the host port all map to 22 in the container. Assumes no more than 21 robots
+      - "2223-2243:22"
     volumes:
       # display stuff
       - $HOME/.Xauthority:/root/.Xauthority
       - /tmp/.X11-unix:/tmp/.X11-unix
       # developer stuff
-      - ./ground_control_station/.bashrc:/root/.bashrc:rw # bash config
+      - ./robot/.bashrc:/root/.bashrc:rw # bash config
       - ~/.bash_history:/root/.bash_history:rw # save cmdline history
       - /var/run/docker.sock:/var/run/docker.sock # access docker API for container name
       # autonomy stack stuff
       - ./..:/root/AirStack:rw
 
   # ==============================================================================
-  robot:
-    build:
-      context: ../
-      dockerfile: docker/Dockerfile.airstack-dev
-      tags:
-        - airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:latest
-        - airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:v0.5.1
-    image: airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:v0.5.1
-    # container_name: robot-1
+  ground-control-station:
+    image: *airstack-dev-image
+    container_name: ground-control-station
     entrypoint: bash -c "service ssh restart && bash"
     # Interactive shell
     stdin_open: true # docker run -i
     tty: true # docker run -t
     # Needed to display graphical applications
+    ipc: host
     privileged: true
-    networks:
-      - airstack_network
     environment:
       - DISPLAY
       - QT_X11_NO_MITSHM=1
@@ -123,14 +121,14 @@ services:
               count: 1
               capabilities: [ gpu ]
     ports:
-      # for ssh, starting from 2223-2243 on the host port all map to 22 in the container. Assumes no more than 21 robots
-      - "2223-2243:22"
+      # for ssh
+      - 2222:22
     volumes:
       # display stuff
       - $HOME/.Xauthority:/root/.Xauthority
       - /tmp/.X11-unix:/tmp/.X11-unix
       # developer stuff
-      - ./robot/.bashrc:/root/.bashrc:rw # bash config
+      - ./ground_control_station/.bashrc:/root/.bashrc:rw # bash config
       - ~/.bash_history:/root/.bash_history:rw # save cmdline history
       - /var/run/docker.sock:/var/run/docker.sock # access docker API for container name
       # autonomy stack stuff

docker/extras/config/omniverse.toml

@@ -1,3 +1,4 @@
 
 [bookmarks]
 "AirLab Nucleus Server" = "omniverse://airlab-storage.andrew.cmu.edu:8443"
+"AirStack" = "omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/"

docker/robot/.bashrc

@@ -4,7 +4,7 @@
 # for examples
 
 # If not running interactively, don't do anything
-[ -z "$PS1" ] && return
+# [ -z "$PS1" ] && return
 
 # don't put duplicate lines in the history. See bash(1) for more options
 # ... or force ignoredups and ignorespace
@@ -120,7 +120,7 @@ export PYTHONWARNINGS
 # Convenience functions for ROS2 workspace
 
 function bws(){
-    echo "Running \`colcon build\` in $ROS2_WS_DIR"
+    echo "Running \`colcon build $@\` in $ROS2_WS_DIR"
     COLCON_LOG_PATH="$ROS2_WS_DIR"/log colcon build --symlink-install --base-paths "$ROS2_WS_DIR"/ --build-base "$ROS2_WS_DIR"/build/ --install-base "$ROS2_WS_DIR"/install/ "$@"
 }
 function sws(){

docs/development/docker_usage.md

@@ -1,3 +1,4 @@
+# General Usage with Docker Compose
 
 AirStack is designed for multi-robot development, and is setup to run multiple robots in simulation.
 
@@ -18,6 +19,29 @@ This lets them communicate with ROS2 on the same network.
 
 Each robot has its own ROS_DOMAIN_ID.
 
+## Pull Images
+To use the AirLab docker registry:
+```bash
+cd AirStack/docker/
+docker login airlab-storage.andrew.cmu.edu:5001
+## <Enter your andrew id (without @andrew.cmu.edu)>
+## <Enter your andrew password>
+
+## Pull the images in the docker compose file
+docker compose pull 
+```
+
+Catelog: [AirLab Registry Images](https://airlab-storage.andrew.cmu.edu:5001/v2/_catalog).
+
+Available image tags: 
+[airstack-dev](https://airlab-storage.andrew.cmu.edu:5001/v2/shared/airstack-dev/tags/list),
+[isaac-sim_ros-humble](https://airlab-storage.andrew.cmu.edu:5001/v2/shared/isaac-sim_ros-humble/tags/list)
+
+## Build Images
+```bash
+docker compose build
+```
+
 ## Start and Stop
 Start
 ```bash
@@ -53,6 +77,20 @@ The ssh password is `airstack`.
 
 ## Container Details
 
+```mermaid
+graph TD
+    A(Isaac Sim) <-- Sensors and Actuation --> B
+    A <-- Sensors and Actuation --> C
+    B(Robot 1) <-- Global Info --> D(Ground Control Station)
+    C(Robot 2) <-- Global Info --> D
+
+    style A fill:#76B900,stroke:#333,stroke-width:2px
+    style B fill:#fbb,stroke:#333,stroke-width:2px
+    style C fill:#fbb,stroke:#333,stroke-width:2px
+    style D fill:#fbf,stroke:#333,stroke-width:2px
+
+```
+
 
 ### Isaac Sim
 Start a bash shell in the Isaac Sim container:
@@ -64,7 +102,7 @@ docker exec -it isaac-sim bash
 `runapp` launches Isaac Sim.
 The `--path` argument can be passed with a path to a `.usd` file to load a scene.
 
-It can also be run in headless mode with `./runheadless.native.sh` to stream to Omniverse Streaming Client or `./runheadless.webrtc.sh` to stream to a web browser.
+It can also be run in headless mode with `./runheadless.native.sh` to stream to [Omniverse Streaming Client](https://docs.omniverse.nvidia.com/streaming-client/latest/user-manual.html) or `./runheadless.webrtc.sh` to [stream to a web browser](https://docs.omniverse.nvidia.com/extensions/latest/ext_livestream/webrtc.html).
 
 The container also has the isaacsim ROS2 package within that can be launched with `ros2 launch isaacsim run_isaacsim.launch.py`. 
 
@@ -78,6 +116,7 @@ docker exec -it docker-robot-1 bash
 # in robot docker
 cws  # cleans workspace
 bws  # builds workspace
+bws --packages-select [your_packages] # builds only desired packages
 sws  # sources workspace
 ros2 launch robot_bringup robot.launch.xml  # top-level launch 
 ```
@@ -96,4 +135,4 @@ docker exec -it ground-control-station bash
 
 The commands are currently the same.
 
-`ROS_DOMAIN_ID` is set to 0.
+On the GCS `ROS_DOMAIN_ID` is set to 0.

docs/development/testing/ci_cd.md

@@ -0,0 +1 @@
+# CI/CD Pipeline

docs/development/testing/integration_testing.md

@@ -0,0 +1 @@
+# Integration Testing

docs/development/testing/testing_frameworks.md

@@ -0,0 +1 @@
+# Testing Frameworks

docs/development/vscode/index.md

@@ -0,0 +1,50 @@
+# VS Code: Docker Integration and Debugger Setup
+
+Start containers
+```bash
+# optionally pass the --scale robot=N argument to start N robots
+dc compose up -d  # --scale robot=2
+```
+
+Open AirStack folder
+
+```bash
+cd AirStack
+code .
+```
+
+Install the ["Dev Containers" extension](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.remote-containers).
+
+Now click the "Remote Explorer" icon on the left side bar, hover over a robot container, and attach to the container.
+
+![attach_container](attach_container.png)
+
+Install recommended extensions within the image. This installs the `ROS`, `C++`, and `Python` extensions in the container.
+![extensions.png](extensions.png)
+
+## Build ROS Workspace
+Hit `Ctrl-Shift-B` to build the project. This is a shortcut for `bws --cmake-args '-DCMAKE_BUILD_TYPE=Debug'`, which adds debug symbols to the build.
+
+Build tasks are defined in `.vscode/tasks.json`.
+
+
+## Launch
+
+Hit `F5` to launch `robot.launch.xml`, or click the "Run and Debug" button on the left side of the screen and click the green play button.
+
+Launch tasks are defined in `.vscode/launch.json`.
+
+![launch](launch.png)
+
+You can now set breakpoints, view variables, step-through code, and debug [as usual in VSCode](https://code.visualstudio.com/docs/editor/debugging).
+
+![debugger](debugger.png)
+
+
+!!! warning "Warning about file permissions"
+
+    Folders and files created within the attached docker container will be owned by root. This can cause issues when trying to edit files from the host machine, especially when using git to switch branches.
+    If you accidentally create files as root, you can change the owner to your user with the following command:
+    ```bash
+    sudo chown -R $USER:$USER .
+    ```