port image_rotate on ros2

image_rotate/CMakeLists.txt

@@ -1,32 +1,61 @@
-cmake_minimum_required(VERSION 2.8)
+cmake_minimum_required(VERSION 3.5)
 project(image_rotate)
 
-find_package(catkin REQUIRED COMPONENTS cmake_modules cv_bridge dynamic_reconfigure image_transport nodelet roscpp tf2 tf2_geometry_msgs)
-
-# generate the dynamic_reconfigure config file
-generate_dynamic_reconfigure_options(cfg/ImageRotate.cfg)
-
-catkin_package()
-
+# 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(cv_bridge REQUIRED)
+find_package(class_loader REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(OpenCV REQUIRED core imgproc)
 
 # add the executable
-include_directories(${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
+include_directories(include)
+
+add_library(${PROJECT_NAME} SHARED src/image_rotate_node.cpp)
+target_link_libraries(${PROJECT_NAME} ${ament_LIBRARIES} ${OpenCV_LIBRARIES})
+ament_target_dependencies(${PROJECT_NAME}
+  "image_transport"
+  "cv_bridge"
+  "class_loader"
+  "rclcpp"
+  "tf2_ros"
+  "tf2_geometry_msgs"
+)
+rclcpp_register_node_plugins(${PROJECT_NAME} "${PROJECT_NAME}::ImageRotateNode")
+set(node_plugins "${node_plugins}${PROJECT_NAME}::ImageRotateNode;$<TARGET_FILE:ImageRotateNode>\n")
 
-add_library(${PROJECT_NAME} SHARED src/nodelet/image_rotate_nodelet.cpp)
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
-install(TARGETS image_rotate
-        DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+install(TARGETS ${PROJECT_NAME}
+        DESTINATION lib
 )
 
-add_executable(image_rotate_exe src/node/image_rotate.cpp)
-SET_TARGET_PROPERTIES(image_rotate_exe PROPERTIES OUTPUT_NAME image_rotate)
-target_link_libraries(image_rotate_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+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 ${ament_LIBRARIES} ${OpenCV_LIBRARIES} ${PROJECT_NAME})
+ament_target_dependencies(image_rotate_bin
+  "rclcpp")
 
-install(TARGETS image_rotate_exe
-        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+install(TARGETS image_rotate_bin
+        DESTINATION lib/${PROJECT_NAME}
 )
-install(FILES nodelet_plugins.xml
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+install(DIRECTORY examples/launch
+  DESTINATION share/${PROJECT_NAME}/
 )
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

image_rotate/examples/launch/image_rotate.launch.py

@@ -0,0 +1,44 @@
+# 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.
+
+from launch import LaunchDescription
+import launch_ros.actions
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        launch_ros.actions.Node(
+            package='image_rotate', node_executable='image_rotate', output='screen',
+            remappings=[('image', '/camera/color/image_raw'),
+                        ('camera_info', '/camera/color/camera_info'),
+                        ('rotated/image', '/camera/color/image_raw_rotated')]),
+    ])

image_rotate/include/image_rotate/image_rotate_node.hpp

@@ -0,0 +1,108 @@
+// 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_ROTATE_NODE_HPP_
+#define IMAGE_ROTATE__IMAGE_ROTATE_NODE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <math.h>
+#include <memory>
+#include <string>
+
+#include "image_rotate/visibility.h"
+
+namespace image_rotate
+{
+
+struct ImageRotateConfig
+{
+  std::string target_frame_id;
+  double target_x;
+  double target_y;
+  double target_z;
+  std::string source_frame_id;
+  double source_x;
+  double source_y;
+  double source_z;
+  std::string output_frame_id;
+  std::string input_frame_id;
+  bool use_camera_info;
+  double max_angular_rate;
+  double output_image_size;
+};
+
+class ImageRotateNode : public rclcpp::Node
+{
+public:
+  IMAGE_ROTATE_PUBLIC ImageRotateNode();
+
+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 std::string input_frame_from_msg);
+  void subscribe();
+  void unsubscribe();
+  void connectCb();
+  void disconnectCb();
+  void onInit();
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_sub_;
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_pub_;
+
+  image_rotate::ImageRotateConfig config_;
+
+  image_transport::Publisher img_pub_;
+  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_;
+};
+}  // namespace image_rotate
+
+#endif  // IMAGE_ROTATE__IMAGE_ROTATE_NODE_HPP_

image_rotate/include/image_rotate/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_ROTATE__VISIBILITY_H_
+#define IMAGE_ROTATE__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_ROTATE_EXPORT __attribute__ ((dllexport))
+    #define IMAGE_ROTATE_IMPORT __attribute__ ((dllimport))
+  #else
+    #define IMAGE_ROTATE_EXPORT __declspec(dllexport)
+    #define IMAGE_ROTATE_IMPORT __declspec(dllimport)
+  #endif
+
+  #ifdef IMAGE_ROTATE_DLL
+    #define IMAGE_ROTATE_PUBLIC IMAGE_ROTATE_EXPORT
+  #else
+    #define IMAGE_ROTATE_PUBLIC IMAGE_ROTATE_IMPORT
+  #endif
+
+  #define IMAGE_ROTATE_PUBLIC_TYPE IMAGE_ROTATE_PUBLIC
+
+  #define IMAGE_ROTATE_LOCAL
+
+#else
+
+  #define IMAGE_ROTATE_EXPORT __attribute__ ((visibility("default")))
+  #define IMAGE_ROTATE_IMPORT
+
+  #if __GNUC__ >= 4
+    #define IMAGE_ROTATE_PUBLIC __attribute__ ((visibility("default")))
+    #define IMAGE_ROTATE_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define IMAGE_ROTATE_PUBLIC
+    #define IMAGE_ROTATE_LOCAL
+  #endif
+
+  #define IMAGE_ROTATE_PUBLIC_TYPE
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // IMAGE_ROTATE__VISIBILITY_H_

image_rotate/package.xml

@@ -1,6 +1,6 @@
-<package>
+<package format="2">
   <name>image_rotate</name>
-  <version>1.12.23</version>
+  <version>2.0.0</version>
   <description>
     <p>
       Contains a node that rotates an image stream in a way that minimizes
@@ -28,30 +28,22 @@
   <license>BSD</license>
   <url>http://ros.org/wiki/image_rotate</url>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <test_depend>rostest</test_depend>
-
-  <build_depend>cmake_modules</build_depend>
-  <build_depend>cv_bridge</build_depend>
-  <build_depend>dynamic_reconfigure</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>image_transport</build_depend>
-  <build_depend>nodelet</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>tf2</build_depend>
-  <build_depend>tf2_geometry_msgs</build_depend>
-  <build_depend>tf2_ros</build_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <build_depend>class_loader</build_depend>
+
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>rclcpp</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
 
-  <run_depend>cv_bridge</run_depend>
-  <run_depend>dynamic_reconfigure</run_depend>
-  <run_depend>image_transport</run_depend>
-  <run_depend>nodelet</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>tf2</run_depend>
-  <run_depend>tf2_geometry_msgs</run_depend>
-  <run_depend>tf2_ros</run_depend>
   <export>
-    <nodelet plugin="${prefix}/nodelet_plugins.xml"/>
+    <build_type>ament_cmake</build_type>
   </export>
 </package>