add image_processor pkgs

.gitignore

@@ -7,3 +7,5 @@ COLCON_IGNORE
 # atk
 .atk-compose.yml
 
+.vscode/
+*.pyc

atk.yml

@@ -33,7 +33,7 @@ services:
         USER_UID: "@{uid}"
         USER_GID: "@{gid}"
         APT_DEPENDENCIES: "bash zsh vim git python3-pip libopencv-dev"
-        PIP_REQUIREMENTS: "pandas matplotlib numpy>=1.19"
+        PIP_REQUIREMENTS: "pandas matplotlib numpy>=1.19 opencv-python"
         USER_GROUPS: "dialout video"
         ROS_DISTRO: galactic
     environment:

workspace/src/cpp_image_processor/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.8)
+project(cpp_image_processor)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies    
+find_package(ament_cmake_auto REQUIRED)    
+ament_auto_find_build_dependencies()    
+
+ament_auto_generate_code()    
+
+ament_auto_add_executable(image_processor_node src/image_processor_node.cpp)    
+
+ament_auto_package(    
+    CONFIG_EXTRAS "cpp_image_processor-extras.cmake"    
+)    
+
+# ament_package() 
+
+
+
+

workspace/src/cpp_image_processor/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>cpp_image_processor</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">ros_sandbox</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>OpenCV</depend>
+  <depend>rclcpp_components</depend>
+  <depend>cv_bridge</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

workspace/src/cpp_image_processor/src/image_processor_node.cpp

@@ -0,0 +1,65 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include <opencv2/opencv.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+class ImageProcessorNode : public rclcpp::Node
+{
+	public:
+		ImageProcessorNode() : Node("image_processor_node"), count_(0), clock_()
+		{
+			image_sub = this->create_subscription<sensor_msgs::msg::Image>("image/original", 10, std::bind(&ImageProcessorNode::image_callback, this, std::placeholders::_1));
+
+            image_pub = this->create_publisher<sensor_msgs::msg::Image>("image/grayscale", 10);
+		}
+	private:
+		void image_callback (const sensor_msgs::msg::Image::SharedPtr msg) 
+		{
+
+			rclcpp::Time cur_time = clock_.now();
+			const auto secs = cur_time.nanoseconds()/1000000000UL - msg->header.stamp.sec;
+			const auto nsecs = cur_time.nanoseconds()%1000000000UL - msg->header.stamp.nanosec;
+
+			std::stringstream ss;
+			ss << "Time diff: " << secs << "." << nsecs << "s";
+			RCLCPP_INFO(this->get_logger(), ss.str().c_str());
+
+            // convert Image msg to CvImagePtr
+            cv_bridge::CvImagePtr colorPtr;
+            colorPtr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
+
+            // get grayscale image
+            cv::Mat colorMat = colorPtr->image;
+            cv::Mat grayMat;
+            cv::cvtColor(colorMat, grayMat, CV_BGR2GRAY);
+
+            // convert cv::Mat to CvImage
+            std_msgs::msg::Header header; // empty header
+            header.stamp = clock_.now();
+            cv_bridge::CvImage grayPtr;
+            grayPtr = cv_bridge::CvImage(header, sensor_msgs::image_encodings::MONO8, grayMat);
+
+            // publish
+            image_pub->publish(*grayPtr.toImageMsg());
+		}
+
+		rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub;
+        rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub;
+		size_t count_;
+		rclcpp::Clock clock_;
+
+};
+
+int main(int argc, char* argv[])
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<ImageProcessorNode>());
+	rclcpp::shutdown();
+	return 0;
+}

workspace/src/py_image_processor/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>py_image_processor</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">ros_sandbox</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

workspace/src/py_image_processor/py_image_processor/image_processor_node.py

@@ -0,0 +1,47 @@
+import time
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+import cv2
+from cv_bridge import CvBridge
+import numpy as np
+
+class ImageProcessorNode(Node):
+    def __init__(self):
+        super().__init__('image_processor_node')
+        self.logger = rclpy.logging.get_logger(self.get_name())
+
+        self.image_sub = self.create_subscription(Image, "image/original", self.image_callback, 10)
+        self.image_sub # avoid unused variable warning
+
+        self.image_pub = self.create_publisher(Image, "image/grayscale", 1)
+
+        self.clock_ = rclpy.clock.Clock()
+        self.bridge = CvBridge()
+
+    def image_callback(self, msg):
+        cur_time = self.clock_.now()
+        secs = cur_time.nanoseconds//1000000000 - msg.header.stamp.sec
+        nsecs = cur_time.nanoseconds%1000000000 - msg.header.stamp.nanosec
+        self.get_logger().info(f"Time diff: {secs}.{nsecs}s")
+
+        cv_image = self.bridge.imgmsg_to_cv2(msg)
+        gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)       
+        gray_msg = self.bridge.cv2_to_imgmsg(gray)
+
+        gray_msg.header.stamp = self.clock_.now().to_msg()
+        self.image_pub.publish(gray_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    image_processor_node = ImageProcessorNode()
+
+    rclpy.spin(image_processor_node)
+
+    image_processor_node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+

workspace/src/py_image_processor/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/py_image_processor
+[install]
+install_scripts=$base/lib/py_image_processor

workspace/src/py_image_processor/setup.py

@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'py_image_processor'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='ros_sandbox',
+    maintainer_email='[email protected]',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'image_processor_node = py_image_processor.image_processor_node:main'
+        ],
+    },
+)

workspace/src/py_image_processor/test/test_copyright.py

@@ -0,0 +1,23 @@
+# 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.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

workspace/src/py_image_processor/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 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.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

workspace/src/py_image_processor/test/test_pep257.py

@@ -0,0 +1,23 @@
+# 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.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

workspace/src/sandbox_launch/launch/cpp_launch.py

@@ -12,6 +12,13 @@ def generate_launch_description():
     )
     ld.add_action(image_spawner)
 
+    # image processor
+    image_processor = Node(
+        package="cpp_image_processor",
+        executable="image_processor_node" 
+    )
+    ld.add_action(image_processor)
+
     # image receiver
     image_receiver = Node(
         package="cpp_image_receiver",

workspace/src/sandbox_launch/launch/py_launch.py

@@ -12,6 +12,13 @@ def generate_launch_description():
     )
     ld.add_action(image_spawner)
 
+    # image processor
+    image_processor = Node(
+        package="py_image_processor",
+        executable="image_processor_node" 
+    )
+    ld.add_action(image_processor)
+
     # image receiver
     image_receiver = Node(
         package="py_image_receiver",