Dashing: Image_proc with old PR comments fixed

.circleci/config.yml

@@ -1,8 +1,8 @@
 version: 2
 jobs:
-  crystal:
+  dashing:
     docker:
-      - image: ros:crystal-ros-base
+      - image: ros:dashing-ros-base
     steps:
       - checkout
       - run:
@@ -29,4 +29,4 @@ workflows:
   version: 2
   ros_build:
     jobs:
-      - crystal
+      - dashing

depth_image_proc/CMakeLists.txt

@@ -12,6 +12,7 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(image_geometry REQUIRED)
@@ -49,22 +50,23 @@ ament_target_dependencies(${PROJECT_NAME}
   "class_loader"
   "message_filters"
   "rclcpp"
+  "rclcpp_components"
   "tf2_ros"
   "tf2"
   "tf2_eigen"
   "stereo_msgs"
   "sensor_msgs"
 )
 
-rclcpp_register_node_plugins(${PROJECT_NAME} "${PROJECT_NAME}::ConvertMetricNode")
-rclcpp_register_node_plugins(${PROJECT_NAME} "${PROJECT_NAME}::CropForemostNode")
-rclcpp_register_node_plugins(${PROJECT_NAME} "${PROJECT_NAME}::DisparityNode")
-rclcpp_register_node_plugins(${PROJECT_NAME} "${PROJECT_NAME}::PointCloudXyzNode")
-rclcpp_register_node_plugins(${PROJECT_NAME} "${PROJECT_NAME}::PointCloudXyzRadialNode")
-rclcpp_register_node_plugins(${PROJECT_NAME} "${PROJECT_NAME}::PointCloudXyziNode")
-rclcpp_register_node_plugins(${PROJECT_NAME} "${PROJECT_NAME}::PointCloudXyziRadialNode")
-rclcpp_register_node_plugins(${PROJECT_NAME} "${PROJECT_NAME}::PointCloudXyzrgbNode")
-rclcpp_register_node_plugins(${PROJECT_NAME} "${PROJECT_NAME}::RegisterNode")
+rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::ConvertMetricNode")
+rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::CropForemostNode")
+rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::DisparityNode")
+rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::PointCloudXyzNode")
+rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::PointCloudXyzRadialNode")
+rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::PointCloudXyziNode")
+rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::PointCloudXyziRadialNode")
+rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::PointCloudXyzrgbNode")
+rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::RegisterNode")
 
 target_link_libraries(${PROJECT_NAME} ${ament_LIBRARIES} ${OpenCV_LIBRARIES})
 

image_proc/CMakeLists.txt

@@ -1,68 +1,50 @@
-cmake_minimum_required(VERSION 2.8)
-project(image_proc)
-
-find_package(catkin REQUIRED)
+cmake_minimum_required(VERSION 3.5)
 
-find_package(catkin REQUIRED cv_bridge dynamic_reconfigure image_geometry image_transport nodelet nodelet_topic_tools roscpp sensor_msgs)
-find_package(OpenCV REQUIRED)
-find_package(Boost REQUIRED COMPONENTS thread)
+project(image_proc)
 
-if(cv_bridge_VERSION VERSION_GREATER "1.12.0")
-  add_compile_options(-std=c++11)
+# ROS2 Flags
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
 endif()
 
-# Dynamic reconfigure support
-generate_dynamic_reconfigure_options(cfg/CropDecimate.cfg cfg/Debayer.cfg cfg/Rectify.cfg cfg/Resize.cfg)
-
-catkin_package(
-  CATKIN_DEPENDS image_geometry roscpp sensor_msgs
-  DEPENDS OpenCV
-  INCLUDE_DIRS include
-  LIBRARIES ${PROJECT_NAME}
-)
-
-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})
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
 
-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}
-)
+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(image_geometry REQUIRED)
+
+include_directories(include)
+add_executable(image_proc
+               src/image_proc.cpp
+               src/debayer.cpp
+               src/rectify.cpp
+               src/edge_aware.cpp)
+target_link_libraries(image_proc ament_index_cpp::ament_index_cpp)
+
+if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+  target_link_libraries(image_proc "stdc++fs")
+endif()
 
-# 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}
+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}/
-)
 
-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) 2019, 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) 2019, 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 → image_proc/include/rectify.hpp

@@ -1,7 +1,7 @@
 /*********************************************************************
 * Software License Agreement (BSD License)
 * 
-*  Copyright (c) 2008, Willow Garage, Inc.
+*  Copyright (c) 2019, Andreas Klintberg.
 *  All rights reserved.
 * 
 *  Redistribution and use in source and binary forms, with or without
@@ -31,47 +31,45 @@
 *  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 <thread>
+#include <memory>
+#include <vector>
+#include <string>
 
-#include <opencv2/core/core.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <image_transport/image_transport.h>
 #include <image_geometry/pinhole_camera_model.h>
-#include <sensor_msgs/Image.h>
-
+#include <cv_bridge/cv_bridge.h>
+#include "visibility.h"
 namespace image_proc {
 
-struct ImageSet
+class RectifyNode : public rclcpp::Node
 {
-  std::string color_encoding;
-  cv::Mat mono;
-  cv::Mat rect;
-  cv::Mat color;
-  cv::Mat rect_color;
-};
+  public:
+    RectifyNode(const rclcpp::NodeOptions&);
+  private:
+    // ROS communication
+    image_transport::CameraSubscriber sub_camera_;
 
-class Processor
-{
-public:
-  Processor()
-    : interpolation_(cv::INTER_LINEAR)
-  {
-  }
-
-  int interpolation_;
+    int queue_size_;
+    int interpolation;
+    std::string camera_namespace_;
+    std::string image_rect;
+    std::string image_topic;
 
-  enum {
-    MONO       = 1 << 0,
-    RECT       = 1 << 1,
-    COLOR      = 1 << 2,
-    RECT_COLOR = 1 << 3,
-    ALL = MONO | RECT | COLOR | RECT_COLOR
-  };
-
-  bool process(const sensor_msgs::ImageConstPtr& raw_image,
-               const image_geometry::PinholeCameraModel& model,
-               ImageSet& output, int flags = ALL) const;
-};
+    std::mutex connect_mutex_;
+    image_transport::Publisher pub_rect_;
+
+    // Processing state (note: only safe because we're using single-threaded NodeHandle!)
+    image_geometry::PinholeCameraModel model_;
 
-} //namespace image_proc
+    void connectCb();
+    void imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & image_msg,
+                const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg);
 
-#endif
+};
+} // namespace image_proc
+#endif