diff --git a/bitbots_misc/bitbots_basler_camera/CMakeLists.txt b/bitbots_misc/bitbots_basler_camera/CMakeLists.txt
index 8fc154e4f..596badf8a 100644
--- 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
deleted file mode 100644
index 7f43f9905..000000000
--- a/bitbots_misc/bitbots_basler_camera/config/binning.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-decimation_x: 4
-decimation_y: 4
-interpolation: 3
diff --git a/bitbots_misc/bitbots_basler_camera/launch/basler_camera.launch b/bitbots_misc/bitbots_basler_camera/launch/basler_camera.launch
index f52e9a44a..8c297b0d9 100644
--- a/bitbots_misc/bitbots_basler_camera/launch/basler_camera.launch
+++ b/bitbots_misc/bitbots_basler_camera/launch/basler_camera.launch
@@ -13,19 +13,12 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
diff --git a/bitbots_misc/bitbots_basler_camera/package.xml b/bitbots_misc/bitbots_basler_camera/package.xml
index b08289f66..7f249425b 100644
--- a/bitbots_misc/bitbots_basler_camera/package.xml
+++ b/bitbots_misc/bitbots_basler_camera/package.xml
@@ -13,8 +13,12 @@
Hamburg Bit-Bots
bitbots_docs
+ cv_bridge
+ image_transport
+ libopencv-dev
pylon_ros2_camera_wrapper
- image_proc
+ rclcpp
+ sensor_msgs
ament_cmake
diff --git a/bitbots_misc/bitbots_basler_camera/src/postprocessing.cpp b/bitbots_misc/bitbots_basler_camera/src/postprocessing.cpp
index 2870407cf..79200d2fc 100644
--- a/bitbots_misc/bitbots_basler_camera/src/postprocessing.cpp
+++ b/bitbots_misc/bitbots_basler_camera/src/postprocessing.cpp
@@ -1,52 +1,112 @@
#include
+#include
+#include
#include
#include
+#include
#include
#include
#include
#include
#include
-#include
-#include
-
-using std::placeholders::_1;
+using std::placeholders::_1, std::placeholders::_2;
namespace postprocessing {
-
class PostProcessor {
std::shared_ptr node_;
- // Declare subscriber
- //rclcpp::Subscription::SharedPtr head_mode_subscriber_;
- //rclcpp::Subscription::SharedPtr joint_state_subscriber_;
+ image_transport::TransportHints transport_hints_;
+ std::unique_ptr image_sub_;
+ std::unique_ptr image_pub_;
- // Declare publisher
- //rclcpp::Publisher::SharedPtr position_publisher_;
+ int binning_factor_x_ = 1;
+ int binning_factor_y_ = 1;
public:
- PostProcessor() : node_(std::make_shared("post_processor")) {
- // Initialize publisher for head motor goals
- //position_publisher_ = node_->create_publisher("head_motor_goals", 10);
-
- // Initialize subscriber for head mode
- //head_mode_subscriber_ = node_->create_subscription(
- // "head_mode", 10, [this](const bitbots_msgs::msg::HeadMode::SharedPtr msg) { head_mode_callback(msg); });
+ PostProcessor()
+ : node_(std::make_shared("post_processor")),
+ transport_hints_(node_.get()),
+ image_sub_(std::make_unique(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::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(&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(*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 get_node() { return node_; }
};
-} // namespace move_head
+} // namespace postprocessing
int main(int argc, char* argv[]) {
rclcpp::init(argc, argv);