ROS 2 node that flips image and publishes updated transform

image_flip/CMakeLists.txt

@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.5)
+project(image_flip)
+
+# 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_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(OpenCV REQUIRED core imgproc)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED src/image_flip_node.cpp)
+target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES})
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "${PROJECT_NAME}::ImageFlipNode"
+  EXECUTABLE image_flip_node
+)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE launch)

image_flip/include/image_flip/image_flip_node.hpp

@@ -0,0 +1,106 @@
+// Copyright (c) 2022, CHRISLab, Christopher Newport University
+// Copyright (c) 2008, Willow Garage, Inc.
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of the Willow Garage nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMAGE_FLIP__IMAGE_FLIP_NODE_HPP_
+#define IMAGE_FLIP__IMAGE_FLIP_NODE_HPP_
+
+#include <math.h>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+
+#include <memory>
+#include <string>
+
+#include <cv_bridge/cv_bridge.hpp>
+#include <image_transport/image_transport.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include "image_flip/visibility.h"
+
+namespace image_flip
+{
+
+struct ImageFlipConfig
+{
+  std::string output_frame_id;
+  int rotation_steps;
+  bool use_camera_info;
+  rmw_qos_profile_t input_qos;  // "default", "sensor_data", etc ..
+  rmw_qos_profile_t output_qos;
+};
+
+class ImageFlipNode : public rclcpp::Node
+{
+public:
+  IMAGE_FLIP_PUBLIC ImageFlipNode(rclcpp::NodeOptions options);
+
+private:
+  const std::string frameWithDefault(const std::string & frame, const std::string & image_frame);
+  void imageCallbackWithInfo(
+    const sensor_msgs::msg::Image::ConstSharedPtr & msg,
+    const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info);
+  void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg);
+  void do_work(
+    const sensor_msgs::msg::Image::ConstSharedPtr & msg,
+    const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info,
+    const std::string input_frame_from_msg);
+  void subscribe();
+  void unsubscribe();
+  void connectCb();
+  void disconnectCb();
+  void onInit();
+
+  rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_sub_;
+  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_pub_;
+  bool tf_unpublished_;
+
+  image_flip::ImageFlipConfig config_;
+
+  image_transport::Publisher img_pub_;
+  image_transport::Subscriber img_sub_;
+  image_transport::CameraSubscriber cam_sub_;
+  image_transport::CameraPublisher cam_pub_;
+
+  int subscriber_count_;
+  double angle_;
+  tf2::TimePoint prev_stamp_;
+  geometry_msgs::msg::TransformStamped transform_;
+};
+}  // namespace image_flip
+
+#endif  // IMAGE_FLIP__IMAGE_FLIP_NODE_HPP_

image_flip/include/image_flip/visibility.h

@@ -0,0 +1,83 @@
+// Copyright (c) 2008, Willow Garage, Inc.
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of the Willow Garage nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMAGE_FLIP__VISIBILITY_H_
+#define IMAGE_FLIP__VISIBILITY_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+
+  #ifdef __GNUC__
+    #define IMAGE_FLIP_EXPORT __attribute__ ((dllexport))
+    #define IMAGE_FLIP_IMPORT __attribute__ ((dllimport))
+  #else
+    #define IMAGE_FLIP_EXPORT __declspec(dllexport)
+    #define IMAGE_FLIP_IMPORT __declspec(dllimport)
+  #endif
+
+  #ifdef IMAGE_FLIP_DLL
+    #define IMAGE_FLIP_PUBLIC IMAGE_FLIP_EXPORT
+  #else
+    #define IMAGE_FLIP_PUBLIC IMAGE_FLIP_IMPORT
+  #endif
+
+  #define IMAGE_FLIP_PUBLIC_TYPE IMAGE_FLIP_PUBLIC
+
+  #define IMAGE_FLIP_LOCAL
+
+#else
+
+  #define IMAGE_FLIP_EXPORT __attribute__ ((visibility("default")))
+  #define IMAGE_FLIP_IMPORT
+
+  #if __GNUC__ >= 4
+    #define IMAGE_FLIP_PUBLIC __attribute__ ((visibility("default")))
+    #define IMAGE_FLIP_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define IMAGE_FLIP_PUBLIC
+    #define IMAGE_FLIP_LOCAL
+  #endif
+
+  #define IMAGE_FLIP_PUBLIC_TYPE
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // IMAGE_FLIP__VISIBILITY_H_

image_flip/launch/image_flip.launch.py

@@ -0,0 +1,64 @@
+# Copyright (c) 2022, CHRISLab, Christopher Newport University
+# All rights reserved.
+#
+# Software License Agreement (BSD License 2.0)
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the Willow Garage nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+"""Demonstration of basic launch of the image_flip_node with remappings."""
+
+import launch_ros.actions
+from launch import LaunchDescription
+
+
+def generate_launch_description():
+    """
+    Launch description for basic launch of the image_flip_node.
+
+    Launch description for basic launch of the image_flip_node,
+    which includes parameters and topic remappings.
+    """
+    return LaunchDescription([
+        launch_ros.actions.Node(
+            package='image_flip', executable='image_flip_node',
+            output='screen', name='camera_flip',
+            remappings=[("image",                'camera/rgb/image_raw'),
+                        ('camera_info',          'camera/rgb/camera_info'),
+                        ('rotated/image',        'camera_rotated/image_rotated'),
+                        ('rotated/camera_info',  'camera_rotated/camera_info'),
+                        ('rotated/image/compressed',
+                            'camera_rotated/image_rotated/compressed'),
+                        ('rotated/image/compressedDepth',
+                            'camera_rotated/image_rotated/compressedDepth'),
+                        ('rotated/image/theora',
+                            'camera_rotated/image_rotated/theora')],
+            parameters=[{'output_frame_id': "camera_rotated",
+                         'rotation_steps': 2,
+                         'use_camera_info': True,
+                         'input_qos': 'best_effort',
+                         'output_qos': 'default'}])])

image_flip/mainpage.dox

@@ -0,0 +1,30 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b image_flip is a simpler image_rotate limited to 90 degree rotations.
+
+It preserves the size data and publishes a camera_info topic,
+as well as a tf transform between original optical frame and rotated frame
+... 
+
+<!--
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

image_flip/package.xml

@@ -0,0 +1,54 @@
+<?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>image_flip</name>
+  <version>2.2.1</version>
+  <description>
+    <p>
+      Contains a node that rotates an image stream in 90 degree increments
+      about the optical axis.
+
+      This is simpler and faster than the general image_rotate
+    </p>
+    <p>
+      This node is intended to allow camera images to be visualized in an
+      orientation that is more intuitive than the hardware-constrained
+      orientation of the physical camera, but is limited to fixed mountings
+      in 90 degree increments.
+
+    </p>
+    <p>
+      This node outputs a camera_info, but users are cautioned that the
+      distortion data is not corrected for flipping and is only valid if
+      symmetric.
+
+    </p>
+  </description>
+
+  <maintainer email="[email protected]">David Conner</maintainer>
+
+  <license>BSD</license>
+  <url type="website">https://index.ros.org/p/image_pipeline/</url>
+  <url type="bugtracker">https://github.com/ros-perception/image_pipeline/issues</url>
+  <url type="repository">https://github.com/ros-perception/image_pipeline</url>
+  <author>David Conner</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>class_loader</build_depend>
+
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>