From 2fac17b851ee0630822d9cb53fd754a417a69ab7 Mon Sep 17 00:00:00 2001 From: Siddharth Vaghela Date: Mon, 29 Jan 2024 13:24:08 -0500 Subject: [PATCH 1/9] Upgraded extract_images_sync script to ROS2 Upgraded extract_images_sync script to ROS2 --- .gitignore | 1 + image_view/package.xml | 2 + image_view/scripts/extract_images_sync | 59 ++++++++++++++------------ 3 files changed, 36 insertions(+), 26 deletions(-) diff --git a/.gitignore b/.gitignore index 72723e50a..e347708b0 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ *pyc +.vscode/settings.json diff --git a/image_view/package.xml b/image_view/package.xml index 81fc59932..3f480ed2b 100644 --- a/image_view/package.xml +++ b/image_view/package.xml @@ -12,6 +12,7 @@ Joshua Whitley Jacob Perron Michael Ferguson + Siddharth Vaghela BSD https://index.ros.org/p/image_view/github-ros-perception-image_pipeline/ @@ -25,6 +26,7 @@ cv_bridge image_transport message_filters + rclpy rclcpp rclcpp_components sensor_msgs diff --git a/image_view/scripts/extract_images_sync b/image_view/scripts/extract_images_sync index cd86dddc6..266751925 100755 --- a/image_view/scripts/extract_images_sync +++ b/image_view/scripts/extract_images_sync @@ -33,47 +33,51 @@ # POSSIBILITY OF SUCH DAMAGE. """Save images of multiple topics with timestamp synchronization. -Usage: rosrun image_view extract_images_sync _inputs:='[, ]' +Usage: ros2 run image_view extract_images_sync _inputs:='[, ]' """ import sys - import cv2 - import cv_bridge -import message_filters -import rospy -from sensor_msgs.msg import Image +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from message_filters import ApproximateTimeSynchronizer, TimeSynchronizer, Subscriber -class ExtractImagesSync(object): +class ExtractImagesSync(Node): def __init__(self): + super().__init__('extract_images_sync') + self.get_logger().info('Extract_Images_Sync Node has been started') self.seq = 0 - self.fname_fmt = rospy.get_param( - '~filename_format', 'frame%04i_%i.jpg') - self.do_dynamic_scaling = rospy.get_param( - '~do_dynamic_scaling', False) - img_topics = rospy.get_param('~inputs', None) + self.fname_fmt = self.declare_parameter( + 'filename_format', 'frame%04i_%i.jpg').value + self.do_dynamic_scaling = self.declare_parameter( + 'do_dynamic_scaling', False).value + img_topics = self.declare_parameter('_inputs', None).value + if img_topics is None: - rospy.logwarn("""\ -extract_images_sync: rosparam '~inputs' has not been specified! \ + self.get_logger().warn("""\ +extract_images_sync: Parameter '_inputs' has not been specified! \ Typical command-line usage: -\t$ rosrun image_view extract_images_sync _inputs:= -\t$ rosrun image_view extract_images_sync \ +\t$ ros2 run image_view extract_images_sync _inputs:= +\t$ ros2 run image_view extract_images_sync \ _inputs:='[, ]'""") sys.exit(1) + if not isinstance(img_topics, list): img_topics = [img_topics] + subs = [] for t in img_topics: - subs.append(message_filters.Subscriber(t, Image)) - if rospy.get_param('~approximate_sync', False): - sync = message_filters.ApproximateTimeSynchronizer( - subs, queue_size=100, slop=.1) + subs.append(Subscriber(self, Image, t)) + + self.approximate_sync = self.declare_parameter('approximate_sync', False).value; + if not self.approximate_sync: + sync = ApproximateTimeSynchronizer(subs, 100, slop=0.1) else: - sync = message_filters.TimeSynchronizer( - subs, queue_size=100) + sync = TimeSynchronizer(subs, 100) sync.registerCallback(self.save) def save(self, *imgmsgs): @@ -84,7 +88,7 @@ _inputs:='[, ]'""") channels = img.shape[2] if img.ndim == 3 else 1 encoding_in = bridge.dtype_with_channels_to_cvtype2( img.dtype, channels) - img = cv_bridge.cvtColorForDisplay( + img = cv_bridge.cvtColor_for_display( img, encoding_in=encoding_in, encoding_out='', do_dynamic_scaling=self.do_dynamic_scaling) fname = self.fname_fmt % (seq, i) @@ -92,8 +96,11 @@ _inputs:='[, ]'""") cv2.imwrite(fname, img) self.seq = seq + 1 +def main(args=None): + rclpy.init(args=args) + extractor = ExtractImagesSync() + rclpy.spin(extractor) + rclpy.shutdown() if __name__ == '__main__': - rospy.init_node('extract_images_sync') - extractor = ExtractImagesSync() - rospy.spin() + main() From dfd3d8d2e52d4e71f0a8abe81894fa70f6300d31 Mon Sep 17 00:00:00 2001 From: Siddharth Vaghela Date: Mon, 29 Jan 2024 15:29:12 -0500 Subject: [PATCH 2/9] Update package.xml --- image_view/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/image_view/package.xml b/image_view/package.xml index 3f480ed2b..c4b75a5df 100644 --- a/image_view/package.xml +++ b/image_view/package.xml @@ -12,7 +12,6 @@ Joshua Whitley Jacob Perron Michael Ferguson - Siddharth Vaghela BSD https://index.ros.org/p/image_view/github-ros-perception-image_pipeline/ @@ -26,13 +25,14 @@ cv_bridge image_transport message_filters - rclpy rclcpp rclcpp_components sensor_msgs std_srvs stereo_msgs + rclpy + ament_lint_auto ament_lint_common From 0f534d8c96e4b17b9f0abf001401f90aa2af498b Mon Sep 17 00:00:00 2001 From: Siddharth Vaghela Date: Mon, 29 Jan 2024 15:35:50 -0500 Subject: [PATCH 3/9] Update extract_images_sync to incorporate feedback --- image_view/scripts/extract_images_sync | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/image_view/scripts/extract_images_sync b/image_view/scripts/extract_images_sync index 266751925..54bde528c 100755 --- a/image_view/scripts/extract_images_sync +++ b/image_view/scripts/extract_images_sync @@ -33,7 +33,7 @@ # POSSIBILITY OF SUCH DAMAGE. """Save images of multiple topics with timestamp synchronization. -Usage: ros2 run image_view extract_images_sync _inputs:='[, ]' +Usage: ros2 run image_view extract_images_sync --ros-args -p inputs:='[, ]' """ import sys @@ -55,15 +55,14 @@ class ExtractImagesSync(Node): 'filename_format', 'frame%04i_%i.jpg').value self.do_dynamic_scaling = self.declare_parameter( 'do_dynamic_scaling', False).value - img_topics = self.declare_parameter('_inputs', None).value + img_topics = self.declare_parameter('inputs', None).value if img_topics is None: self.get_logger().warn("""\ -extract_images_sync: Parameter '_inputs' has not been specified! \ +extract_images_sync: Parameter 'inputs' has not been specified! \ Typical command-line usage: -\t$ ros2 run image_view extract_images_sync _inputs:= -\t$ ros2 run image_view extract_images_sync \ -_inputs:='[, ]'""") +\t$ ros2 run image_view extract_images_sync --ros-args -p inputs:= +\t$ ros2 run image_view extract_images_sync --ros-args -p inputs:='[, ]'""") sys.exit(1) if not isinstance(img_topics, list): @@ -74,7 +73,7 @@ _inputs:='[, ]'""") subs.append(Subscriber(self, Image, t)) self.approximate_sync = self.declare_parameter('approximate_sync', False).value; - if not self.approximate_sync: + if self.approximate_sync: sync = ApproximateTimeSynchronizer(subs, 100, slop=0.1) else: sync = TimeSynchronizer(subs, 100) From 02e738c0b4ec743458555d7f71c48803687ebe6b Mon Sep 17 00:00:00 2001 From: Siddharth Vaghela Date: Mon, 29 Jan 2024 16:11:03 -0500 Subject: [PATCH 4/9] Update extract_images_sync to revert change to cvtColorForDisplay api call --- image_view/scripts/extract_images_sync | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/image_view/scripts/extract_images_sync b/image_view/scripts/extract_images_sync index 54bde528c..035e31e2a 100755 --- a/image_view/scripts/extract_images_sync +++ b/image_view/scripts/extract_images_sync @@ -87,7 +87,7 @@ Typical command-line usage: channels = img.shape[2] if img.ndim == 3 else 1 encoding_in = bridge.dtype_with_channels_to_cvtype2( img.dtype, channels) - img = cv_bridge.cvtColor_for_display( + img = cv_bridge.cvtColorForDisplay( img, encoding_in=encoding_in, encoding_out='', do_dynamic_scaling=self.do_dynamic_scaling) fname = self.fname_fmt % (seq, i) From d8ada8d694f3fd5880b86ed7dd47fe202720613a Mon Sep 17 00:00:00 2001 From: Siddharth Vaghela Date: Tue, 30 Jan 2024 09:11:23 -0500 Subject: [PATCH 5/9] Update image_view/package.xml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- image_view/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/image_view/package.xml b/image_view/package.xml index c4b75a5df..1ec083f5f 100644 --- a/image_view/package.xml +++ b/image_view/package.xml @@ -31,7 +31,7 @@ std_srvs stereo_msgs - rclpy + rclpy ament_lint_auto ament_lint_common From 38a311691ecca1a2d5cca706c1ebf1ea42aa4f0c Mon Sep 17 00:00:00 2001 From: Siddharth Vaghela Date: Tue, 30 Jan 2024 11:33:05 -0500 Subject: [PATCH 6/9] Update extract_images_sync to add sec_per_frame parameter --- image_view/scripts/extract_images_sync | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/image_view/scripts/extract_images_sync b/image_view/scripts/extract_images_sync index 035e31e2a..a203308f7 100755 --- a/image_view/scripts/extract_images_sync +++ b/image_view/scripts/extract_images_sync @@ -51,10 +51,18 @@ class ExtractImagesSync(Node): super().__init__('extract_images_sync') self.get_logger().info('Extract_Images_Sync Node has been started') self.seq = 0 + self.time = self.get_clock().now() self.fname_fmt = self.declare_parameter( - 'filename_format', 'frame%04i_%i.jpg').value + 'filename_format', 'frame%04i_%i.jpg').value + + # Do not scale dynamically by default. self.do_dynamic_scaling = self.declare_parameter( 'do_dynamic_scaling', False).value + + # Limit the throughput to 10fps by default. + self.sec_per_frame = self.declare_parameter( + 'sec_per_frame', 0.1).value + img_topics = self.declare_parameter('inputs', None).value if img_topics is None: @@ -80,6 +88,17 @@ Typical command-line usage: sync.registerCallback(self.save) def save(self, *imgmsgs): + delay = self.get_clock().now() - self.time + + # Decimation is enabled if it is set to greater than + # zero. If it is enabled, exit if delay is less than + # the decimation rate specified. + if (self.sec_per_frame > 0) and (delay < self.sec_per_frame): + return + + # Update the current time + self.time = self.get_clock().now() + seq = self.seq bridge = cv_bridge.CvBridge() for i, imgmsg in enumerate(imgmsgs): From a8f4382bf6153221161c589a15099c8fb474e056 Mon Sep 17 00:00:00 2001 From: Siddharth Vaghela Date: Wed, 31 Jan 2024 09:28:43 -0500 Subject: [PATCH 7/9] Revert "Update extract_images_sync to add sec_per_frame parameter" This reverts commit 38a311691ecca1a2d5cca706c1ebf1ea42aa4f0c. --- image_view/scripts/extract_images_sync | 21 +-------------------- 1 file changed, 1 insertion(+), 20 deletions(-) diff --git a/image_view/scripts/extract_images_sync b/image_view/scripts/extract_images_sync index a203308f7..035e31e2a 100755 --- a/image_view/scripts/extract_images_sync +++ b/image_view/scripts/extract_images_sync @@ -51,18 +51,10 @@ class ExtractImagesSync(Node): super().__init__('extract_images_sync') self.get_logger().info('Extract_Images_Sync Node has been started') self.seq = 0 - self.time = self.get_clock().now() self.fname_fmt = self.declare_parameter( - 'filename_format', 'frame%04i_%i.jpg').value - - # Do not scale dynamically by default. + 'filename_format', 'frame%04i_%i.jpg').value self.do_dynamic_scaling = self.declare_parameter( 'do_dynamic_scaling', False).value - - # Limit the throughput to 10fps by default. - self.sec_per_frame = self.declare_parameter( - 'sec_per_frame', 0.1).value - img_topics = self.declare_parameter('inputs', None).value if img_topics is None: @@ -88,17 +80,6 @@ Typical command-line usage: sync.registerCallback(self.save) def save(self, *imgmsgs): - delay = self.get_clock().now() - self.time - - # Decimation is enabled if it is set to greater than - # zero. If it is enabled, exit if delay is less than - # the decimation rate specified. - if (self.sec_per_frame > 0) and (delay < self.sec_per_frame): - return - - # Update the current time - self.time = self.get_clock().now() - seq = self.seq bridge = cv_bridge.CvBridge() for i, imgmsg in enumerate(imgmsgs): From c91c305eef63224f9dd81d65906504156bdb8336 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Wed, 31 Jan 2024 14:25:14 -0500 Subject: [PATCH 8/9] add install, change shebang to python3 --- image_view/CMakeLists.txt | 4 ++++ image_view/scripts/extract_images_sync | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/image_view/CMakeLists.txt b/image_view/CMakeLists.txt index da98a0979..19610df04 100644 --- a/image_view/CMakeLists.txt +++ b/image_view/CMakeLists.txt @@ -109,4 +109,8 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +install(PROGRAMS + scripts/extract_images_sync + DESTINATION lib/${PROJECT_NAME}) + ament_auto_package() diff --git a/image_view/scripts/extract_images_sync b/image_view/scripts/extract_images_sync index 035e31e2a..ec28c9667 100755 --- a/image_view/scripts/extract_images_sync +++ b/image_view/scripts/extract_images_sync @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # -*- coding: utf-8 -*- # Software License Agreement (BSD License) # From ef85850ae516361b44b76e97d514f4f9fe7ced84 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Wed, 31 Jan 2024 14:28:49 -0500 Subject: [PATCH 9/9] clean up formatting of cmake --- image_view/CMakeLists.txt | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/image_view/CMakeLists.txt b/image_view/CMakeLists.txt index 19610df04..1148b0669 100644 --- a/image_view/CMakeLists.txt +++ b/image_view/CMakeLists.txt @@ -109,8 +109,9 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -install(PROGRAMS - scripts/extract_images_sync - DESTINATION lib/${PROJECT_NAME}) +install( + PROGRAMS scripts/extract_images_sync + DESTINATION lib/${PROJECT_NAME} +) ament_auto_package()