Skip to content

GitLab

Commit 44ce9d81 authored by Barcis, Michal's avatar Barcis, Michal
Browse files

initial commit

parents
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt 0 → 100644
View file @ 44ce9d81
cmake_minimum_required(VERSION 3.5)
project(camera_streaming)
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(OpenCV REQUIRED)
include_directories(include)
add_executable(cam2image
src/burger.cpp
src/cam2image.cpp
src/options.cpp
)
ament_target_dependencies(cam2image
"rclcpp"
"sensor_msgs"
"std_msgs"
"OpenCV")
add_executable(showimage
src/options.cpp
src/showimage.cpp
)
ament_target_dependencies(showimage
"rclcpp"
"sensor_msgs"
"std_msgs"
"OpenCV")
install(TARGETS
cam2image
showimage
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
find_package(ament_cmake_pytest REQUIRED)
find_package(rmw_implementation_cmake REQUIRED)
# 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)
set(RCLCPP_DEMO_CAM2IMAGE_EXECUTABLE $<TARGET_FILE:cam2image>)
set(RCLCPP_DEMO_SHOWIMAGE_EXECUTABLE $<TARGET_FILE:showimage>)
configure_file(
test/test_executables_demo.py.in
test_showimage_cam2image${target_suffix}.py.genexp
@ONLY
)
file(GENERATE
OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test_showimage_cam2image${target_suffix}_$<CONFIG>.py"
INPUT "${CMAKE_CURRENT_BINARY_DIR}/test_showimage_cam2image${target_suffix}.py.genexp"
)
ament_add_pytest_test(test_showimage_cam2image${target_suffix}
"${CMAKE_CURRENT_BINARY_DIR}/test_showimage_cam2image${target_suffix}_$<CONFIG>.py"
ENV
RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation}
RMW_IMPLEMENTATION=${rmw_implementation}
TIMEOUT 30
)
endmacro()
call_for_each_rmw_implementation(testing_targets)
endif()
ament_package()
camera_streaming/CMakeLists.txt 0 → 100644
View file @ 44ce9d81
cmake_minimum_required(VERSION 3.5)
project(camera_streaming)
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(OpenCV REQUIRED)
include_directories(include)
add_executable(cam2image
src/burger.cpp
src/cam2image.cpp
src/options.cpp
)
ament_target_dependencies(cam2image
"rclcpp"
"sensor_msgs"
"std_msgs"
"OpenCV")
add_executable(showimage
src/options.cpp
src/showimage.cpp
)
ament_target_dependencies(showimage
"rclcpp"
"sensor_msgs"
"std_msgs"
"OpenCV")
install(TARGETS
cam2image
showimage
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
find_package(ament_cmake_pytest REQUIRED)
find_package(rmw_implementation_cmake REQUIRED)
# 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)
set(RCLCPP_DEMO_CAM2IMAGE_EXECUTABLE $<TARGET_FILE:cam2image>)
set(RCLCPP_DEMO_SHOWIMAGE_EXECUTABLE $<TARGET_FILE:showimage>)
configure_file(
test/test_executables_demo.py.in
test_showimage_cam2image${target_suffix}.py.genexp
@ONLY
)
file(GENERATE
OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test_showimage_cam2image${target_suffix}_$<CONFIG>.py"
INPUT "${CMAKE_CURRENT_BINARY_DIR}/test_showimage_cam2image${target_suffix}.py.genexp"
)
ament_add_pytest_test(test_showimage_cam2image${target_suffix}
"${CMAKE_CURRENT_BINARY_DIR}/test_showimage_cam2image${target_suffix}_$<CONFIG>.py"
ENV
RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation}
RMW_IMPLEMENTATION=${rmw_implementation}
TIMEOUT 30
)
endmacro()
call_for_each_rmw_implementation(testing_targets)
endif()
ament_package()
camera_streaming/include/camera_streaming/options.hpp 0 → 100644
View file @ 44ce9d81
// Copyright 2015 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 IMAGE_TOOLS__OPTIONS_HPP_
#define IMAGE_TOOLS__OPTIONS_HPP_
#include <string>
#include <vector>
#include "rmw/types.h"
/// Find "option" in the argument vector.
/**
* \param[in] args The argument vector
* \param[in] option The option to search for
* \return True if option was found in args, false otherwise.
*/
bool find_command_option(
const std::vector<std::string> & args, const std::string & option);
/// Get the value corresponding to option.
/**
* \param[in] args The argument vector to search in
* \param[in] option The option to search for
* \return The value that comes after "option"
*/
std::string get_command_option(
const std::vector<std::string> & args, const std::string & option);
/// Parse the C-style argument vector and return demo-specific parameters.
/**
* \param[in] argc Size of the argument vector.
* \param[in] argv Argument vector, an array of C-style strings.
* \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, double * freq = nullptr,
size_t * width = nullptr, size_t * height = nullptr, bool * burger_mode = nullptr,
std::string * topic = nullptr);
#endif // IMAGE_TOOLS__OPTIONS_HPP_
camera_streaming/package.xml 0 → 100644
View file @ 44ce9d81
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>camera_streaming</name>
<version>0.0.0</version>
<description>Tools to capture and play back images to and from DDS subscriptions and publications.</description>
<maintainer email="michal.barcis@aau.at">Michał Barciś</maintainer>
<author email="dthomas@osrfoundation.org">Dirk Thomas</author>
<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>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<exec_depend>libopencv-dev</exec_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<test_depend>ament_cmake_pytest</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>
<test_depend>rmw_implementation_cmake</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
camera_streaming/src/burger.cpp 0 → 100644
View file @ 44ce9d81
// 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 (uint8_t i = 'A'; i <= 'Z'; i++) {
base64_map[i] = i - 'A';
}
for (uint8_t i = 'a'; i <= 'z'; i++) {
base64_map[i] = i - 'a' + 26;
}
for (uint8_t 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;
}
camera_streaming/src/burger.hpp 0 → 100644
View file @ 44ce9d81
// 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_
camera_streaming/src/cam2image.cpp 0 → 100644
View file @ 44ce9d81
// Copyright 2015 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.
#include <cstdio>
#include <iostream>
#include <memory>
#include <string>
#include "opencv2/highgui/highgui.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "std_msgs/msg/bool.hpp"
#include "std_msgs/msg/float32.hpp"
#include "camera_streaming/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.
* \return A string representing the encoding type.
*/
std::string
mat_type2encoding(int mat_type)
{
switch (mat_type) {
case CV_8UC1:
return "mono8";
case CV_8UC3:
return "bgr8";
case CV_16SC1:
return "mono16";
case CV_8UC4:
return "rgba8";
default:
throw std::runtime_error("Unsupported encoding type");
}
}
/// Convert an OpenCV matrix (cv::Mat) to a ROS Image message.
/**
* \param[in] frame The OpenCV matrix/image to convert.
* \param[in] frame_id ID for the ROS message.
* \param[out] Allocated shared pointer for the ROS Image message.
*/
void convert_frame_to_message(
const cv::Mat & frame, size_t frame_id, sensor_msgs::msg::Image::SharedPtr msg)
{
// copy cv information into ros message
msg->height = frame.rows;
msg->width = frame.cols;
msg->encoding = mat_type2encoding(frame.type());
msg->step = static_cast<sensor_msgs::msg::Image::_step_type>(frame.step);
size_t size = frame.step * frame.rows;
msg->data.resize(size);
memcpy(&msg->data[0], frame.data, size);
msg->header.frame_id = std::to_string(frame_id);
}
void adapt(
double factor,
double* rate_adaptation,
size_t* adapted_width,
size_t* adapted_height)
{
*rate_adaptation = 1;
if(factor >= 1) {
*adapted_width = 320;
*adapted_height = 240;
}else{
*adapted_width = 160;
*adapted_height = 120;
}
}
int main(int argc, char * argv[])
{
// Pass command line arguments to rclcpp.
rclcpp::init(argc, argv);
// Initialize default demo parameters
bool show_camera = false;
size_t depth = 10;
double freq = 5.0;
rmw_qos_reliability_policy_t reliability_policy = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
rmw_qos_history_policy_t history_policy = RMW_QOS_POLICY_HISTORY_KEEP_ALL;
size_t width = 320;
size_t height = 240;
bool burger_mode = false;
std::string topic("image");
// Force flush of the stdout buffer.
// This ensures a correct sync of all prints
// even when executed simultaneously within a launch file.
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
// Configure demo parameters with command line options.
bool success = parse_command_options(
argc, argv, &depth, &reliability_policy, &history_policy, &show_camera, &freq, &width, &height,
&burger_mode, &topic);
if (!success) {
return 0;
}
// Initialize a ROS 2 node to publish images read from the OpenCV interface to the camera.
auto node = rclcpp::Node::make_shared("cam2image");
rclcpp::Logger node_logger = node->get_logger();
// Set the parameters of the quality of service profile. Initialize as the default profile
// and set the QoS parameters specified on the command line.
rmw_qos_profile_t custom_camera_qos_profile = rmw_qos_profile_default;
// Depth represents how many messages to store in history when the history policy is KEEP_LAST.
custom_camera_qos_profile.depth = 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 = 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 = history_policy;
RCLCPP_INFO(node_logger, "Publishing data on topic '%s'", topic.c_str())
// Create the image publisher with our custom QoS profile.
auto pub = node->create_publisher<sensor_msgs::msg::Image>(
topic, custom_camera_qos_profile);
// is_flipped will cause the incoming camera image message to flip about the y-axis.
bool is_flipped = false;
// Subscribe to a message that will toggle flipping or not flipping, and manage the state in a
// callback.
auto callback =
[&is_flipped, &node_logger](const std_msgs::msg::Bool::SharedPtr msg) -> void
{
is_flipped = msg->data;
RCLCPP_INFO(node_logger, "Set flip mode to: %s", is_flipped ? "on" : "off")
};
// Set the QoS profile for the subscription to the flip message.
rmw_qos_profile_t custom_flip_qos_profile = rmw_qos_profile_sensor_data;
custom_flip_qos_profile.depth = 10;
auto sub = node->create_subscription<std_msgs::msg::Bool>(