Merge pull request #426 from klintan/image-proc-dashing

Dashing: Image_proc with old PR comments fixed

image_proc/CMakeLists.txt

@@ -1,68 +1,97 @@
-cmake_minimum_required(VERSION 2.8)
+cmake_minimum_required(VERSION 3.5)
+
 project(image_proc)
 
-find_package(catkin REQUIRED)
+# ROS2 Flags
+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(catkin REQUIRED cv_bridge dynamic_reconfigure image_geometry image_transport nodelet nodelet_topic_tools roscpp sensor_msgs)
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(OpenCV REQUIRED)
-find_package(Boost REQUIRED COMPONENTS thread)
+find_package(image_geometry REQUIRED)
 
-if(cv_bridge_VERSION VERSION_GREATER "1.12.0")
-  add_compile_options(-std=c++11)
-endif()
+include_directories(include)
 
-# Dynamic reconfigure support
-generate_dynamic_reconfigure_options(cfg/CropDecimate.cfg cfg/Debayer.cfg cfg/Rectify.cfg cfg/Resize.cfg)
+add_library(rectify SHARED
+  src/rectify.cpp)
+target_compile_definitions(rectify
+  PRIVATE "COMPOSITION_BUILDING_DLL")
 
-catkin_package(
-  CATKIN_DEPENDS image_geometry roscpp sensor_msgs
-  DEPENDS OpenCV
-  INCLUDE_DIRS include
-  LIBRARIES ${PROJECT_NAME}
-)
+ament_target_dependencies(rectify
+  "image_geometry"
+  "sensor_msgs"
+  "cv_bridge"
+  "class_loader"
+  "image_transport"
+  "rclcpp"
+  "rclcpp_components"
+  )
+
+rclcpp_components_register_nodes(rectify "image_proc::RectifyNode")
+set(node_plugins "${node_plugins}image_proc::RectifyNode;$<TARGET_FILE:rectify>\n")
 
-include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
-
-# Temporary fix for DataType deprecation in OpenCV 3.3.1. We continue to use the deprecated form for now because
-# the new one is not available in OpenCV 2.4 (on Trusty).
-add_definitions(-DOPENCV_TRAITS_ENABLE_DEPRECATED)
-
-# Nodelet library
-add_library(${PROJECT_NAME} src/libimage_proc/processor.cpp
-                                src/nodelets/debayer.cpp
-                                src/nodelets/rectify.cpp
-                                src/nodelets/resize.cpp
-                                src/nodelets/crop_decimate.cpp
-                                src/libimage_proc/advertisement_checker.cpp
-                                src/nodelets/edge_aware.cpp
-                                src/nodelets/crop_non_zero.cpp
-)
-add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenCV_LIBRARIES})
+add_library(debayer SHARED
+  src/debayer.cpp
+  src/edge_aware.cpp
+  )
 
