add image_flip node

image_rotate/CMakeLists.txt

@@ -24,6 +24,13 @@ ament_auto_add_executable(image_rotate_bin src/image_rotate.cpp)
 set_target_properties(image_rotate_bin PROPERTIES OUTPUT_NAME ${PROJECT_NAME})
 target_link_libraries(image_rotate_bin ${OpenCV_LIBRARIES} ${PROJECT_NAME})
 
+ament_auto_add_library(image_flip SHARED src/image_flip.cpp)
+target_link_libraries(image_flip ${OpenCV_LIBRARIES})
+rclcpp_components_register_node(image_flip
+  PLUGIN "${PROJECT_NAME}::ImageFlipNode"
+  EXECUTABLE image_flip_node
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

image_rotate/include/image_rotate/image_flip.hpp

@@ -0,0 +1,103 @@
+// 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_ROTATE__IMAGE_FLIP_HPP_
+#define IMAGE_ROTATE__IMAGE_FLIP_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_rotate/visibility.h"
+
+namespace image_rotate
+{
+
+struct ImageFlipConfig
+{
+  std::string output_frame_id;
+  int rotation_steps;
+  bool use_camera_info;
+};
+
+class ImageFlipNode : public rclcpp::Node
+{
+public:
+  IMAGE_ROTATE_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 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_;
+
+  ImageFlipConfig config_;
+
+  // Subscriber - only one is used at a time - depends on use_camera_info
+  image_transport::Subscriber img_sub_;
+  image_transport::CameraSubscriber cam_sub_;
+
+  // Publisher - only one is used at a time - depends on use_camera_info
+  image_transport::Publisher img_pub_;
+  image_transport::CameraPublisher cam_pub_;
+
+  double angle_;
+  tf2::TimePoint prev_stamp_;
+  geometry_msgs::msg::TransformStamped transform_;
+};
+}  // namespace image_rotate
+
+#endif  // IMAGE_ROTATE__IMAGE_FLIP_HPP_

image_rotate/include/image_rotate/image_rotate_node.hpp

@@ -93,13 +93,14 @@ class ImageRotateNode : public rclcpp::Node
   image_rotate::ImageRotateConfig config_;
 
   image_transport::Publisher img_pub_;
+
+  // Subscriber - only one is used at a time - depends on use_camera_info
   image_transport::Subscriber img_sub_;
   image_transport::CameraSubscriber cam_sub_;
 
   geometry_msgs::msg::Vector3Stamped target_vector_;
   geometry_msgs::msg::Vector3Stamped source_vector_;
 
-  int subscriber_count_;
   double angle_;
   tf2::TimePoint prev_stamp_;
 };

image_rotate/launch/image_flip.launch.py

@@ -0,0 +1,51 @@
+# 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."""
+
+from launch import LaunchDescription
+import launch_ros.actions
+
+
+def generate_launch_description():
+    """Launch description for basic launch of the image_flip."""
+    return LaunchDescription([
+        launch_ros.actions.Node(
+            package='image_rotate', executable='image_flip',
+            output='screen', name='camera_flip',
+            remappings=[('image',                'camera/rgb/image_raw'),
+                        ('rotated/image',        'camera_rotated/image_rotated')],
+            parameters=[{'output_frame_id': 'camera_rotated',
+                         'rotation_steps': 2,
+                         'use_camera_info': True,
+                         'input_qos': 'best_effort',
+                         'output_qos': 'default'}])])

image_rotate/package.xml

@@ -3,26 +3,7 @@
 <package format="3">
   <name>image_rotate</name>
   <version>5.0.0</version>
-  <description>
-    <p>
-      Contains a node that rotates an image stream in a way that minimizes
-      the angle between a vector in some arbitrary frame and a vector in the
-      camera frame. The frame of the outgoing image is published by the node.
-    </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. This is particularly helpful, for
-      example, to show images from the PR2's forearm cameras with a
-      consistent up direction, despite the fact that the forearms need to
-      rotate in arbitrary ways during manipulation.
-    </p>
-    <p>
-      It is not recommended to use the output from this node for further
-      computation, as it interpolates the source image, introduces black
-      borders, and does not output a camera_info.
-    </p>
-  </description>
+  <description>Contains nodes for rotating images.</description>
 
   <maintainer email="[email protected]">Vincent Rabaud</maintainer>
   <maintainer email="[email protected]">Joshua Whitley</maintainer>
@@ -34,6 +15,7 @@
   <url type="bugtracker">https://github.com/ros-perception/image_pipeline/issues</url>
   <url type="repository">https://github.com/ros-perception/image_pipeline</url>
   <author>Blaise Gassend</author>
+  <author email="[email protected]">David Conner</author>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>