#35 Created dummy cpp nodes for cpp prototype

ros2_ws/src/cpp_package/src/CameraNode.cpp

@@ -0,0 +1,146 @@
+#include <chrono>
+#include <opencv2/opencv.hpp>
+#include <sl/Camera.hpp>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/header.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "custom_interfaces/msg/image_input.hpp"
+#include "cv_bridge/cv_bridge.h"
+
+using namespace std::chrono_literals;
+
+
+// ZED SDK Integration:
+//      Uses sl::Mat for accessing ZED images.
+//      Handles different views based on camera_side.
+// ROS 2 Standard Practices:
+//      Uses rclcpp::Node for ROS integration.
+//      Publishes to a custom ROS message type.
+// OpenCV Conversion:
+//      Converts ZED's sl::Mat to OpenCV's cv::Mat for processing.
+// Performance Logging:
+//      Measures and logs preprocessing time.
+
+class CameraNode : public rclcpp::Node {
+public:
+    CameraNode() : Node("camera_node") {
+        RCLCPP_INFO(this->get_logger(), "Initializing Camera Node");
+
+        // Declare and get parameters
+        this->declare_parameter<std::string>("camera_side", "left");
+        this->declare_parameter<std::vector<int>>("roi_dimensions", {0, 0, 100, 100});
+        this->declare_parameter<int>("frame_rate", 30);
+
+        camera_side_ = this->get_parameter("camera_side").as_string();
+        roi_dimensions_ = this->get_parameter("roi_dimensions").as_integer_array();
+        frame_rate_ = this->get_parameter("frame_rate").as_int();
+
+        model_dimensions_ = cv::Size(640, 640);
+        velocity_ = 0.0;
+        shift_constant_ = (camera_side_ == "left") ? 0 : -1;
+
+        // Publishers
+        image_publisher_ = this->create_publisher<custom_interfaces::msg::ImageInput>(
+            camera_side_ + std::string("_image_input"), 10);
+
+        // Start ZED camera processing
+        start_camera();
+    }
+
+private:
+    void start_camera() {
+        sl::Camera zed;
+        sl::InitParameters init_params;
+        init_params.camera_resolution = sl::RESOLUTION::HD1080;
+        init_params.camera_fps = frame_rate_;
+
+        if (camera_side_ == "left") {
+            init_params.set_from_serial_number(left_camera_serial_number_);
+        } else if (camera_side_ == "right") {
+            init_params.set_from_serial_number(right_camera_serial_number_);
+        }
+
+        auto status = zed.open(init_params);
+        if (status != sl::ERROR_CODE::SUCCESS) {
+            RCLCPP_ERROR(this->get_logger(), "Failed to open ZED camera: %s", sl::toString(status).c_str());
+            return;
+        }
+
+        sl::RuntimeParameters runtime_params;
+        sl::Mat image_zed;
+
+        while (rclcpp::ok()) {
+            if (zed.grab(runtime_params) == sl::ERROR_CODE::SUCCESS) {
+                zed.retrieve_image(image_zed, (camera_side_ == "left") ? sl::VIEW::LEFT_UNRECTIFIED : sl::VIEW::RIGHT_UNRECTIFIED);
+                cv::Mat cv_image = slMatToCvMat(image_zed);
+                preprocess_and_publish(cv_image);
+                std::this_thread::sleep_for(std::chrono::milliseconds(1000 / frame_rate_));
+            } else {
+                RCLCPP_ERROR(this->get_logger(), "Failed to grab ZED frame");
+            }
+        }
+
+        zed.close();
+        RCLCPP_INFO(this->get_logger(), "Closing ZED camera");
+    }
+
+    void preprocess_and_publish(const cv::Mat &image) {
+        auto start_time = std::chrono::high_resolution_clock::now();
+
+        // Extract ROI
+        int x1 = roi_dimensions_[0] + shift_constant_;
+        int y1 = roi_dimensions_[1];
+        int x2 = roi_dimensions_[2] + shift_constant_;
+        int y2 = roi_dimensions_[3];
+        cv::Mat roi_image = image(cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)));
+
+        // Resize and convert to ROS message
+        cv::Mat preprocessed_image;
+        cv::resize(roi_image, preprocessed_image, model_dimensions_);
+
+        auto raw_image_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", image).toImageMsg();
+        auto preprocessed_image_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", preprocessed_image).toImageMsg();
+
+        // Publish custom message
+        custom_interfaces::msg::ImageInput image_input;
+        image_input.header.frame_id = "camera_frame";
+        image_input.raw_image = *raw_image_msg;
+        image_input.preprocessed_image = *preprocessed_image_msg;
+        image_input.velocity = velocity_;
+
+        image_publisher_->publish(image_input);
+
+        auto end_time = std::chrono::high_resolution_clock::now();
+        RCLCPP_INFO(this->get_logger(), "Preprocessing time: %.2f ms",
+                    std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count());
+    }
+
+    cv::Mat slMatToCvMat(const sl::Mat &input) {
+        int cv_type = (input.getDataType() == sl::MAT_TYPE::U8_C4) ? CV_8UC4 : CV_8UC3;
+        return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(sl::MEM::CPU));
+    }
+
+    // Parameters
+    std::string camera_side_;
+    std::vector<int64_t> roi_dimensions_;
+    int frame_rate_;
+
+    // Camera settings
+    int left_camera_serial_number_ = 26853647;
+    int right_camera_serial_number_ = 0;
+    double velocity_;
+    int shift_constant_;
+    cv::Size model_dimensions_;
+
+    // ROS Publisher
+    rclcpp::Publisher<custom_interfaces::msg::ImageInput>::SharedPtr image_publisher_;
+};
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<CameraNode>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

