1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
|
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Software License Agreement (BSD License)
#
# Copyright (c) 2015, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of the Willow Garage nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
"""Save images of multiple topics with timestamp synchronization.
Usage: rosrun image_view extract_images_sync _inputs:='[<topic_0>, <topic_1>]'
"""
import sys
import cv2
import cv_bridge
import message_filters
import rospy
from sensor_msgs.msg import Image
class ExtractImagesSync(object):
def __init__(self):
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)
if img_topics is None:
rospy.logwarn("""\
extract_images_sync: rosparam '~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>]'""")
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)
else:
sync = message_filters.TimeSynchronizer(
subs, queue_size=100)
sync.registerCallback(self.save)
def save(self, *imgmsgs):
seq = self.seq
bridge = cv_bridge.CvBridge()
for i, imgmsg in enumerate(imgmsgs):
img = bridge.imgmsg_to_cv2(imgmsg)
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, encoding_in=encoding_in, encoding_out='',
do_dynamic_scaling=self.do_dynamic_scaling)
fname = self.fname_fmt % (seq, i)
print('Save image as {0}'.format(fname))
cv2.imwrite(fname, img)
self.seq = seq + 1
if __name__ == '__main__':
rospy.init_node('extract_images_sync')
extractor = ExtractImagesSync()
rospy.spin()
|
