updated subscriber and publisher for oneformer inferences

docker/perception/camera_segmentation.Dockerfile

@@ -46,6 +46,7 @@ WORKDIR /home/docker/ament_ws/src
 COPY src/perception/camera_segmentation/ camera_segmentation
 COPY src/wato_msgs/sample_msgs sample_msgs
 
+# RUN sudo apt-get install ros-humble-cv-bridge
 COPY src/perception/camera_segmentation/requirements.txt camera_segmentation/requirements.txt
 RUN pip3 install --no-cache-dir --upgrade --trusted-host pypi.org --trusted-host shi-labs.com --trusted-host files.pythonhosted.org -r camera_segmentation/requirements.txt
 

src/perception/camera_segmentation/camera_segmentation/camera_segmentation_node.py

@@ -2,14 +2,15 @@
 from rclpy.node import Node
 
 from std_msgs.msg import String
-from sensor_msgs.msg import Image
+from sensor_msgs.msg import Image, CompressedImage
 
 import detectron2
 from detectron2.utils.logger import setup_logger
 
 # Import libraries
 import numpy as np
 import cv2
+from cv_bridge import CvBridge, CvBridgeError
 import torch
 import imutils
 import os
@@ -38,7 +39,7 @@ def __init__(self):
         # log initialization
         self.get_logger().info('Initializing node...')
         # Fetch parameters from yaml file
-        self.declare_parameter('camera_topic', '/camera_topic')
+        self.declare_parameter('camera_topic', '/CAM_FRONT/image_rect_compressed')
         self.declare_parameter('publish_topic', '/camera_segmentation')
         camera_topic = self.get_parameter('camera_topic').get_parameter_value().string_value
         publish_topic = self.get_parameter('publish_topic').get_parameter_value().string_value
@@ -61,20 +62,38 @@ def __init__(self):
             "instance": self.instance_run, 
             "semantic": self.semantic_run}
 
-    def test_run(self):
-        predictor, metadata = self.setup_modules("cityscapes", "/home/docker/ament_ws/src/camera_segmentation/camera_segmentation/models/250_16_dinat_l_oneformer_cityscapes_90k.pth", False)
+        self.predictor, self.metadata = self.setup_modules("cityscapes", "/home/docker/ament_ws/src/camera_segmentation/camera_segmentation/models/250_16_dinat_l_oneformer_cityscapes_90k.pth", False)
+        self.cv_bridge = CvBridge()
+
+    # def test_run(self):
+    #     predictor, metadata = self.setup_modules("cityscapes", "/home/docker/ament_ws/src/camera_segmentation/camera_segmentation/models/250_16_dinat_l_oneformer_cityscapes_90k.pth", False)
 
-        img = cv2.imread("/home/docker/ament_ws/src/camera_segmentation/camera_segmentation/samples/test.png")
-        img = imutils.resize(img, width=512)
-        task = "panoptic"
-        out = self.TASK_INFER[task](img, predictor, metadata).get_image()
-        cv2.imwrite("/home/docker/ament_ws/src/camera_segmentation/camera_segmentation/samples/test_results_new.png", out)
+    #     img = cv2.imread("/home/docker/ament_ws/src/camera_segmentation/camera_segmentation/samples/test.png")
+    #     img = imutils.resize(img, width=512)
+    #     task = "panoptic"
+    #     out = self.TASK_INFER[task](img, predictor, metadata).get_image()
+    #     cv2.imwrite("/home/docker/ament_ws/src/camera_segmentation/camera_segmentation/samples/test_results_new.png", out)
 
-        print("At the end")
+    #     print("At the end")
 
-    def image_callback(self):
-        msg = Image()
-        self.publisher_.publish(msg)
+    def image_callback(self, msg):
+        if self.compressed:
+            np_arr = np.frombuffer(msg.data, np.uint8)
+            cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
+        else:
+            try:
+                cv_image = self.cv_bridge.imgmsg_to_cv2(
+                    msg, desired_encoding="passthrough")
+            except CvBridgeError as e:
+                self.get_logger().error(str(e))
+                return
+
+        cv_image = imutils.resize(cv_image, width=512)
+        out = self.TASK_INFER["panoptic"](cv_image, self.predictor, self.metadata).get_image()
+
+        img_msg = self.cv_bridge.cv2_to_imgmsg(out, "bgr8")
+
+        self.publisher_.publish(img_msg)
         self.get_logger().info('Publishing image: "%s"' % msg.data)
         self.i += 1
 
@@ -145,7 +164,7 @@ def main(args=None):
 
     rclpy.init(args=args)
     node = CameraSegmentationNode()
-    node.test_run()
+    # node.test_run()
     rclpy.spin(node)
 
     # Destroy the node explicitly

src/perception/camera_segmentation/package.xml

@@ -7,6 +7,11 @@
   <maintainer email="[email protected]">docker</maintainer>
   <license>TODO: License declaration</license>
 
+  <depend>cv_bridge</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

watod-config.sh

@@ -7,7 +7,7 @@
 ##   - production    		  :   configs for all containers required in production
 ##   - samples             :   starts sample ROS2 pubsub nodes
 
-ACTIVE_PROFILES="perception"
+ACTIVE_PROFILES="perception vis_tools data_stream"
 
 
 ## Name to append to docker containers. DEFAULT = <your_watcloud_username>
@@ -19,3 +19,5 @@ ACTIVE_PROFILES="perception"
 ## DEFAULT = <your_current_github_branch> 
 
 TAG="justin"
+
+FOXGLOVE_BRIDGE_PORT=5768