Use own image post processing node

bit-bots · Mar 19, 2024 · 31d216f · 31d216f
1 parent bbcd3eb
commit 31d216f
Show file tree

Hide file tree

Showing 5 changed files with 103 additions and 44 deletions.
diff --git a/bitbots_misc/bitbots_basler_camera/CMakeLists.txt b/bitbots_misc/bitbots_basler_camera/CMakeLists.txt
@@ -6,25 +6,30 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
 
-find_package(rclcpp REQUIRED)
 find_package(ament_cmake REQUIRED)
-find_package(bitbots_docs REQUIRED)
 find_package(backward_ros REQUIRED)
+find_package(bitbots_docs REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(image_transport REQUIRED)
 find_package(OpenCV REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
 
 add_compile_options(-Wall -Werror -Wno-unused)
 
 add_executable(postprocessing src/postprocessing.cpp)
 
-target_link_libraries(postprocessing
-  ${OpenCV_LIBRARIES}
-)
+target_link_libraries(postprocessing ${OpenCV_LIBRARIES})
 
 ament_target_dependencies(
   postprocessing
   ament_cmake
   bitbots_docs
-  rclcpp)
+  cv_bridge
+  image_transport
+  rclcpp
+  sensor_msgs
+  OpenCV)
 
 enable_bitbots_docs()
 

diff --git a/bitbots_misc/bitbots_basler_camera/config/binning.yaml b/bitbots_misc/bitbots_basler_camera/config/binning.yaml
diff --git a/bitbots_misc/bitbots_basler_camera/launch/basler_camera.launch b/bitbots_misc/bitbots_basler_camera/launch/basler_camera.launch
@@ -13,19 +13,12 @@
         <param name="enable_current_params_publisher" value="true" />
     </node>
     <!-- Debayer + Binning -->
-    <node_container pkg="rclcpp_components" exec="component_container" name="head_camera_image_proc_container" namespace="" launch-prefix="$(var taskset)">
-        <composable_node pkg="image_proc" plugin="image_proc::DebayerNode" name="debayer">
-            <remap from="image_raw" to="/pylon_camera_node/image_raw"/>
-            <remap from="camera_info" to="/pylon_camera_node/camera_info"/>
-            <remap from="image_color" to="/camera/image_raw"/>
-            <remap from="image_mono" to="/camera/image_mono"/>
-        </composable_node>
-        <composable_node pkg="image_proc" plugin="image_proc::CropDecimateNode" name="crop_decimate">
-            <remap from="in/image_raw" to="/camera/image_raw"/>
-            <remap from="in/camera_info" to="/pylon_camera_node/camera_info"/>
-            <remap from="out/image_raw" to="/camera/image_proc"/>
-            <remap from="out/camera_info" to="/camera/camera_info"/>
-            <param from="$(find-pkg-share bitbots_basler_camera)/config/binning.yaml"/>
-        </composable_node>
-    </node_container>
+    <node pkg="bitbots_basler_camera" exec="postprocessing" name="basler_camera_post_processing" output="screen" launch-prefix="$(var taskset)">
+        <remap from="in/image_raw" to="/pylon_camera_node/image_raw" />
+        <remap from="in/camera_info" to="/pylon_camera_node/camera_info" />
+        <remap from="out/image_proc" to="/camera/image_proc" />
+        <remap from="out/camera_info" to="/camera/camera_info" />
+        <param name="binning_factor_x" value="4" />
+        <param name="binning_factor_y" value="4" />
+    </node>
 </launch>
diff --git a/bitbots_misc/bitbots_basler_camera/package.xml b/bitbots_misc/bitbots_basler_camera/package.xml
@@ -13,8 +13,12 @@
   <author email="[email protected]">Hamburg Bit-Bots</author>
 
   <depend>bitbots_docs</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>libopencv-dev</depend>
   <depend>pylon_ros2_camera_wrapper</depend>
-  <depend>image_proc</depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

diff --git a/bitbots_misc/bitbots_basler_camera/src/postprocessing.cpp b/bitbots_misc/bitbots_basler_camera/src/postprocessing.cpp
@@ -1,52 +1,112 @@
 #include <cmath>
+#include <cv_bridge/cv_bridge.hpp>
+#include <image_transport/image_transport.hpp>
 #include <iostream>
 #include <memory>
+#include <opencv2/imgproc/imgproc.hpp>
 #include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
 #include <rclcpp/logger.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <string>
 #include <vector>
 
-#include <opencv2/imgproc/imgproc.hpp>
-#include <image_transport/image_transport.hpp>
-
-using std::placeholders::_1;
+using std::placeholders::_1, std::placeholders::_2;
 
 namespace postprocessing {
 
-
 class PostProcessor {
   std::shared_ptr<rclcpp::Node> node_;
 
-  // Declare subscriber
-  //rclcpp::Subscription<bitbots_msgs::msg::HeadMode>::SharedPtr head_mode_subscriber_;
-  //rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_subscriber_;
+  image_transport::TransportHints transport_hints_;
+  std::unique_ptr<image_transport::CameraSubscriber> image_sub_;
+  std::unique_ptr<image_transport::CameraPublisher> image_pub_;
 
-  // Declare publisher
-  //rclcpp::Publisher<bitbots_msgs::msg::JointCommand>::SharedPtr position_publisher_;
+  int binning_factor_x_ = 1;
+  int binning_factor_y_ = 1;
 
  public:
-  PostProcessor() : node_(std::make_shared<rclcpp::Node>("post_processor")) {
-    // Initialize publisher for head motor goals
-    //position_publisher_ = node_->create_publisher<bitbots_msgs::msg::JointCommand>("head_motor_goals", 10);
-
-    // Initialize subscriber for head mode
-    //head_mode_subscriber_ = node_->create_subscription<bitbots_msgs::msg::HeadMode>(
-    //    "head_mode", 10, [this](const bitbots_msgs::msg::HeadMode::SharedPtr msg) { head_mode_callback(msg); });
+  PostProcessor()
+      : node_(std::make_shared<rclcpp::Node>("post_processor")),
+        transport_hints_(node_.get()),
+        image_sub_(std::make_unique<image_transport::CameraSubscriber>(image_transport::create_camera_subscription(
+            node_.get(), "in/image_raw", std::bind(&PostProcessor::image_callback, this, _1, _2),
+            transport_hints_.getTransport()))),
+        image_pub_(std::make_unique<image_transport::CameraPublisher>(
+            image_transport::create_camera_publisher(node_.get(), "out/image_proc"))) {
+    // Declare parameters
+    node_->declare_parameter("binning_factor_x", 4);
+    node_->declare_parameter("binning_factor_y", 4);
 
+    // Get parameters
+    node_->get_parameter("binning_factor_x", binning_factor_x_);
+    node_->get_parameter("binning_factor_y", binning_factor_y_);
   }
 
   /**
    * @brief Callback used to update the head mode
    */
-  //void head_mode_callback(const bitbots_msgs::msg::HeadMode::SharedPtr msg) { head_mode_ = msg->head_mode; }
+  void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& image_msg,
+                      const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info_msg) {
+    // Convert to cv::Mat
+    cv::Mat image = cv_bridge::toCvShare(image_msg)->image;
+
+    // Do some processing
+    int bit_depth = sensor_msgs::image_encodings::bitDepth(image_msg->encoding);
+
+    int type = bit_depth == 8 ? CV_8U : CV_16U;
+
+    // Create cv::Mat for color image
+    cv::Mat color(image_msg->height, image_msg->width, CV_MAKETYPE(type, 3));
+
+    // Create cv::Mat for
+    const cv::Mat bayer(image_msg->height, image_msg->width, CV_MAKETYPE(type, 1),
+                        const_cast<uint8_t*>(&image_msg->data[0]), image_msg->step);
+
+    // Get the correct conversion code
+    int code = -1;
+    if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB8 ||
+        image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB16) {
+      code = cv::COLOR_BayerBG2BGR;
+    } else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR8 ||  // NOLINT
+               image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR16) {
+      code = cv::COLOR_BayerRG2BGR;
+    } else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG8 ||  // NOLINT
+               image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG16) {
+      code = cv::COLOR_BayerGR2BGR;
+    } else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG8 ||  // NOLINT
+               image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG16) {
+      code = cv::COLOR_BayerGB2BGR;
+    }
+    code += cv::COLOR_BayerBG2BGR_VNG - cv::COLOR_BayerBG2BGR;
+
+    // Debayer the image
+    cv::cvtColor(bayer, color, code);
+
+    // Perform binning by a given factor
+    cv::Mat binned(image_msg->height / binning_factor_y_, image_msg->width / binning_factor_x_, CV_MAKETYPE(type, 3));
+    cv::resize(color, binned, cv::Size(), 1.0 / binning_factor_x_, 1.0 / binning_factor_y_, cv::INTER_AREA);
+
+    // Add the binning to the camera info
+    auto new_camera_info = std::make_shared<sensor_msgs::msg::CameraInfo>(*info_msg);
+    new_camera_info->binning_x = binning_factor_x_;
+    new_camera_info->binning_y = binning_factor_y_;
+
+    // Convert back to sensor_msgs::Image
+    cv_bridge::CvImage color_msg;
+    color_msg.header = image_msg->header;
+    color_msg.encoding = sensor_msgs::image_encodings::BGR8;
+    color_msg.image = binned;
+
+    // Publish the image
+    image_pub_->publish(color_msg.toImageMsg(), new_camera_info);
+  }
 
   /**
    * @brief A getter that returns the node
    */
   std::shared_ptr<rclcpp::Node> get_node() { return node_; }
 };
-}  // namespace move_head
+}  // namespace postprocessing
 
 int main(int argc, char* argv[]) {
   rclcpp::init(argc, argv);