MAC-VO Dense Mapping mode & adaptation (#247)

* Add macvo2 codebase

* switch to macvo2

* Fix MACVO2

* Fix visualization

* Minor fix

* cleanup

* working version

* Add disparity publisher

---------

Co-authored-by: haleqiu <[email protected]>

Author: MarkChenYutian

.env

@@ -1,6 +1,6 @@
 # This top-level .env file under AirStack/ defines variables that are propagated through docker-compose.yaml
 PROJECT_NAME="airstack"
-PROJECT_VERSION="0.13.0"
+PROJECT_VERSION="0.13.1"
 # 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/AFCA/fire_academy_faro_with_sky.scene.usd"

.gitmodules

@@ -10,3 +10,6 @@
 [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
+[submodule "robot/ros_ws/src/autonomy/2_perception/macvo2"]
+	path = robot/ros_ws/src/autonomy/2_perception/macvo2
+	url = [email protected]:castacks/MAC-VO-ROS2.git

robot/docker/Dockerfile.robot

@@ -115,7 +115,7 @@ RUN pip3 install \
     tabulate \
     einops \
     timm==0.9.12 \
-    rerun-sdk==0.17 \
+    rerun-sdk==0.22.0 \
     yacs \
     wandb
 

robot/docker/robot.env

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

robot/ros_ws/src/autonomy/2_perception/macvo/config/rviz_macvo.rviz

@@ -59,7 +59,7 @@ Visualization Manager:
       Name: Pose
       Shaft Length: 1
       Shaft Radius: 0.05000000074505806
-      Shape: Arrow
+      Shape: Axes
       Topic:
         Depth: 5
         Durability Policy: Volatile

robot/ros_ws/src/autonomy/2_perception/macvo2

@@ -4,7 +4,7 @@
     <!-- State Estimation -->
     <!-- Visual Odometry -->
 
-    <node pkg="macvo" exec="macvo" if="$(env USE_MACVO)">
+    <!-- <node pkg="macvo" exec="macvo" if="$(env USE_MACVO)">
         <param name="camera_config" value="$(find-pkg-share macvo)/config/model_config.yaml" />
         <param from="$(find-pkg-share macvo)/config/interface_config.yaml" />
         <remap from="left/image_rect"
@@ -18,6 +18,22 @@
         <remap from="visual_odometry_points"
             to="/$(env ROBOT_NAME)/perception/visual_odometry_points" />
         <remap from="visual_odometry_img" to="/$(env ROBOT_NAME)/perception/visual_odometry_img" />
+    </node> -->
+    <node pkg="macvo2" exec="macvo2" if="$(env USE_MACVO)">
+        <param name="camera_config" value="$(find-pkg-share macvo2)/config/model_config.yaml" />
+        <param from="$(find-pkg-share macvo2)/config/interface_config.yaml" />
+        <remap from="left/image_rect"
+            to="/$(env ROBOT_NAME)/sensors/front_stereo/left/image_rect" />
+        <remap from="right/image_rect"
+            to="/$(env ROBOT_NAME)/sensors/front_stereo/right/image_rect" />
+        <remap from="/sensors/get_camera_params"
+            to="/$(env ROBOT_NAME)/sensors/get_camera_params" />
+        <remap from="visual_odometry_pose"
+            to="/$(env ROBOT_NAME)/perception/visual_odometry_pose" />
+        <remap from="visual_odometry_points"
+            to="/$(env ROBOT_NAME)/perception/visual_odometry_points" />
+        <remap from="visual_odometry_img"
+            to="/$(env ROBOT_NAME)/perception/visual_odometry_img" />
     </node>
 
     <!-- Depth Estimation -->

robot/ros_ws/src/autonomy/2_perception/perception_bringup/launch/perception.launch.xml

@@ -259,15 +259,14 @@ Visualization Manager:
           Name: Visual Odometry
           Shaft Length: 1
           Shaft Radius: 0.05000000074505806
-          Shape: Arrow
+          Shape: Axes
           Topic:
             Depth: 5
             Durability Policy: Volatile
             Filter size: 10
             History Policy: Keep Last
             Reliability Policy: Reliable
             Value: /robot_1/perception/visual_odometry_pose
-          Value: true
       Enabled: true
       Name: Perception
     - Class: rviz_common/Group