From 16c3bc5d41462bbace90bff66d45ad1f3e42f91a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 7 Sep 2016 16:13:11 -0700 Subject: [PATCH] Image tools tests redux (#67) * add tests for cam2image and showimage * use burger mode (-b) instead of movie for tests * add a frequency option to the cam2image program * transparent burgers * proper burger masking * windows * missing Windows include * fix warnings for Windows * warnings on Windows * fix signed/unsigned comparison warnings * fix Windows warnings --- image_tools/CMakeLists.txt | 57 +++++++- image_tools/include/image_tools/options.hpp | 6 +- image_tools/package.xml | 4 +- image_tools/src/burger.cpp | 133 +++++++++++++++++++ image_tools/src/burger.hpp | 44 ++++++ image_tools/src/cam2image.cpp | 38 ++++-- image_tools/src/options.cpp | 36 ++++- image_tools/src/showimage.cpp | 52 +++++--- image_tools/test/cam2image.regex | 1 + image_tools/test/showimage.regex | 1 + image_tools/test/test_executables_demo.py.in | 70 ++++++++++ 11 files changed, 399 insertions(+), 43 deletions(-) create mode 100644 image_tools/src/burger.cpp create mode 100644 image_tools/src/burger.hpp create mode 100644 image_tools/test/cam2image.regex create mode 100644 image_tools/test/showimage.regex create mode 100644 image_tools/test/test_executables_demo.py.in diff --git a/image_tools/CMakeLists.txt b/image_tools/CMakeLists.txt index c8dd87784..5fedd41cb 100644 --- a/image_tools/CMakeLists.txt +++ b/image_tools/CMakeLists.txt @@ -22,8 +22,10 @@ macro(targets) endif() add_executable(cam2image${target_suffix} + src/burger.cpp src/cam2image.cpp - src/options.cpp) + src/options.cpp + ) ament_target_dependencies(cam2image${target_suffix} "rclcpp${target_suffix}" "sensor_msgs" @@ -31,8 +33,9 @@ macro(targets) "OpenCV") add_executable(showimage${target_suffix} + src/options.cpp src/showimage.cpp - src/options.cpp) + ) ament_target_dependencies(showimage${target_suffix} "rclcpp${target_suffix}" "sensor_msgs" @@ -50,6 +53,56 @@ call_for_each_rmw_implementation(targets GENERATE_DEFAULT) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + # get the rmw implementations ahead of time + get_available_rmw_implementations(middleware_implementations) + foreach(middleware_impl ${middleware_implementations}) + find_package("${middleware_impl}" REQUIRED) + endforeach() + + # These are the regex's for validating the output of the executables. + set(RCLCPP_DEMO_SHOWIMAGE_EXPECTED_OUTPUT "${CMAKE_CURRENT_SOURCE_DIR}/test/showimage") + set(RCLCPP_DEMO_CAM2IMAGE_EXPECTED_OUTPUT "${CMAKE_CURRENT_SOURCE_DIR}/test/cam2image") + + macro(testing_targets) + if( + # TODO(wjwwood): connext and connext_dynamic fail with: + # + # COMMENDSrWriterService_write:!write. Reliable large data requires asynchronous writer. + # + # We can remove this disable for connext when that's fixed. + "${rmw_implementation} " STREQUAL "rmw_connext_cpp " OR + "${rmw_implementation} " STREQUAL "rmw_connext_dynamic_cpp " OR + # TODO(wjwwood): fastrtps fails with: + # + # uncaught exception of type std::runtime_error: vector overcomes the maximum length + # + # We can remove this disable for fastrtps when that's fixed. + "${rmw_implementation} " STREQUAL "rmw_fastrtps_cpp " + ) + message(STATUS "Skipping tests for '${rmw_implementation}'") + return() + endif() + + set(RCLCPP_DEMO_CAM2IMAGE_EXECUTABLE $) + set(RCLCPP_DEMO_SHOWIMAGE_EXECUTABLE $) + + configure_file( + test/test_executables_demo.py.in + test_showimage_cam2image${target_suffix}.py + @ONLY + ) + file(GENERATE + OUTPUT test_showimage_cam2image${target_suffix}.py + INPUT test_showimage_cam2image${target_suffix}.py) + + ament_add_nose_test(test_demo_showimage_cam2image${target_suffix} + "${CMAKE_CURRENT_BINARY_DIR}/test_showimage_cam2image${target_suffix}.py" + TIMEOUT 60) + endmacro() + + call_for_each_rmw_implementation(testing_targets) + endif() ament_package() diff --git a/image_tools/include/image_tools/options.hpp b/image_tools/include/image_tools/options.hpp index e7e129904..d8b3f4ffd 100644 --- a/image_tools/include/image_tools/options.hpp +++ b/image_tools/include/image_tools/options.hpp @@ -45,13 +45,15 @@ std::string get_command_option( * \param[in] depth The queue size for the KEEP_LAST QoS policy. * \param[in] reliability_policy The reliability policy (RELIABLE or BEST_EFFORT). * \param[in] show_camera True to show the input stream, false or NULL otherwise. + * \param[in] freq The frequency at which to publish images. * \param[in] width The width of the image to get, 320 by default. * \param[in] height The height of the image to get, 240 by default. + * \param[in] burger_mode If true, produce images of burgers rather than use a camera. */ bool parse_command_options( int argc, char ** argv, size_t * depth, rmw_qos_reliability_policy_t * reliability_policy, - rmw_qos_history_policy_t * history_policy, bool * show_camera = nullptr, - size_t * width = nullptr, size_t * height = nullptr); + rmw_qos_history_policy_t * history_policy, bool * show_camera = nullptr, double * freq = nullptr, + size_t * width = nullptr, size_t * height = nullptr, bool * burger_mode = nullptr); #endif // IMAGE_TOOLS__OPTIONS_HPP_ diff --git a/image_tools/package.xml b/image_tools/package.xml index adb70a352..0cb25b103 100644 --- a/image_tools/package.xml +++ b/image_tools/package.xml @@ -8,7 +8,6 @@ Apache License 2.0 ament_cmake - libopencv-dev rclcpp rmw_implementation_cmake @@ -20,8 +19,11 @@ sensor_msgs std_msgs + ament_cmake_nose ament_lint_auto ament_lint_common + launch + launch_testing ament_cmake diff --git a/image_tools/src/burger.cpp b/image_tools/src/burger.cpp new file mode 100644 index 000000000..3ee5d6d43 --- /dev/null +++ b/image_tools/src/burger.cpp @@ -0,0 +1,133 @@ +// 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. + +#ifdef _WIN32 +#include // For GetTickCount(). +#endif + +#include +#include +#include + +#include "./burger.hpp" +#include "opencv2/imgproc/imgproc.hpp" + +using burger::Burger; // i've always wanted to write that + +#ifndef _WIN32 +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wchar-subscripts" +#endif +// here lies the world's slowest portable base64 decoder +void decode_base64(const char * cstr, std::vector & out) +{ + int len = static_cast(strlen(cstr)); + if (len < 2) { + return; // would have to think too hard about trivial inputs + } + out.resize(len * 3 / 4); // deal with padding bytes later + uint8_t base64_map[256] = {0}; + for (int i = 'A'; i <= 'Z'; i++) { + base64_map[i] = i - 'A'; + } + for (int i = 'a'; i <= 'z'; i++) { + base64_map[i] = i - 'a' + 26; + } + for (int i = '0'; i <= '9'; i++) { + base64_map[i] = i - '0' + 52; + } + base64_map['+'] = 62; + base64_map['/'] = 63; + int ridx = 0, widx = 0; + for (ridx = 0; ridx < len; ridx += 4) { + // we expand each group of 4 code bytes into 3 output bytes + uint32_t block = 0; + block = (base64_map[cstr[ridx]] << 18) | + (base64_map[cstr[ridx + 1]] << 12) | + (base64_map[cstr[ridx + 2]] << 6) | + (base64_map[cstr[ridx + 3]] << 0); + out[widx++] = (block >> 16) & 0xff; + out[widx++] = (block >> 8) & 0xff; + out[widx++] = (block >> 0) & 0xff; + } + // fix padding now. (these branches are untested so they're probably wrong) + if (cstr[len - 1] == '=' && cstr[len - 2] == '=') { + // there were two padding bytes. remove the two last output bytes + out.pop_back(); + out.pop_back(); + } else if (cstr[len - 1] == '=') { + // there was only one padding byte. remove the last output byte. + out.pop_back(); + } +} +#ifndef _WIN32 +#pragma GCC diagnostic pop +#endif + +Burger::Burger() +{ + size_t burger_size = strlen(BURGER); + std::vector burger_png; + burger_png.resize(burger_size); + decode_base64(BURGER, burger_png); + burger_template = cv::imdecode(burger_png, CV_LOAD_IMAGE_COLOR); + cv::floodFill(burger_template, cv::Point(1, 1), CV_RGB(1, 1, 1)); + cv::compare(burger_template, 1, burger_mask, cv::CMP_NE); +#ifndef _WIN32 + srand(time(NULL)); +#else + srand(GetTickCount()); +#endif +} + +cv::Mat & Burger::render_burger(size_t width, size_t height) +{ + int width_i = static_cast(width); + int height_i = static_cast(height); + if (burger_buf.size().width != width_i || burger_buf.size().height != height_i) { + int num_burgers = rand() % 10 + 2; // NOLINT + x.resize(num_burgers); + y.resize(num_burgers); + x_inc.resize(num_burgers); + y_inc.resize(num_burgers); + for (int b = 0; b < num_burgers; b++) { + x[b] = rand() % (width - burger_template.size().width - 1); // NOLINT + y[b] = rand() % (height - burger_template.size().height - 1); // NOLINT + x_inc[b] = rand() % 3 + 1; // NOLINT + y_inc[b] = rand() % 3 + 1; // NOLINT + } + burger_buf = cv::Mat(height_i, width_i, CV_8UC3); + } + burger_buf = cvScalar(0, 0, 0); + for (int b = 0; b < static_cast(x.size()); b++) { + burger_template.copyTo(burger_buf(cv::Rect( + x[b], + y[b], + burger_template.size().height, + burger_template.size().width + )), burger_mask); + x[b] += x_inc[b]; + y[b] += y_inc[b]; + // bounce as needed + if (x[b] < 0 || x[b] > width_i - burger_template.size().width - 1) { + x_inc[b] *= -1; + x[b] += 2 * x_inc[b]; + } + if (y[b] < 0 || y[b] > height_i - burger_template.size().height - 1) { + y_inc[b] *= -1; + y[b] += 2 * y_inc[b]; + } + } + return burger_buf; +} diff --git a/image_tools/src/burger.hpp b/image_tools/src/burger.hpp new file mode 100644 index 000000000..52095f900 --- /dev/null +++ b/image_tools/src/burger.hpp @@ -0,0 +1,44 @@ +// 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. + +#ifndef BURGER_HPP_ +#define BURGER_HPP_ + +#include + +#include "opencv2/highgui/highgui.hpp" + +namespace burger +{ + +class Burger +{ +public: + Burger(); + cv::Mat & render_burger(size_t width, size_t height); + cv::Mat burger_buf; + +private: + cv::Mat burger_template, burger_mask; + std::vector x, y, x_inc, y_inc; +}; + +// THE FOLLOWING IS A BURGER IN AN AWESOME C BASE64 MACRO. RESPECT IT + +#define BURGER \ + "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" // NOLINT + +} // namespace burger + +#endif // BURGER_HPP_ diff --git a/image_tools/src/cam2image.cpp b/image_tools/src/cam2image.cpp index cf511e3e3..de1cb85d5 100644 --- a/image_tools/src/cam2image.cpp +++ b/image_tools/src/cam2image.cpp @@ -24,6 +24,8 @@ #include "image_tools/options.hpp" +#include "./burger.hpp" + /// Convert an OpenCV matrix encoding type to a string format recognized by sensor_msgs::Image. /** * \param[in] mat_type The OpenCV encoding type. @@ -74,14 +76,17 @@ int main(int argc, char * argv[]) // Initialize default demo parameters bool show_camera = false; size_t depth = 10; + double freq = 30.0; rmw_qos_reliability_policy_t reliability_policy = RMW_QOS_POLICY_RELIABLE; rmw_qos_history_policy_t history_policy = RMW_QOS_POLICY_KEEP_ALL_HISTORY; size_t width = 320; size_t height = 240; + bool burger_mode = false; // Configure demo parameters with command line options. bool success = parse_command_options( - argc, argv, &depth, &reliability_policy, &history_policy, &show_camera, &width, &height); + argc, argv, &depth, &reliability_policy, &history_policy, &show_camera, &freq, &width, &height, + &burger_mode); if (!success) { return 0; } @@ -131,19 +136,22 @@ int main(int argc, char * argv[]) "flip_image", callback, custom_flip_qos_profile); // Set a loop rate for our main event loop. - rclcpp::WallRate loop_rate(30); + rclcpp::WallRate loop_rate(freq); - // Initialize OpenCV video capture stream. cv::VideoCapture cap; - // Always open device 0. - cap.open(0); - - // Set the width and height based on command line arguments. - cap.set(CV_CAP_PROP_FRAME_WIDTH, static_cast(width)); - cap.set(CV_CAP_PROP_FRAME_HEIGHT, static_cast(height)); - if (!cap.isOpened()) { - fprintf(stderr, "Could not open video stream\n"); - return 1; + burger::Burger burger_cap; + if (!burger_mode) { + // Initialize OpenCV video capture stream. + // Always open device 0. + cap.open(0); + + // Set the width and height based on command line arguments. + cap.set(CV_CAP_PROP_FRAME_WIDTH, static_cast(width)); + cap.set(CV_CAP_PROP_FRAME_HEIGHT, static_cast(height)); + if (!cap.isOpened()) { + fprintf(stderr, "Could not open video stream\n"); + return 1; + } } // Initialize OpenCV image matrices. @@ -159,7 +167,11 @@ int main(int argc, char * argv[]) // Our main event loop will spin until the user presses CTRL-C to exit. while (rclcpp::ok()) { // Get the frame from the video capture. - cap >> frame; + if (burger_mode) { + frame = burger_cap.render_burger(width, height); + } else { + cap >> frame; + } // Check if the frame was grabbed correctly if (!frame.empty()) { // Convert to a ROS image diff --git a/image_tools/src/options.cpp b/image_tools/src/options.cpp index 1c23668f0..8ae95dc3f 100644 --- a/image_tools/src/options.cpp +++ b/image_tools/src/options.cpp @@ -34,11 +34,20 @@ std::string get_command_option(const std::vector & args, const std: return std::string(); } +bool get_flag_option(const std::vector & args, const std::string & option) +{ + auto it = std::find(args.begin(), args.end(), option); + if (it != args.end()) { + return true; + } + return false; +} + bool parse_command_options( int argc, char ** argv, size_t * depth, rmw_qos_reliability_policy_t * reliability_policy, rmw_qos_history_policy_t * history_policy, bool * show_camera, - size_t * width, size_t * height) + double * freq, size_t * width, size_t * height, bool * burger_mode) { std::vector args(argv, argv + argc); @@ -50,23 +59,30 @@ bool parse_command_options( ss << " 0 - best effort" << std::endl; ss << " 1 - reliable (default)" << std::endl; ss << " -d: Queue depth. 10 (default)" << std::endl; + ss << " -f: Publish frequency in Hz. 30 (default)" << std::endl; ss << " -k: History QoS setting:" << std::endl; ss << " 0 - only keep last sample" << std::endl; ss << " 1 - keep all the samples (default)" << std::endl; if (show_camera != nullptr) { - ss << " -s: Display camera stream." << std::endl; + ss << " -s: Camera stream:" << std::endl; + ss << " 0 - Do not show the camera stream" << std::endl; + ss << " 1 - Show the camera stream" << std::endl; } if (width != nullptr && height != nullptr) { ss << " -x WIDTH and -y HEIGHT. Resolution. " << std::endl; ss << " Please type v4l2-ctl --list-formats-ext " << std::endl; ss << " to obtain a list of valid values." << std::endl; } + if (burger_mode != nullptr) { + ss << " -b: produce images of burgers rather than connecting to a camera" << std::endl; + } std::cout << ss.str(); return false; } - if (show_camera != nullptr && find_command_option(args, "-s")) { - *show_camera = true; + auto show_camera_str = get_command_option(args, "-s"); + if (!show_camera_str.empty()) { + *show_camera = std::stoul(show_camera_str.c_str()) != 0 ? true : false; } auto depth_str = get_command_option(args, "-d"); @@ -74,6 +90,13 @@ bool parse_command_options( *depth = std::stoul(depth_str.c_str()); } + if (freq != nullptr) { + auto freq_str = get_command_option(args, "-f"); + if (!freq_str.empty()) { + *freq = std::stod(freq_str.c_str()); + } + } + auto reliability_str = get_command_option(args, "-r"); if (!reliability_str.empty()) { unsigned int r = std::stoul(reliability_str.c_str()); @@ -94,5 +117,10 @@ bool parse_command_options( *height = std::stoul(height_str.c_str()); } } + + if (burger_mode) { + *burger_mode = get_flag_option(args, "-b"); + } + return true; } diff --git a/image_tools/src/showimage.cpp b/image_tools/src/showimage.cpp index 83567f6eb..d1b09e56d 100644 --- a/image_tools/src/showimage.cpp +++ b/image_tools/src/showimage.cpp @@ -49,24 +49,24 @@ encoding2mat_type(const std::string & encoding) /// Convert the ROS Image message to an OpenCV matrix and display it to the user. // \param[in] msg The image message to show. -void show_image(const sensor_msgs::msg::Image::SharedPtr msg) +void show_image(const sensor_msgs::msg::Image::SharedPtr msg, bool show_camera) { - std::stringstream ss; - ss << "Received image #" << msg->header.frame_id << std::endl; - std::cout << ss.str(); - - // Convert to an OpenCV matrix by assigning the data. - cv::Mat frame( - msg->height, msg->width, encoding2mat_type(msg->encoding), - const_cast(msg->data.data()), msg->step); - - // NOTE(esteve): Use C version of cvShowImage to avoid this on Windows: - // http://stackoverflow.com/questions/20854682/opencv-multiple-unwanted-window-with-garbage-name - // Show the image in a window called "showimage". - CvMat cvframe = frame; - cvShowImage("showimage", &cvframe); - // Draw the screen and wait for 1 millisecond. - cv::waitKey(1); + printf("Received image #%s\n", msg->header.frame_id.c_str()); + + if (show_camera) { + // Convert to an OpenCV matrix by assigning the data. + cv::Mat frame( + msg->height, msg->width, encoding2mat_type(msg->encoding), + const_cast(msg->data.data()), msg->step); + + // NOTE(esteve): Use C version of cvShowImage to avoid this on Windows: + // http://stackoverflow.com/q/20854682 + // Show the image in a window called "showimage". + CvMat cvframe = frame; + cvShowImage("showimage", &cvframe); + // Draw the screen and wait for 1 millisecond. + cv::waitKey(1); + } } int main(int argc, char * argv[]) @@ -78,14 +78,19 @@ int main(int argc, char * argv[]) size_t depth = 10; rmw_qos_reliability_policy_t reliability_policy = RMW_QOS_POLICY_RELIABLE; rmw_qos_history_policy_t history_policy = RMW_QOS_POLICY_KEEP_ALL_HISTORY; + bool show_camera = true; // Configure demo parameters with command line options. - if (!parse_command_options(argc, argv, &depth, &reliability_policy, &history_policy)) { + if (!parse_command_options(argc, argv, &depth, &reliability_policy, &history_policy, + &show_camera)) + { return 0; } - // Initialize an OpenCV named window called "showimage". - cvNamedWindow("showimage", CV_WINDOW_AUTOSIZE); + if (show_camera) { + // Initialize an OpenCV named window called "showimage". + cvNamedWindow("showimage", CV_WINDOW_AUTOSIZE); + } // Initialize a ROS node. auto node = rclcpp::node::Node::make_shared("showimage"); @@ -107,9 +112,14 @@ int main(int argc, char * argv[]) // makes no guarantees about the order or reliability of delivery. custom_qos_profile.reliability = reliability_policy; + auto callback = [show_camera](const sensor_msgs::msg::Image::SharedPtr msg) + { + show_image(msg, show_camera); + }; + // Initialize a subscriber that will receive the ROS Image message to be displayed. auto sub = node->create_subscription( - "image", show_image, custom_qos_profile); + "image", callback, custom_qos_profile); rclcpp::spin(node); diff --git a/image_tools/test/cam2image.regex b/image_tools/test/cam2image.regex new file mode 100644 index 000000000..917645836 --- /dev/null +++ b/image_tools/test/cam2image.regex @@ -0,0 +1 @@ +Publishing image #100 diff --git a/image_tools/test/showimage.regex b/image_tools/test/showimage.regex new file mode 100644 index 000000000..fe331b99a --- /dev/null +++ b/image_tools/test/showimage.regex @@ -0,0 +1 @@ +Received image #\d+ diff --git a/image_tools/test/test_executables_demo.py.in b/image_tools/test/test_executables_demo.py.in new file mode 100644 index 000000000..23b014430 --- /dev/null +++ b/image_tools/test/test_executables_demo.py.in @@ -0,0 +1,70 @@ +# 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 import LaunchDescriptor +from launch.exit_handler import default_exit_handler +from launch.launcher import DefaultLauncher +from launch_testing import create_handler + + +def setup(): + os.environ['OSPL_VERBOSITY'] = '8' # 8 = OS_NONE + + +def test_executable(): + output_handlers = [] + + launch_descriptor = LaunchDescriptor() + + showimage_executable = '@RCLCPP_DEMO_SHOWIMAGE_EXECUTABLE@' + showimage_output_file = '@RCLCPP_DEMO_SHOWIMAGE_EXPECTED_OUTPUT@' + showimage_name = 'test_showimage' + showimage_handler = create_handler(showimage_name, launch_descriptor, showimage_output_file) + assert showimage_handler, 'Cannot find appropriate handler for %s' % showimage_output_file + output_handlers.append(showimage_handler) + launch_descriptor.add_process( + cmd=[showimage_executable, '-r', '0', '-s', '0'], + name=showimage_name, + exit_handler=default_exit_handler, + output_handlers=[showimage_handler], + ) + + cam2image_executable = '@RCLCPP_DEMO_CAM2IMAGE_EXECUTABLE@' + cam2image_output_file = '@RCLCPP_DEMO_CAM2IMAGE_EXPECTED_OUTPUT@' + cam2image_name = 'test_cam2image' + cam2image_handler = create_handler(cam2image_name, launch_descriptor, cam2image_output_file) + assert cam2image_handler, 'Cannot find appropriate handler for %s' % cam2image_output_file + output_handlers.append(cam2image_handler) + launch_descriptor.add_process( + cmd=[cam2image_executable, '-r', '0', '-s', '0', '-b'], + name=cam2image_name, + exit_handler=default_exit_handler, + output_handlers=[cam2image_handler], + ) + + launcher = DefaultLauncher() + launcher.add_launch_descriptor(launch_descriptor) + rc = launcher.launch() + + assert rc == 0, \ + "The launch file failed with exit code '" + str(rc) + "'. " \ + 'Maybe the client did not receive any messages?' + + for handler in output_handlers: + handler.check() + +if __name__ == '__main__': + test_executable()