From cdfd2162c08dd3bb78fcef9ebe2a0d69bfe80d1c Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Fri, 11 Oct 2024 17:08:15 +0200 Subject: [PATCH] Use image_raw instead of image_color (#10) --- README.md | 4 +- .../launch/multi_view_demo.launch.py | 5 -- .../launch/single_view_demo.launch.py | 5 -- .../rviz/happypose_example.rviz | 2 +- .../rviz/happypose_multiview_example.rviz | 6 +-- happypose_ros/happypose_ros/camera_wrapper.py | 4 +- happypose_ros/test/happypose_testing_utils.py | 4 +- happypose_ros/test/single_view_base.py | 4 +- .../test/test_multi_view_integration.py | 6 +-- resources/happypose_ros_diagram.drawio.svg | 52 +++++++++---------- 10 files changed, 41 insertions(+), 51 deletions(-) diff --git a/README.md b/README.md index 5501874..12281a4 100644 --- a/README.md +++ b/README.md @@ -93,13 +93,13 @@ The node provides the following ROS topics: ### Subscribers -- **\/image_color** [sensor_msgs/msg/Image] +- **\/image_raw** [sensor_msgs/msg/Image] Video stream from a given camera. This is a default topic subscribed by a given camera. Can be changed to compressed image via parameters. Disabled by the parameter **cameras.\.compressed**. -- **\/image_color/compressed** [sensor_msgs/msg/CompressedImage] +- **\/image_raw/compressed** [sensor_msgs/msg/CompressedImage] Compressed video stream from a given camera. diff --git a/happypose_examples/launch/multi_view_demo.launch.py b/happypose_examples/launch/multi_view_demo.launch.py index 5bf17c5..c990650 100644 --- a/happypose_examples/launch/multi_view_demo.launch.py +++ b/happypose_examples/launch/multi_view_demo.launch.py @@ -46,11 +46,6 @@ def launch_setup( "camera_info_url": camera_info_url, } ], - remappings=[ - # Remapped topics are created to match camera names in - # happypose_examples/config/cosypose_params_multiview.yaml - ("image_raw", "image_color"), - ], ) ) diff --git a/happypose_examples/launch/single_view_demo.launch.py b/happypose_examples/launch/single_view_demo.launch.py index c75ebad..81059bc 100644 --- a/happypose_examples/launch/single_view_demo.launch.py +++ b/happypose_examples/launch/single_view_demo.launch.py @@ -42,11 +42,6 @@ def launch_setup( "camera_info_url": camera_info_url, } ], - remappings=[ - # Remapped topics have to match the names from - # happypose_examples/config/cosypose_params.yaml - ("image_raw", "image_color"), - ], ) # Include common part of the demo launch files diff --git a/happypose_examples/rviz/happypose_example.rviz b/happypose_examples/rviz/happypose_example.rviz index f88a5cc..fdaa805 100644 --- a/happypose_examples/rviz/happypose_example.rviz +++ b/happypose_examples/rviz/happypose_example.rviz @@ -89,7 +89,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /cam_1/image_color + Value: /cam_1/image_raw Value: true Visibility: Grid: false diff --git a/happypose_examples/rviz/happypose_multiview_example.rviz b/happypose_examples/rviz/happypose_multiview_example.rviz index 481a406..51ef991 100644 --- a/happypose_examples/rviz/happypose_multiview_example.rviz +++ b/happypose_examples/rviz/happypose_multiview_example.rviz @@ -96,7 +96,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /cam_1/image_color + Value: /cam_1/image_raw Value: true - Class: rviz_default_plugins/Image Enabled: true @@ -110,7 +110,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /cam_2/image_color + Value: /cam_2/image_raw Value: true - Class: rviz_default_plugins/Image Enabled: true @@ -124,7 +124,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /cam_3/image_color + Value: /cam_3/image_raw Value: true Enabled: true Global Options: diff --git a/happypose_ros/happypose_ros/camera_wrapper.py b/happypose_ros/happypose_ros/camera_wrapper.py index f150032..2eada75 100644 --- a/happypose_ros/happypose_ros/camera_wrapper.py +++ b/happypose_ros/happypose_ros/camera_wrapper.py @@ -52,10 +52,10 @@ def __init__( if params.get_entry(self._camera_name).compressed: img_msg_type = CompressedImage - topic_postfix = "/image_color/compressed" + topic_postfix = "/image_raw/compressed" else: img_msg_type = Image - topic_postfix = "/image_color" + topic_postfix = "/image_raw" self._image = None self._cvb = CvBridge() diff --git a/happypose_ros/test/happypose_testing_utils.py b/happypose_ros/test/happypose_testing_utils.py index f343f78..7a506aa 100644 --- a/happypose_ros/test/happypose_testing_utils.py +++ b/happypose_ros/test/happypose_testing_utils.py @@ -98,9 +98,9 @@ def __init__( cam[1], # Choose topic name based on the type ( - f"{cam[0]}/image_color" + f"{cam[0]}/image_raw" if isinstance(cam[1], Metaclass_Image) - else f"{cam[0]}/image_color/compressed" + else f"{cam[0]}/image_raw/compressed" ), 10, ), diff --git a/happypose_ros/test/single_view_base.py b/happypose_ros/test/single_view_base.py index fd7551b..5049b95 100644 --- a/happypose_ros/test/single_view_base.py +++ b/happypose_ros/test/single_view_base.py @@ -71,12 +71,12 @@ def test_02_check_topics(self) -> None: # Check if node subscribes to correct topics if self.compressed: self.node.assert_node_is_subscriber( - "cam_1/image_color/compressed", + "cam_1/image_raw/compressed", timeout=3.0, ) else: self.node.assert_node_is_subscriber( - "cam_1/image_color", + "cam_1/image_raw", timeout=3.0, ) self.node.assert_node_is_subscriber("cam_1/camera_info", timeout=3.0) diff --git a/happypose_ros/test/test_multi_view_integration.py b/happypose_ros/test/test_multi_view_integration.py index ccd1c92..e9add73 100644 --- a/happypose_ros/test/test_multi_view_integration.py +++ b/happypose_ros/test/test_multi_view_integration.py @@ -94,11 +94,11 @@ def test_01_node_startup(self, proc_output: ActiveIoHandler) -> None: def test_02_check_topics(self) -> None: # Check if node subscribes to correct topics - self.node.assert_node_is_subscriber("cam_1/image_color", timeout=3.0) + self.node.assert_node_is_subscriber("cam_1/image_raw", timeout=3.0) self.node.assert_node_is_subscriber("cam_1/camera_info", timeout=3.0) - self.node.assert_node_is_subscriber("cam_2/image_color/compressed", timeout=3.0) + self.node.assert_node_is_subscriber("cam_2/image_raw/compressed", timeout=3.0) self.node.assert_node_is_subscriber("cam_2/camera_info", timeout=3.0) - self.node.assert_node_is_subscriber("cam_3/image_color", timeout=3.0) + self.node.assert_node_is_subscriber("cam_3/image_raw", timeout=3.0) self.node.assert_node_is_subscriber("cam_3/camera_info", timeout=3.0) self.node.assert_node_is_publisher("happypose/detections", timeout=3.0) self.node.assert_node_is_publisher("happypose/vision_info", timeout=3.0) diff --git a/resources/happypose_ros_diagram.drawio.svg b/resources/happypose_ros_diagram.drawio.svg index 4cd6d92..7143d16 100644 --- a/resources/happypose_ros_diagram.drawio.svg +++ b/resources/happypose_ros_diagram.drawio.svg @@ -1,4 +1,4 @@ - + @@ -21,7 +21,7 @@ - + @@ -39,9 +39,9 @@ - + - + @@ -95,18 +95,18 @@ -
+
- /cam_1/image_color + /cam_1/image_raw
- - /cam_1/image_color + + /cam_1/image_raw @@ -158,7 +158,7 @@ - + @@ -196,18 +196,18 @@ -
+
- /cam_2/image_color/compressed + /cam_2/image_raw/compressed
- - /cam_2/image_color/compressed + + /cam_2/image_raw/compressed @@ -267,18 +267,18 @@ -
+
- /cam_n/image_color + /cam_n/image_raw
- - /cam_n/image_color + + /cam_n/image_raw @@ -300,7 +300,7 @@ - + @@ -338,7 +338,7 @@ -
+
@@ -475,7 +475,7 @@ -
+
@@ -487,7 +487,7 @@
- + Function call @@ -559,7 +559,7 @@ -
+
@@ -569,7 +569,7 @@
- + Spawns thread @@ -597,12 +597,12 @@ - - + + -
+