From 64b8ab2e84ba856d03e49b55e5f9e4e9aeffd46e Mon Sep 17 00:00:00 2001 From: Siddharth Vaghela Date: Wed, 31 Jan 2024 15:46:53 -0500 Subject: [PATCH 1/4] Update extract_images_sync to add sec_per_frame parameter --- image_view/scripts/extract_images_sync | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/image_view/scripts/extract_images_sync b/image_view/scripts/extract_images_sync index ec28c9667..f6b60d8ad 100755 --- a/image_view/scripts/extract_images_sync +++ b/image_view/scripts/extract_images_sync @@ -51,10 +51,19 @@ 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 +89,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 40fcdd2e9668ffcb0becbfd619a4df2b10a74cef Mon Sep 17 00:00:00 2001 From: Siddharth Vaghela Date: Wed, 31 Jan 2024 16:09:42 -0500 Subject: [PATCH 2/4] Update extract_images_sync 1. Set default value for sec_per_frame to 0. 2. changed time variable to last_frame_time. --- image_view/scripts/extract_images_sync | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/image_view/scripts/extract_images_sync b/image_view/scripts/extract_images_sync index f6b60d8ad..7a89ed426 100755 --- a/image_view/scripts/extract_images_sync +++ b/image_view/scripts/extract_images_sync @@ -51,7 +51,7 @@ 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.last_frame_time = self.get_clock().now() self.fname_fmt = self.declare_parameter( 'filename_format', 'frame%04i_%i.jpg').value @@ -62,7 +62,7 @@ class ExtractImagesSync(Node): # Limit the throughput to 10fps by default. self.sec_per_frame = self.declare_parameter( - 'sec_per_frame', 0.1).value + 'sec_per_frame', 0).value img_topics = self.declare_parameter('inputs', None).value @@ -89,7 +89,7 @@ Typical command-line usage: sync.registerCallback(self.save) def save(self, *imgmsgs): - delay = self.get_clock().now() - self.time + delay = self.get_clock().now() - self.last_frame_time # Decimation is enabled if it is set to greater than # zero. If it is enabled, exit if delay is less than @@ -98,7 +98,7 @@ Typical command-line usage: return # Update the current time - self.time = self.get_clock().now() + self.last_frame_time = self.get_clock().now() seq = self.seq bridge = cv_bridge.CvBridge() From 4062818dd863d182125ea5f7a759381d81c70c94 Mon Sep 17 00:00:00 2001 From: Siddharth Vaghela Date: Wed, 31 Jan 2024 16:19:18 -0500 Subject: [PATCH 3/4] Update extract_images_sync Ensure delay is compared with the sec_per_frame value as a float. --- 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 7a89ed426..6e787d850 100755 --- a/image_view/scripts/extract_images_sync +++ b/image_view/scripts/extract_images_sync @@ -94,7 +94,7 @@ Typical command-line usage: # 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): + if (self.sec_per_frame > 0) and (delay.to_sec() < self.sec_per_frame): return # Update the current time From 3dc23f46496cffee046798b5e217f15225f9c41a Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Wed, 31 Jan 2024 16:30:15 -0500 Subject: [PATCH 4/4] make param float, proper comparison --- image_view/scripts/extract_images_sync | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/image_view/scripts/extract_images_sync b/image_view/scripts/extract_images_sync index 6e787d850..445ceac67 100755 --- a/image_view/scripts/extract_images_sync +++ b/image_view/scripts/extract_images_sync @@ -42,6 +42,7 @@ import cv_bridge import rclpy from rclpy.node import Node +from rclpy.duration import Duration from sensor_msgs.msg import Image from message_filters import ApproximateTimeSynchronizer, TimeSynchronizer, Subscriber @@ -62,7 +63,7 @@ class ExtractImagesSync(Node): # Limit the throughput to 10fps by default. self.sec_per_frame = self.declare_parameter( - 'sec_per_frame', 0).value + 'sec_per_frame', 0.0).value img_topics = self.declare_parameter('inputs', None).value @@ -94,7 +95,7 @@ Typical command-line usage: # 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.to_sec() < self.sec_per_frame): + if (self.sec_per_frame > 0) and (delay < Duration(seconds=self.sec_per_frame)): return # Update the current time