|
1 | 1 | #include <cmath> |
| 2 | +#include <cv_bridge/cv_bridge.hpp> |
| 3 | +#include <image_transport/image_transport.hpp> |
2 | 4 | #include <iostream> |
3 | 5 | #include <memory> |
| 6 | +#include <opencv2/imgproc/imgproc.hpp> |
4 | 7 | #include <rclcpp/experimental/executors/events_executor/events_executor.hpp> |
5 | 8 | #include <rclcpp/logger.hpp> |
6 | 9 | #include <rclcpp/rclcpp.hpp> |
7 | 10 | #include <string> |
8 | 11 | #include <vector> |
9 | 12 |
|
10 | | -#include <opencv2/imgproc/imgproc.hpp> |
11 | | -#include <image_transport/image_transport.hpp> |
12 | | - |
13 | | -using std::placeholders::_1; |
| 13 | +using std::placeholders::_1, std::placeholders::_2; |
14 | 14 |
|
15 | 15 | namespace postprocessing { |
16 | 16 |
|
17 | | - |
18 | 17 | class PostProcessor { |
19 | 18 | std::shared_ptr<rclcpp::Node> node_; |
20 | 19 |
|
21 | | - // Declare subscriber |
22 | | - //rclcpp::Subscription<bitbots_msgs::msg::HeadMode>::SharedPtr head_mode_subscriber_; |
23 | | - //rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_subscriber_; |
| 20 | + image_transport::TransportHints transport_hints_; |
| 21 | + std::unique_ptr<image_transport::CameraSubscriber> image_sub_; |
| 22 | + std::unique_ptr<image_transport::CameraPublisher> image_pub_; |
24 | 23 |
|
25 | | - // Declare publisher |
26 | | - //rclcpp::Publisher<bitbots_msgs::msg::JointCommand>::SharedPtr position_publisher_; |
| 24 | + int binning_factor_x_ = 1; |
| 25 | + int binning_factor_y_ = 1; |
27 | 26 |
|
28 | 27 | public: |
29 | | - PostProcessor() : node_(std::make_shared<rclcpp::Node>("post_processor")) { |
30 | | - // Initialize publisher for head motor goals |
31 | | - //position_publisher_ = node_->create_publisher<bitbots_msgs::msg::JointCommand>("head_motor_goals", 10); |
32 | | - |
33 | | - // Initialize subscriber for head mode |
34 | | - //head_mode_subscriber_ = node_->create_subscription<bitbots_msgs::msg::HeadMode>( |
35 | | - // "head_mode", 10, [this](const bitbots_msgs::msg::HeadMode::SharedPtr msg) { head_mode_callback(msg); }); |
| 28 | + PostProcessor() |
| 29 | + : node_(std::make_shared<rclcpp::Node>("post_processor")), |
| 30 | + transport_hints_(node_.get()), |
| 31 | + image_sub_(std::make_unique<image_transport::CameraSubscriber>(image_transport::create_camera_subscription( |
| 32 | + node_.get(), "in/image_raw", std::bind(&PostProcessor::image_callback, this, _1, _2), |
| 33 | + transport_hints_.getTransport()))), |
| 34 | + image_pub_(std::make_unique<image_transport::CameraPublisher>( |
| 35 | + image_transport::create_camera_publisher(node_.get(), "out/image_proc"))) { |
| 36 | + // Declare parameters |
| 37 | + node_->declare_parameter("binning_factor_x", 4); |
| 38 | + node_->declare_parameter("binning_factor_y", 4); |
36 | 39 |
|
| 40 | + // Get parameters |
| 41 | + node_->get_parameter("binning_factor_x", binning_factor_x_); |
| 42 | + node_->get_parameter("binning_factor_y", binning_factor_y_); |
37 | 43 | } |
38 | 44 |
|
39 | 45 | /** |
40 | 46 | * @brief Callback used to update the head mode |
41 | 47 | */ |
42 | | - //void head_mode_callback(const bitbots_msgs::msg::HeadMode::SharedPtr msg) { head_mode_ = msg->head_mode; } |
| 48 | + void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& image_msg, |
| 49 | + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info_msg) { |
| 50 | + // Convert to cv::Mat |
| 51 | + cv::Mat image = cv_bridge::toCvShare(image_msg)->image; |
| 52 | + |
| 53 | + // Do some processing |
| 54 | + int bit_depth = sensor_msgs::image_encodings::bitDepth(image_msg->encoding); |
| 55 | + |
| 56 | + int type = bit_depth == 8 ? CV_8U : CV_16U; |
| 57 | + |
| 58 | + // Create cv::Mat for color image |
| 59 | + cv::Mat color(image_msg->height, image_msg->width, CV_MAKETYPE(type, 3)); |
| 60 | + |
| 61 | + // Create cv::Mat for |
| 62 | + const cv::Mat bayer(image_msg->height, image_msg->width, CV_MAKETYPE(type, 1), |
| 63 | + const_cast<uint8_t*>(&image_msg->data[0]), image_msg->step); |
| 64 | + |
| 65 | + // Get the correct conversion code |
| 66 | + int code = -1; |
| 67 | + if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB8 || |
| 68 | + image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB16) { |
| 69 | + code = cv::COLOR_BayerBG2BGR; |
| 70 | + } else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR8 || // NOLINT |
| 71 | + image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR16) { |
| 72 | + code = cv::COLOR_BayerRG2BGR; |
| 73 | + } else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG8 || // NOLINT |
| 74 | + image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG16) { |
| 75 | + code = cv::COLOR_BayerGR2BGR; |
| 76 | + } else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG8 || // NOLINT |
| 77 | + image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG16) { |
| 78 | + code = cv::COLOR_BayerGB2BGR; |
| 79 | + } |
| 80 | + code += cv::COLOR_BayerBG2BGR_VNG - cv::COLOR_BayerBG2BGR; |
| 81 | + |
| 82 | + // Debayer the image |
| 83 | + cv::cvtColor(bayer, color, code); |
| 84 | + |
| 85 | + // Perform binning by a given factor |
| 86 | + cv::Mat binned(image_msg->height / binning_factor_y_, image_msg->width / binning_factor_x_, CV_MAKETYPE(type, 3)); |
| 87 | + cv::resize(color, binned, cv::Size(), 1.0 / binning_factor_x_, 1.0 / binning_factor_y_, cv::INTER_AREA); |
| 88 | + |
| 89 | + // Add the binning to the camera info |
| 90 | + auto new_camera_info = std::make_shared<sensor_msgs::msg::CameraInfo>(*info_msg); |
| 91 | + new_camera_info->binning_x = binning_factor_x_; |
| 92 | + new_camera_info->binning_y = binning_factor_y_; |
| 93 | + |
| 94 | + // Convert back to sensor_msgs::Image |
| 95 | + cv_bridge::CvImage color_msg; |
| 96 | + color_msg.header = image_msg->header; |
| 97 | + color_msg.encoding = sensor_msgs::image_encodings::BGR8; |
| 98 | + color_msg.image = binned; |
| 99 | + |
| 100 | + // Publish the image |
| 101 | + image_pub_->publish(color_msg.toImageMsg(), new_camera_info); |
| 102 | + } |
43 | 103 |
|
44 | 104 | /** |
45 | 105 | * @brief A getter that returns the node |
46 | 106 | */ |
47 | 107 | std::shared_ptr<rclcpp::Node> get_node() { return node_; } |
48 | 108 | }; |
49 | | -} // namespace move_head |
| 109 | +} // namespace postprocessing |
50 | 110 |
|
51 | 111 | int main(int argc, char* argv[]) { |
52 | 112 | rclcpp::init(argc, argv); |
|
0 commit comments