2828# Local imports
2929import image_qos_profile
3030
31- args = None
32-
3331
3432def encoding2mat (encoding ):
3533 if encoding .startswith ('mono' ):
@@ -51,30 +49,7 @@ def encoding2mat(encoding):
5149 return dtype , channels
5250
5351
54- def image_cb (msg ):
55- global args
56-
57- print ("Received image #" + msg .header .frame_id )
58-
59- if args .show_camera :
60- dtype , n_channels = encoding2mat (msg .encoding )
61- dtype = numpy .dtype (dtype )
62- dtype = dtype .newbyteorder ('>' if msg .is_bigendian else '<' )
63- if n_channels == 1 :
64- frame = numpy .ndarray (shape = (msg .height , msg .width ),
65- dtype = dtype , buffer = bytes (msg .data ))
66- else :
67- frame = numpy .ndarray (shape = (msg .height , msg .width , n_channels ),
68- dtype = dtype , buffer = bytes (msg .data ))
69-
70- cv2 .imshow ("showimagepy" , frame )
71- # Draw the image to the screen and wait 1 millisecond
72- cv2 .waitKey (1 )
73-
74-
7552def main ():
76- global args
77-
7853 # Pass command-line arguments to rclpy.
7954 rclpy .init (sys .argv )
8055
@@ -108,6 +83,25 @@ def main():
10883 args .reliability_policy ,
10984 args .keep )
11085
86+ def image_cb (msg ):
87+ print ("Received image #" + msg .header .frame_id )
88+
89+ if args .show_camera :
90+ dtype , n_channels = encoding2mat (msg .encoding )
91+ dtype = numpy .dtype (dtype )
92+ dtype = dtype .newbyteorder ('>' if msg .is_bigendian else '<' )
93+ if n_channels == 1 :
94+ frame = numpy .ndarray (shape = (msg .height , msg .width ),
95+ dtype = dtype , buffer = bytes (msg .data ))
96+ else :
97+ frame = numpy .ndarray (shape = (msg .height , msg .width , n_channels ),
98+ dtype = dtype , buffer = bytes (msg .data ))
99+
100+ cv2 .imshow ("showimagepy" , frame )
101+ # Draw the image to the screen and wait 1 millisecond
102+ cv2 .waitKey (1 )
103+
104+
111105 node .create_subscription (sensor_msgs .msg .Image , 'image' ,
112106 image_cb , custom_qos_profile )
113107
0 commit comments