camera streaming scripts

CMakeLists.txt

-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/camera_stream.py

+#! /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()

camera_streaming/camera_subscriber.py

+#! /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()

include/camera_streaming/options.hpp

-// 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_

package.xml

@@ -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>

setup.cfg

+[develop]
+script-dir=$base/lib/camera_streaming
+[install]
+install-scripts=$base/lib/camera_streaming

setup.py

+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',
+        ],
+    },
+)

src/burger.cpp

-// 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);