[Draft] enable ROS example for COMPASS policy

ros2_deployment/README.md

@@ -199,3 +199,24 @@ That's it!  If everything worked, you should see the robot moving towards the go
 
 https://github.com/user-attachments/assets/32931225-60b1-4fbb-8f17-1b7e7641808f
 
+
+# COMPASS Integration
+
+To run this navigator example with the COMPASS policy, you can follow the following steps:
+
+1. Clone the COMPASS repo (https://github.com/NVlabs/COMPASS) and download the generalist checkpoint from: https://huggingface.co/nvidia/COMPASS/blob/main/compass_generalist.ckpt
+
+2. Follow the instructions in COMPASS [README](https://github.com/NVlabs/COMPASS?tab=readme-ov-file#model-export) to setup the environment and export the ONNX file from the generalist checkpoint:
+```
+python3 onnx_conversion.py -b <x-mobility-ckpt> -g <downloaded-generalist-ckpt> -e carter -o <onnx-output-path>
+```
+NOTE: Update the image size in the `onnx_conversion.py` to match the camera resolution.
+
+3. Follow the instructions in Step 5 to build the TensorRT engine.
+
+4. Point the `runtime_path` parameter in the `x_mobility_navigator.launch.py` to the newly built engine.
+
+5. Launch the X-Mobility navigator with the COMPASS policy:
+```
+ros2 launch x_mobility_navigator x_mobility_navigator.launch.py
+```

ros2_deployment/x_mobility_navigator/launch/x_mobility_navigator.launch.py

@@ -41,7 +41,7 @@ def generate_launch_description():
                 LaunchConfiguration("is_mapless", default="True"),
                 'runtime_path':
                 LaunchConfiguration("runtime_path",
-                                    default="/tmp/x_mobility.engine"),
+                                    default="/tmp/compass_carter.engine"),
             }],
         ),
         Node(

ros2_deployment/x_mobility_navigator/x_mobility_navigator/x_mobility_navigator.py

@@ -38,7 +38,7 @@
 RUNTIME_PATH = 'runtime_path'
 MAPLESS_FLAG = 'is_mapless'
 
-NUM_ROUTE_POINTS = 20
+NUM_ROUTE_POINTS = 10
 # Route vector with 4 values representing start and end positions
 ROUTE_VECTOR_SIZE = 4
 ROBOT_FRAME = 'base_link'
@@ -121,6 +121,11 @@ def load_model(self):
         runtime = trt.Runtime(trt.Logger(trt.Logger.INFO))
         engine = runtime.deserialize_cuda_engine(engine_data)
         self.runtime_context = engine.create_execution_context()
+        for idx in range(engine.num_io_tensors):
+            name = engine.get_tensor_name(idx) 
+            print(name)
+            print(engine.get_tensor_dtype(name))
+            print(engine.get_tensor_shape(name))
 
     def image_callback(self, image_msg):
         self.camera_image = self.process_image_msg(image_msg)
@@ -231,7 +236,7 @@ def _trt_inference(self):
         history_input = cuda.mem_alloc(self.history.nbytes)
         sample_input = cuda.mem_alloc(self.sample.nbytes)
         action_output = cuda.mem_alloc(self.action.nbytes)
-        path_output = cuda.mem_alloc(self.path.nbytes)
+        # path_output = cuda.mem_alloc(self.path.nbytes)
         history_output = cuda.mem_alloc(self.history.nbytes)
         sample_ouput = cuda.mem_alloc(self.sample.nbytes)
 
@@ -253,7 +258,7 @@ def _trt_inference(self):
             int(history_input),
             int(sample_input),
             int(action_output),
-            int(path_output),
+            # int(path_output),
             int(history_output),
             int(sample_ouput),
         ]
@@ -263,7 +268,7 @@ def _trt_inference(self):
 
         # Copy action back to host and publish
         cuda.memcpy_dtoh(self.action, action_output)
-        cuda.memcpy_dtoh(self.path, path_output)
+        # cuda.memcpy_dtoh(self.path, path_output)
         cuda.memcpy_dtoh(self.history, history_output)
         cuda.memcpy_dtoh(self.sample, sample_ouput)