ros2_ws/src/cpp_package/src/ExterminationNode.cpp

@@ -0,0 +1,127 @@
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <std_msgs/msg/int8.hpp>
+#include <custom_interfaces/msg/inference_output.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/opencv.hpp>
+#include "utils/model_inference.hpp"
+
+// C++ Equivalent of cv_bridge:
+//      Used CvBridge to convert ROS Image messages to OpenCV Mat.
+// Custom Utility Integration:
+//      ModelInference class should provide postprocess and draw_boxes functions similar to Python.
+// Multi-threaded Executor:
+//      Ensures responsiveness by managing callbacks efficiently.
+
+class ExterminationNode : public rclcpp::Node {
+public:
+    ExterminationNode() : Node("extermination_node") {
+        RCLCPP_INFO(this->get_logger(), "Initializing Extermination Node");
+
+        // Declare and retrieve parameters
+        this->declare_parameter<bool>("use_display_node", true);
+        this->declare_parameter<std::string>("camera_side", "left");
+        use_display_node_ = this->get_parameter("use_display_node").as_bool();
+        camera_side_ = this->get_parameter("camera_side").as_string();
+
+        window_ = (camera_side_ == "left") ? "Left Camera" : "Right Camera";
+        if (!use_display_node_) {
+            window_ = "";
+        }
+
+        publishing_rate_ = 1.0;
+        lower_range_ = cv::Scalar(78, 158, 124);
+        upper_range_ = cv::Scalar(60, 255, 255);
+        minimum_area_ = 100;
+        minimum_confidence_ = 0.5;
+        boxes_present_ = 0;
+
+        model_ = std::make_shared<ModelInference>();
+        bridge_ = std::make_shared<cv_bridge::CvBridge>();
+
+        boxes_msg_.data = 0;
+
+        // Create subscription and publisher
+        inference_subscription_ = this->create_subscription<custom_interfaces::msg::InferenceOutput>(
+            camera_side_ + "_inference_output", 10, 
+            std::bind(&ExterminationNode::inference_callback, this, std::placeholders::_1));
+
+        box_publisher_ = this->create_publisher<std_msgs::msg::Int8>(
+            camera_side_ + "_extermination_output", 10);
+
+        timer_ = this->create_wall_timer(
+            std::chrono::milliseconds(static_cast<int>(1000 / publishing_rate_)),
+            std::bind(&ExterminationNode::timer_callback, this));
+    }
+
+private:
+    void inference_callback(const custom_interfaces::msg::InferenceOutput::SharedPtr msg) {
+        RCLCPP_INFO(this->get_logger(), "Received Bounding Boxes");
+
+        // Convert ROS image messages to OpenCV
+        cv::Mat preprocessed_image = bridge_->imgmsg_to_cv2(msg->preprocessed_image, "passthrough");
+        cv::Mat raw_image = bridge_->imgmsg_to_cv2(msg->raw_image, "passthrough");
+
+        // Perform post-processing and draw bounding boxes
+        auto bounding_boxes = model_->postprocess(
+            msg->confidences, msg->bounding_boxes, raw_image, msg->velocity);
+
+        cv::Mat final_image = model_->draw_boxes(raw_image, bounding_boxes, msg->velocity);
+
+        // Display the image if required
+        if (use_display_node_) {
+            cv::imshow(window_, final_image);
+            cv::waitKey(10);
+        }
+
+        // Update presence of bounding boxes
+        boxes_present_ = !bounding_boxes.empty() ? 1 : 0;
+        boxes_msg_.data = boxes_present_;
+    }
+
+    void timer_callback() {
+        box_publisher_->publish(boxes_msg_);
+        RCLCPP_INFO(this->get_logger(), "Published results to Proxy Node");
+    }
+
+    // Parameters
+    bool use_display_node_;
+    std::string camera_side_;
+    std::string window_;
+
+    // Model and preprocessing
+    double publishing_rate_;
+    cv::Scalar lower_range_;
+    cv::Scalar upper_range_;
+    int minimum_area_;
+    double minimum_confidence_;
+    int boxes_present_;
+
+    std_msgs::msg::Int8 boxes_msg_;
+
+    // Utilities
+    std::shared_ptr<ModelInference> model_;
+    std::shared_ptr<cv_bridge::CvBridge> bridge_;
+
+    // ROS 2 communication
+    rclcpp::Subscription<custom_interfaces::msg::InferenceOutput>::SharedPtr inference_subscription_;
+    rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr box_publisher_;
+    rclcpp::TimerBase::SharedPtr timer_;
+};
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<ExterminationNode>();
+
+    rclcpp::executors::MultiThreadedExecutor executor;
+    executor.add_node(node);
+
+    try {
+        executor.spin();
+    } catch (const std::exception &e) {
+        RCLCPP_ERROR(node->get_logger(), "Shutting down Extermination Node: %s", e.what());
+    }
+
+    rclcpp::shutdown();
+    return 0;
+}