-install(TARGETS ${PROJECT_NAME}
-        DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-)
-install(DIRECTORY include/${PROJECT_NAME}/
-        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
-install(FILES nodelet_plugins.xml
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+target_compile_definitions(debayer
+  PRIVATE "COMPOSITION_BUILDING_DLL")
+
+ament_target_dependencies(debayer
+  "sensor_msgs"
+  "cv_bridge"
+  "class_loader"
+  "image_transport"
+  "rclcpp"
+  "rclcpp_components"
+  )
+
+rclcpp_components_register_nodes(debayer "image_proc::DebayerNode")
+set(node_plugins "${node_plugins}image_proc::DebayerNode;$<TARGET_FILE:debayer>\n")
+
+add_executable(image_proc
+               src/image_proc.cpp
 )
+target_link_libraries(image_proc
+  debayer
+  rectify
+  ament_index_cpp::ament_index_cpp)
 
-# Standalone node
-add_executable(image_proc_exe src/nodes/image_proc.cpp)
-target_link_libraries(image_proc_exe ${PROJECT_NAME}  ${Boost_LIBRARIES} ${OpenCV_LIBRARIES})
-SET_TARGET_PROPERTIES(image_proc_exe PROPERTIES OUTPUT_NAME image_proc)
-install(TARGETS image_proc_exe
-        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+  target_link_libraries(image_proc "stdc++fs")
+endif()
+
+ament_target_dependencies(image_proc
+  "image_geometry"
+  "sensor_msgs"
+  "cv_bridge"
+  "class_loader"
+  "image_transport"
+  "rclcpp"
+  "rclcpp_components"
+  "rcutils"
 )
 
-# install the launch file
-install(DIRECTORY launch
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/
+install(TARGETS
+  debayer
+  rectify
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib
 )
 
-if(CATKIN_ENABLE_TESTING)
-  add_subdirectory(test)
-endif()
+install(TARGETS
+        image_proc
+        DESTINATION lib/${PROJECT_NAME})
+ament_package()

image_proc/include/image_proc/advertisement_checker.h → image_proc/include/debayer.hpp

@@ -1,7 +1,7 @@
 /*********************************************************************
 * Software License Agreement (BSD License)
 * 
-*  Copyright (c) 2008, Willow Garage, Inc.
+*  Copyright (c) 2008, 2019, Willow Garage, Inc., Andreas Klintberg.
 *  All rights reserved.
 * 
 *  Redistribution and use in source and binary forms, with or without
@@ -31,31 +31,47 @@
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
-#ifndef IMAGE_PROC_ADVERTISEMENT_CHECKER_H
-#define IMAGE_PROC_ADVERTISEMENT_CHECKER_H
 
-#include <ros/ros.h>
+#include <cstring>
+#include <memory>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <thread>
+#include <rclcpp/rclcpp.hpp>
+#include <image_transport/image_transport.h>
 
-namespace image_proc {
+#include <ament_index_cpp/get_resource.hpp>
+#include "rclcpp/rclcpp.hpp"
+#ifndef IMAGE_PROC_DEBAYER_HPP
+#define IMAGE_PROC_DEBAYER_HPP
 
-class AdvertisementChecker
+
+namespace image_proc
+{
+class DebayerNode : public rclcpp::Node
 {
-  ros::NodeHandle nh_;
-  std::string name_;
-  ros::WallTimer timer_;
-  ros::V_string topics_;
+  // ROS communication
+  public:
+    DebayerNode(const rclcpp::NodeOptions&);
+  private:
+    image_transport::Subscriber sub_raw_;
 
-  void timerCb();
+    std::string camera_namespace_;
+    int debayer_;
 
-public:
-  AdvertisementChecker(const ros::NodeHandle& nh = ros::NodeHandle(),
-                       const std::string& name = std::string());
-
-  void start(const ros::V_string& topics, double duration);
+    int debayer_bilinear_ = 0;
+    int debayer_edgeaware_ = 1;
+    int debayer_edgeaware_weighted_ = 2;
+    int debayer_vng_ = 3;
 
-  void stop();
-};
+    std::mutex connect_mutex_;
 
-} // namespace image_proc
+    image_transport::Publisher pub_mono_;
+    image_transport::Publisher pub_color_;
 
-#endif
+    void connectCb();
+    void imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg);
+};
+} // namespace image_proc
+#endif

image_proc/src/nodelets/edge_aware.h → image_proc/include/edge_aware.h

@@ -1,7 +1,7 @@
 /*********************************************************************
 * Software License Agreement (BSD License)
 * 
-*  Copyright (c) 2008, Willow Garage, Inc.
+*  Copyright (c) 2008, 2019, Willow Garage, Inc., Andreas Klintberg.
 *  All rights reserved.
 * 
 *  Redistribution and use in source and binary forms, with or without
@@ -31,6 +31,7 @@
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
+
 #ifndef IMAGE_PROC_EDGE_AWARE
 #define IMAGE_PROC_EDGE_AWARE
 
@@ -45,5 +46,4 @@ void debayerEdgeAware(const cv::Mat& bayer, cv::Mat& color);
 void debayerEdgeAwareWeighted(const cv::Mat& bayer, cv::Mat& color);
 
 } // namespace image_proc
-
 #endif

image_proc/include/image_proc/processor.h

@@ -1,7 +1,7 @@
 /*********************************************************************
 * Software License Agreement (BSD License)
 * 
-*  Copyright (c) 2008, Willow Garage, Inc.
+*  Copyright (c) 2008, 2019, Willow Garage, Inc., Andreas Klintberg.
 *  All rights reserved.
 * 
 *  Redistribution and use in source and binary forms, with or without
@@ -31,12 +31,13 @@
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
+
 #ifndef IMAGE_PROC_PROCESSOR_H
 #define IMAGE_PROC_PROCESSOR_H
 
 #include <opencv2/core/core.hpp>
 #include <image_geometry/pinhole_camera_model.h>
-#include <sensor_msgs/Image.h>
+#include "sensor_msgs/msg/image.hpp"
 
 namespace image_proc {
 
@@ -67,7 +68,7 @@ class Processor
     ALL = MONO | RECT | COLOR | RECT_COLOR
   };
 
-  bool process(const sensor_msgs::ImageConstPtr& raw_image,
+  bool process(const sensor_msgs::msg::Image::SharedConstPtr& raw_image,
                const image_geometry::PinholeCameraModel& model,
                ImageSet& output, int flags = ALL) const;
 };

image_proc/src/libimage_proc/advertisement_checker.cpp → image_proc/include/rectify.hpp

@@ -1,7 +1,7 @@
 /*********************************************************************
 * Software License Agreement (BSD License)
 * 
-*  Copyright (c) 2008, Willow Garage, Inc.
+*  Copyright (c) 2008, 2019, Willow Garage, Inc., Andreas Klintberg.
 *  All rights reserved.
 * 
 *  Redistribution and use in source and binary forms, with or without
@@ -31,62 +31,44 @@
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
-#include "image_proc/advertisement_checker.h"
-#include <boost/foreach.hpp>
 
-namespace image_proc {
+#ifndef IMAGE_PROC_PROCESSOR_H
+#define IMAGE_PROC_PROCESSOR_H
+#include <thread>
+#include <memory>
+#include <vector>
+#include <string>
 
-AdvertisementChecker::AdvertisementChecker(const ros::NodeHandle& nh,
-                                           const std::string& name)
-  : nh_(nh),
-    name_(name)
-{
-}
+#include <rclcpp/rclcpp.hpp>
+#include <image_transport/image_transport.h>
+#include <image_geometry/pinhole_camera_model.h>
+#include <cv_bridge/cv_bridge.h>
+namespace image_proc {
 
-void AdvertisementChecker::timerCb()
+class RectifyNode : public rclcpp::Node
 {
-  ros::master::V_TopicInfo topic_info;
-  if (!ros::master::getTopics(topic_info)) return;
+  public:
+    RectifyNode(const rclcpp::NodeOptions&);
+  private:
+    // ROS communication
+    image_transport::CameraSubscriber sub_camera_;
 
-  ros::V_string::iterator topic_it = topics_.begin();
-  while (topic_it != topics_.end())
-  {
-    // Should use std::find_if
-    bool found = false;
-    ros::master::V_TopicInfo::iterator info_it = topic_info.begin();
-    while (!found && info_it != topic_info.end())
-    {
-      found = (*topic_it == info_it->name);
-      ++info_it;
-    }
-    if (found)
-      topic_it = topics_.erase(topic_it);
-    else
-    {
-      ROS_WARN_NAMED(name_, "The input topic '%s' is not yet advertised", topic_it->c_str());
-      ++topic_it;
-    }
-  }
+    int queue_size_;
+    int interpolation;
+    std::string camera_namespace_;
+    std::string image_rect;
+    std::string image_topic;
 
-  if (topics_.empty())
-    stop();
-}
+    std::mutex connect_mutex_;
+    image_transport::Publisher pub_rect_;
 
-void AdvertisementChecker::start(const ros::V_string& topics, double duration)
-{
-  topics_.clear();
-  BOOST_FOREACH(const std::string& topic, topics)
-    topics_.push_back(nh_.resolveName(topic));
-
-  ros::NodeHandle nh;
-  timer_ = nh.createWallTimer(ros::WallDuration(duration),
-                              boost::bind(&AdvertisementChecker::timerCb, this));
-  timerCb();
-}
+    // Processing state (note: only safe because we're using single-threaded NodeHandle!)
+    image_geometry::PinholeCameraModel model_;
 
-void AdvertisementChecker::stop()
-{
-  timer_.stop();
-}
+    void connectCb();
+    void imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & image_msg,
+                const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg);
 
+};
 } // namespace image_proc
+#endif