Skip to content

Commit ff19924

Browse files
committed
Review feedback.
Signed-off-by: Chris Lalancette <[email protected]>
1 parent 745fc5f commit ff19924

File tree

4 files changed

+78
-64
lines changed

4 files changed

+78
-64
lines changed

image_tools_py/README.md

Lines changed: 41 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,16 @@
11
## Image Tools Demo in Python
22

3-
This is a demonstration of the Quality of Service (QoS) features of ROS 2 using Python.
3+
This is a demonstration of the Quality of Service (QoS) features of ROS 2 using
4+
Python.
45
There are two programs implemented here: cam2image_py, and showimage_py.
56

67
### CAM2IMAGE_PY
7-
This is a Python program that will take data from an attached camera, and publish the
8-
data to a topic called "image", with the type sensor_msgs/msg/Image. If a camera
9-
isn't available, this program can also generate a default image and smoothly "move"
10-
it across the screen, simulating motion. The usage output from the program looks like
11-
this:
8+
This is a Python program that will take data from an attached camera, and
9+
publish the data to a topic called "image", with the type
10+
`sensor_msgs/msg/Image`.
11+
If a camera isn't available, this program can also generate a default image and
12+
smoothly "move" it across the screen, simulating motion. The usage output from
13+
the program looks like this:
1214

1315
```
1416
usage: cam2image_py [-h] [-b] [-d DEPTH] [-f FREQUENCY] [-k {0,1}] [-r {0,1}]
@@ -39,25 +41,30 @@ optional arguments:
3941
Image height (default: 240)
4042
```
4143

42-
The -d, -k, and -r parameters control various aspects of the QoS implementation, and
43-
are the most interesting to play with when testing out QoS.
44+
The `-d`, `-k`, and `-r` parameters control various aspects of the QoS
45+
implementation, and are the most interesting to play with when testing out QoS.
4446

4547
Note that this program also subscribes to a topic called "flip_image" of type
46-
std_msgs/msg/Bool. If flip_image is set to False, the data coming out of the camera
47-
is sent as usual. If flip_image is set to True, the data coming out of the camera is
48-
flipped around the Y axis.
49-
50-
If the -s parameter is set to 1, then this program opens up a (local) window to show
51-
the images that are being published. However, these images are *not* coming in through
52-
the ROS 2 pub/sub model, so this window cannot show off the QoS parameters (it is mostly
53-
useful for debugging). See SHOWIMAGE_PY below for a program that can show QoS over the
54-
pub/sub model.
48+
`std_msgs/msg/Bool`.
49+
If "flip_image" is set to `False`, the data coming out of the camera is sent as
50+
usual.
51+
If "flip_image" is set to `True`, the data coming out of the camera is flipped
52+
around the Y axis.
53+
54+
If the `-s` parameter is set to 1, then this program opens up a (local) window
55+
to show the images that are being published.
56+
However, these images are *not* coming in through the ROS 2 pub/sub model, so
57+
this window cannot show off the QoS parameters (it is mostly useful for
58+
debugging).
59+
See SHOWIMAGE_PY below for a program that can show QoS over the pub/sub model.
5560

5661
### SHOWIMAGE_PY
57-
This is a Python program that subscribes to the "image" topic, waiting for data. As
58-
new data comes in, this program accepts the data and can optionally render it to
59-
the screen. The usage output from the program looks like this:
62+
This is a Python program that subscribes to the "image" topic, waiting for data.
63+
As new data comes in, this program accepts the data and can optionally render
64+
it to the screen.
65+
The usage output from the program looks like this:
6066

67+
```
6168
usage: showimage_py [-h] [-d QUEUE_DEPTH] [-k {0,1}] [-r {0,1}] [-s {0,1}]
6269
[-t TOPIC]
6370
@@ -75,32 +82,34 @@ optional arguments:
7582
Show the camera stream (default: 1)
7683
-t TOPIC, --topic TOPIC
7784
use topic TOPIC instead of the default (default: image)
85+
```
7886

79-
The -d, -k, and -r parameters control various aspects of the QoS implementation, and
80-
are the most interesting to play with when testing out QoS.
87+
The `-d`, `-k`, and `-r` parameters control various aspects of the QoS
88+
implementation, and are the most interesting to play with when testing out QoS.
8189

82-
If the -s parameter is set to 1, then this program opens up a window to show the images
83-
that have been received over the ROS 2 pub/sub model. This program should be used
84-
in conjunction with cam2image_py to demonstrate the ROS 2 QoS capabilities over lossy/slow
85-
links.
90+
If the `-s` parameter is set to 1, then this program opens up a window to show
91+
the images that have been received over the ROS 2 pub/sub model.
92+
This program should be used in conjunction with cam2image_py to demonstrate the
93+
ROS 2 QoS capabilities over lossy/slow links.
8694

