Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Upgraded extract_images_sync script to ROS 2 #919

Merged
merged 9 commits into from
Jan 31, 2024
1 change: 1 addition & 0 deletions .gitignore
Original file line number Diff line number Diff line change
@@ -1 +1,2 @@
*pyc
.vscode/settings.json
2 changes: 2 additions & 0 deletions image_view/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,8 @@
<depend>std_srvs</depend>
<depend>stereo_msgs</depend>

<exec_depend>rclpy</depend>
artineering marked this conversation as resolved.
Show resolved Hide resolved

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

Expand Down
58 changes: 32 additions & 26 deletions image_view/scripts/extract_images_sync
Original file line number Diff line number Diff line change
Expand Up @@ -33,47 +33,50 @@
# POSSIBILITY OF SUCH DAMAGE.
"""Save images of multiple topics with timestamp synchronization.

Usage: rosrun image_view extract_images_sync _inputs:='[<topic_0>, <topic_1>]'
Usage: ros2 run image_view extract_images_sync --ros-args -p inputs:='[<topic_0>, <topic_1>]'
"""

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:=<image_topic>
\t$ rosrun image_view extract_images_sync \
_inputs:='[<image_topic>, <image_topic>]'""")
\t$ ros2 run image_view extract_images_sync --ros-args -p inputs:=<image_topic>
\t$ ros2 run image_view extract_images_sync --ros-args -p inputs:='[<image_topic>, <image_topic>]'""")
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):
Expand All @@ -92,8 +95,11 @@ _inputs:='[<image_topic>, <image_topic>]'""")
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()