ros2_ws/src/cpp_package/src/InferenceNode.cpp

@@ -0,0 +1,119 @@
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <std_msgs/msg/float32_multi_array.hpp>
+#include <custom_interfaces/msg/image_input.hpp>
+#include <custom_interfaces/msg/inference_output.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/opencv.hpp>
+#include <chrono>
+#include "utils/model_inference.hpp"
+
+// Custom ModelInference Class:
+
+// Should support inference as a method accepting an input image and returning output image, confidences, and bounding boxes.
+// Must handle loading models using the weights_path and precision.
+// ROS 2 Message Handling:
+
+// custom_interfaces::msg::ImageInput and custom_interfaces::msg::InferenceOutput must have fields matching the Python implementation.
+// Conversion from Python's list (Float32MultiArray) is straightforward using std::vector.
+// Timing:
+
+// Uses std::chrono for precise inference timing.
+
+class InferenceNode : public rclcpp::Node {
+public:
+    InferenceNode() : Node("inference_node") {
+        RCLCPP_INFO(this->get_logger(), "Initializing Inference Node");
+
+        // Declare and retrieve parameters
+        this->declare_parameter<std::string>("weights_path", "/home/user/ROS/models/maize/Maize.onnx");
+        this->declare_parameter<std::string>("precision", "fp32"); // Options: fp32, fp16
+        this->declare_parameter<std::string>("camera_side", "left");
+
+        weights_path_ = this->get_parameter("weights_path").as_string();
+        precision_ = this->get_parameter("precision").as_string();
+        camera_side_ = this->get_parameter("camera_side").as_string();
+
+        model_ = std::make_shared<ModelInference>(weights_path_, precision_);
+        bridge_ = std::make_shared<cv_bridge::CvBridge>();
+
+        // Create subscription and publisher
+        image_subscription_ = this->create_subscription<custom_interfaces::msg::ImageInput>(
+            camera_side_ + "_image_input", 10,
+            std::bind(&InferenceNode::image_callback, this, std::placeholders::_1));
+
+        box_publisher_ = this->create_publisher<custom_interfaces::msg::InferenceOutput>(
+            camera_side_ + "_inference_output", 10);
+    }
+
+private:
+    void image_callback(const custom_interfaces::msg::ImageInput::SharedPtr msg) {
+        RCLCPP_INFO(this->get_logger(), "Received Image");
+
+        // Convert ROS image message to OpenCV format
+        cv::Mat opencv_img = bridge_->imgmsg_to_cv2(msg->preprocessed_image, "passthrough");
+
+        // Perform inference and measure time
+        auto start = std::chrono::high_resolution_clock::now();
+        cv::Mat output_img;
+        std::vector<float> confidences;
+        std::vector<float> boxes;
+        model_->inference(opencv_img, output_img, confidences, boxes);
+        auto end = std::chrono::high_resolution_clock::now();
+        auto inference_time = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
+
+        // Prepare the output message
+        custom_interfaces::msg::InferenceOutput output_msg;
+        output_msg.num_boxes = boxes.size() / 4;  // Assuming each box has 4 values (x, y, width, height)
+        output_msg.raw_image = msg->raw_image;
+        output_msg.velocity = msg->velocity;
+        output_msg.preprocessed_image = msg->preprocessed_image;
+
+        // Fill bounding boxes and confidences
+        std_msgs::msg::Float32MultiArray bounding_boxes;
+        std_msgs::msg::Float32MultiArray confidences_msg;
+
+        if (!boxes.empty()) {
+            bounding_boxes.data = boxes;
+            confidences_msg.data = confidences;
+        }
+
+        output_msg.bounding_boxes = bounding_boxes;
+        output_msg.confidences = confidences_msg;
+
+        // Publish the output message
+        box_publisher_->publish(output_msg);
+
+        RCLCPP_INFO(this->get_logger(), "Inference: %ld ms", inference_time);
+    }
+
+    // Parameters
+    std::string weights_path_;
+    std::string precision_;
+    std::string camera_side_;
+
+    // Model and utilities
+    std::shared_ptr<ModelInference> model_;
+    std::shared_ptr<cv_bridge::CvBridge> bridge_;
+
+    // ROS 2 communication
+    rclcpp::Subscription<custom_interfaces::msg::ImageInput>::SharedPtr image_subscription_;
+    rclcpp::Publisher<custom_interfaces::msg::InferenceOutput>::SharedPtr box_publisher_;
+};
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<InferenceNode>();
+
+    rclcpp::executors::MultiThreadedExecutor executor;
+    executor.add_node(node);
+
+    try {
+        executor.spin();
+    } catch (const std::exception &e) {
+        RCLCPP_ERROR(node->get_logger(), "Shutting down Inference Node: %s", e.what());
+    }
+
+    rclcpp::shutdown();
+    return 0;
+}