diff --git a/docker/perception/camera_segmentation.Dockerfile b/docker/perception/camera_segmentation.Dockerfile
index eb2e65b3..5e9a2698 100644
--- a/docker/perception/camera_segmentation.Dockerfile
+++ b/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
diff --git a/src/perception/camera_segmentation/camera_segmentation/camera_segmentation_node.py b/src/perception/camera_segmentation/camera_segmentation/camera_segmentation_node.py
index 421ea466..48b7e919 100755
--- a/src/perception/camera_segmentation/camera_segmentation/camera_segmentation_node.py
+++ b/src/perception/camera_segmentation/camera_segmentation/camera_segmentation_node.py
@@ -2,7 +2,7 @@
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
@@ -10,6 +10,7 @@
# 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
diff --git a/src/perception/camera_segmentation/package.xml b/src/perception/camera_segmentation/package.xml
index f8f6f0c6..37aec452 100755
--- a/src/perception/camera_segmentation/package.xml
+++ b/src/perception/camera_segmentation/package.xml
@@ -7,6 +7,11 @@
docker
TODO: License declaration
+ cv_bridge
+ sensor_msgs
+ geometry_msgs
+ std_msgs
+
ament_copyright
ament_flake8
ament_pep257
diff --git a/watod-config.sh b/watod-config.sh
index dea6ed05..3634981f 100755
--- a/watod-config.sh
+++ b/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 =
@@ -19,3 +19,5 @@ ACTIVE_PROFILES="perception"
## DEFAULT =
TAG="justin"
+
+FOXGLOVE_BRIDGE_PORT=5768