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