Skip to content

Commit

Permalink
Image tools tests redux (#67)
Browse files Browse the repository at this point in the history
* 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
  • Loading branch information
wjwwood authored Sep 7, 2016
1 parent 1f2bbe0 commit 16c3bc5
Show file tree
Hide file tree
Showing 11 changed files with 399 additions and 43 deletions.
57 changes: 55 additions & 2 deletions image_tools/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -22,17 +22,20 @@ 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"
"std_msgs"
"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"
Expand All @@ -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 $<TARGET_FILE:cam2image${target_suffix}>)
set(RCLCPP_DEMO_SHOWIMAGE_EXECUTABLE $<TARGET_FILE:showimage${target_suffix}>)

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)

This comment has been minimized.

Copy link
@dirk-thomas

dirk-thomas Dec 20, 2016

Member

This approach does not work on Windows where CMake expands the files for every configuration. It is only viable to use the same file across all configuration if the content of the generated file is the same. But since this file contains configuration specific TARGET_FILEs it needs to expand into configuration specific files. See #102 for the fix.


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()
6 changes: 4 additions & 2 deletions image_tools/include/image_tools/options.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_
4 changes: 3 additions & 1 deletion image_tools/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
<license>Apache License 2.0</license>

<buildtool_depend>ament_cmake</buildtool_depend>

<build_depend>libopencv-dev</build_depend>
<build_depend>rclcpp</build_depend>
<build_depend>rmw_implementation_cmake</build_depend>
Expand All @@ -20,8 +19,11 @@
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>

<test_depend>ament_cmake_nose</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>launch</test_depend>
<test_depend>launch_testing</test_depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
133 changes: 133 additions & 0 deletions image_tools/src/burger.cpp
Original file line number Diff line number Diff line change
@@ -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 <Windows.h> // For GetTickCount().
#endif

#include <cstring>
#include <cstdio>
#include <vector>

#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<uint8_t> & out)
{
int len = static_cast<int>(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<uint8_t> 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<int>(width);
int height_i = static_cast<int>(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<int>(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;
}
44 changes: 44 additions & 0 deletions image_tools/src/burger.hpp
Original file line number Diff line number Diff line change
@@ -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 <vector>

#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<int> 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_
38 changes: 25 additions & 13 deletions image_tools/src/cam2image.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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<double>(width));
cap.set(CV_CAP_PROP_FRAME_HEIGHT, static_cast<double>(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<double>(width));
cap.set(CV_CAP_PROP_FRAME_HEIGHT, static_cast<double>(height));
if (!cap.isOpened()) {
fprintf(stderr, "Could not open video stream\n");
return 1;
}
}

// Initialize OpenCV image matrices.
Expand All @@ -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
Expand Down
Loading

0 comments on commit 16c3bc5

Please sign in to comment.