Try clang formatter

docker/perception/semantic_segmentation/semantic_segmentation.Dockerfile

@@ -31,6 +31,11 @@ RUN apt-get -qq update && rosdep update && \
 ################################# Dependencies ################################
 FROM ${BASE_BUILD_IMAGE} as dependencies
 
+# install tensorrt
+RUN apt update && apt install -y tensorrt
+
+RUN apt install -y ros-humble-cv-bridge libopencv-dev
+
 # Install Rosdep requirements
 COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list
 RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list)
@@ -47,8 +52,6 @@ RUN apt-get -qq autoremove -y && apt-get -qq autoclean && apt-get -qq clean && \
 ################################ Build ################################
 FROM dependencies as build
 
-# install tensorrt
-RUN apt update && apt install -y tensorrt
 
 # Build ROS2 packages
 WORKDIR ${AMENT_WS}

src/perception/semantic_segmentation/CMakeLists.txt

@@ -32,6 +32,8 @@ find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(image_transport REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(cv_bridge REQUIRED)
 find_package(CUDA REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(yaml-cpp REQUIRED)
@@ -82,5 +84,13 @@ install(FILES ${PADDLE_LIB}/paddle/lib/libpaddle_inference.so
               ${PADDLE_LIB}/third_party/install/mklml/lib/libmklml_intel.so
         DESTINATION /usr/local/lib)
 
-ament_target_dependencies(semantic_segmentation rclcpp std_msgs sensor_msgs geometry_msgs image_transport)
+ament_target_dependencies(semantic_segmentation
+  rclcpp
+  std_msgs
+  sensor_msgs
+  cv_bridge
+  geometry_msgs
+  image_transport
+  OpenCV
+)
 ament_package()

src/perception/semantic_segmentation/package.xml

@@ -14,6 +14,8 @@
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>OpenCV</depend>
+  <depend>cv_bridge</depend>
   <depend>image_transport</depend>
 
   <build_depend>rosidl_default_generators</build_depend>

src/perception/semantic_segmentation/src/semantic_segmentation_node.cpp

@@ -1,11 +1,15 @@
+#include <cv_bridge/cv_bridge.h>
 #include <chrono>
 #include <functional>
 #include <memory>
+#include <opencv2/opencv.hpp>
 #include <string>
+#include <vector>
 
 #include "paddle/include/paddle_inference_api.h"
 #include "yaml-cpp/yaml.h"
 
+#include <sensor_msgs/msg/image.hpp>
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 
@@ -49,7 +53,6 @@ YamlConfig load_yaml(const std::string& yaml_path) {
 
 class SemanticSegmentationNode : public rclcpp::Node {
  public:
-
   bool FLAGS_use_mkldnn = true;
   bool FLAGS_use_trt = true;
   std::string FLAGS_trt_precision = "fp32";
@@ -119,19 +122,53 @@ class SemanticSegmentationNode : public rclcpp::Node {
     return predictor;
   }
 
+  void hwc_img_2_chw_data(const cv::Mat& hwc_img, float* data) {
+    int rows = hwc_img.rows;
+    int cols = hwc_img.cols;
+    int chs = hwc_img.channels();
+    for (int i = 0; i < chs; ++i) {
+      cv::extractChannel(hwc_img, cv::Mat(rows, cols, CV_32FC1, data + i * rows * cols), i);
+    }
+  }
+
+  void process_image(cv::Mat& img, const YamlConfig& yaml_config) {
+    // cv::Mat img = cv::imread(FLAGS_img_path, cv::IMREAD_COLOR);
+    cv::cvtColor(img, img, cv::COLOR_BGR2RGB);
+    if (yaml_config.is_resize) {
+      cv::resize(img, img, cv::Size(yaml_config.resize_width, yaml_config.resize_height));
+    }
+    if (yaml_config.is_normalize) {
+      img.convertTo(img, CV_32F, 1.0 / 255, 0);
+      img = (img - 0.5) / 0.5;
+    }
+  }
+
   SemanticSegmentationNode() : Node("semantic_segmentation_node"), count_(0) {
+    // Get the image topic parameter from the launch file)
+    std::string image_topic = this->declare_parameter("input_topic", "/camera/right/image_color");
+
+    RCLCPP_INFO(this->get_logger(), "subscribing to image_topic: %s", image_topic.c_str());
+
     publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
     timer_ =
         this->create_wall_timer(500ms, std::bind(&SemanticSegmentationNode::timer_callback, this));
-    YamlConfig yaml_config = load_yaml(resource_path + "deploy.yaml");
+    yaml_config_ = load_yaml(resource_path + "deploy.yaml");
     // Print out the yaml config
-    RCLCPP_INFO(this->get_logger(), "model_file: %s", yaml_config.model_file.c_str());
-    RCLCPP_INFO(this->get_logger(), "params_file: %s", yaml_config.params_file.c_str());
-    RCLCPP_INFO(this->get_logger(), "is_normalize: %d", yaml_config.is_normalize);
-    RCLCPP_INFO(this->get_logger(), "is_resize: %d", yaml_config.is_resize);
+    RCLCPP_INFO(this->get_logger(), "model_file: %s", yaml_config_.model_file.c_str());
+    RCLCPP_INFO(this->get_logger(), "params_file: %s", yaml_config_.params_file.c_str());
+    RCLCPP_INFO(this->get_logger(), "is_normalize: %d", yaml_config_.is_normalize);
+    RCLCPP_INFO(this->get_logger(), "is_resize: %d", yaml_config_.is_resize);
 
     // Create the paddle predictor
-    predictor_ = create_predictor(yaml_config);
+    predictor_ = create_predictor(yaml_config_);
+
+    // Subscribe to the image topic
+    subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
+        image_topic, 10,
+        std::bind(&SemanticSegmentationNode::image_callback, this, std::placeholders::_1));
+
+    image_publisher_ =
+        this->create_publisher<sensor_msgs::msg::Image>("semantic_segmentation_image", 10);
   }
 
  private:
@@ -141,10 +178,83 @@ class SemanticSegmentationNode : public rclcpp::Node {
     RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
     publisher_->publish(message);
   }
+
+  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) {
+    RCLCPP_INFO(this->get_logger(), "Received image");
+    cv::Mat img;
+
+    // Convert msg to a cv mat
+    try {
+      img = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8)->image;
+    } catch (cv_bridge::Exception& e) {
+      RCLCPP_ERROR(this->get_logger(), "Could not convert from '%s' to 'bgr8'.",
+                   msg->encoding.c_str());
+      return;
+    }
+
+    // Process the image according to yaml config
+    process_image(img, yaml_config_);
+
+    int rows = img.rows;
+    int cols = img.cols;
+    int chs = img.channels();
+    std::vector<float> input_data(1 * chs * rows * cols, 0.0f);
+    hwc_img_2_chw_data(img, input_data.data());
+
+    // Set input
+    auto input_names = predictor_->GetInputNames();
+    auto input_t = predictor_->GetInputHandle(input_names[0]);
+    std::vector<int> input_shape = {1, chs, rows, cols};
+    input_t->Reshape(input_shape);
+    input_t->CopyFromCpu(input_data.data());
+
+    // Start timing
+    auto start = std::chrono::high_resolution_clock::now();
+
+    // Run
+    predictor_->Run();
+
+    // Stop timing
+    auto end = std::chrono::high_resolution_clock::now();
+
+    // Calculate elapsed time
+    std::chrono::duration<double, std::milli> elapsed = end - start;
+    RCLCPP_INFO(this->get_logger(), "Inference time: %f ms", elapsed.count());
+
+    // Get output
+    auto output_names = predictor_->GetOutputNames();
+    auto output_t = predictor_->GetOutputHandle(output_names[0]);
+    std::vector<int> output_shape = output_t->shape();
+    int out_num =
+        std::accumulate(output_shape.begin(), output_shape.end(), 1, std::multiplies<int>());
+    std::vector<int32_t> out_data(out_num);
+    output_t->CopyToCpu(out_data.data());
+
+    // Get pseudo image
+    std::vector<uint8_t> out_data_u8(out_num);
+    for (int i = 0; i < out_num; i++) {
+      out_data_u8[i] = static_cast<uint8_t>(out_data[i]);
+    }
+    cv::Mat out_gray_img(output_shape[1], output_shape[2], CV_8UC1, out_data_u8.data());
+    cv::Mat out_eq_img;
+    cv::equalizeHist(out_gray_img, out_eq_img);
+    cv::imwrite("out_img.jpg", out_eq_img);
+
+    // Publish out_eq_img to a topic
+    sensor_msgs::msg::Image::SharedPtr out_img_msg =
+        cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", out_eq_img).toImageMsg();
+
+    image_publisher_->publish(*out_img_msg);
+
+    RCLCPP_INFO(this->get_logger(), "Finish, the result is saved in out_img.jpg");
+  }
   rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_;
   size_t count_;
   std::shared_ptr<paddle_infer::Predictor> predictor_;
+  YamlConfig yaml_config_;
 };
 
 int main(int argc, char* argv[]) {

watod-config.sh

@@ -12,7 +12,7 @@
 ##	 - simulation			:	starts simulation
 ##   - samples             	:   starts sample ROS2 pubsub nodes
 
-ACTIVE_MODULES="perception"
+ACTIVE_MODULES="perception infrastructure"
 
 ############################## OPTIONAL CONFIGURATIONS ##############################
 ## Name to append to docker containers. DEFAULT = "<your_watcloud_username>"