ROS2: How to publish cv Mat image?
I'm having trouble understanding how to publish a cv::Mat image in C++, and I can find very little documentation about this.
Here's my publish code. I based it on this tutorial: https://docs.ros.org/en/galactic/Tuto...
#include <chrono>
#include <memory>
#include "cv_bridge/cv_bridge.h"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "std_msgs/msg/header.hpp"
#include <opencv2/opencv.hpp>
#include <stdio.h>
// for Size
#include <opencv2/core/types.hpp>
// for CV_8UC3
#include <opencv2/core/hal/interface.h>
// for compressing the image
#include <image_transport/image_transport.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using namespace std::chrono_literals;
class MinimalPublisher : public rclcpp::Node {
public:
MinimalPublisher() : Node("minimal_publisher"), count_(0) {
publisher_ =
this->create_publisher<sensor_msgs::msg::Image>("topic", 10);
timer_ = this->create_wall_timer(
300ms, std::bind(&MinimalPublisher::timer_callback, this));
}
private:
void timer_callback() {
cv_bridge::CvImagePtr cv_ptr;
cv::Mat img(cv::Size(1280, 720), CV_8UC3);
cv::randu(img, cv::Scalar(0, 0, 0), cv::Scalar(255, 255, 255));
sensor_msgs::msg::Image::SharedPtr msg =
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", img)
.toImageMsg();
publisher_->publish(msg);
std::cout << "Published!" << std::endl;
}
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
size_t count_;
};
int main(int argc, char *argv[]) {
printf("Starting...");
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MinimalPublisher>());
rclcpp::shutdown();
return 0;
}
The publisher_->publish(msg)
line throws the following error:
Starting >>> cpp_pubsub
--- stderr: cpp_pubsub
/app/tests/ros2_cpp_test/src/cpp_pubsub/src/publisher_member_function.cpp: In member function ‘void MinimalPublisher::timer_callback()’:
/app/tests/ros2_cpp_test/src/cpp_pubsub/src/publisher_member_function.cpp:67:26: error: no matching function for call to ‘rclcpp::Publisher<sensor_msgs::msg::Image_<std::allocator<void> > >::publish(sensor_msgs::msg::Image_<std::allocator<void> >::SharedPtr&)’
publisher_->publish(msg);
^
In file included from /opt/ros/galactic/install/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp:31:0,
from /opt/ros/galactic/install/include/rclcpp/subscription.hpp:50,
from /opt/ros/galactic/install/include/rclcpp/any_executable.hpp:25,
from /opt/ros/galactic/install/include/rclcpp/memory_strategy.hpp:25,
from /opt/ros/galactic/install/include/rclcpp/memory_strategies.hpp:18,
from /opt/ros/galactic/install/include/rclcpp/executor_options.hpp:20,
from /opt/ros/galactic/install/include/rclcpp/executor.hpp:36,
from /opt/ros/galactic/install/include/rclcpp/executors/multi_threaded_executor.hpp:26,
from /opt/ros/galactic/install/include/rclcpp/executors.hpp:21,
from /opt/ros/galactic/install/include/rclcpp/rclcpp.hpp:156,
from /app/tests/ros2_cpp_test/src/cpp_pubsub/src/publisher_member_function.cpp:19:
/opt/ros/galactic/install/include/rclcpp/publisher.hpp:187:3: note: candidate: void rclcpp::Publisher<MessageT, AllocatorT>::publish(std::unique_ptr<MessageT, typename std::conditional<std::is_same<typename std::allocator_traits<typename std::allocator_traits<_Alloc>::rebind_traits<MessageT>::allocator_type>::rebind_alloc<MessageT>, typename std::allocator<void>::rebind<_Tp1>::other>::value, std::default_delete<_Tp>, rclcpp::allocator::AllocatorDeleter<typename std::allocator_traits<_Alloc>::rebind_traits<MessageT>::allocator_type> >::type>) [with MessageT = sensor_msgs::msg::Image_<std::allocator<void> >; AllocatorT = std::allocator<void>; typename std::conditional<std::is_same<typename std::allocator_traits<typename std::allocator_traits<_Alloc>::rebind_traits<MessageT>::allocator_type>::rebind_alloc<MessageT>, typename std::allocator<void>::rebind<_Tp1>::other>::value, std::default_delete<_Tp>, rclcpp::allocator::AllocatorDeleter<typename std::allocator_traits<_Alloc>::rebind_traits<MessageT>::allocator_type> >::type = std::default_delete<sensor_msgs::msg::Image_<std::allocator<void> > >]
publish(std::unique_ptr<MessageT, MessageDeleter> msg)
^~~~~~~
/opt/ros/galactic/install/include/rclcpp/publisher.hpp:187:3: note: no known conversion for argument 1 from ‘sensor_msgs ...