diff --git a/CMakeLists.txt b/CMakeLists.txt index cd9a64b..4beb037 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,48 +1,42 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.27) project(computer_vision) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -fPIC) endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(image_transport REQUIRED) find_package(cv_bridge REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(std_msgs REQUIRED) -find_package(OpenCV REQUIRED) -find_package(PCL 1.8 REQUIRED QUIET) -find_package(pcl_conversions REQUIRED) -find_package(image_geometry REQUIRED) find_package(depth_image_proc REQUIRED) find_package(Eigen3 REQUIRED) +find_package(image_geometry REQUIRED) +find_package(image_transport REQUIRED) find_package(message_filters REQUIRED) +find_package(OpenCV REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) - -include_directories(${PCL_INCLUDE_DIRS}) -link_directories(${PCL_LIBRARY_DIRS}) -add_definitions(${PCL_DEFINITIONS}) set(dependencies - rclcpp - image_transport cv_bridge - sensor_msgs - std_msgs - OpenCV - PCL - pcl_conversions - image_geometry depth_image_proc Eigen3 - message_filters - tf2_ros geometry_msgs + image_geometry + image_transport + message_filters + OpenCV + pcl_ros + rclcpp + sensor_msgs + std_msgs tf2_geometry_msgs + tf2_ros ) # OpenCV and PCL node diff --git a/include/computer_vision/CVSubscriber.hpp b/include/computer_vision/CVSubscriber.hpp index 3203e99..79d6701 100644 --- a/include/computer_vision/CVSubscriber.hpp +++ b/include/computer_vision/CVSubscriber.hpp @@ -1,5 +1,5 @@ /* - Copyright (c) 2024 José Miguel Guerrero Hernández + Copyright (c) 2025 José Miguel Guerrero Hernández This file is licensed under the terms of the MIT license. See the LICENSE file in the root of this repository @@ -8,45 +8,56 @@ #ifndef INCLUDE_COMPUTER_VISION__CVSUBSCRIBER_HPP_ #define INCLUDE_COMPUTER_VISION__CVSUBSCRIBER_HPP_ +#include "cv_bridge/cv_bridge.hpp" +#include "image_geometry/pinhole_camera_model.hpp" #include "image_transport/image_transport.hpp" #include "opencv2/highgui/highgui.hpp" #include "opencv2/imgproc/imgproc.hpp" #include "opencv2/opencv.hpp" -#include "sensor_msgs/msg/camera_info.hpp" -#include "image_geometry/pinhole_camera_model.hpp" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/subscriber.h" #include "pcl/point_types.h" -#include "pcl_conversions/pcl_conversions.h" #include "pcl/point_types_conversion.h" -#include "message_filters/subscriber.h" -#include "message_filters/sync_policies/approximate_time.h" -#include "cv_bridge/cv_bridge.hpp" +#include "pcl_conversions/pcl_conversions.h" #include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/image.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" +#include "stereo_msgs/msg/disparity_image.hpp" namespace computer_vision { -using std::placeholders::_1; -using std::placeholders::_2; -using std::placeholders::_3; - class CVGroup { public: - CVGroup(cv::Mat image_rgb, cv::Mat image_depth, pcl::PointCloud pointcloud) + CVGroup( + cv::Mat rgb, + cv::Mat depth, cv::Mat disparity, + cv::Mat left_rect, cv::Mat right_rect, + cv::Mat left_raw, cv::Mat right_raw, + pcl::PointCloud pointcloud) { - image_rgb_ = image_rgb; - image_depth_ = image_depth; + rgb_ = rgb; + depth_ = depth; + disparity_ = disparity; + left_rect_ = left_rect; + right_rect_ = right_rect; + left_raw_ = left_raw; + right_raw_ = right_raw; pointcloud_ = pointcloud; } - cv::Mat getImageRGB() {return image_rgb_;} - cv::Mat getImageDepth() {return image_depth_;} + cv::Mat getImageRGB() {return rgb_;} + cv::Mat getImageDepth() {return depth_;} + cv::Mat getImageDisparity() {return disparity_;} + cv::Mat getImageLeftRect() {return left_rect_;} + cv::Mat getImageRightRect() {return right_rect_;} + cv::Mat getImageLeftRaw() {return left_raw_;} + cv::Mat getImageRightRaw() {return right_raw_;} pcl::PointCloud getPointCloud() {return pointcloud_;} private: - cv::Mat image_rgb_; - cv::Mat image_depth_; + cv::Mat rgb_, depth_, disparity_, left_rect_, right_rect_, left_raw_, right_raw_; pcl::PointCloud pointcloud_; }; @@ -56,149 +67,378 @@ class CVSubscriber : public rclcpp::Node CVSubscriber() : Node("cv_subscriber") { - this->declare_parameter("check_subscription_count", false); - this->get_parameter("check_subscription_count", check_subscription_count_); - subscription_info_ = create_subscription( "/camera_info", 1, - std::bind(&CVSubscriber::topic_callback_info, this, _1)); + [this](sensor_msgs::msg::CameraInfo::UniquePtr msg) { + RCLCPP_INFO(get_logger(), "Camera info received"); + camera_model_ = std::make_shared(); + camera_model_->fromCameraInfo(*msg); + subscription_info_ = nullptr; + }); + + subscription_rgb_ = create_subscription( + "/rgb_in", 1, + [this](sensor_msgs::msg::Image::UniquePtr msg) { + last_rgb_ = std::move(msg); + }); + + subscription_depth_ = create_subscription( + "/depth_in", 1, + [this](sensor_msgs::msg::Image::UniquePtr msg) { + last_depth_ = std::move(msg); + }); + + subscription_disparity_ = create_subscription( + "/disparity_in", 1, + [this](stereo_msgs::msg::DisparityImage::UniquePtr msg) { + last_disparity_ = std::move(msg); + }); + + subscription_left_rect_ = create_subscription( + "/left_rect_in", 1, + [this](sensor_msgs::msg::Image::UniquePtr msg) { + last_left_rect_ = std::move(msg); + }); + + subscription_right_rect_ = create_subscription( + "/right_rect_in", 1, + [this](sensor_msgs::msg::Image::UniquePtr msg) { + last_right_rect_ = std::move(msg); + }); + + subscription_left_raw_ = create_subscription( + "/left_raw_in", 1, + [this](sensor_msgs::msg::Image::UniquePtr msg) { + last_left_raw_ = std::move(msg); + }); + + subscription_right_raw_ = create_subscription( + "/right_raw_in", 1, + [this](sensor_msgs::msg::Image::UniquePtr msg) { + last_right_raw_ = std::move(msg); + }); + + subscription_pointcloud_ = create_subscription( + "/pointcloud_in", 1, + [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) { + last_pointcloud_ = std::move(msg); + }); + + publisher_rgb_ = this->create_publisher( + "rgb", + rclcpp::SensorDataQoS().reliable()); - subscription_rgb_ = std::make_shared>( - this, "/image_rgb_in", rclcpp::SensorDataQoS().reliable().get_rmw_qos_profile()); + publisher_depth_ = this->create_publisher( + "depth", + rclcpp::SensorDataQoS().reliable()); - subscription_depth_ = std::make_shared>( - this, "/image_depth_in", rclcpp::SensorDataQoS().reliable().get_rmw_qos_profile()); + publisher_disparity_ = this->create_publisher( + "disparity", + rclcpp::SensorDataQoS().reliable()); - subscription_pointcloud_ = - std::make_shared>( - this, "/pointcloud_in", rclcpp::SensorDataQoS().reliable().get_rmw_qos_profile()); + publisher_left_raw_ = this->create_publisher( + "left_raw", + rclcpp::SensorDataQoS().reliable()); - sync_ = std::make_shared>( - MySyncPolicy(10), *subscription_rgb_, *subscription_depth_, *subscription_pointcloud_); - sync_->registerCallback( - std::bind( - &CVSubscriber::topic_callback_multi, this, _1, _2, _3)); + publisher_right_raw_ = this->create_publisher( + "right_raw", + rclcpp::SensorDataQoS().reliable()); - publisher_depth_ = this->create_publisher( - "image_depth", + publisher_left_rect_ = this->create_publisher( + "left_rect", rclcpp::SensorDataQoS().reliable()); - publisher_rgb_ = this->create_publisher( - "image_rgb", + publisher_right_rect_ = this->create_publisher( + "right_rect", rclcpp::SensorDataQoS().reliable()); publisher_pointcloud_ = this->create_publisher( "pointcloud", rclcpp::SensorDataQoS().reliable()); + + // 30Hz + timer_ = this->create_wall_timer( + std::chrono::milliseconds(33), + std::bind(&CVSubscriber::timer_callback, this)); } private: - bool check_subscription_count_; - CVGroup processing( - const cv::Mat in_image_rgb, - const cv::Mat in_image_depth, + const cv::Mat rgb, + const cv::Mat depth, + const cv::Mat disparity, + const cv::Mat left_rect, + const cv::Mat right_rect, + const cv::Mat left_raw, + const cv::Mat right_raw, const pcl::PointCloud in_pointcloud) const; - void topic_callback_info(sensor_msgs::msg::CameraInfo::UniquePtr msg) + void timer_callback() { - RCLCPP_INFO(get_logger(), "Camera info received"); - - camera_model_ = std::make_shared(); - camera_model_->fromCameraInfo(*msg); - - subscription_info_ = nullptr; - } - - void topic_callback_multi( - const sensor_msgs::msg::Image::ConstSharedPtr & image_rgb_msg, - const sensor_msgs::msg::Image::ConstSharedPtr & image_depth_msg, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg) const - { - // Check if camera model has been received + // Check if camera model has been received if (camera_model_ == nullptr) { RCLCPP_WARN(get_logger(), "Camera Model not yet available"); return; } - // Check if depth image has been received - if (image_depth_msg->encoding != "16UC1" && image_depth_msg->encoding != "32FC1") { - RCLCPP_ERROR(get_logger(), "The image type has not depth info"); + // Convert ROS Image to OpenCV Image | sensor_msgs::msg::Image -> cv::Mat + cv_bridge::CvImagePtr image_rgb_ptr, image_depth_ptr, image_disparity_ptr, + image_left_rect_ptr, image_right_rect_ptr, image_left_raw_ptr, image_right_raw_ptr; + + cv::Mat image_rgb, image_depth, image_disparity, image_left_rect, image_right_rect, + image_left_raw, image_right_raw; + + try { + if (last_rgb_ != nullptr) { + image_rgb_ptr = cv_bridge::toCvCopy(*last_rgb_, sensor_msgs::image_encodings::BGR8); + image_rgb = image_rgb_ptr->image; + if (counter_rgb_ == 0) { + RCLCPP_INFO(get_logger(), "RGB image received"); + counter_rgb_++; + } + } + + if (last_depth_ != nullptr) { + if (last_depth_->encoding != "16UC1") { + RCLCPP_ERROR(get_logger(), "The depth image type has not 16UC1 encoding: %s", + (last_depth_->encoding).c_str()); + return; + } + image_depth_ptr = cv_bridge::toCvCopy(*last_depth_, + sensor_msgs::image_encodings::TYPE_16UC1); + image_depth = image_depth_ptr->image; + if (counter_depth_ == 0) { + RCLCPP_INFO(get_logger(), "Depth image received"); + counter_depth_++; + } + } + + if (last_disparity_ != nullptr) { + if (last_disparity_->image.encoding != "32FC1") { + RCLCPP_ERROR(get_logger(), "The disparity image type has not 16UC1 encoding: %s", + (last_disparity_->image.encoding).c_str()); + return; + } + image_disparity_ptr = cv_bridge::toCvCopy(last_disparity_->image, + sensor_msgs::image_encodings::TYPE_32FC1); + image_disparity = image_disparity_ptr->image; + if (counter_disparity_ == 0) { + RCLCPP_INFO(get_logger(), "Disparity image received"); + counter_disparity_++; + } + } + + if (last_left_rect_ != nullptr) { + image_left_rect_ptr = cv_bridge::toCvCopy(*last_left_rect_, + sensor_msgs::image_encodings::BGR8); + image_left_rect = image_left_rect_ptr->image; + if (counter_left_rect_ == 0) { + RCLCPP_INFO(get_logger(), "Left rectified image received"); + counter_left_rect_++; + } + } + + if (last_right_rect_ != nullptr) { + image_right_rect_ptr = cv_bridge::toCvCopy(*last_right_rect_, + sensor_msgs::image_encodings::BGR8); + image_right_rect = image_right_rect_ptr->image; + if (counter_right_rect_ == 0) { + RCLCPP_INFO(get_logger(), "Right rectified image received"); + counter_right_rect_++; + } + } + + if (last_left_raw_ != nullptr) { + image_left_raw_ptr = cv_bridge::toCvCopy(*last_left_raw_, + sensor_msgs::image_encodings::BGR8); + image_left_raw = image_left_raw_ptr->image; + if (counter_left_raw_ == 0) { + RCLCPP_INFO(get_logger(), "Left raw image received"); + counter_left_raw_++; + } + } + + if (last_right_raw_ != nullptr) { + image_right_raw_ptr = cv_bridge::toCvCopy(*last_right_raw_, + sensor_msgs::image_encodings::BGR8); + image_right_raw = image_right_raw_ptr->image; + if (counter_right_raw_ == 0) { + RCLCPP_INFO(get_logger(), "Right raw image received"); + counter_right_raw_++; + } + } + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", e.what()); return; } - // Set "check_subscription_count" to False in the launch file if you want to process it always - if (!check_subscription_count_ || - ((publisher_rgb_->get_subscription_count() > 0 ) && - (publisher_depth_->get_subscription_count() > 0) && - (publisher_pointcloud_->get_subscription_count() > 0))) - { - // Convert ROS Image to OpenCV Image | sensor_msgs::msg::Image -> cv::Mat - cv_bridge::CvImagePtr image_rgb_ptr, image_depth_ptr; - try { - image_rgb_ptr = cv_bridge::toCvCopy(*image_rgb_msg, sensor_msgs::image_encodings::BGR8); - image_depth_ptr = cv_bridge::toCvCopy( - *image_depth_msg, - sensor_msgs::image_encodings::TYPE_32FC1); - } catch (cv_bridge::Exception & e) { - RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", e.what()); - return; + // Convert to PCL data type + pcl::PointCloud pointcloud; + if (last_pointcloud_ != nullptr) { + pcl::fromROSMsg(*last_pointcloud_, pointcloud); + if (counter_pointcloud_ == 0) { + RCLCPP_INFO(get_logger(), "Pointcloud received"); + counter_pointcloud_++; } - cv::Mat image_rgb_raw = image_rgb_ptr->image; - cv::Mat image_depth_raw = image_depth_ptr->image; - - // Convert to PCL data type - pcl::PointCloud pointcloud; - pcl::fromROSMsg(*pointcloud_msg, pointcloud); + } - // Image and PointCloud processing - CVGroup cvgroup = processing( - image_rgb_raw, image_depth_raw, pointcloud); + // Image and PointCloud processing + CVGroup cvgroup = processing(image_rgb, image_depth, image_disparity, + image_left_rect, image_right_rect, image_left_raw, image_right_raw, pointcloud); + if (!cvgroup.getImageRGB().empty()) { // Convert OpenCV Image to ROS Image cv_bridge::CvImage image_rgb_bridge = cv_bridge::CvImage( - image_rgb_msg->header, sensor_msgs::image_encodings::BGR8, - cvgroup.getImageRGB()); + last_rgb_->header, sensor_msgs::image_encodings::BGR8, + cvgroup.getImageRGB()); + // >> message to be sent + sensor_msgs::msg::Image out_image_rgb; + // from cv_bridge to sensor_msgs::Image + image_rgb_bridge.toImageMsg(out_image_rgb); + // Publish the data + publisher_rgb_->publish(out_image_rgb); + } + if (!cvgroup.getImageDepth().empty()) { + // Convert to ROS data type cv_bridge::CvImage image_depth_bridge = cv_bridge::CvImage( - image_depth_msg->header, sensor_msgs::image_encodings::TYPE_32FC1, - cvgroup.getImageDepth()); + last_depth_->header, sensor_msgs::image_encodings::TYPE_16UC1, + cvgroup.getImageDepth()); + // >> message to be sent + sensor_msgs::msg::Image out_image_depth; + // from cv_bridge to sensor_msgs::Image + image_depth_bridge.toImageMsg(out_image_depth); + // Publish the data + publisher_depth_->publish(out_image_depth); + } + if (!cvgroup.getImageDisparity().empty()) { + // Convert to ROS data type + cv_bridge::CvImage image_disparity_bridge = + cv_bridge::CvImage( + last_disparity_->image.header, sensor_msgs::image_encodings::TYPE_32FC1, + cvgroup.getImageDisparity()); // >> message to be sent - sensor_msgs::msg::Image out_image_rgb, out_image_depth; + sensor_msgs::msg::Image out_image_disparity; + // from cv_bridge to sensor_msgs::Image + image_disparity_bridge.toImageMsg(out_image_disparity); + // out_image_disparity.is_bigendian = true; + // Publish the data + publisher_disparity_->publish(out_image_disparity); + } + if (!cvgroup.getImageLeftRect().empty()) { + // Convert to ROS data type + cv_bridge::CvImage image_left_rect_bridge = + cv_bridge::CvImage( + last_left_rect_->header, sensor_msgs::image_encodings::BGR8, + cvgroup.getImageLeftRect()); + // >> message to be sent + sensor_msgs::msg::Image out_image_left_rect; // from cv_bridge to sensor_msgs::Image - image_rgb_bridge.toImageMsg(out_image_rgb); - image_depth_bridge.toImageMsg(out_image_depth); + image_left_rect_bridge.toImageMsg(out_image_left_rect); + // Publish the data + publisher_left_rect_->publish(out_image_left_rect); + } + if (!cvgroup.getImageRightRect().empty()) { // Convert to ROS data type - sensor_msgs::msg::PointCloud2 out_pointcloud; - pcl::toROSMsg(cvgroup.getPointCloud(), out_pointcloud); - out_pointcloud.header = pointcloud_msg->header; + cv_bridge::CvImage image_right_rect_bridge = + cv_bridge::CvImage( + last_right_rect_->header, sensor_msgs::image_encodings::BGR8, + cvgroup.getImageRightRect()); + // >> message to be sent + sensor_msgs::msg::Image out_image_right_rect; + // from cv_bridge to sensor_msgs::Image + image_right_rect_bridge.toImageMsg(out_image_right_rect); + // Publish the data + publisher_right_rect_->publish(out_image_right_rect); + } + if (!cvgroup.getImageLeftRaw().empty()) { + // Convert to ROS data type + cv_bridge::CvImage image_left_raw_bridge = + cv_bridge::CvImage( + last_left_raw_->header, sensor_msgs::image_encodings::BGR8, + cvgroup.getImageLeftRaw()); + // >> message to be sent + sensor_msgs::msg::Image out_image_left_raw; + // from cv_bridge to sensor_msgs::Image + image_left_raw_bridge.toImageMsg(out_image_left_raw); // Publish the data - publisher_rgb_->publish(out_image_rgb); - publisher_depth_->publish(out_image_depth); + publisher_left_raw_->publish(out_image_left_raw); + } + + if (!cvgroup.getImageRightRaw().empty()) { + // Convert to ROS data type + cv_bridge::CvImage image_right_raw_bridge = + cv_bridge::CvImage( + last_right_raw_->header, sensor_msgs::image_encodings::BGR8, + cvgroup.getImageRightRaw()); + // >> message to be sent + sensor_msgs::msg::Image out_image_right_raw; + // from cv_bridge to sensor_msgs::Image + image_right_raw_bridge.toImageMsg(out_image_right_raw); + // Publish the data + publisher_right_raw_->publish(out_image_right_raw); + } + + if (!cvgroup.getPointCloud().empty()) { + // Convert to ROS data type + sensor_msgs::msg::PointCloud2 out_pointcloud; + pcl::toROSMsg(cvgroup.getPointCloud(), out_pointcloud); + out_pointcloud.header = last_pointcloud_->header; publisher_pointcloud_->publish(out_pointcloud); } } - typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; - std::shared_ptr> sync_; - std::shared_ptr> subscription_rgb_; - std::shared_ptr> subscription_depth_; - std::shared_ptr> - subscription_pointcloud_; - + // Last received data + sensor_msgs::msg::Image::UniquePtr last_rgb_ = nullptr; + sensor_msgs::msg::Image::UniquePtr last_depth_ = nullptr; + stereo_msgs::msg::DisparityImage::UniquePtr last_disparity_ = nullptr; + sensor_msgs::msg::Image::UniquePtr last_left_rect_ = nullptr; + sensor_msgs::msg::Image::UniquePtr last_right_rect_ = nullptr; + sensor_msgs::msg::Image::UniquePtr last_left_raw_ = nullptr; + sensor_msgs::msg::Image::UniquePtr last_right_raw_ = nullptr; + sensor_msgs::msg::PointCloud2::UniquePtr last_pointcloud_ = nullptr; + std::shared_ptr camera_model_ = nullptr; + + // Subscriptions + rclcpp::Subscription::SharedPtr subscription_rgb_; + rclcpp::Subscription::SharedPtr subscription_depth_; + rclcpp::Subscription::SharedPtr subscription_disparity_; + rclcpp::Subscription::SharedPtr subscription_left_rect_; + rclcpp::Subscription::SharedPtr subscription_right_rect_; + rclcpp::Subscription::SharedPtr subscription_left_raw_; + rclcpp::Subscription::SharedPtr subscription_right_raw_; + rclcpp::Subscription::SharedPtr subscription_pointcloud_; rclcpp::Subscription::SharedPtr subscription_info_; + + // Publishers rclcpp::Publisher::SharedPtr publisher_rgb_; rclcpp::Publisher::SharedPtr publisher_depth_; + rclcpp::Publisher::SharedPtr publisher_disparity_; //Publish Image instead of DisparityImage + rclcpp::Publisher::SharedPtr publisher_left_rect_; + rclcpp::Publisher::SharedPtr publisher_right_rect_; + rclcpp::Publisher::SharedPtr publisher_left_raw_; + rclcpp::Publisher::SharedPtr publisher_right_raw_; rclcpp::Publisher::SharedPtr publisher_pointcloud_; - std::shared_ptr camera_model_; + + // Timers + rclcpp::TimerBase::SharedPtr timer_; + + //Counters + size_t counter_rgb_ = 0; + size_t counter_depth_ = 0; + size_t counter_disparity_ = 0; + size_t counter_left_rect_ = 0; + size_t counter_right_rect_ = 0; + size_t counter_left_raw_ = 0; + size_t counter_right_raw_ = 0; + size_t counter_pointcloud_ = 0; }; } // namespace computer_vision diff --git a/launch/cv.launch.py b/launch/cv.launch.py index 7934d42..eeceacb 100644 --- a/launch/cv.launch.py +++ b/launch/cv.launch.py @@ -1,4 +1,4 @@ -# Copyright (c) 2023 José Miguel Guerrero Hernández +# Copyright (c) 2025 José Miguel Guerrero Hernández # # This file is licensed under the terms of the MIT license. # See the LICENSE file in the root of this repository. @@ -16,17 +16,17 @@ def generate_launch_description(): executable='cv_program', output='both', emulate_tty=True, - # Set to True to process just if there is a subscription, - # False to process always - parameters=[ - {'check_subscription_count': False} - ], - # Use topics from robot + # Use topics from camera remappings=[ - ('/camera_info', '/head_front_camera/rgb/camera_info'), - ('/image_rgb_in', '/head_front_camera/rgb/image_raw'), - ('/image_depth_in', '/head_front_camera/depth_registered/image_raw'), - ('/pointcloud_in', '/head_front_camera/depth_registered/points'), + ('/camera_info', '/rgb/camera_info'), + ('/rgb_in', '/rgb/image'), + ('/depth_in', '/stereo/depth'), + ('/disparity_in', '/stereo/disparity'), + ('/left_raw_in', '/left/image'), + ('/right_raw_in', '/right/image'), + ('/left_rect_in', '/left_rect/image'), + ('/right_rect_in', '/right_rect/image'), + ('/pointcloud_in', '/stereo/points'), ], ) ]) diff --git a/src/computer_vision/CVSubscriber.cpp b/src/computer_vision/CVSubscriber.cpp index e3fff06..f00e60f 100644 --- a/src/computer_vision/CVSubscriber.cpp +++ b/src/computer_vision/CVSubscriber.cpp @@ -1,5 +1,5 @@ /* - Copyright (c) 2024 José Miguel Guerrero Hernández + Copyright (c) 2025 José Miguel Guerrero Hernández This file is licensed under the terms of the MIT license. See the LICENSE file in the root of this repository @@ -11,30 +11,62 @@ namespace computer_vision { /** - TO-DO: Default - the output images and pointcloud are the same as the input + TO-DO: Default - the output images are half the size of the input and pointcloud are the same as the input */ CVGroup CVSubscriber::processing( - const cv::Mat in_image_rgb, - const cv::Mat in_image_depth, + const cv::Mat rgb, + const cv::Mat depth, + const cv::Mat disparity, + const cv::Mat left_rect, + const cv::Mat right_rect, + const cv::Mat left_raw, + const cv::Mat right_raw, const pcl::PointCloud in_pointcloud) const { // Create output images - cv::Mat out_image_rgb, out_image_depth; + cv::Mat out_rgb, out_depth, out_disparity, out_left_rect, out_right_rect, out_left_raw, + out_right_raw; // Create output pointcloud pcl::PointCloud out_pointcloud; - // Processing - out_image_rgb = in_image_rgb; - out_image_depth = in_image_depth; - out_pointcloud = in_pointcloud; + // Processing - important: check if the input images are empty before to process them + if (!rgb.empty()) { + cv::resize(rgb, out_rgb, cv::Size(), 0.5, 0.5); + cv::imshow("out_image_rgb", out_rgb); + } + if (!depth.empty()) { + cv::resize(depth, out_depth, cv::Size(), 0.5, 0.5); + cv::imshow("out_image_depth", out_depth); + } + if (!disparity.empty()) { + cv::resize(disparity, out_disparity, cv::Size(), 0.5, 0.5); + cv::imshow("out_disparity", out_disparity); + } + if (!left_rect.empty()) { + cv::resize(left_rect, out_left_rect, cv::Size(), 0.5, 0.5); + cv::imshow("out_left_rect", out_left_rect); + } + if (!right_rect.empty()) { + cv::resize(right_rect, out_right_rect, cv::Size(), 0.5, 0.5); + cv::imshow("out_right_rect", out_right_rect); + } + if (!left_raw.empty()) { + cv::resize(left_raw, out_left_raw, cv::Size(), 0.5, 0.5); + cv::imshow("out_left_raw", out_left_raw); + } + if (!right_raw.empty()) { + cv::resize(right_raw, out_right_raw, cv::Size(), 0.5, 0.5); + cv::imshow("out_right_raw", out_right_raw); + } + if (!in_pointcloud.empty()) { + out_pointcloud = in_pointcloud; + } - // Show images in a different windows - cv::imshow("out_image_rgb", out_image_rgb); - cv::imshow("out_image_depth", out_image_depth); cv::waitKey(3); - return CVGroup(out_image_rgb, out_image_depth, out_pointcloud); + return CVGroup(out_rgb, out_depth, out_disparity, out_left_rect, out_right_rect, + out_left_raw, out_right_raw, out_pointcloud); } } // namespace computer_vision