From 6372f18e9b4944342a2ec93fa6464c13080d4b66 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 6 Jan 2017 14:44:25 -0800 Subject: [PATCH 01/16] Add in a demo of camera capture with python tools. This is useful for showing off dropped messages with QOS changes. Signed-off-by: Chris Lalancette --- image_tools_py/README | 109 ++++++++++++++ image_tools_py/burger_py.py | 109 ++++++++++++++ image_tools_py/cam2image_py.py | 195 +++++++++++++++++++++++++ image_tools_py/package.xml | 26 ++++ image_tools_py/resource/image_tools_py | 0 image_tools_py/setup.cfg | 4 + image_tools_py/setup.py | 41 ++++++ image_tools_py/showimage_py.py | 123 ++++++++++++++++ image_tools_py/test/test_copyright.py | 23 +++ image_tools_py/test/test_pep257.py | 23 +++ 10 files changed, 653 insertions(+) create mode 100644 image_tools_py/README create mode 100644 image_tools_py/burger_py.py create mode 100644 image_tools_py/cam2image_py.py create mode 100644 image_tools_py/package.xml create mode 100644 image_tools_py/resource/image_tools_py create mode 100644 image_tools_py/setup.cfg create mode 100644 image_tools_py/setup.py create mode 100644 image_tools_py/showimage_py.py create mode 100644 image_tools_py/test/test_copyright.py create mode 100644 image_tools_py/test/test_pep257.py diff --git a/image_tools_py/README b/image_tools_py/README new file mode 100644 index 000000000..42230ae69 --- /dev/null +++ b/image_tools_py/README @@ -0,0 +1,109 @@ +This is a demonstration of the Quality of Service (QoS) features of ROS 2 using Python. +There are two programs implemented here: cam2image_py, and showimage_py. Note that in +order for these programs to work, an OpenCV binding for Python3 must be available. As +of this writing (January 11, 2017), only OpenCV 3 or later supports Python3. Instructions +for compiling OpenCV3 for Python3 are available here: + +http://stackoverflow.com/questions/20953273/install-opencv-for-python-3-3 + +The condensed rundown that works on Ubuntu16.04 and will install to /usr/local is: +$ sudo apt install python3 build-essential cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev python3.5-dev libpython3-dev python3-numpy libtbb2 libtbb-dev libjpeg-dev libpng-dev libtiff-dev libjasper-dev libdc1394-22-dev +$ git clone https://github.com/opencv/opencv +$ cd opencv +$ git checkout 3.2.0 +$ mkdir release +$ cd release +$ cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr/local .. +$ make -j8 +$ sudo make install + +CAM2IMAGE_PY +------------ +This is a Python program that will take data from an attached camera, and publish the +data to a topic called "image", with the type sensor_msgs::msg::Image. If a camera +isn't available, this program can also generate a default image and smoothly "move" +it across the screen, simulating motion. The usage output from the program looks like +this: + +usage: cam2image_py.py [-h] [-b] [-d QUEUE_DEPTH] [-f FREQUENCY] [-k {0,1}] + [-r {0,1}] [-s {0,1}] [-x WIDTH] [-y HEIGHT] + +optional arguments: + -h, --help show this help message and exit + -b, --burger Produce images of burgers rather than connecting to a + camera (default: False) + -d QUEUE_DEPTH, --depth QUEUE_DEPTH + Queue depth (default: 10) + -f FREQUENCY, --frequency FREQUENCY + Publish frequency in Hz (default: 30) + -k {0,1}, --keep {0,1} + History QoS setting, 0 - keep last sample, 1 - keep + all the samples (default: 1) + -r {0,1}, --reliability {0,1} + Reliability QoS setting, 0 - best effort, 1 - reliable + (default: 1) + -s {0,1}, --show {0,1} + Show the camera stream (default: 0) + -x WIDTH, --width WIDTH + Image width (default: 320) + -y HEIGHT, --height HEIGHT + Image height (default: 240) + +The -d, -k, and -r parameters control various aspects of the QoS implementation, and +are the most interesting to play with when testing out QoS. + +Note that this program also subscribes to a topic called "flip_image" of type +std_msgs::msg::Bool. If flip_image is set to False, the data coming out of the camera +is sent as usual. If flip_image is set to True, the data coming out of the camera is +flipped around the Y axis. + +If the -s parameter is set to 1, then this program opens up a (local) window to show +the images that are being published. However, these images are *not* coming in through +the ROS 2 pub/sub model, so this window cannot show off the QoS parameters (it is mostly +useful for debugging). See SHOWIMAGE_PY below for a program that can show QoS over the +pub/sub model. + +SHOWIMAGE_PY +------------ +This is a Python program that subscribes to the "image" topic, waiting for data. As +new data comes in, this program accepts the data and can optionally render it to +the screen. The usage output from the program looks like this: + +usage: showimage_py.py [-h] [-d QUEUE_DEPTH] [-k {0,1}] [-r {0,1}] [-s {0,1}] + +optional arguments: + -h, --help show this help message and exit + -d QUEUE_DEPTH, --depth QUEUE_DEPTH + Queue depth (default: 10) + -k {0,1}, --keep {0,1} + History QoS setting, 0 - keep last sample, 1 - keep + all the samples (default: 1) + -r {0,1}, --reliability {0,1} + Reliability QoS setting, 0 - best effort, 1 - reliable + (default: 1) + -s {0,1}, --show {0,1} + Show the camera stream (default: 0) + +The -d, -k, and -r parameters control various aspects of the QoS implementation, and +are the most interesting to play with when testing out QoS. + +If the -s parameter is set to 1, then this program opens up a window to show the images +that have been received over the ROS 2 pub/sub model. This program should be used +in conjunction with CAM2IMAGE_PY to demonstrate the ROS 2 QoS capabilities over lossy/slow +links. + +EXAMPLE USAGE +------------- +To use the above programs, you would run them something like the following: + +# In the first terminal, run the data publisher. This will connect to the 1st camera +# available, and print out "Publishing image #" for each image it publishes. +$ python3 cam2image_py.py + +# In a second terminal, run the data subscriber. This will subscribe to the "image" +# topic and render any frames it receives. +$ python3 showimage_py.py -s 1 + +# If you don't have a local camera, you can use the -b parameter to generate data on +# the fly rather than get data from a camera: +$ python3 cam2image_py.py -b diff --git a/image_tools_py/burger_py.py b/image_tools_py/burger_py.py new file mode 100644 index 000000000..dc29e985b --- /dev/null +++ b/image_tools_py/burger_py.py @@ -0,0 +1,109 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# System imports +import base64 +import random + +# OpenCV +import cv2 + +# Numpy +import numpy + +# THE FOLLOWING IS A BURGER IN BASE64. RESPECT IT + +BURGER_BASE64 = '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' # noqa + + +class Burger(object): + + def __init__(self): + burger_png = base64.b64decode(BURGER_BASE64) + np_img = numpy.fromstring(burger_png, dtype=numpy.uint8) + self.burger_template = cv2.imdecode(np_img, cv2.IMREAD_COLOR) + + # We flood fill the burger template with "1,1,1" (BGR) so that we can + # remove the borders during rendering. + h, w = self.burger_template.shape[:2] + mask = numpy.zeros((h + 2, w + 2), numpy.uint8) + cv2.floodFill(self.burger_template, mask, (1, 1), (1, 1, 1)) + + random.seed() + + self.width = 0 + self.height = 0 + self.num_burgers = 0 + self.burger_list = [] + + def render_burger(self, width, height): + # The basic idea here is to render a number of burgers into a OpenCV mat, + # moving them on each successful iteration. So when the requested + # resolution changes (this includes the very first time we are called), + # we generate a random number of burgers, at random starting locations, + # moving at random speeds, and store that list of burgers in the object. + # We then render the burgers onto the mat and return it. On subsequent + # render_burger() method calls, we keep the same list of burgers from + # before, but move them according to their X and Y speed, "bouncing" them + # off of the side of the mat if they run into it. + class OneBurger(object): + + def __init__(self, x, y, x_inc, y_inc): + self.x = x + self.y = y + self.x_inc = x_inc + self.y_inc = y_inc + + if self.width != width or self.height != height: + num_burgers = random.randrange(2, 10) + width_max = width - self.burger_template.shape[1] - 1 + height_max = height - self.burger_template.shape[0] - 1 + for b in range(0, num_burgers): + x = random.randrange(0, width_max) + y = random.randrange(0, height_max) + x_inc = random.randrange(1, 3) + y_inc = random.randrange(1, 3) + self.burger_list.append(OneBurger(x, y, x_inc, y_inc)) + self.width = width + self.height = height + + # We want an OpenCV Mat with CV_8UC3, which is 3 channels + burger_buf = numpy.zeros((height, width, 3), numpy.uint8) + + # TODO(clalancette): This is the slow way to do this, in that we iterate + # over every pixel by hand, looking for the flood fill and thus whether + # we should render that pixel. However, I was not able to figure out the + # OpenCV python calls to do this in a nicer way, so we leave this for now. + for b in self.burger_list: + for y in range(0, self.burger_template.shape[0]): + for x in range(0, self.burger_template.shape[1]): + bitem = self.burger_template.item(y, x, 0) + gitem = self.burger_template.item(y, x, 1) + ritem = self.burger_template.item(y, x, 2) + if bitem != 1 or gitem != 1 or ritem != 1: + burger_buf.itemset((b.y + y, b.x + x, 0), bitem) + burger_buf.itemset((b.y + y, b.x + x, 1), gitem) + burger_buf.itemset((b.y + y, b.x + x, 2), ritem) + b.x += b.x_inc + b.y += b.y_inc + + # Bounce if needed + if b.x < 0 or b.x > (width - self.burger_template.shape[1] - 1): + b.x_inc *= -1 + b.x += 2 * b.x_inc + if b.y < 0 or b.y > (height - self.burger_template.shape[0] - 1): + b.y_inc *= -1 + b.y += 2 * b.y_inc + + return burger_buf diff --git a/image_tools_py/cam2image_py.py b/image_tools_py/cam2image_py.py new file mode 100644 index 000000000..d6990b811 --- /dev/null +++ b/image_tools_py/cam2image_py.py @@ -0,0 +1,195 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# System imports +import argparse +import sys + +# Local imports +import burger_py + +# OpenCV imports +import cv2 + +# ROS2 imports +import rclpy +from rclpy.qos import qos_profile_default, qos_profile_sensor_data +import sensor_msgs.msg +import std_msgs.msg + + +# Convert an OpenCV matrix encoding type to a string format recognized by +# sensor_msgs::Image +def mat2encoding(frame): + encoding = '' + + encodings = {1: 'mono', 3: 'bgr', 4: 'rgba'} + for channels, prefix in encodings.items(): + if frame.shape[2] == channels: + encoding += prefix + break + else: + raise ValueError('Unsupported frame shape %d' % (frame.shape[2])) + + types = {'uint8': '8', 'int16': '16'} + for dtype, num in types.items(): + if frame.dtype == dtype: + encoding += num + break + else: + raise ValueError('Unsupported frame type ' + frame.dtype) + + return encoding + + +# Convert an OpenCV matrix to a ROS Image message. +def convert_frame_to_message(frame, frame_id, msg): + msg.height = frame.shape[0] + msg.width = frame.shape[1] + msg.encoding = mat2encoding(frame) + msg.data = frame.data.tobytes() + msg.step = int(len(msg.data) / msg.height) + msg.header.frame_id = str(frame_id) + + +def main(args=None): + # Pass command-line arguments to rclpy. + rclpy.init(args=args) + + # Parse the command-line options. + parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument( + '-b', '--burger', dest='burger_mode', action='store_true', default=False, + help='Produce images of burgers rather than connecting to a camera') + parser.add_argument( + '-d', '--depth', dest='depth', action='store', default=qos_profile_default.depth, type=int, + help='Queue depth') + parser.add_argument( + '-f', '--frequency', dest='frequency', action='store', default=30, type=int, + help='Publish frequency in Hz') + parser.add_argument( + '-k', '--keep', dest='history_policy', action='store', default=qos_profile_default.history, + type=int, choices=[0, 1], + help='History QoS setting, 0 - keep last sample, 1 - keep all the samples') + parser.add_argument( + '-r', '--reliability', dest='reliability_policy', action='store', + default=qos_profile_default.reliability, type=int, choices=[0, 1], + help='Reliability QoS setting, 0 - best effort, 1 - reliable') + parser.add_argument( + '-s', '--show', dest='show_camera', action='store', default=0, type=int, choices=[0, 1], + help='Show the camera stream') + parser.add_argument( + '-x', '--width', dest='width', action='store', default=320, type=int, + help='Image width') + parser.add_argument( + '-y', '--height', dest='height', action='store', default=240, type=int, + help='Image height') + args = parser.parse_args() + + # Initialize a ROS 2 node to publish images read from the OpenCV interface to + # the camera. + node = rclpy.create_node('cam2imagepy') + + # Set the parameters of the quality of service profile. Initialize as the + # default profile and set the QoS parameters specified on the command line. + custom_camera_qos_profile = qos_profile_default + + # Depth represents how many messages to store in history when the history policy is KEEP_LAST + custom_camera_qos_profile.depth = args.depth + + # The reliability policy can be reliable, meaning that the underlying transport layer will try + # ensure that every message gets received in order, or best effort, meaning that the transport + # makes no guarantees about the order or reliability of delivery. + custom_camera_qos_profile.reliability = args.reliability_policy + + # The history policy determines how messages are saved until the message is taken by the reader + # KEEP_ALL saves all messages until they are taken. + # KEEP_LAST enforces a limit on the number of messages that are saved, specified by the "depth" + # parameter. + custom_camera_qos_profile.history = args.history_policy + + # Create the image publisher with our custom QoS profile. + pub = node.create_publisher( + sensor_msgs.msg.Image, 'image', + qos_profile=custom_camera_qos_profile) + + is_flipped = False + + msg = sensor_msgs.msg.Image() + msg.is_bigendian = False + + def flip_image_cb(msg): + nonlocal is_flipped + + is_flipped = msg.data + + output = 'on' if is_flipped else 'off' + + print('Set flip mode to: ' + output) + + custom_flip_qos_profile = qos_profile_sensor_data + custom_flip_qos_profile.depth = 10 + node.create_subscription( + std_msgs.msg.Bool, 'flip_image', flip_image_cb, qos_profile=custom_flip_qos_profile) + + if args.burger_mode: + burger_cap = burger_py.Burger() + else: + # Initialize OpenCV video capture stream. Always open device 0. + cam_cap = cv2.VideoCapture(0) + + # Set the width and height based on command-line arguments. + cam_cap.set(cv2.CAP_PROP_FRAME_WIDTH, args.width) + cam_cap.set(cv2.CAP_PROP_FRAME_HEIGHT, args.height) + if not cam_cap.isOpened(): + print('Could not open video stream') + sys.exit(1) + + # Our main event loop will spin until the user presses CTRL-C to exit. + frame_number = 1 + while rclpy.ok(): + # Get the frame from the video capture. + frame = None + if args.burger_mode: + frame = burger_cap.render_burger(args.width, args.height) + else: + ret, frame = cam_cap.read() + + # Check if the frame was grabbed correctly. + if frame is not None: + # Convert to a ROS image + if is_flipped: + # Flip the frame if needed + flipped_frame = cv2.flip(frame, 1) + convert_frame_to_message(flipped_frame, frame_number, msg) + else: + convert_frame_to_message(frame, frame_number, msg) + + if args.show_camera == 1: + # Show the image in a window called 'cam2image_py', if requested. + cv2.imshow('cam2image_py', frame) + # Draw the image to the screen and wait 1 millisecond + cv2.waitKey(1) + + # Publish the image message and increment the frame_id. + print('Publishing image #%d' % (frame_number)) + pub.publish(msg) + frame_number += 1 + + # Do some work in rclpy and wait for more to come in. + rclpy.spin_once(node, timeout_sec=1.0 / float(args.frequency)) + + +if __name__ == '__main__': + main() diff --git a/image_tools_py/package.xml b/image_tools_py/package.xml new file mode 100644 index 000000000..f5ba062b6 --- /dev/null +++ b/image_tools_py/package.xml @@ -0,0 +1,26 @@ + + + + image_tools_py + 0.0.0 + Python tools to capture/play back images to/from DDS subscriptions/publications. + Chris Lalancette + Apache License 2.0 + Chris Lalancette + + python3-opencv + rclpy + std_msgs + sensor_msgs + + + ament_copyright + ament_pep257 + ament_pep8 + ament_pyflakes + + + ament_python + + diff --git a/image_tools_py/resource/image_tools_py b/image_tools_py/resource/image_tools_py new file mode 100644 index 000000000..e69de29bb diff --git a/image_tools_py/setup.cfg b/image_tools_py/setup.cfg new file mode 100644 index 000000000..3778bb622 --- /dev/null +++ b/image_tools_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/image_tools_py +[install] +install-scripts=$base/lib/image_tools_py diff --git a/image_tools_py/setup.py b/image_tools_py/setup.py new file mode 100644 index 000000000..6a9f95949 --- /dev/null +++ b/image_tools_py/setup.py @@ -0,0 +1,41 @@ +from setuptools import setup + +package_name = 'image_tools_py' + +setup( + name=package_name, + version='0.0.0', + packages=[], + py_modules=[ + 'burger_py', + 'cam2image_py', + 'showimage_py', + ], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + author='Chris Lalancette', + author_email='clalancette@osrfoundation.org', + maintainer='Chris Lalancette', + maintainer_email='clalancette@osrfoundation.org', + keywords=['ROS'], + classifiers=[ + 'Intended Audience :: Developers', + 'License :: OSI Approved :: Apache Software License', + 'Programming Language :: Python', + 'Topic :: Software Development', + ], + description='Python tools to capture/play back images to/from DDS subscriptions/publications.', + license='Apache License, Version 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'cam2image_py = cam2image_py:main', + 'showimage_py = showimage_py:main', + ], + }, +) diff --git a/image_tools_py/showimage_py.py b/image_tools_py/showimage_py.py new file mode 100644 index 000000000..7c2aeeaa4 --- /dev/null +++ b/image_tools_py/showimage_py.py @@ -0,0 +1,123 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# System imports +import argparse + +# OpenCV imports +import cv2 + +# Numpy imports +import numpy + +# ROS2 imports +import rclpy +from rclpy.qos import qos_profile_default +import sensor_msgs.msg + + +def encoding2mat(encoding): + encodings = {'mono': 1, 'bgr': 3, 'rgba': 4} + for prefix, channels in encodings.items(): + if encoding.startswith(prefix): + break + else: + raise ValueError('Unsupported encoding ' + encoding) + + types = {'8': 'uint8', '16': 'int16'} + for prefix, dtype in types.items(): + if encoding[channels:] == prefix: + break + else: + raise ValueError('Unsupported encoding ' + encoding) + + return dtype, channels + + +def main(args=None): + # Pass command-line arguments to rclpy. + rclpy.init(args=args) + + # Parse the command-line options. + parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument( + '-d', '--depth', dest='depth', action='store', default=qos_profile_default.depth, type=int, + help='Queue depth') + parser.add_argument( + '-k', '--keep', dest='history_policy', action='store', default=qos_profile_default.history, + type=int, choices=[0, 1], + help='History QoS setting, 0 - keep last sample, 1 - keep all the samples') + parser.add_argument( + '-r', '--reliability', dest='reliability_policy', action='store', + default=qos_profile_default.reliability, type=int, choices=[0, 1], + help='Reliability QoS setting, 0 - best effort, 1 - reliable') + parser.add_argument( + '-s', '--show', dest='show_camera', action='store', default=1, type=int, choices=[0, 1], + help='Show the camera stream') + args = parser.parse_args() + + if args.show_camera == 1: + cv2.namedWindow('showimage_py') + + # Initialize a ROS 2 node to subscribe to images read from the OpenCV interface to + # the camera. + node = rclpy.create_node('showimagepy') + + custom_qos_profile = qos_profile_default + + # Depth represents how many messages to store in history when the history policy is KEEP_LAST. + custom_qos_profile.depth = args.depth + + # The reliability policy can be reliable, meaning that the underlying transport layer will try + # ensure that every message gets received in order, or best effort, meaning that the transport + # makes no guarantees about the order or reliability of delivery. + custom_qos_profile.reliability = args.reliability_policy + + # The history policy determines how messages are saved until the message is taken by the reader + # KEEP_ALL saves all messages until they are taken. + # KEEP_LAST enforces a limit on the number of messages that are saved, specified by the "depth" + # parameter. + custom_qos_profile.history = args.history_policy + + def image_cb(msg): + print('Received image #' + msg.header.frame_id) + + if args.show_camera: + dtype, n_channels = encoding2mat(msg.encoding) + dtype = numpy.dtype(dtype) + dtype = dtype.newbyteorder('>' if msg.is_bigendian else '<') + if n_channels == 1: + frame = numpy.ndarray( + shape=(msg.height, msg.width), + dtype=dtype, + buffer=bytes(msg.data)) + else: + frame = numpy.ndarray( + shape=(msg.height, msg.width, n_channels), + dtype=dtype, + buffer=bytes(msg.data)) + + cv2.imshow('showimage_py', frame) + # Draw the image to the screen and wait 1 millisecond + cv2.waitKey(1) + + node.create_subscription( + sensor_msgs.msg.Image, 'image', image_cb, qos_profile=custom_qos_profile) + + while rclpy.ok(): + rclpy.spin_once(node) + + +if __name__ == '__main__': + main() diff --git a/image_tools_py/test/test_copyright.py b/image_tools_py/test/test_copyright.py new file mode 100644 index 000000000..06965244d --- /dev/null +++ b/image_tools_py/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.']) + assert rc == 0, 'Found errors' diff --git a/image_tools_py/test/test_pep257.py b/image_tools_py/test/test_pep257.py new file mode 100644 index 000000000..c0b87bd81 --- /dev/null +++ b/image_tools_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.']) + assert rc == 0, 'Found code style errors / warnings' From bbde938a8afaae91b8d19a5c91ba3989a5e24b05 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 18 Jul 2018 19:08:06 +0000 Subject: [PATCH 02/16] Add in a test for showimage_py and cam2image_py. Signed-off-by: Chris Lalancette --- image_tools_py/cam2image_py.py | 5 + image_tools_py/package.xml | 4 +- image_tools_py/setup.py | 48 ++++++++- image_tools_py/test/cam2image_py.txt | 2 + image_tools_py/test/showimage_py.regex | 1 + image_tools_py/test/test_flake8.py | 23 ++++ .../test/test_showimage_cam2image.py.in | 102 ++++++++++++++++++ 7 files changed, 181 insertions(+), 4 deletions(-) create mode 100644 image_tools_py/test/cam2image_py.txt create mode 100644 image_tools_py/test/showimage_py.regex create mode 100644 image_tools_py/test/test_flake8.py create mode 100644 image_tools_py/test/test_showimage_cam2image.py.in diff --git a/image_tools_py/cam2image_py.py b/image_tools_py/cam2image_py.py index d6990b811..03fec02f0 100644 --- a/image_tools_py/cam2image_py.py +++ b/image_tools_py/cam2image_py.py @@ -89,6 +89,9 @@ def main(args=None): parser.add_argument( '-s', '--show', dest='show_camera', action='store', default=0, type=int, choices=[0, 1], help='Show the camera stream') + parser.add_argument( + '-t', '--topic', dest='topic', action='store', default='image', type=str, + help='Topic to publish on') parser.add_argument( '-x', '--width', dest='width', action='store', default=320, type=int, help='Image width') @@ -119,6 +122,8 @@ def main(args=None): # parameter. custom_camera_qos_profile.history = args.history_policy + print("Publishing data on topic '%s'" % (args.topic)) + # Create the image publisher with our custom QoS profile. pub = node.create_publisher( sensor_msgs.msg.Image, 'image', diff --git a/image_tools_py/package.xml b/image_tools_py/package.xml index f5ba062b6..cc2a15022 100644 --- a/image_tools_py/package.xml +++ b/image_tools_py/package.xml @@ -4,9 +4,9 @@ image_tools_py 0.0.0 Python tools to capture/play back images to/from DDS subscriptions/publications. - Chris Lalancette + Chris Lalancette Apache License 2.0 - Chris Lalancette + Chris Lalancette python3-opencv rclpy diff --git a/image_tools_py/setup.py b/image_tools_py/setup.py index 6a9f95949..ffd654d9b 100644 --- a/image_tools_py/setup.py +++ b/image_tools_py/setup.py @@ -1,7 +1,50 @@ +import os + from setuptools import setup +from setuptools.command.install_scripts import install_scripts package_name = 'image_tools_py' + +# This configuration deserves some explanation. The goal is to generate a +# test file called something like 'test_showimage_cam2image__rmw_fastrtps_.py' +# that contains the test from test_showimage_cam2image.py.in, customized +# for the rmw_implementation we want to test and for absolute paths of this +# installation. To do that, we override the 'install_scripts' stage of +# setuptools, since that is the stage that knows the paths to the 'cam2image_py' +# and 'showimage_py' executables. +# +# TODO(clalancette): One downside to what is being done below is that the output +# file is being left in the source directory. That's currently necessary +# because the pytest invocation only looks there for test scripts. It would +# be nicer to generate the output file in the 'build' directory and then point +# pytest at it, but I don't know how to do that. +class custom_install_scripts(install_scripts): + + def run(self): + rmw_implementations = ['rmw_fastrtps_cpp'] + + for rmw_impl in rmw_implementations: + substs = { + '@rmw_implementation@': rmw_impl, + '@showimage_py_exe@': os.path.join(self.install_dir, 'showimage_py'), + '@showimage_py_outfile@': os.path.realpath(os.path.join('test', 'showimage_py')), + '@cam2image_py_exe@': os.path.join(self.install_dir, 'cam2image_py'), + '@cam2image_py_outfile@': os.path.realpath(os.path.join('test', 'cam2image_py')), + } + + infile = os.path.join('test', 'test_showimage_cam2image.py.in') + outfile = os.path.join('test', 'test_showimage_cam2image__%s_.py' % (rmw_impl)) + with open(infile, 'r') as infp: + with open(outfile, 'w') as outfp: + for line in infp: + for subst in substs: + line = line.replace(subst, "'%s' # noqa" % (substs[subst])) + outfp.write(line) + + install_scripts.run(self) + + setup( name=package_name, version='0.0.0', @@ -19,9 +62,9 @@ install_requires=['setuptools'], zip_safe=True, author='Chris Lalancette', - author_email='clalancette@osrfoundation.org', + author_email='clalancette@openrobotics.org', maintainer='Chris Lalancette', - maintainer_email='clalancette@osrfoundation.org', + maintainer_email='clalancette@openrobotics.org', keywords=['ROS'], classifiers=[ 'Intended Audience :: Developers', @@ -38,4 +81,5 @@ 'showimage_py = showimage_py:main', ], }, + cmdclass={'install_scripts': custom_install_scripts}, ) diff --git a/image_tools_py/test/cam2image_py.txt b/image_tools_py/test/cam2image_py.txt new file mode 100644 index 000000000..f03d92ed2 --- /dev/null +++ b/image_tools_py/test/cam2image_py.txt @@ -0,0 +1,2 @@ +Publishing data on topic 'image' +Publishing image #1 diff --git a/image_tools_py/test/showimage_py.regex b/image_tools_py/test/showimage_py.regex new file mode 100644 index 000000000..fe331b99a --- /dev/null +++ b/image_tools_py/test/showimage_py.regex @@ -0,0 +1 @@ +Received image #\d+ diff --git a/image_tools_py/test/test_flake8.py b/image_tools_py/test/test_flake8.py new file mode 100644 index 000000000..eff829969 --- /dev/null +++ b/image_tools_py/test/test_flake8.py @@ -0,0 +1,23 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc = main(argv=[]) + assert rc == 0, 'Found errors' diff --git a/image_tools_py/test/test_showimage_cam2image.py.in b/image_tools_py/test/test_showimage_cam2image.py.in new file mode 100644 index 000000000..7d5d34893 --- /dev/null +++ b/image_tools_py/test/test_showimage_cam2image.py.in @@ -0,0 +1,102 @@ +# Copyright 2016 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from launch.legacy import LaunchDescriptor +from launch.legacy.exit_handler import ignore_exit_handler +from launch.legacy.exit_handler import primary_ignore_returncode_exit_handler +from launch.legacy.launcher import DefaultLauncher +from launch.legacy.output_handler import ConsoleOutput +from launch_testing import create_handler + + +def setup(): + os.environ['OSPL_VERBOSITY'] = '8' # 8 = OS_NONE + # bare minimum formatting for console output matching + os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' + + +def test_reliable_qos(): + pub_executable_args = ['-r', '1', '-s', '0', '-b', '-f', '5'] + sub_executable_args = ['-r', '1', '-s', '0'] + _test_executables(pub_executable_args, sub_executable_args) + + +def _test_executables(publisher_executable_args, subscriber_executable_args): + output_handlers = [] + launch_descriptor = LaunchDescriptor() + + rmw_implementation = @rmw_implementation@ + env = dict(os.environ) + env['RMW_IMPLEMENTATION'] = rmw_implementation + env['PYTHONUNBUFFERED'] = '1' + + # Launch the process that will receive the images. + # This is the process that gets to decide when to tear the launcher down. + # It will exit when the regex for receiving images is matched. + showimage_executable = @showimage_py_exe@ + showimage_output_file = @showimage_py_outfile@ + showimage_name = 'test_showimage_py' + showimage_handler = create_handler( + showimage_name, launch_descriptor, showimage_output_file, exit_on_match=True, + filtered_rmw_implementation=rmw_implementation) + assert showimage_handler, 'Cannot find appropriate handler for %s' % showimage_output_file + output_handlers.append(showimage_handler) + + command = [showimage_executable] + command.extend(subscriber_executable_args) + launch_descriptor.add_process( + cmd=command, + name=showimage_name, + exit_handler=primary_ignore_returncode_exit_handler, + output_handlers=[showimage_handler, ConsoleOutput()], + env=env, + ) + + # Launch the process that will publish the images. + # This process will be exited when the launcher is torn down. + cam2image_executable = @cam2image_py_exe@ + cam2image_output_file = @cam2image_py_outfile@ + cam2image_name = 'test_cam2image_py' + cam2image_handler = create_handler( + cam2image_name, launch_descriptor, cam2image_output_file, exit_on_match=False, + filtered_rmw_implementation=rmw_implementation) + assert cam2image_handler, 'Cannot find appropriate handler for %s' % cam2image_output_file + output_handlers.append(cam2image_handler) + + command = [cam2image_executable] + command.extend(publisher_executable_args) + launch_descriptor.add_process( + cmd=command, + name=cam2image_name, + exit_handler=ignore_exit_handler, + output_handlers=[cam2image_handler, ConsoleOutput()], + env=env, + ) + + launcher = DefaultLauncher() + launcher.add_launch_descriptor(launch_descriptor) + rc = launcher.launch() + + assert rc == 0, \ + "The launch file failed with exit code '" + str(rc) + "'. " + + # Check that the output received by the handlers is what was expected. + for handler in output_handlers: + handler.check() + + +if __name__ == '__main__': + test_reliable_qos() From f87a10aa13c2b3c0962ba9a8c8a5486366be0f4a Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 19 Dec 2018 14:44:40 +0000 Subject: [PATCH 03/16] Add in some test_depend. Signed-off-by: Chris Lalancette --- image_tools_py/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/image_tools_py/package.xml b/image_tools_py/package.xml index cc2a15022..db577b1a8 100644 --- a/image_tools_py/package.xml +++ b/image_tools_py/package.xml @@ -16,9 +16,11 @@ ament_copyright + ament_flake8 ament_pep257 ament_pep8 ament_pyflakes + launch ament_python From 99dfdd7a33abbbd389837459aeaeef8fc7501230 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 19 Dec 2018 15:00:38 +0000 Subject: [PATCH 04/16] One more test_depend. Signed-off-by: Chris Lalancette --- image_tools_py/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/image_tools_py/package.xml b/image_tools_py/package.xml index db577b1a8..ce5ec81f7 100644 --- a/image_tools_py/package.xml +++ b/image_tools_py/package.xml @@ -21,6 +21,7 @@ ament_pep8 ament_pyflakes launch + launch_testing ament_python From 0fc7d54f4f9c29e548f0eb6ddcab3794ede64a30 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 27 Dec 2018 22:18:04 +0000 Subject: [PATCH 05/16] Make sure to use raw strings for Windows-like paths. Signed-off-by: Chris Lalancette --- image_tools_py/setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/image_tools_py/setup.py b/image_tools_py/setup.py index ffd654d9b..3bc919b08 100644 --- a/image_tools_py/setup.py +++ b/image_tools_py/setup.py @@ -39,7 +39,7 @@ def run(self): with open(outfile, 'w') as outfp: for line in infp: for subst in substs: - line = line.replace(subst, "'%s' # noqa" % (substs[subst])) + line = line.replace(subst, "r'%s' # noqa" % (substs[subst])) outfp.write(line) install_scripts.run(self) From ca256692d4a8ad9308636c2241e23844d7a6082a Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 28 Dec 2018 16:43:59 +0000 Subject: [PATCH 06/16] Switch from print to using the logger. Signed-off-by: Chris Lalancette --- image_tools_py/cam2image_py.py | 8 ++++---- image_tools_py/showimage_py.py | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/image_tools_py/cam2image_py.py b/image_tools_py/cam2image_py.py index 03fec02f0..37bebb067 100644 --- a/image_tools_py/cam2image_py.py +++ b/image_tools_py/cam2image_py.py @@ -122,7 +122,7 @@ def main(args=None): # parameter. custom_camera_qos_profile.history = args.history_policy - print("Publishing data on topic '%s'" % (args.topic)) + node.get_logger().info("Publishing data on topic '%s'" % (args.topic)) # Create the image publisher with our custom QoS profile. pub = node.create_publisher( @@ -141,7 +141,7 @@ def flip_image_cb(msg): output = 'on' if is_flipped else 'off' - print('Set flip mode to: ' + output) + node.get_logger().info('Set flip mode to: ' + output) custom_flip_qos_profile = qos_profile_sensor_data custom_flip_qos_profile.depth = 10 @@ -158,7 +158,7 @@ def flip_image_cb(msg): cam_cap.set(cv2.CAP_PROP_FRAME_WIDTH, args.width) cam_cap.set(cv2.CAP_PROP_FRAME_HEIGHT, args.height) if not cam_cap.isOpened(): - print('Could not open video stream') + node.get_logger().fatal('Could not open video stream') sys.exit(1) # Our main event loop will spin until the user presses CTRL-C to exit. @@ -188,7 +188,7 @@ def flip_image_cb(msg): cv2.waitKey(1) # Publish the image message and increment the frame_id. - print('Publishing image #%d' % (frame_number)) + node.get_logger().info('Publishing image #%d' % (frame_number)) pub.publish(msg) frame_number += 1 diff --git a/image_tools_py/showimage_py.py b/image_tools_py/showimage_py.py index 7c2aeeaa4..34d2cfcb3 100644 --- a/image_tools_py/showimage_py.py +++ b/image_tools_py/showimage_py.py @@ -91,7 +91,7 @@ def main(args=None): custom_qos_profile.history = args.history_policy def image_cb(msg): - print('Received image #' + msg.header.frame_id) + node.get_logger().info('Received image #' + msg.header.frame_id) if args.show_camera: dtype, n_channels = encoding2mat(msg.encoding) From ecffc52e394fb6119d3b01125e00d4c3eb53c776 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 17 May 2019 19:36:34 +0000 Subject: [PATCH 07/16] Update to latest APIs. Signed-off-by: Chris Lalancette --- image_tools_py/cam2image_py.py | 10 +++++----- image_tools_py/showimage_py.py | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/image_tools_py/cam2image_py.py b/image_tools_py/cam2image_py.py index 37bebb067..2f45a1086 100644 --- a/image_tools_py/cam2image_py.py +++ b/image_tools_py/cam2image_py.py @@ -24,7 +24,7 @@ # ROS2 imports import rclpy -from rclpy.qos import qos_profile_default, qos_profile_sensor_data +from rclpy.qos import qos_profile_system_default, qos_profile_sensor_data import sensor_msgs.msg import std_msgs.msg @@ -73,18 +73,18 @@ def main(args=None): '-b', '--burger', dest='burger_mode', action='store_true', default=False, help='Produce images of burgers rather than connecting to a camera') parser.add_argument( - '-d', '--depth', dest='depth', action='store', default=qos_profile_default.depth, type=int, + '-d', '--depth', dest='depth', action='store', default=qos_profile_system_default.depth, type=int, help='Queue depth') parser.add_argument( '-f', '--frequency', dest='frequency', action='store', default=30, type=int, help='Publish frequency in Hz') parser.add_argument( - '-k', '--keep', dest='history_policy', action='store', default=qos_profile_default.history, + '-k', '--keep', dest='history_policy', action='store', default=qos_profile_system_default.history, type=int, choices=[0, 1], help='History QoS setting, 0 - keep last sample, 1 - keep all the samples') parser.add_argument( '-r', '--reliability', dest='reliability_policy', action='store', - default=qos_profile_default.reliability, type=int, choices=[0, 1], + default=qos_profile_system_default.reliability, type=int, choices=[0, 1], help='Reliability QoS setting, 0 - best effort, 1 - reliable') parser.add_argument( '-s', '--show', dest='show_camera', action='store', default=0, type=int, choices=[0, 1], @@ -106,7 +106,7 @@ def main(args=None): # Set the parameters of the quality of service profile. Initialize as the # default profile and set the QoS parameters specified on the command line. - custom_camera_qos_profile = qos_profile_default + custom_camera_qos_profile = qos_profile_system_default # Depth represents how many messages to store in history when the history policy is KEEP_LAST custom_camera_qos_profile.depth = args.depth diff --git a/image_tools_py/showimage_py.py b/image_tools_py/showimage_py.py index 34d2cfcb3..daf864f81 100644 --- a/image_tools_py/showimage_py.py +++ b/image_tools_py/showimage_py.py @@ -23,7 +23,7 @@ # ROS2 imports import rclpy -from rclpy.qos import qos_profile_default +from rclpy.qos import qos_profile_system_default import sensor_msgs.msg @@ -52,15 +52,15 @@ def main(args=None): # Parse the command-line options. parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument( - '-d', '--depth', dest='depth', action='store', default=qos_profile_default.depth, type=int, + '-d', '--depth', dest='depth', action='store', default=qos_profile_system_default.depth, type=int, help='Queue depth') parser.add_argument( - '-k', '--keep', dest='history_policy', action='store', default=qos_profile_default.history, + '-k', '--keep', dest='history_policy', action='store', default=qos_profile_system_default.history, type=int, choices=[0, 1], help='History QoS setting, 0 - keep last sample, 1 - keep all the samples') parser.add_argument( '-r', '--reliability', dest='reliability_policy', action='store', - default=qos_profile_default.reliability, type=int, choices=[0, 1], + default=qos_profile_system_default.reliability, type=int, choices=[0, 1], help='Reliability QoS setting, 0 - best effort, 1 - reliable') parser.add_argument( '-s', '--show', dest='show_camera', action='store', default=1, type=int, choices=[0, 1], @@ -74,7 +74,7 @@ def main(args=None): # the camera. node = rclpy.create_node('showimagepy') - custom_qos_profile = qos_profile_default + custom_qos_profile = qos_profile_system_default # Depth represents how many messages to store in history when the history policy is KEEP_LAST. custom_qos_profile.depth = args.depth From 77d03d895320cc9f3be3053525b79cfa31dccd3b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 1 Jul 2019 13:40:56 +0000 Subject: [PATCH 08/16] Fix topic specification. Signed-off-by: Chris Lalancette --- image_tools_py/cam2image_py.py | 2 +- image_tools_py/showimage_py.py | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/image_tools_py/cam2image_py.py b/image_tools_py/cam2image_py.py index 2f45a1086..f174f364e 100644 --- a/image_tools_py/cam2image_py.py +++ b/image_tools_py/cam2image_py.py @@ -126,7 +126,7 @@ def main(args=None): # Create the image publisher with our custom QoS profile. pub = node.create_publisher( - sensor_msgs.msg.Image, 'image', + sensor_msgs.msg.Image, args.topic, qos_profile=custom_camera_qos_profile) is_flipped = False diff --git a/image_tools_py/showimage_py.py b/image_tools_py/showimage_py.py index daf864f81..fe2ef11ff 100644 --- a/image_tools_py/showimage_py.py +++ b/image_tools_py/showimage_py.py @@ -65,6 +65,9 @@ def main(args=None): parser.add_argument( '-s', '--show', dest='show_camera', action='store', default=1, type=int, choices=[0, 1], help='Show the camera stream') + parser.add_argument( + '-t', '--topic', dest='topic', action='store', default='image', type=str, + help='use topic TOPIC instead of the default') args = parser.parse_args() if args.show_camera == 1: @@ -113,7 +116,7 @@ def image_cb(msg): cv2.waitKey(1) node.create_subscription( - sensor_msgs.msg.Image, 'image', image_cb, qos_profile=custom_qos_profile) + sensor_msgs.msg.Image, args.topic, image_cb, qos_profile=custom_qos_profile) while rclpy.ok(): rclpy.spin_once(node) From 064ccfbd62588bd7f95a295158006745558f3eaa Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 1 Jul 2019 13:31:35 +0000 Subject: [PATCH 09/16] Cleanup the README.md Signed-off-by: Chris Lalancette --- image_tools_py/{README => README.md} | 73 +++++++++++++--------------- 1 file changed, 35 insertions(+), 38 deletions(-) rename image_tools_py/{README => README.md} (69%) diff --git a/image_tools_py/README b/image_tools_py/README.md similarity index 69% rename from image_tools_py/README rename to image_tools_py/README.md index 42230ae69..0eefd7ef5 100644 --- a/image_tools_py/README +++ b/image_tools_py/README.md @@ -1,38 +1,24 @@ +## Image Tools Demo in Python + This is a demonstration of the Quality of Service (QoS) features of ROS 2 using Python. -There are two programs implemented here: cam2image_py, and showimage_py. Note that in -order for these programs to work, an OpenCV binding for Python3 must be available. As -of this writing (January 11, 2017), only OpenCV 3 or later supports Python3. Instructions -for compiling OpenCV3 for Python3 are available here: - -http://stackoverflow.com/questions/20953273/install-opencv-for-python-3-3 - -The condensed rundown that works on Ubuntu16.04 and will install to /usr/local is: -$ sudo apt install python3 build-essential cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev python3.5-dev libpython3-dev python3-numpy libtbb2 libtbb-dev libjpeg-dev libpng-dev libtiff-dev libjasper-dev libdc1394-22-dev -$ git clone https://github.com/opencv/opencv -$ cd opencv -$ git checkout 3.2.0 -$ mkdir release -$ cd release -$ cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr/local .. -$ make -j8 -$ sudo make install - -CAM2IMAGE_PY ------------- +There are two programs implemented here: cam2image_py, and showimage_py. + +### CAM2IMAGE_PY This is a Python program that will take data from an attached camera, and publish the -data to a topic called "image", with the type sensor_msgs::msg::Image. If a camera +data to a topic called "image", with the type sensor_msgs/msg/Image. If a camera isn't available, this program can also generate a default image and smoothly "move" it across the screen, simulating motion. The usage output from the program looks like this: -usage: cam2image_py.py [-h] [-b] [-d QUEUE_DEPTH] [-f FREQUENCY] [-k {0,1}] - [-r {0,1}] [-s {0,1}] [-x WIDTH] [-y HEIGHT] +``` +usage: cam2image_py [-h] [-b] [-d DEPTH] [-f FREQUENCY] [-k {0,1}] [-r {0,1}] + [-s {0,1}] [-t TOPIC] [-x WIDTH] [-y HEIGHT] optional arguments: -h, --help show this help message and exit -b, --burger Produce images of burgers rather than connecting to a camera (default: False) - -d QUEUE_DEPTH, --depth QUEUE_DEPTH + -d DEPTH, --depth DEPTH Queue depth (default: 10) -f FREQUENCY, --frequency FREQUENCY Publish frequency in Hz (default: 30) @@ -42,18 +28,22 @@ optional arguments: -r {0,1}, --reliability {0,1} Reliability QoS setting, 0 - best effort, 1 - reliable (default: 1) + -t TOPIC, --topic TOPIC + Topic to publish on (default: image) + -s {0,1}, --show {0,1} Show the camera stream (default: 0) -x WIDTH, --width WIDTH Image width (default: 320) -y HEIGHT, --height HEIGHT Image height (default: 240) +``` The -d, -k, and -r parameters control various aspects of the QoS implementation, and are the most interesting to play with when testing out QoS. Note that this program also subscribes to a topic called "flip_image" of type -std_msgs::msg::Bool. If flip_image is set to False, the data coming out of the camera +std_msgs/msg/Bool. If flip_image is set to False, the data coming out of the camera is sent as usual. If flip_image is set to True, the data coming out of the camera is flipped around the Y axis. @@ -63,13 +53,13 @@ the ROS 2 pub/sub model, so this window cannot show off the QoS parameters (it i useful for debugging). See SHOWIMAGE_PY below for a program that can show QoS over the pub/sub model. -SHOWIMAGE_PY ------------- +### SHOWIMAGE_PY This is a Python program that subscribes to the "image" topic, waiting for data. As new data comes in, this program accepts the data and can optionally render it to the screen. The usage output from the program looks like this: -usage: showimage_py.py [-h] [-d QUEUE_DEPTH] [-k {0,1}] [-r {0,1}] [-s {0,1}] +usage: showimage_py [-h] [-d QUEUE_DEPTH] [-k {0,1}] [-r {0,1}] [-s {0,1}] + [-t TOPIC] optional arguments: -h, --help show this help message and exit @@ -82,28 +72,35 @@ optional arguments: Reliability QoS setting, 0 - best effort, 1 - reliable (default: 1) -s {0,1}, --show {0,1} - Show the camera stream (default: 0) + Show the camera stream (default: 1) + -t TOPIC, --topic TOPIC + use topic TOPIC instead of the default (default: image) The -d, -k, and -r parameters control various aspects of the QoS implementation, and are the most interesting to play with when testing out QoS. If the -s parameter is set to 1, then this program opens up a window to show the images that have been received over the ROS 2 pub/sub model. This program should be used -in conjunction with CAM2IMAGE_PY to demonstrate the ROS 2 QoS capabilities over lossy/slow +in conjunction with cam2image_py to demonstrate the ROS 2 QoS capabilities over lossy/slow links. -EXAMPLE USAGE -------------- +### EXAMPLE USAGE To use the above programs, you would run them something like the following: # In the first terminal, run the data publisher. This will connect to the 1st camera # available, and print out "Publishing image #" for each image it publishes. -$ python3 cam2image_py.py - -# In a second terminal, run the data subscriber. This will subscribe to the "image" -# topic and render any frames it receives. -$ python3 showimage_py.py -s 1 +``` +$ ros2 run image_tools_py cam2image_py +``` # If you don't have a local camera, you can use the -b parameter to generate data on # the fly rather than get data from a camera: -$ python3 cam2image_py.py -b +``` +$ ros2 run image_tools_py cam2image_py -b +``` + +# In a second terminal, run the data subscriber. This will subscribe to the "image" +# topic and render any frames it receives. +``` +$ ros2 run image_tools_py showimage_py +``` From 4bd4e8aaf8a619ded10cb1381d4438cce4065f1b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 1 Jul 2019 13:48:56 +0000 Subject: [PATCH 10/16] Review feedback. Signed-off-by: Chris Lalancette --- image_tools_py/burger_py.py | 3 --- image_tools_py/cam2image_py.py | 15 +++++++-------- image_tools_py/setup.py | 2 +- image_tools_py/showimage_py.py | 4 ---- 4 files changed, 8 insertions(+), 16 deletions(-) diff --git a/image_tools_py/burger_py.py b/image_tools_py/burger_py.py index dc29e985b..25cd3726d 100644 --- a/image_tools_py/burger_py.py +++ b/image_tools_py/burger_py.py @@ -12,14 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -# System imports import base64 import random -# OpenCV import cv2 -# Numpy import numpy # THE FOLLOWING IS A BURGER IN BASE64. RESPECT IT diff --git a/image_tools_py/cam2image_py.py b/image_tools_py/cam2image_py.py index f174f364e..9ccaa40fc 100644 --- a/image_tools_py/cam2image_py.py +++ b/image_tools_py/cam2image_py.py @@ -12,26 +12,24 @@ # See the License for the specific language governing permissions and # limitations under the License. -# System imports import argparse import sys -# Local imports import burger_py -# OpenCV imports import cv2 -# ROS2 imports import rclpy from rclpy.qos import qos_profile_system_default, qos_profile_sensor_data import sensor_msgs.msg import std_msgs.msg -# Convert an OpenCV matrix encoding type to a string format recognized by -# sensor_msgs::Image def mat2encoding(frame): + ''' + Convert an OpenCV matrix encoding type to a string format recognized by + sensor_msgs::msg::Image + ''' encoding = '' encodings = {1: 'mono', 3: 'bgr', 4: 'rgba'} @@ -53,8 +51,10 @@ def mat2encoding(frame): return encoding -# Convert an OpenCV matrix to a ROS Image message. def convert_frame_to_message(frame, frame_id, msg): + ''' + Convert an OpenCV matrix to a ROS Image message. + ''' msg.height = frame.shape[0] msg.width = frame.shape[1] msg.encoding = mat2encoding(frame) @@ -165,7 +165,6 @@ def flip_image_cb(msg): frame_number = 1 while rclpy.ok(): # Get the frame from the video capture. - frame = None if args.burger_mode: frame = burger_cap.render_burger(args.width, args.height) else: diff --git a/image_tools_py/setup.py b/image_tools_py/setup.py index 3bc919b08..0866d90f2 100644 --- a/image_tools_py/setup.py +++ b/image_tools_py/setup.py @@ -47,7 +47,7 @@ def run(self): setup( name=package_name, - version='0.0.0', + version='0.1.0', packages=[], py_modules=[ 'burger_py', diff --git a/image_tools_py/showimage_py.py b/image_tools_py/showimage_py.py index fe2ef11ff..4fa2dca60 100644 --- a/image_tools_py/showimage_py.py +++ b/image_tools_py/showimage_py.py @@ -12,16 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. -# System imports import argparse -# OpenCV imports import cv2 -# Numpy imports import numpy -# ROS2 imports import rclpy from rclpy.qos import qos_profile_system_default import sensor_msgs.msg From 75ec783c927859ffe915aa1a4781942e6043e85b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 1 Jul 2019 13:56:49 +0000 Subject: [PATCH 11/16] Fix up argparse help output. Signed-off-by: Chris Lalancette --- image_tools_py/cam2image_py.py | 6 +++--- image_tools_py/showimage_py.py | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/image_tools_py/cam2image_py.py b/image_tools_py/cam2image_py.py index 9ccaa40fc..e6efddfb0 100644 --- a/image_tools_py/cam2image_py.py +++ b/image_tools_py/cam2image_py.py @@ -73,18 +73,18 @@ def main(args=None): '-b', '--burger', dest='burger_mode', action='store_true', default=False, help='Produce images of burgers rather than connecting to a camera') parser.add_argument( - '-d', '--depth', dest='depth', action='store', default=qos_profile_system_default.depth, type=int, + '-d', '--depth', dest='depth', action='store', default=int(qos_profile_system_default.depth), type=int, help='Queue depth') parser.add_argument( '-f', '--frequency', dest='frequency', action='store', default=30, type=int, help='Publish frequency in Hz') parser.add_argument( - '-k', '--keep', dest='history_policy', action='store', default=qos_profile_system_default.history, + '-k', '--keep', dest='history_policy', action='store', default=int(qos_profile_system_default.history), type=int, choices=[0, 1], help='History QoS setting, 0 - keep last sample, 1 - keep all the samples') parser.add_argument( '-r', '--reliability', dest='reliability_policy', action='store', - default=qos_profile_system_default.reliability, type=int, choices=[0, 1], + default=int(qos_profile_system_default.reliability), type=int, choices=[0, 1], help='Reliability QoS setting, 0 - best effort, 1 - reliable') parser.add_argument( '-s', '--show', dest='show_camera', action='store', default=0, type=int, choices=[0, 1], diff --git a/image_tools_py/showimage_py.py b/image_tools_py/showimage_py.py index 4fa2dca60..671186b54 100644 --- a/image_tools_py/showimage_py.py +++ b/image_tools_py/showimage_py.py @@ -48,15 +48,15 @@ def main(args=None): # Parse the command-line options. parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument( - '-d', '--depth', dest='depth', action='store', default=qos_profile_system_default.depth, type=int, + '-d', '--depth', dest='depth', action='store', default=int(qos_profile_system_default.depth), type=int, help='Queue depth') parser.add_argument( - '-k', '--keep', dest='history_policy', action='store', default=qos_profile_system_default.history, + '-k', '--keep', dest='history_policy', action='store', default=int(qos_profile_system_default.history), type=int, choices=[0, 1], help='History QoS setting, 0 - keep last sample, 1 - keep all the samples') parser.add_argument( '-r', '--reliability', dest='reliability_policy', action='store', - default=qos_profile_system_default.reliability, type=int, choices=[0, 1], + default=int(qos_profile_system_default.reliability), type=int, choices=[0, 1], help='Reliability QoS setting, 0 - best effort, 1 - reliable') parser.add_argument( '-s', '--show', dest='show_camera', action='store', default=1, type=int, choices=[0, 1], From c8d1b494d4b6c95c29b037be4b88167478092223 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 1 Jul 2019 14:24:26 +0000 Subject: [PATCH 12/16] Style cleanups. Signed-off-by: Chris Lalancette --- image_tools_py/cam2image_py.py | 22 +++++++++++----------- image_tools_py/package.xml | 2 +- image_tools_py/showimage_py.py | 8 ++++---- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/image_tools_py/cam2image_py.py b/image_tools_py/cam2image_py.py index e6efddfb0..7f51ebb91 100644 --- a/image_tools_py/cam2image_py.py +++ b/image_tools_py/cam2image_py.py @@ -20,16 +20,18 @@ import cv2 import rclpy -from rclpy.qos import qos_profile_system_default, qos_profile_sensor_data +from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default import sensor_msgs.msg import std_msgs.msg def mat2encoding(frame): - ''' + """ + Convert an OpenCV matrix encoding type to a string format. + Convert an OpenCV matrix encoding type to a string format recognized by - sensor_msgs::msg::Image - ''' + sensor_msgs::msg::Image. + """ encoding = '' encodings = {1: 'mono', 3: 'bgr', 4: 'rgba'} @@ -52,9 +54,7 @@ def mat2encoding(frame): def convert_frame_to_message(frame, frame_id, msg): - ''' - Convert an OpenCV matrix to a ROS Image message. - ''' + """Convert an OpenCV matrix to a ROS Image message.""" msg.height = frame.shape[0] msg.width = frame.shape[1] msg.encoding = mat2encoding(frame) @@ -73,14 +73,14 @@ def main(args=None): '-b', '--burger', dest='burger_mode', action='store_true', default=False, help='Produce images of burgers rather than connecting to a camera') parser.add_argument( - '-d', '--depth', dest='depth', action='store', default=int(qos_profile_system_default.depth), type=int, - help='Queue depth') + '-d', '--depth', dest='depth', action='store', + default=int(qos_profile_system_default.depth), type=int, help='Queue depth') parser.add_argument( '-f', '--frequency', dest='frequency', action='store', default=30, type=int, help='Publish frequency in Hz') parser.add_argument( - '-k', '--keep', dest='history_policy', action='store', default=int(qos_profile_system_default.history), - type=int, choices=[0, 1], + '-k', '--keep', dest='history_policy', action='store', + default=int(qos_profile_system_default.history), type=int, choices=[0, 1], help='History QoS setting, 0 - keep last sample, 1 - keep all the samples') parser.add_argument( '-r', '--reliability', dest='reliability_policy', action='store', diff --git a/image_tools_py/package.xml b/image_tools_py/package.xml index ce5ec81f7..051813177 100644 --- a/image_tools_py/package.xml +++ b/image_tools_py/package.xml @@ -2,7 +2,7 @@ image_tools_py - 0.0.0 + 0.1.0 Python tools to capture/play back images to/from DDS subscriptions/publications. Chris Lalancette Apache License 2.0 diff --git a/image_tools_py/showimage_py.py b/image_tools_py/showimage_py.py index 671186b54..fa73be65f 100644 --- a/image_tools_py/showimage_py.py +++ b/image_tools_py/showimage_py.py @@ -48,11 +48,11 @@ def main(args=None): # Parse the command-line options. parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument( - '-d', '--depth', dest='depth', action='store', default=int(qos_profile_system_default.depth), type=int, - help='Queue depth') + '-d', '--depth', dest='depth', action='store', + default=int(qos_profile_system_default.depth), type=int, help='Queue depth') parser.add_argument( - '-k', '--keep', dest='history_policy', action='store', default=int(qos_profile_system_default.history), - type=int, choices=[0, 1], + '-k', '--keep', dest='history_policy', action='store', + default=int(qos_profile_system_default.history), type=int, choices=[0, 1], help='History QoS setting, 0 - keep last sample, 1 - keep all the samples') parser.add_argument( '-r', '--reliability', dest='reliability_policy', action='store', From 745fc5fc4b99618794aa8bf5144f7e4931afb809 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 1 Jul 2019 14:55:04 +0000 Subject: [PATCH 13/16] Revamp the test. Make it use the new test infrastructure. Signed-off-by: Chris Lalancette --- image_tools_py/package.xml | 6 +- image_tools_py/setup.py | 10 +- image_tools_py/test/cam2image_py.txt | 2 - image_tools_py/test/showimage_py.regex | 1 - .../test/test_showimage_cam2image.py.in | 188 ++++++++++-------- 5 files changed, 105 insertions(+), 102 deletions(-) delete mode 100644 image_tools_py/test/cam2image_py.txt delete mode 100644 image_tools_py/test/showimage_py.regex diff --git a/image_tools_py/package.xml b/image_tools_py/package.xml index 051813177..64e21deb4 100644 --- a/image_tools_py/package.xml +++ b/image_tools_py/package.xml @@ -13,15 +13,11 @@ std_msgs sensor_msgs - ament_copyright ament_flake8 ament_pep257 - ament_pep8 - ament_pyflakes launch - launch_testing + python3-pytest ament_python diff --git a/image_tools_py/setup.py b/image_tools_py/setup.py index 0866d90f2..ab674ab1e 100644 --- a/image_tools_py/setup.py +++ b/image_tools_py/setup.py @@ -26,11 +26,9 @@ def run(self): for rmw_impl in rmw_implementations: substs = { - '@rmw_implementation@': rmw_impl, - '@showimage_py_exe@': os.path.join(self.install_dir, 'showimage_py'), - '@showimage_py_outfile@': os.path.realpath(os.path.join('test', 'showimage_py')), - '@cam2image_py_exe@': os.path.join(self.install_dir, 'cam2image_py'), - '@cam2image_py_outfile@': os.path.realpath(os.path.join('test', 'cam2image_py')), + '@RMW_IMPLEMENTATION@': rmw_impl, + '@RCLPY_DEMO_SHOWIMAGE_EXE@': os.path.join(self.install_dir, 'showimage_py'), + '@RCLPY_DEMO_CAM2IMAGE_EXE@': os.path.join(self.install_dir, 'cam2image_py'), } infile = os.path.join('test', 'test_showimage_cam2image.py.in') @@ -39,7 +37,7 @@ def run(self): with open(outfile, 'w') as outfp: for line in infp: for subst in substs: - line = line.replace(subst, "r'%s' # noqa" % (substs[subst])) + line = line.replace(subst, substs[subst]) outfp.write(line) install_scripts.run(self) diff --git a/image_tools_py/test/cam2image_py.txt b/image_tools_py/test/cam2image_py.txt deleted file mode 100644 index f03d92ed2..000000000 --- a/image_tools_py/test/cam2image_py.txt +++ /dev/null @@ -1,2 +0,0 @@ -Publishing data on topic 'image' -Publishing image #1 diff --git a/image_tools_py/test/showimage_py.regex b/image_tools_py/test/showimage_py.regex deleted file mode 100644 index fe331b99a..000000000 --- a/image_tools_py/test/showimage_py.regex +++ /dev/null @@ -1 +0,0 @@ -Received image #\d+ diff --git a/image_tools_py/test/test_showimage_cam2image.py.in b/image_tools_py/test/test_showimage_cam2image.py.in index 7d5d34893..148abb15a 100644 --- a/image_tools_py/test/test_showimage_cam2image.py.in +++ b/image_tools_py/test/test_showimage_cam2image.py.in @@ -1,4 +1,4 @@ -# Copyright 2016 Open Source Robotics Foundation, Inc. +# Copyright 2019 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -13,90 +13,102 @@ # limitations under the License. import os - -from launch.legacy import LaunchDescriptor -from launch.legacy.exit_handler import ignore_exit_handler -from launch.legacy.exit_handler import primary_ignore_returncode_exit_handler -from launch.legacy.launcher import DefaultLauncher -from launch.legacy.output_handler import ConsoleOutput -from launch_testing import create_handler - - -def setup(): - os.environ['OSPL_VERBOSITY'] = '8' # 8 = OS_NONE - # bare minimum formatting for console output matching - os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' - - -def test_reliable_qos(): - pub_executable_args = ['-r', '1', '-s', '0', '-b', '-f', '5'] - sub_executable_args = ['-r', '1', '-s', '0'] - _test_executables(pub_executable_args, sub_executable_args) - - -def _test_executables(publisher_executable_args, subscriber_executable_args): - output_handlers = [] - launch_descriptor = LaunchDescriptor() - - rmw_implementation = @rmw_implementation@ - env = dict(os.environ) - env['RMW_IMPLEMENTATION'] = rmw_implementation - env['PYTHONUNBUFFERED'] = '1' - - # Launch the process that will receive the images. - # This is the process that gets to decide when to tear the launcher down. - # It will exit when the regex for receiving images is matched. - showimage_executable = @showimage_py_exe@ - showimage_output_file = @showimage_py_outfile@ - showimage_name = 'test_showimage_py' - showimage_handler = create_handler( - showimage_name, launch_descriptor, showimage_output_file, exit_on_match=True, - filtered_rmw_implementation=rmw_implementation) - assert showimage_handler, 'Cannot find appropriate handler for %s' % showimage_output_file - output_handlers.append(showimage_handler) - - command = [showimage_executable] - command.extend(subscriber_executable_args) - launch_descriptor.add_process( - cmd=command, - name=showimage_name, - exit_handler=primary_ignore_returncode_exit_handler, - output_handlers=[showimage_handler, ConsoleOutput()], - env=env, - ) - - # Launch the process that will publish the images. - # This process will be exited when the launcher is torn down. - cam2image_executable = @cam2image_py_exe@ - cam2image_output_file = @cam2image_py_outfile@ - cam2image_name = 'test_cam2image_py' - cam2image_handler = create_handler( - cam2image_name, launch_descriptor, cam2image_output_file, exit_on_match=False, - filtered_rmw_implementation=rmw_implementation) - assert cam2image_handler, 'Cannot find appropriate handler for %s' % cam2image_output_file - output_handlers.append(cam2image_handler) - - command = [cam2image_executable] - command.extend(publisher_executable_args) - launch_descriptor.add_process( - cmd=command, - name=cam2image_name, - exit_handler=ignore_exit_handler, - output_handlers=[cam2image_handler, ConsoleOutput()], - env=env, - ) - - launcher = DefaultLauncher() - launcher.add_launch_descriptor(launch_descriptor) - rc = launcher.launch() - - assert rc == 0, \ - "The launch file failed with exit code '" + str(rc) + "'. " - - # Check that the output received by the handlers is what was expected. - for handler in output_handlers: - handler.check() - - -if __name__ == '__main__': - test_reliable_qos() +import time +import unittest + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import EmitEvent +from launch.actions import ExecuteProcess +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessIO +from launch.events import Shutdown + + +# TODO(clalancette): we can't currently use launch_testing since there is +# currently no integration with pure python setup.py. Once we have a solution +# for https://github.com/ros2/launch/issues/237, we should revisit this. +class TestExecutablesDemo(unittest.TestCase): + + def __init__(self, name): + super().__init__(name) + self._start_time = time.time() + self._ls = LaunchService() + self._ls.include_launch_description(self.generate_launch_description()) + self._saw_cam2image_output = False + self._saw_showimage_output = False + + def generate_launch_description(self): + launch_description = LaunchDescription() + publisher_executable_args = ['-r', '1', '-s', '0', '-b', '-f', '5'] + subscriber_executable_args = ['-r', '1', '-s', '0'] + + env = dict(os.environ) + env['PYTHONUNBUFFERED'] = '1' + env['OSPL_VERBOSITY'] = '8' # 8 = OS_NONE + # bare minimum formatting for console output matching + env['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' + env['RMW_IMPLEMENTATION'] = r'@RMW_IMPLEMENTATION@' + + # Launch the process that will receive the images. + # This is the process that gets to decide when to tear the launcher down. + # It will exit when the regex for receiving images is matched. + showimage_executable = r'@RCLPY_DEMO_SHOWIMAGE_EXE@' # noqa: E501 + showimage_name = 'test_showimage_py' + + command = [showimage_executable] + command.extend(subscriber_executable_args) + showimage_process = ExecuteProcess( + cmd=command, + name=showimage_name, + env=env, + output='screen' + ) + launch_description.add_action(showimage_process) + + # Launch the process that will publish the images. + # This process will be exited when the launcher is torn down. + cam2image_executable = r'@RCLPY_DEMO_CAM2IMAGE_EXE@' # noqa: E501 + cam2image_name = 'test_cam2image_py' + + command = [cam2image_executable] + command.extend(publisher_executable_args) + cam2image_process = ExecuteProcess( + cmd=command, + name=cam2image_name, + env=env, + output='screen' + ) + launch_description.add_action(cam2image_process) + + launch_description.add_action( + RegisterEventHandler( + OnProcessIO( + on_stdout=self.append, + on_stderr=self.append, + ) + ) + ) + + return launch_description + + def append(self, process_io): + if 'cam2image' in process_io.process_name: + if 'Publishing image #' in process_io.text.decode(): + self._saw_cam2image_output = True + elif 'showimage' in process_io.process_name: + if 'Received image #' in process_io.text.decode(): + self._saw_showimage_output = True + + if self._saw_cam2image_output and self._saw_showimage_output: + # We've seen all required arguments from the test, quit + return EmitEvent(event=Shutdown(reason='finished', due_to_sigint=False)) + + if time.time() - self._start_time > 10.0: + # We've waited more than 10 seconds with no output; raise an error + return EmitEvent(event=Shutdown(reason='timeout', due_to_sigint=False)) + + def test_reliable_qos(self): + self._ls.run() + self._ls.shutdown() + self.assertTrue(self._saw_cam2image_output and self._saw_showimage_output) From ff19924dda8c7039276db9993200d25d26f625d3 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 9 Jul 2019 15:24:46 +0000 Subject: [PATCH 14/16] Review feedback. Signed-off-by: Chris Lalancette --- image_tools_py/README.md | 73 +++++++++++-------- image_tools_py/cam2image_py.py | 61 ++++++++-------- image_tools_py/showimage_py.py | 7 +- .../test/test_showimage_cam2image.py.in | 1 - 4 files changed, 78 insertions(+), 64 deletions(-) diff --git a/image_tools_py/README.md b/image_tools_py/README.md index 0eefd7ef5..57a6efea7 100644 --- a/image_tools_py/README.md +++ b/image_tools_py/README.md @@ -1,14 +1,16 @@ ## Image Tools Demo in Python -This is a demonstration of the Quality of Service (QoS) features of ROS 2 using Python. +This is a demonstration of the Quality of Service (QoS) features of ROS 2 using +Python. There are two programs implemented here: cam2image_py, and showimage_py. ### CAM2IMAGE_PY -This is a Python program that will take data from an attached camera, and publish the -data to a topic called "image", with the type sensor_msgs/msg/Image. If a camera -isn't available, this program can also generate a default image and smoothly "move" -it across the screen, simulating motion. The usage output from the program looks like -this: +This is a Python program that will take data from an attached camera, and +publish the data to a topic called "image", with the type +`sensor_msgs/msg/Image`. +If a camera isn't available, this program can also generate a default image and +smoothly "move" it across the screen, simulating motion. The usage output from +the program looks like this: ``` usage: cam2image_py [-h] [-b] [-d DEPTH] [-f FREQUENCY] [-k {0,1}] [-r {0,1}] @@ -39,25 +41,30 @@ optional arguments: Image height (default: 240) ``` -The -d, -k, and -r parameters control various aspects of the QoS implementation, and -are the most interesting to play with when testing out QoS. +The `-d`, `-k`, and `-r` parameters control various aspects of the QoS +implementation, and are the most interesting to play with when testing out QoS. Note that this program also subscribes to a topic called "flip_image" of type -std_msgs/msg/Bool. If flip_image is set to False, the data coming out of the camera -is sent as usual. If flip_image is set to True, the data coming out of the camera is -flipped around the Y axis. - -If the -s parameter is set to 1, then this program opens up a (local) window to show -the images that are being published. However, these images are *not* coming in through -the ROS 2 pub/sub model, so this window cannot show off the QoS parameters (it is mostly -useful for debugging). See SHOWIMAGE_PY below for a program that can show QoS over the -pub/sub model. +`std_msgs/msg/Bool`. +If "flip_image" is set to `False`, the data coming out of the camera is sent as +usual. +If "flip_image" is set to `True`, the data coming out of the camera is flipped +around the Y axis. + +If the `-s` parameter is set to 1, then this program opens up a (local) window +to show the images that are being published. +However, these images are *not* coming in through the ROS 2 pub/sub model, so +this window cannot show off the QoS parameters (it is mostly useful for +debugging). +See SHOWIMAGE_PY below for a program that can show QoS over the pub/sub model. ### SHOWIMAGE_PY -This is a Python program that subscribes to the "image" topic, waiting for data. As -new data comes in, this program accepts the data and can optionally render it to -the screen. The usage output from the program looks like this: +This is a Python program that subscribes to the "image" topic, waiting for data. +As new data comes in, this program accepts the data and can optionally render +it to the screen. +The usage output from the program looks like this: +``` usage: showimage_py [-h] [-d QUEUE_DEPTH] [-k {0,1}] [-r {0,1}] [-s {0,1}] [-t TOPIC] @@ -75,32 +82,34 @@ optional arguments: Show the camera stream (default: 1) -t TOPIC, --topic TOPIC use topic TOPIC instead of the default (default: image) +``` -The -d, -k, and -r parameters control various aspects of the QoS implementation, and -are the most interesting to play with when testing out QoS. +The `-d`, `-k`, and `-r` parameters control various aspects of the QoS +implementation, and are the most interesting to play with when testing out QoS. -If the -s parameter is set to 1, then this program opens up a window to show the images -that have been received over the ROS 2 pub/sub model. This program should be used -in conjunction with cam2image_py to demonstrate the ROS 2 QoS capabilities over lossy/slow -links. +If the `-s` parameter is set to 1, then this program opens up a window to show +the images that have been received over the ROS 2 pub/sub model. +This program should be used in conjunction with cam2image_py to demonstrate the +ROS 2 QoS capabilities over lossy/slow links. ### EXAMPLE USAGE To use the above programs, you would run them something like the following: -# In the first terminal, run the data publisher. This will connect to the 1st camera -# available, and print out "Publishing image #" for each image it publishes. +In the first terminal, run the data publisher. +This will connect to the first camera available, and print out +"Publishing image #" for each image it publishes. ``` $ ros2 run image_tools_py cam2image_py ``` -# If you don't have a local camera, you can use the -b parameter to generate data on -# the fly rather than get data from a camera: +If you don't have a local camera, you can use the `-b` parameter to generate +data on the fly rather than get data from a camera: ``` $ ros2 run image_tools_py cam2image_py -b ``` -# In a second terminal, run the data subscriber. This will subscribe to the "image" -# topic and render any frames it receives. +In a second terminal, run the data subscriber. +This will subscribe to the "image" topic and render any frames it receives. ``` $ ros2 run image_tools_py showimage_py ``` diff --git a/image_tools_py/cam2image_py.py b/image_tools_py/cam2image_py.py index 7f51ebb91..77da0e522 100644 --- a/image_tools_py/cam2image_py.py +++ b/image_tools_py/cam2image_py.py @@ -163,36 +163,39 @@ def flip_image_cb(msg): # Our main event loop will spin until the user presses CTRL-C to exit. frame_number = 1 - while rclpy.ok(): - # Get the frame from the video capture. - if args.burger_mode: - frame = burger_cap.render_burger(args.width, args.height) - else: - ret, frame = cam_cap.read() - - # Check if the frame was grabbed correctly. - if frame is not None: - # Convert to a ROS image - if is_flipped: - # Flip the frame if needed - flipped_frame = cv2.flip(frame, 1) - convert_frame_to_message(flipped_frame, frame_number, msg) + try: + while rclpy.ok(): + # Get the frame from the video capture. + if args.burger_mode: + frame = burger_cap.render_burger(args.width, args.height) else: - convert_frame_to_message(frame, frame_number, msg) - - if args.show_camera == 1: - # Show the image in a window called 'cam2image_py', if requested. - cv2.imshow('cam2image_py', frame) - # Draw the image to the screen and wait 1 millisecond - cv2.waitKey(1) - - # Publish the image message and increment the frame_id. - node.get_logger().info('Publishing image #%d' % (frame_number)) - pub.publish(msg) - frame_number += 1 - - # Do some work in rclpy and wait for more to come in. - rclpy.spin_once(node, timeout_sec=1.0 / float(args.frequency)) + ret, frame = cam_cap.read() + + # Check if the frame was grabbed correctly. + if frame is not None: + # Convert to a ROS image + if is_flipped: + # Flip the frame if needed + flipped_frame = cv2.flip(frame, 1) + convert_frame_to_message(flipped_frame, frame_number, msg) + else: + convert_frame_to_message(frame, frame_number, msg) + + if args.show_camera == 1: + # Show the image in a window called 'cam2image_py', if requested. + cv2.imshow('cam2image_py', frame) + # Draw the image to the screen and wait 1 millisecond + cv2.waitKey(1) + + # Publish the image message and increment the frame_id. + node.get_logger().info('Publishing image #%d' % (frame_number)) + pub.publish(msg) + frame_number += 1 + + # Do some work in rclpy and wait for more to come in. + rclpy.spin_once(node, timeout_sec=1.0 / float(args.frequency)) + except KeyboardInterrupt: + pass if __name__ == '__main__': diff --git a/image_tools_py/showimage_py.py b/image_tools_py/showimage_py.py index fa73be65f..2c5e82ffc 100644 --- a/image_tools_py/showimage_py.py +++ b/image_tools_py/showimage_py.py @@ -114,8 +114,11 @@ def image_cb(msg): node.create_subscription( sensor_msgs.msg.Image, args.topic, image_cb, qos_profile=custom_qos_profile) - while rclpy.ok(): - rclpy.spin_once(node) + try: + while rclpy.ok(): + rclpy.spin_once(node) + except KeyboardInterrupt: + pass if __name__ == '__main__': diff --git a/image_tools_py/test/test_showimage_cam2image.py.in b/image_tools_py/test/test_showimage_cam2image.py.in index 148abb15a..8fe1bde75 100644 --- a/image_tools_py/test/test_showimage_cam2image.py.in +++ b/image_tools_py/test/test_showimage_cam2image.py.in @@ -110,5 +110,4 @@ class TestExecutablesDemo(unittest.TestCase): def test_reliable_qos(self): self._ls.run() - self._ls.shutdown() self.assertTrue(self._saw_cam2image_output and self._saw_showimage_output) From 9cf7e9ff733255a9d9fdf10e7771a2567166141c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 11 Jul 2019 14:12:16 +0000 Subject: [PATCH 15/16] Fixes for Windows. Signed-off-by: Chris Lalancette --- .../test/test_showimage_cam2image.py.in | 65 +++++++++++++------ 1 file changed, 45 insertions(+), 20 deletions(-) diff --git a/image_tools_py/test/test_showimage_cam2image.py.in b/image_tools_py/test/test_showimage_cam2image.py.in index 8fe1bde75..b92e7006f 100644 --- a/image_tools_py/test/test_showimage_cam2image.py.in +++ b/image_tools_py/test/test_showimage_cam2image.py.in @@ -12,17 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. +import io import os import time import unittest from launch import LaunchDescription from launch import LaunchService -from launch.actions import EmitEvent -from launch.actions import ExecuteProcess -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessIO -from launch.events import Shutdown +import launch.actions +import launch.event_handlers +import launch.events # TODO(clalancette): we can't currently use launch_testing since there is @@ -37,6 +36,10 @@ class TestExecutablesDemo(unittest.TestCase): self._ls.include_launch_description(self.generate_launch_description()) self._saw_cam2image_output = False self._saw_showimage_output = False + # Windows can emit partial lines, so buffer the data and only check + # check when we have a complete line + self._cam2image_buffer = io.StringIO() + self._showimage_buffer = io.StringIO() def generate_launch_description(self): launch_description = LaunchDescription() @@ -58,7 +61,7 @@ class TestExecutablesDemo(unittest.TestCase): command = [showimage_executable] command.extend(subscriber_executable_args) - showimage_process = ExecuteProcess( + showimage_process = launch.actions.ExecuteProcess( cmd=command, name=showimage_name, env=env, @@ -73,7 +76,7 @@ class TestExecutablesDemo(unittest.TestCase): command = [cam2image_executable] command.extend(publisher_executable_args) - cam2image_process = ExecuteProcess( + cam2image_process = launch.actions.ExecuteProcess( cmd=command, name=cam2image_name, env=env, @@ -82,31 +85,53 @@ class TestExecutablesDemo(unittest.TestCase): launch_description.add_action(cam2image_process) launch_description.add_action( - RegisterEventHandler( - OnProcessIO( + launch.actions.RegisterEventHandler( + launch.event_handlers.OnProcessIO( on_stdout=self.append, on_stderr=self.append, ) ) ) + launch_description.add_action( + launch.actions.TimerAction( + period=10.0, + actions=[launch.actions.Shutdown(reason='Timer expired')]) + ) + return launch_description + def append_and_check(self, process_io, process_name, text_to_check): + if process_name in process_io.process_name: + buffer = getattr(self, '_' + process_name + '_buffer') + buffer.write(process_io.text.decode(errors='replace')) + buffer.seek(0) + last_line = None + for line in buffer: + # Note that this does not use os.linesep; apparently rclpy + # node.get_logger().info doesn't use the OS line separator + if line.endswith('\n'): + # We have a complete line, see if it has what we want + if text_to_check in line[:-len(os.linesep)]: + setattr(self, '_saw_' + process_name + '_output', True) + break + else: + last_line = line + break + buffer.seek(0) + buffer.truncate(0) + if last_line is not None: + buffer.write(last_line) + def append(self, process_io): - if 'cam2image' in process_io.process_name: - if 'Publishing image #' in process_io.text.decode(): - self._saw_cam2image_output = True - elif 'showimage' in process_io.process_name: - if 'Received image #' in process_io.text.decode(): - self._saw_showimage_output = True + self.append_and_check(process_io, 'cam2image', 'Publishing image #') + self.append_and_check(process_io, 'showimage', 'Received image #') if self._saw_cam2image_output and self._saw_showimage_output: # We've seen all required arguments from the test, quit - return EmitEvent(event=Shutdown(reason='finished', due_to_sigint=False)) - - if time.time() - self._start_time > 10.0: - # We've waited more than 10 seconds with no output; raise an error - return EmitEvent(event=Shutdown(reason='timeout', due_to_sigint=False)) + return launch.actions.EmitEvent( + event=launch.events.Shutdown(reason='finished', due_to_sigint=False) + ) def test_reliable_qos(self): self._ls.run() From c09cf43f70345e32ee65f9c491a4e3335859ba43 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 11 Jul 2019 14:22:06 +0000 Subject: [PATCH 16/16] Minor comment fix. Signed-off-by: Chris Lalancette --- image_tools_py/test/test_showimage_cam2image.py.in | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/image_tools_py/test/test_showimage_cam2image.py.in b/image_tools_py/test/test_showimage_cam2image.py.in index b92e7006f..36817b256 100644 --- a/image_tools_py/test/test_showimage_cam2image.py.in +++ b/image_tools_py/test/test_showimage_cam2image.py.in @@ -24,9 +24,9 @@ import launch.event_handlers import launch.events -# TODO(clalancette): we can't currently use launch_testing since there is -# currently no integration with pure python setup.py. Once we have a solution -# for https://github.com/ros2/launch/issues/237, we should revisit this. +# TODO(clalancette): we can't use launch_testing since there is currently no +# integration with pure python setup.py. Once we have a solution for +# https://github.com/ros2/launch/issues/237, we should revisit this. class TestExecutablesDemo(unittest.TestCase): def __init__(self, name):