56 lines
1.9 KiB
C++
56 lines
1.9 KiB
C++
#include <rclcpp/rclcpp.hpp>
|
|
#include <sensor_msgs/msg/image.hpp>
|
|
#include <sensor_msgs/msg/compressed_image.hpp>
|
|
#include <std_msgs/msg/header.hpp>
|
|
#include <cv_bridge/cv_bridge.hpp>
|
|
|
|
#include <opencv2/core.hpp>
|
|
#include <opencv2/imgproc.hpp>
|
|
|
|
|
|
#include <chrono>
|
|
|
|
using namespace std::chrono_literals;
|
|
|
|
|
|
namespace j7s {
|
|
class ImagePublisher : public rclcpp::Node {
|
|
public:
|
|
ImagePublisher(const rclcpp::NodeOptions& options);
|
|
private:
|
|
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Image>> m_publisher;
|
|
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::CompressedImage>> m_compressedPublisher;
|
|
rclcpp::TimerBase::SharedPtr m_timer;
|
|
|
|
std::tuple<sensor_msgs::msg::Image::SharedPtr, sensor_msgs::msg::CompressedImage::SharedPtr> draw_image();
|
|
};
|
|
};
|
|
|
|
namespace j7s {
|
|
ImagePublisher::ImagePublisher(const rclcpp::NodeOptions& options):
|
|
Node("image_publisher", options),
|
|
m_publisher(this->create_publisher<sensor_msgs::msg::Image>("image", 1)),
|
|
m_compressedPublisher(this->create_publisher<sensor_msgs::msg::CompressedImage>("image/compressed", 1)),
|
|
m_timer{}
|
|
{
|
|
m_timer = this->create_timer(10ms, [this]() -> void {
|
|
const auto [image, compressedImage] = draw_image();
|
|
m_publisher->publish(*image);
|
|
m_compressedPublisher->publish(*compressedImage);
|
|
});
|
|
}
|
|
|
|
std::tuple<sensor_msgs::msg::Image::SharedPtr, sensor_msgs::msg::CompressedImage::SharedPtr> ImagePublisher::draw_image()
|
|
{
|
|
cv::Mat mat = cv::Mat::zeros(200, 200, CV_8UC3);
|
|
|
|
std_msgs::msg::Header header;
|
|
header.stamp = this->now();
|
|
cv_bridge::CvImage image(header, "mono8", mat);
|
|
return std::tuple(image.toImageMsg(), image.toCompressedImageMsg());
|
|
}
|
|
};
|
|
|
|
#include <rclcpp_components/register_node_macro.hpp>
|
|
RCLCPP_COMPONENTS_REGISTER_NODE(j7s::ImagePublisher)
|