8795
### EXAMPLE USAGE
8896
To use the above programs, you would run them something like the following:
8997

90-
# In the first terminal, run the data publisher. This will connect to the 1st camera
91-
# available, and print out "Publishing image #" for each image it publishes.
98+
In the first terminal, run the data publisher.
99+
This will connect to the first camera available, and print out
100+
"Publishing image #" for each image it publishes.
92101
```
93102
$ ros2 run image_tools_py cam2image_py
94103
```
95104

96-
# If you don't have a local camera, you can use the -b parameter to generate data on
97-
# the fly rather than get data from a camera:
105+
If you don't have a local camera, you can use the `-b` parameter to generate
106+
data on the fly rather than get data from a camera:
98107
```
99108
$ ros2 run image_tools_py cam2image_py -b
100109
```
101110

102-
# In a second terminal, run the data subscriber. This will subscribe to the "image"
103-
# topic and render any frames it receives.
111+
In a second terminal, run the data subscriber.
112+
This will subscribe to the "image" topic and render any frames it receives.
104113
```
105114
$ ros2 run image_tools_py showimage_py
106115
```

image_tools_py/cam2image_py.py

Lines changed: 32 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -163,36 +163,39 @@ def flip_image_cb(msg):
163163

164164
# Our main event loop will spin until the user presses CTRL-C to exit.
165165
frame_number = 1
166-
while rclpy.ok():
167-
# Get the frame from the video capture.
168-
if args.burger_mode:
169-
frame = burger_cap.render_burger(args.width, args.height)
170-
else:
171-
ret, frame = cam_cap.read()
172-
173-
# Check if the frame was grabbed correctly.
174-
if frame is not None:
175-
# Convert to a ROS image
176-
if is_flipped:
177-
# Flip the frame if needed
178-
flipped_frame = cv2.flip(frame, 1)
179-
convert_frame_to_message(flipped_frame, frame_number, msg)
166+
try:
167+
while rclpy.ok():
168+
# Get the frame from the video capture.
169+
if args.burger_mode:
170+
frame = burger_cap.render_burger(args.width, args.height)
180171
else:
181-
convert_frame_to_message(frame, frame_number, msg)
182-
183-
if args.show_camera == 1:
184-
# Show the image in a window called 'cam2image_py', if requested.
185-
cv2.imshow('cam2image_py', frame)
186-
# Draw the image to the screen and wait 1 millisecond
187-
cv2.waitKey(1)
188-
189-
# Publish the image message and increment the frame_id.
190-
node.get_logger().info('Publishing image #%d' % (frame_number))
191-
pub.publish(msg)
192-
frame_number += 1
193-
194-
# Do some work in rclpy and wait for more to come in.
195-
rclpy.spin_once(node, timeout_sec=1.0 / float(args.frequency))
172+
ret, frame = cam_cap.read()
173+
174+
# Check if the frame was grabbed correctly.
175+
if frame is not None:
176+
# Convert to a ROS image
177+
if is_flipped:
178+
# Flip the frame if needed
179+
flipped_frame = cv2.flip(frame, 1)
180+
convert_frame_to_message(flipped_frame, frame_number, msg)
181+
else:
182+
convert_frame_to_message(frame, frame_number, msg)
183+
184+
if args.show_camera == 1:
185+
# Show the image in a window called 'cam2image_py', if requested.
186+
cv2.imshow('cam2image_py', frame)
187+
# Draw the image to the screen and wait 1 millisecond
188+
cv2.waitKey(1)
189+
190+
# Publish the image message and increment the frame_id.
191+
node.get_logger().info('Publishing image #%d' % (frame_number))
192+
pub.publish(msg)
193+
frame_number += 1
194+
195+
# Do some work in rclpy and wait for more to come in.
196+
rclpy.spin_once(node, timeout_sec=1.0 / float(args.frequency))
197+
except KeyboardInterrupt:
198+
pass
196199

197200

198201
if __name__ == '__main__':

image_tools_py/showimage_py.py

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -114,8 +114,11 @@ def image_cb(msg):
114114
node.create_subscription(
115115
sensor_msgs.msg.Image, args.topic, image_cb, qos_profile=custom_qos_profile)
116116

117-
while rclpy.ok():
118-
rclpy.spin_once(node)
117+
try:
118+
while rclpy.ok():
119+
rclpy.spin_once(node)
120+
except KeyboardInterrupt:
121+
pass
119122

120123

121124
if __name__ == '__main__':

image_tools_py/test/test_showimage_cam2image.py.in

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -110,5 +110,4 @@ class TestExecutablesDemo(unittest.TestCase):
110110

111111
def test_reliable_qos(self):
112112
self._ls.run()
113-
self._ls.shutdown()
114113
self.assertTrue(self._saw_cam2image_output and self._saw_showimage_output)

0 commit comments

Comments
 (0)