From 0866bf14dff4842f9f9716a0464226b82be1fad9 Mon Sep 17 00:00:00 2001 From: Siddharth Vaghela Date: Wed, 31 Jan 2024 14:48:03 -0500 Subject: [PATCH] Port extract_images_sync script to ROS 2 (#919) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Change Description: The extract_images_sync python script was upgraded to ROS 2. Testing: Upgraded node tested against Iron on Ubuntu 22.04.3 LTS Issue: fixes #860 --------- Co-authored-by: Alejandro Hernández Cordero Co-authored-by: Michael Ferguson --- .gitignore | 1 + image_view/CMakeLists.txt | 5 +++ image_view/package.xml | 2 + image_view/scripts/extract_images_sync | 60 ++++++++++++++------------ 4 files changed, 41 insertions(+), 27 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/CMakeLists.txt b/image_view/CMakeLists.txt index da98a0979..1148b0669 100644 --- a/image_view/CMakeLists.txt +++ b/image_view/CMakeLists.txt @@ -109,4 +109,9 @@ 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/package.xml b/image_view/package.xml index 81fc59932..1ec083f5f 100644 --- a/image_view/package.xml +++ b/image_view/package.xml @@ -31,6 +31,8 @@ std_srvs stereo_msgs + rclpy + ament_lint_auto ament_lint_common diff --git a/image_view/scripts/extract_images_sync b/image_view/scripts/extract_images_sync index cd86dddc6..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) # @@ -33,47 +33,50 @@ # 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 --ros-args -p 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 \ -_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): 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 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): @@ -92,8 +95,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()