Commit a7178889 authored by Barciś, Agata's avatar Barciś, Agata

camera streaming scripts

parent 82ab4e22
Pipeline #21956 failed with stages
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()
#! /usr/bin/env python
import rclpy
from rclpy.node import Node
from std_msgs.msg import Header
from sensor_msgs.msg import CompressedImage
import cv2
from builtin_interfaces.msg import Time
import socket
# from datetime import datetime
from rclpy.qos import (
QoSProfile,
QoSDurabilityPolicy,
QoSHistoryPolicy,
QoSReliabilityPolicy,
)
CUSTOM_QOS = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
# RMW_QOS_POLICY_RELIABILITY_RELIABLE,
depth=1,
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE,
)
class CameraStream():
def __init__(self):
self.node = Node('camera_stream')
hostname = socket.gethostname()
self.publisher = self.node.create_publisher(
CompressedImage,
'/{}/camera'.format(hostname),
qos_profile=CUSTOM_QOS,
)
timer_period = 0.5
self.timer = self.node.create_timer(timer_period, self.timer_callback)
self.cam = cv2.VideoCapture(0)
self.cam.set(cv2.CAP_PROP_FPS, 60)
print(cv2.CAP_PROP_BUFFERSIZE)
self.cam.set(cv2.CAP_PROP_BUFFERSIZE, 1)
self.cam.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG'))
self.cam.read()
def timer_callback(self):
for i in range(16):
self.cam.grab()
frame = self.cam.retrieve()[1]
now = self.node.get_clock().now()
frame = cv2.resize(frame, (320, 240))
jpg = bytearray(cv2.imencode('.jpg', frame)[1])
msg = CompressedImage()
nanosec, sec = map(
int,
(now.nanoseconds % 10**9, now.nanoseconds/10**9)
)
header = Header(
frame_id="",
stamp=Time(sec=sec, nanosec=nanosec)
)
msg.header = header
msg.format = 'jpeg'
msg.data = jpg
self.publisher.publish(msg)
print("image sent")
# reading = 0
# resizing = 0
# encoding = 0
#
# frame = cam.read()[1]
# for i in range(100):
# start_time = datetime.now()
# frame = cam.read()[1]
# read_time = datetime.now()
# frame = resize(frame, (320, 240))
# resize_time = datetime.now()
# jpg = imencode('.jpg', frame)[1].tostring()
# end_time = datetime.now()
# reading += (read_time - start_time).total_seconds()
# resizing += (resize_time - read_time).total_seconds()
# encoding += (end_time - resize_time).total_seconds()
#
# print('reading', reading/100)
# print('resizing', resizing/100)
# print('encoding', encoding/100)
# print('total', reading + resizing + encoding)
def main():
rclpy.init()
camera_stream = CameraStream()
rclpy.spin(camera_stream.node)
camera_stream.node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
#! /usr/bin/env python
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import CompressedImage
import cv2
import numpy as np
from datetime import datetime
import matplotlib.pyplot as plt
from rclpy.qos import (
QoSProfile,
QoSDurabilityPolicy,
QoSHistoryPolicy,
QoSReliabilityPolicy,
)
CUSTOM_QOS = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
# RMW_QOS_POLICY_RELIABILITY_RELIABLE,
depth=1,
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE,
)
class CameraSubscriber():
def __init__(self):
self.node = Node('camera_subscriber')
self.hosts = {
'fdrones_t90': 0,
'fdrones_t91': 1,
'fdrones_t92': 2,
'fdrones_t93': 3,
'fdrones_t94': 4,
'fdrones_t95': 5,
'fdrones_t96': 6,
'fdrones_t97': 7,
}
self.delays = {
key: 0
for key in self.hosts.keys()
}
self.fps = {
key: 0
for key in self.hosts.keys()
}
self.last_frame = {
key: datetime.now()
for key in self.hosts.keys()
}
self.resolution = (240, 320, 3)
hosts_num = len(self.hosts.keys())
self.shape = (
int(np.floor(np.sqrt(hosts_num))),
int(np.ceil(hosts_num / int(np.sqrt(hosts_num))))
)
self.images = np.zeros(self.shape + self.resolution)
self.camera_subscriptions = [
self.node.create_subscription(
CompressedImage,
'/{}/camera'.format(host),
self.gen_camera_callback(host),
qos_profile=CUSTOM_QOS,
) for host in self.hosts.keys()
]
# self.figure, self.display = plt.subplots(2, 4)
# plt.show(block=False)
def check_for_old_images(self):
for host in self.hosts.keys():
now = datetime.now()
if self.fps[host] and (now - self.last_frame[host]).total_seconds() > 10:
host_id = self.hosts[host]
place = (int(host_id / self.shape[1]), int(host_id % self.shape[1]))
self.images[place[0], place[1]] *= 0.25
self.fps[host] = 0
def display_streams(self, # 2d np array of imgs (each img an np array)
pad=10, # number of pixels to use for padding, must be even
win_name='camera streams' # name of cv2 window
):
self.check_for_old_images()
rows, cols = self.shape
subplot_shapes = np.array([list(map(np.shape, x)) for x in self.images])
sp_height, sp_width, depth = np.max(
np.max(subplot_shapes, axis=0),
axis=0
)
self.titles = np.array([
f'{host}, ' +
(f'{self.fps[host]:.02f} FPS, '
if self.fps[host] else 'OFFLINE, ') +
f'latency: {self.delays[host]:.02f}, '
f'red: {0}'
for host in self.hosts.keys()
]).reshape(self.shape)
title_pad = 30
pad_top = pad + title_pad
frame = np.zeros(
(rows * (sp_height + pad_top),
cols * (sp_width + pad), depth))
for r in range(rows):
for c in range(cols):
img = self.images[r, c]
h, w, _ = img.shape
y0 = r * (sp_height+pad_top) + pad_top//2
x0 = c * (sp_width+pad) + pad//2
frame[y0:y0+h, x0:x0+w] = img
frame = cv2.putText(frame, self.titles[r, c], (x0, y0-title_pad//4), cv2.FONT_HERSHEY_COMPLEX, .35, (255,255,255))
# print('images', self.images)
# print('frame', frame)
cv2.imshow(win_name, frame)
cv2.waitKey(1)
def gen_camera_callback(self, host):
def _camera_callback(msg):
now = datetime.now()
img = msg.data
npimg = np.frombuffer(img, dtype=np.uint8)
img = cv2.imdecode(npimg, cv2.IMREAD_COLOR)/256
# cv2.imwrite('/home/agniewek/longTemp/test.jpg', img)
host_id = self.hosts[host]
place = (int(host_id / self.shape[1]), int(host_id % self.shape[1]))
self.images[place[0], place[1]] = img
self.display_streams()
# self.display[int(host_id / 4), host_id % 4].imshow(img)
# self.figure.canvas.draw()
# cv2.waitKey(1)
t_gen = datetime.fromtimestamp(
msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
)
self.fps[host] = round(
1/(t_gen - self.last_frame[host]).total_seconds(),
2
)
self.last_frame[host] = t_gen
now2 = datetime.now()
self.delays[host] = (now - t_gen).total_seconds()
# print('delay: ', (now - t_gen).total_seconds())
# print('delay2: ', (now2 - t_gen).total_seconds())
return _camera_callback
def main():
rclpy.init()
camera_subscriber = CameraSubscriber()
rclpy.spin(camera_subscriber.node)
camera_subscriber.node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
// 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_
......@@ -5,28 +5,20 @@
<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>
<author email="agata.barcis@aau.at">Agata Barciś</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>rclpy</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>
<!-- These test dependencies are optional
Their purpose is to make sure that the code passes the linters -->
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<export>
<build_type>ament_cmake</build_type>
<build_type>ament_python</build_type>
</export>
</package>
[develop]
script-dir=$base/lib/camera_streaming
[install]
install-scripts=$base/lib/camera_streaming
from setuptools import find_packages
from setuptools import setup
package_name = 'camera_streaming'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
author='Michal Barcis',
author_email='michal.barcis@aau.at',
maintainer='Michal Barcis',
maintainer_email='michal.barcis@aau.at',
keywords=['ROS'],
classifiers=[
'Intended Audience :: Developers',
'License :: OSI Approved :: Apache Software License',
'Programming Language :: Python',
'Topic :: Software Development',
],
description='TODO',
license='TODO',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'camera_streaming = camera_streaming.camera_streaming:main',
],
},
)
// 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);