From 09a0980fa9c26c6513ad64927398749674a49006 Mon Sep 17 00:00:00 2001 From: Jessica Ding Date: Fri, 16 Feb 2024 00:27:19 +0000 Subject: [PATCH 1/7] basic test --- .../test/camera_object_detection_test.py | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100644 src/perception/camera_object_detection/test/camera_object_detection_test.py diff --git a/src/perception/camera_object_detection/test/camera_object_detection_test.py b/src/perception/camera_object_detection/test/camera_object_detection_test.py new file mode 100644 index 00000000..743ee2be --- /dev/null +++ b/src/perception/camera_object_detection/test/camera_object_detection_test.py @@ -0,0 +1,38 @@ +import unittest +import numpy +from unittest.mock import patch, MagicMock +from rclpy.node import Node +from sensor_msgs.msg import Image +from camera_object_detection.yolov8_detection import CameraDetectionNode + +class TestCameraDetectionNode(unittest.TestCase): + + @patch.object(Node, '__init__') + @patch('rclpy.node.Node.get_logger') + def test_image_processing(self, + get_logger_mock, + node_init_mock): + node_init_mock.return_value = None # Mock the Node's __init__ method + + + node = CameraDetectionNode() + # Mock the publish method to capture its input + node.create_publisher = MagicMock() + node.create_publisher().publish = MagicMock() + + # Create a dummy image message + dummy_image = Image() + dummy_image.height = 480 + dummy_image.width = 640 + dummy_image.encoding = 'rgb8' + dummy_image.step = dummy_image.width * 3 + dummy_image.data = [255] * (dummy_image.step * dummy_image.height) + + node.image_callback(dummy_image) + + # Check if the publish method was called with a bounding box + node.create_publisher().publish.assert_called() # changed publish to publish_detections ! + +if __name__ == '__main__': + unittest.main() + From bfea3ace09f38bd7309583c2fe3d42f7616b8577 Mon Sep 17 00:00:00 2001 From: Steven Gong Date: Fri, 16 Feb 2024 02:58:06 +0000 Subject: [PATCH 2/7] Add working test --- .../test/camera_object_detection_test.py | 48 +++++++++---------- 1 file changed, 23 insertions(+), 25 deletions(-) diff --git a/src/perception/camera_object_detection/test/camera_object_detection_test.py b/src/perception/camera_object_detection/test/camera_object_detection_test.py index 743ee2be..bc2397c2 100644 --- a/src/perception/camera_object_detection/test/camera_object_detection_test.py +++ b/src/perception/camera_object_detection/test/camera_object_detection_test.py @@ -1,38 +1,36 @@ import unittest -import numpy -from unittest.mock import patch, MagicMock -from rclpy.node import Node from sensor_msgs.msg import Image +import time from camera_object_detection.yolov8_detection import CameraDetectionNode +import rclpy class TestCameraDetectionNode(unittest.TestCase): - @patch.object(Node, '__init__') - @patch('rclpy.node.Node.get_logger') - def test_image_processing(self, - get_logger_mock, - node_init_mock): - node_init_mock.return_value = None # Mock the Node's __init__ method + def test_image_processing(self): + rclpy.init() + node = CameraDetectionNode() - node = CameraDetectionNode() - # Mock the publish method to capture its input - node.create_publisher = MagicMock() - node.create_publisher().publish = MagicMock() + # Create a dummy image message + dummy_image = Image() + dummy_image.height = 480 + dummy_image.width = 640 + dummy_image.encoding = 'rgb8' + dummy_image.step = dummy_image.width * 3 + dummy_image.data = [255] * (dummy_image.step * dummy_image.height) - # Create a dummy image message - dummy_image = Image() - dummy_image.height = 480 - dummy_image.width = 640 - dummy_image.encoding = 'rgb8' - dummy_image.step = dummy_image.width * 3 - dummy_image.data = [255] * (dummy_image.step * dummy_image.height) + node.image_callback(dummy_image) + # Delay to allow the callback to be called + time.sleep(1) + # Check if the publish method was called with a bounding box + # node.create_publisher().publish.assert_called() + node.destroy_node() + rclpy.shutdown() + + def listener_callback(self, msg): + self.msg_received = True + self.received_msg = msg - node.image_callback(dummy_image) - - # Check if the publish method was called with a bounding box - node.create_publisher().publish.assert_called() # changed publish to publish_detections ! if __name__ == '__main__': unittest.main() - From b58f35c2b50e2a8ce69b675064ae2d85ffcd3796 Mon Sep 17 00:00:00 2001 From: Steven Gong Date: Mon, 19 Feb 2024 19:21:56 -0500 Subject: [PATCH 3/7] Update launch file for 3 cameras (#74) * Update launch file for 3 cameras * Rename to eve * Fix Linting --- modules/docker-compose.perception.yaml | 2 +- .../config/eve_config.yaml | 23 ++++++++++ .../config/sim_config.yaml | 7 ---- .../launch/eve_launch.py | 42 +++++++++++++++++++ .../launch/sim_launch.py | 26 ------------ 5 files changed, 66 insertions(+), 34 deletions(-) create mode 100755 src/perception/camera_object_detection/config/eve_config.yaml delete mode 100755 src/perception/camera_object_detection/config/sim_config.yaml create mode 100755 src/perception/camera_object_detection/launch/eve_launch.py delete mode 100755 src/perception/camera_object_detection/launch/sim_launch.py diff --git a/modules/docker-compose.perception.yaml b/modules/docker-compose.perception.yaml index fd6ce9b3..3614af7d 100644 --- a/modules/docker-compose.perception.yaml +++ b/modules/docker-compose.perception.yaml @@ -28,7 +28,7 @@ services: - driver: nvidia count: 1 capabilities: [ gpu ] - command: /bin/bash -c "ros2 launch camera_object_detection nuscenes_launch.py" + command: /bin/bash -c "ros2 launch camera_object_detection eve_launch.py" volumes: - /mnt/wato-drive2/perception_models/yolov8s.pt:/perception_models/yolov8s.pt diff --git a/src/perception/camera_object_detection/config/eve_config.yaml b/src/perception/camera_object_detection/config/eve_config.yaml new file mode 100755 index 00000000..11687c55 --- /dev/null +++ b/src/perception/camera_object_detection/config/eve_config.yaml @@ -0,0 +1,23 @@ +left_camera_object_detection_node: + ros__parameters: + camera_topic: /camera/left/image_color + publish_vis_topic: /camera/left/annotated_img + publish_detection_topic: /camera/left/detections + model_path: /perception_models/yolov8s.pt + image_size: 480 + +center_camera_object_detection_node: + ros__parameters: + camera_topic: /camera/center/image_color + publish_vis_topic: /camera/center/annotated_img + publish_detection_topic: /camera/center/detections + model_path: /perception_models/yolov8s.pt + image_size: 480 + +right_camera_object_detection_node: + ros__parameters: + camera_topic: /camera/right/image_color + publish_vis_topic: /camera/right/annotated_img + publish_detection_topic: /camera/right/detections + model_path: /perception_models/yolov8s.pt + image_size: 480 diff --git a/src/perception/camera_object_detection/config/sim_config.yaml b/src/perception/camera_object_detection/config/sim_config.yaml deleted file mode 100755 index ec13a71b..00000000 --- a/src/perception/camera_object_detection/config/sim_config.yaml +++ /dev/null @@ -1,7 +0,0 @@ -camera_object_detection_node: - ros__parameters: - camera_topic: /camera/right/image_color - publish_vis_topic: /annotated_img - publish_obstacle_topic: /obstacles - model_path: /perception_models/yolov8s.pt - image_size: 480 diff --git a/src/perception/camera_object_detection/launch/eve_launch.py b/src/perception/camera_object_detection/launch/eve_launch.py new file mode 100755 index 00000000..94ac35df --- /dev/null +++ b/src/perception/camera_object_detection/launch/eve_launch.py @@ -0,0 +1,42 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + ld = LaunchDescription() + config = os.path.join( + get_package_share_directory('camera_object_detection'), + 'config', + 'eve_config.yaml' + ) + + # nodes + left_camera_object_detection_node = Node( + package='camera_object_detection', + executable='camera_object_detection_node', + name='left_camera_object_detection_node', + parameters=[config] + ) + + center_camera_object_detection_node = Node( + package='camera_object_detection', + executable='camera_object_detection_node', + name='center_camera_object_detection_node', + parameters=[config] + ) + + right_camera_object_detection_node = Node( + package='camera_object_detection', + executable='camera_object_detection_node', + name='right_camera_object_detection_node', + parameters=[config] + ) + + # finalize + ld.add_action(left_camera_object_detection_node) + ld.add_action(center_camera_object_detection_node) + ld.add_action(right_camera_object_detection_node) + + return ld diff --git a/src/perception/camera_object_detection/launch/sim_launch.py b/src/perception/camera_object_detection/launch/sim_launch.py deleted file mode 100755 index 12af5eb0..00000000 --- a/src/perception/camera_object_detection/launch/sim_launch.py +++ /dev/null @@ -1,26 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -import os - - -def generate_launch_description(): - ld = LaunchDescription() - config = os.path.join( - get_package_share_directory('camera_object_detection'), - 'config', - 'sim_config.yaml' - ) - - # nodes - camera_object_detection_node = Node( - package='camera_object_detection', - executable='camera_object_detection_node', - name='camera_object_detection_node', - parameters=[config] - ) - - # finalize - ld.add_action(camera_object_detection_node) - - return ld From cadee99ac5a54ad7f5b2a1a2bb12a0d676595b14 Mon Sep 17 00:00:00 2001 From: Jessica Ding Date: Thu, 22 Feb 2024 02:25:01 +0000 Subject: [PATCH 4/7] add completed test and launch file --- .../camera_object_detection_test.launch.py | 39 +++++++++ .../test/camera_object_detection_test.py | 82 +++++++++++++++---- 2 files changed, 104 insertions(+), 17 deletions(-) create mode 100644 src/perception/camera_object_detection/launch/camera_object_detection_test.launch.py diff --git a/src/perception/camera_object_detection/launch/camera_object_detection_test.launch.py b/src/perception/camera_object_detection/launch/camera_object_detection_test.launch.py new file mode 100644 index 00000000..67f1457f --- /dev/null +++ b/src/perception/camera_object_detection/launch/camera_object_detection_test.launch.py @@ -0,0 +1,39 @@ +from ament_index_python import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import IncludeLaunchDescription +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +import os + + +def generate_launch_description(): + + ld = LaunchDescription() + + config = os.path.join( + get_package_share_directory('camera_object_detection'), + 'launch', + 'camera_object_detection_test.launch.py' + ) + + # NODES + camera_object_detection_node = Node( + package='camera_object_detection', + executable='camera_object_detection_node', + name='camera_object_detection_node', + parameters=[{ + 'camera_topic': '/camera_pkg/display_mjpeg', + 'publish_vis_topic': '/annotated_img', + 'publish_obstacle_topic': '/obstacles', + 'model_path': '/perception_models/yolov8s.pt', + 'image_size': 480 + }] + ) + + ld.add_action(camera_object_detection_node) + + return ld \ No newline at end of file diff --git a/src/perception/camera_object_detection/test/camera_object_detection_test.py b/src/perception/camera_object_detection/test/camera_object_detection_test.py index bc2397c2..154d7019 100644 --- a/src/perception/camera_object_detection/test/camera_object_detection_test.py +++ b/src/perception/camera_object_detection/test/camera_object_detection_test.py @@ -1,36 +1,84 @@ +import os import unittest +import rclpy +from rcl_interfaces.msg import Log +import launch_testing +import launch +from launch.actions import ExecuteProcess +from ament_index_python.packages import get_package_share_directory +import pytest from sensor_msgs.msg import Image -import time from camera_object_detection.yolov8_detection import CameraDetectionNode -import rclpy -class TestCameraDetectionNode(unittest.TestCase): +@pytest.mark.launch_test +def generate_test_description(): + # Get launch file and set up for starting the test + pkg_share = get_package_share_directory('camera_object_detection') + launch_dir = os.path.join(pkg_share, 'launch') + camera_detection_launch = os.path.join(launch_dir, 'camera_object_detection_test.launch.py') + + return launch.LaunchDescription([ + ExecuteProcess( + cmd=['ros2', 'launch', camera_detection_launch], + output='screen', + additional_env={'PYTHONUNBUFFERED': '1'} + ), + # Delay the test until the ROS nodes are up and running + launch_testing.actions.ReadyToTest() + ]) - def test_image_processing(self): + +class TestCameraObjectDetectionLaunch(unittest.TestCase): + @classmethod + def setUpClass(cls): rclpy.init() + cls.node = CameraDetectionNode() # create a CameraDetectionNode to test + cls.logs_received = [] - node = CameraDetectionNode() + # Subscribe to /rosout where get_logger messages are outputted + cls.subscription = cls.node.create_subscription( + Log, + '/rosout', + cls.log_callback, + 10 + ) - # Create a dummy image message + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown() + + @classmethod + def log_callback(cls, msg: Log): + # Append the log message to the class's list of received messages + cls.logs_received.append(msg) + + def test_camera_detection_node_logging(self): + # create a dummy image to test dummy_image = Image() dummy_image.height = 480 dummy_image.width = 640 dummy_image.encoding = 'rgb8' dummy_image.step = dummy_image.width * 3 dummy_image.data = [255] * (dummy_image.step * dummy_image.height) + dummy_image.header.stamp = self.node.get_clock().now().to_msg() - node.image_callback(dummy_image) - # Delay to allow the callback to be called - time.sleep(1) - # Check if the publish method was called with a bounding box - # node.create_publisher().publish.assert_called() - node.destroy_node() - rclpy.shutdown() - - def listener_callback(self, msg): - self.msg_received = True - self.received_msg = msg + # Test the image_callback function of the CameraDetectionNode node + self.node.image_callback(dummy_image) + + # Spin some time to collect logs + end_time = self.node.get_clock().now() + rclpy.time.Duration(seconds=10) + while rclpy.ok() and self.node.get_clock().now() < end_time: + rclpy.spin_once(self.node, timeout_sec=0.1) + # Check if the expected log messages were received + expected_log_messages = ["Successfully created node listening on camera topic:", "Finished in:"] # From CameraDetectionNode logger + for expected_msg in expected_log_messages: + message_logged = any( + expected_msg in log.msg and log.name == "camera_object_detection_node" + for log in self.logs_received + ) + self.assertTrue(message_logged, f"Expected log message '{expected_msg}' not found in the logs.") if __name__ == '__main__': unittest.main() From 12693b704ae2a9dea6fa72c5d3fb495bd066029a Mon Sep 17 00:00:00 2001 From: Jessica Ding Date: Thu, 22 Feb 2024 03:03:02 +0000 Subject: [PATCH 5/7] add subscriber to /annotated_img in test --- .../test/camera_object_detection_test.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/perception/camera_object_detection/test/camera_object_detection_test.py b/src/perception/camera_object_detection/test/camera_object_detection_test.py index 154d7019..88971d41 100644 --- a/src/perception/camera_object_detection/test/camera_object_detection_test.py +++ b/src/perception/camera_object_detection/test/camera_object_detection_test.py @@ -34,6 +34,7 @@ def setUpClass(cls): rclpy.init() cls.node = CameraDetectionNode() # create a CameraDetectionNode to test cls.logs_received = [] + cls.images_received = [] # Subscribe to /rosout where get_logger messages are outputted cls.subscription = cls.node.create_subscription( @@ -43,6 +44,14 @@ def setUpClass(cls): 10 ) + # Subscribe to the /annotated_img publisher of CameraDetectionNode node + cls.subscription_images = cls.node.create_subscription( + Image, + '/annotated_img', + cls.image_callback, + 10 + ) + @classmethod def tearDownClass(cls): cls.node.destroy_node() @@ -53,6 +62,10 @@ def log_callback(cls, msg: Log): # Append the log message to the class's list of received messages cls.logs_received.append(msg) + @classmethod + def image_callback(cls, msg): + cls.images_received.append(msg) + def test_camera_detection_node_logging(self): # create a dummy image to test dummy_image = Image() @@ -80,5 +93,7 @@ def test_camera_detection_node_logging(self): ) self.assertTrue(message_logged, f"Expected log message '{expected_msg}' not found in the logs.") + self.assertTrue(len(self.images_received) > 0, "No visualization images were published.") # Check that given an image, the node is publishing + if __name__ == '__main__': unittest.main() From 1e21ca7fcb94b73db60cee7ad907b4378d920a28 Mon Sep 17 00:00:00 2001 From: Jessica Ding Date: Thu, 22 Feb 2024 03:21:40 +0000 Subject: [PATCH 6/7] autopep8 linting fixes --- .../test/camera_object_detection_test.py | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/perception/camera_object_detection/test/camera_object_detection_test.py b/src/perception/camera_object_detection/test/camera_object_detection_test.py index 88971d41..de6b9d48 100644 --- a/src/perception/camera_object_detection/test/camera_object_detection_test.py +++ b/src/perception/camera_object_detection/test/camera_object_detection_test.py @@ -10,12 +10,14 @@ from sensor_msgs.msg import Image from camera_object_detection.yolov8_detection import CameraDetectionNode + @pytest.mark.launch_test def generate_test_description(): # Get launch file and set up for starting the test pkg_share = get_package_share_directory('camera_object_detection') launch_dir = os.path.join(pkg_share, 'launch') - camera_detection_launch = os.path.join(launch_dir, 'camera_object_detection_test.launch.py') + camera_detection_launch = os.path.join( + launch_dir, 'camera_object_detection_test.launch.py') return launch.LaunchDescription([ ExecuteProcess( @@ -32,7 +34,7 @@ class TestCameraObjectDetectionLaunch(unittest.TestCase): @classmethod def setUpClass(cls): rclpy.init() - cls.node = CameraDetectionNode() # create a CameraDetectionNode to test + cls.node = CameraDetectionNode() # create a CameraDetectionNode to test cls.logs_received = [] cls.images_received = [] @@ -78,22 +80,28 @@ def test_camera_detection_node_logging(self): # Test the image_callback function of the CameraDetectionNode node self.node.image_callback(dummy_image) - + # Spin some time to collect logs end_time = self.node.get_clock().now() + rclpy.time.Duration(seconds=10) while rclpy.ok() and self.node.get_clock().now() < end_time: rclpy.spin_once(self.node, timeout_sec=0.1) # Check if the expected log messages were received - expected_log_messages = ["Successfully created node listening on camera topic:", "Finished in:"] # From CameraDetectionNode logger + # From CameraDetectionNode logger + expected_log_messages = [ + "Successfully created node listening on camera topic:", "Finished in:"] for expected_msg in expected_log_messages: message_logged = any( expected_msg in log.msg and log.name == "camera_object_detection_node" for log in self.logs_received ) - self.assertTrue(message_logged, f"Expected log message '{expected_msg}' not found in the logs.") + self.assertTrue( + message_logged, f"Expected log message '{expected_msg}' not found in the logs.") + + # Check that given an image, the node is publishing + self.assertTrue(len(self.images_received) > 0, + "No visualization images were published.") - self.assertTrue(len(self.images_received) > 0, "No visualization images were published.") # Check that given an image, the node is publishing if __name__ == '__main__': unittest.main() From a8acbbde81500b89549c6bd9a8c37355eb02cb87 Mon Sep 17 00:00:00 2001 From: Jessica Ding Date: Thu, 22 Feb 2024 03:25:28 +0000 Subject: [PATCH 7/7] autopep8 linting for launch file --- .../launch/camera_object_detection_test.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/perception/camera_object_detection/launch/camera_object_detection_test.launch.py b/src/perception/camera_object_detection/launch/camera_object_detection_test.launch.py index 67f1457f..60d09d19 100644 --- a/src/perception/camera_object_detection/launch/camera_object_detection_test.launch.py +++ b/src/perception/camera_object_detection/launch/camera_object_detection_test.launch.py @@ -36,4 +36,4 @@ def generate_launch_description(): ld.add_action(camera_object_detection_node) - return ld \ No newline at end of file + return ld