Tharp/mac vo integration (#188)

* adding mac-vo, working docker, node does not spin up yet

* adding MAC-VO update

* MACVO node can be intialized, have not tested it with image stream

* MACVO runs with camera parameter server, but crashes after couple of frames

* working inference

* trying to get manuel dimension setting working

* working manual dimensions, now removing from submodule

* removing MAC-VO-ROS2 submodule for custom wrapper

* adding mac-vo submoudle

* merging robot docker file into existing docker file

* fixing error in docker file

* working commit with transfer of macvo and updated docker file, not adding submodule and cant connect to nucleus server

* adding corrected macvo submodule

* removing old submodules from .gitmodules

* working macvo integration

* testing and adding visualization file

* removing frequency print statements

* integrated macvo into robot launch, now working

* fixing issue with timing if the subscibers are initialized too soon

* adding docs for mac vo

* adding camera param server and mac-vo docs

* Bump version to 1.1.0 for new mac-vo feature

* Update isaac to 4.5.0; make sure the ros_ws download URL is version-specific

* Parameterize isaac sim version in docker

* Add some to robot docker

* Remove traj debug from default rviz

* Add MACVO to perception

* Update with option and docs to enable/disable MACVO

* Set USE_MACVO to false by default for now

* Fix dockerfile build

https://stackoverflow.com/a/56748289

---------

Co-authored-by: Andrew Jong <[email protected]>

Author: tharp789

.env

@@ -1,6 +1,9 @@
+# This top-level .env file under AirStack/ defines variables that are propagated through docker-compose.yaml
 PROJECT_NAME="airstack"
-PROJECT_VERSION="1.0.5"
+PROJECT_VERSION="1.1.0"
 # can replace with your docker hub username
 PROJECT_DOCKER_REGISTRY="airlab-storage.andrew.cmu.edu:5001/shared"
 DEFAULT_ISAAC_SCENE="omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/fire_academy.scene.usd"
 PLAY_SIM_ON_START="true"
+# the file under robot/docker/ that contains the robot's environment variables
+ROBOT_ENV_FILE_NAME="robot.env"

.gitmodules

@@ -7,3 +7,6 @@
 [submodule "ground_control_station/ros_ws/src/ros-gst-bridge"]
 	path = ground_control_station/ros_ws/src/ros-gst-bridge
 	url = https://github.com/BrettRD/ros-gst-bridge.git
+[submodule "robot/ros_ws/src/autonomy/2_perception/macvo/macvo/src"]
+	path = robot/ros_ws/src/autonomy/2_perception/macvo/macvo/src
+	url = https://github.com/MAC-VO/MAC-VO.git

docs/development/project_configuration.md

@@ -1,4 +1,12 @@
 # Project Configuration
 
 The project as a whole can be configured using the `.env` file under the project root.
-The variables in the `.env` file gets populated into the root `docker-compose.yml`.
+The variables in the `.env` file get propagated into the `docker-compose.yml` files.
+
+
+The top-level env file is reproduced below:
+```bash
+--8<-- ".env"
+```
+
+

docs/robot/common_topics.md

@@ -0,0 +1,2 @@
+# Configuration
+

docs/robot/configuration/index.md

@@ -1,4 +1,48 @@
 # Robot
 
+## Directory Structure
+Underneath `AirStack/robot`, there are these directories:
+- `docker/`: Contains files related to building and launching the robot Docker container.
+- `installation/`: Contains files related to installing the robot software on a physical robot (TODO).
+- `ros_ws/`: Contains the ROS 2 workspace for the robot.
+
 ## Launch Structure
-Each high-level module has a `*_bringup` package that contains the launch files for that module. The launch files are located in the `launch` directory of the `*_bringup` package. The launch files are named `*.launch.(xml/yaml/py)` and can be launched with `ros2 launch <module_name>_bringup <module_name>.launch.(xml/yaml/py)`.
+Each high-level module under `ros_ws/` has a `[module]_bringup` package that contains the launch files for that module. The launch files are located in the `launch` directory of the `[module]_bringup` package. The launch files are named `*.launch.(xml/yaml/py)` and can be launched with `ros2 launch <module_name>_bringup <module_name>.launch.(xml/yaml/py)`.
+
+At a high level, the launch files are organized as follows:
+
+
+```
+- robot_bringup/: robot.launch.xml
+    - autonomy_bringup/: autonomy.launch.xml
+        - interface_bringup/: interface.launch.xml
+        - sensors_bringup/: sensors.launch.xml
+        - perception_bringup/: perception.launch.xml
+        - local_bringup/: local.launch.xml
+        - global_bringup/: global.launch.xml
+        - behavior_bringup/: behavior.launch.xml
+```
+
+## Configuration
+
+### Desktop vs Jetson
+If you look at the `robot/docker/docker-compose.yaml` file, you'll see it contains two services. `robot` is meant for x86-64 desktop development whereas `robot_l4t` is meant to run on NVIDIA Jetson devices. Both extend a base service in `robot/docker/robot-base-docker-compose.yaml`.
+
+### Environment Variables
+Environment variables are used to configure the robot Docker container. The top level `AirStack/.env` file points to a `ROBOT_ENV_FILE_NAME` (default: `robot.env`), that in turn is used to load environment variables for the robot Docker container. The file that `ROBOT_ENV_FILE_NAME` points to gets added into the container under `robot/docker/robot-base-docker-compose.yaml`.
+
+The environment variables can be used to trigger nodes to launch. For example, the `USE_MACVO` environmental variable is checked by `perception.launch.xml` to determine whether to launch the `macvo` node.
+
+The file `robot.env` is reproduced below:
+```bash
+--8<-- "robot/docker/robot.env"
+```
+
+## Common Topics
+| Topic                          | Type              | Description                                                                                                                             |
+| -------------------------------| ------------------| ---------------------------------------------------------------------------------------------------------------------------|
+| `/$ROBOT_NAME/odometry`        | [nav_msgs/Odometry](https://docs.ros.org/en/rolling/p/nav_msgs/interfaces/msg/Odometry.html) | Best estimate of robot odometry
+| `/$ROBOT_NAME/global_plan`     | [nav_msgs/Path](https://docs.ros.org/en/rolling/p/nav_msgs/interfaces/msg/Path.html)     | Current target global trajectory for the robot to follow. See [global planning](4_global/planning.md) for more details.
+
+### Rough System Diagram
+![AirStack System Diagram](airstack_system_diagram.png)

docs/robot/index.md

@@ -56,7 +56,6 @@ nav:
       - docs/development/frame_conventions.md
   - Robot:
       - docs/robot/index.md
-      - docs/robot/common_topics.md
       - Autonomy Modules:
           - Robot Interface: 
               - docs/robot/autonomy/0_interface/index.md
@@ -130,7 +129,7 @@ theme:
     - navigation.indexes
     - navigation.path
     - navigation.tabs
-    - navigation.expand
+    # - navigation.expand
     - navigation.footer
     - navigation.top
     - navigation.sections

mkdocs.yml

@@ -38,7 +38,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
 RUN sudo add-apt-repository universe \
   && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg \
   && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null \
-  && apt-get update && apt-get install -y --no-install-recommends \
+  && apt-get update && apt upgrade -y && apt-get install -y --no-install-recommends \
     ros-humble-desktop \
     python3-argcomplete \
   && rm -rf /var/lib/apt/lists/*
@@ -63,6 +63,7 @@ RUN apt update && apt install -y \
     cmake build-essential \
     less htop jq \
     python3-pip \
+    python3-rosdep \
     tmux \
     gdb
 
@@ -78,16 +79,16 @@ RUN apt update -y && apt install -y \
     ros-humble-domain-bridge \
     libcgal-dev \
     python3-colcon-common-extensions
-RUN /opt/ros/humble/lib/mavros/install_geographiclib_datasets.sh
 
+RUN /opt/ros/humble/lib/mavros/install_geographiclib_datasets.sh
 
 # Install Python dependencies
 RUN pip3 install \
     empy \
     future \
     lxml \
-    matplotlib \
-    numpy \
+    matplotlib==3.8.4 \
+    numpy==1.24.0 \
     pkgconfig \
     psutil \
     pygments \
@@ -98,8 +99,22 @@ RUN pip3 install \
     setuptools \
     six \
     toml \
-    scipy
-
+    scipy \
+    torch \
+    torchvision \
+    pypose \
+    rich \
+    tqdm \
+    pillow \ 
+    flow_vis \
+    h5py \
+    evo \
+    tabulate \
+    einops \
+    timm==0.9.12 \
+    rerun-sdk==0.17 \
+    yacs \
+    wandb
 
 # Override install newer openvdb 9.1.0 for compatibility with Ubuntu 22.04  https://bugs.launchpad.net/bugs/1970108
 RUN apt remove -y libopenvdb*; \
@@ -124,18 +139,29 @@ EXPOSE 22
 ARG REAL_ROBOT=false
 RUN if [ "$REAL_ROBOT"  = "true" ]; then \
   # Put commands here that should run for the real robot but not the sim
-  
   echo "REAL_ROBOT is true"; \
   apt-get update && apt-get install -y libimath-dev; \
 else \
   # Put commands here that should be run for the sim but not the real robot
-  
   echo "REAL_ROBOT is false"; \
 fi
 
+# Downloading model weights for MACVO
+WORKDIR /root/model_weights
+RUN wget -r "https://github.com/MAC-VO/MAC-VO/releases/download/model/MACVO_FrontendCov.pth" && \ 
+    mv /root/model_weights/github.com/MAC-VO/MAC-VO/releases/download/model/MACVO_FrontendCov.pth /root/model_weights/MACVO_FrontendCov.pth && \
+    rm -rf /root/model_weights/github.com
+
+WORKDIR /root/ros_ws
 # Cleanup. Prevent people accidentally doing git commits as root in Docker
 RUN apt purge git -y \
     && apt autoremove -y \
     && apt clean -y \
     && rm -rf /var/lib/apt/lists/*
 
+# Install colcon, seems to be getting removed
+RUN pip install -U colcon-common-extensions
+
+# Fixes for MACVO Integration
+RUN pip install huggingface_hub
+RUN pip uninstall matplotlib -y

robot/docker/Dockerfile.robot

@@ -10,6 +10,8 @@ services:
     environment:
       - DISPLAY
       - QT_X11_NO_MITSHM=1
+    env_file:
+      - ${ROBOT_ENV_FILE_NAME}
     deploy:
       # let it use the GPU
       resources:

robot/docker/robot-base-docker-compose.yaml

@@ -0,0 +1,2 @@
+# These become environment variables in the robot container
+USE_MACVO="false"

robot/docker/robot.env

@@ -0,0 +1,51 @@
+
+# Camera Parameter Server
+
+## Summary
+
+The camera parameter server was designed to eliminate the need for multiple nodes to subscribe to each camera individually, reducing unnecessary subscribers and callbacks. Its sole purpose is to listen to the camera info topics, store relevant information about the cameras, and provide it on demand for other nodes to query.
+
+## Configuration
+
+The camera parameter server is currently configurable through a non-ROS configuration file. This file allows users to define a list of cameras, specifying their types and topic names. At the top level of the configuration, a base link name is provided to indicate the `tf` name for the robot's center. Additionally, a parameter called `camera_list` contains a list of dictionaries, with each dictionary representing an individual camera. Currently, two camera types are supported: monocular and stereo.
+
+## Parameters
+
+Below are the parameters needed for the meta level camera parameter server configuration, as well as the camera fields needed to specify individual camera types.
+
+### Meta Level Parameters
+
+| <div style="width:220px">Parameter</div>  | Description
+|----------------------------|---------------------------------------------------------------
+| `base_link_frame_id`  | The frame name of the base link, or center frame of the robot|
+| `camera_list`         | A list of dictionaries that define each camera of the system|
+
+### Monocular Camera Parameters
+
+| <div style="width:220px">Parameter</div>  | Description
+|----------------------------|---------------------------------------------------------------
+| `camera_name`            | The name of the camera|
+| `camera_type`            | The type of camera, for monocular being `mono` |
+| `camera_info_sub_topic`  | The info topic name for the camera, normally `camera_info`|
+| `camera_frame_id`        | The frame name of the camera to find its tf |
+
+### Stereo Camera Parameters
+
+| <div style="width:220px">Parameter</div>  | Description
+|----------------------------|---------------------------------------------------------------
+| `camera_name`            | The name of the camera|
+| `camera_type`            | The type of camera, for stereo being `stereo` |
+| `camera_info_sub_topic`  | The info topic name for the camera, normally `camera_info`|
+| `left_camera_frame_id`   | The frame name of the left camera for find its tf |
+| `right_camera_frame_id`  | The frame name of the right camera to find its tf |
+
+## Services
+| <div style="width:220px">Parameter</div> | Type | Description
+|----------------------------|----------------------------------------|-----------------------|
+| `~/get_camera_params`      | sensor_interfaces/GetCameraParams | The service to get info about the desired camera. This provides camera intrinsics, transform frame ids, and baseline if the camera type is a stereo|
+
+## Subscriptions
+| <div style="width:220px">Parameter</div> | Type | Description
+|----------------------------|----------------------------------------|-----------------------|
+| `tf/`           | tfMessage | Listens for the tf for the specified cameras |
+| `~/camera_info` | sensor_msgs/CameraInfo | Listens for the info for specified cameras|