ROS2——发布图像话题_ros2发布图片

#include <iostream>
#include <numeric>

#include "cmdline.h"
#include "utils.h"
#include "detector.h"
#include "include.h"

#include <rclcpp/rclcpp.hpp>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/msg/image.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/header.h>

#include <opencv2/core/types.hpp>
#include <opencv2/core/hal/interface.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;
using std::placeholders::_1;

cv::Mat res_img, depth_mat, aligned_depth_mat;
std::vector<Detection> result;

// hyper-parameter setting
const std::string modelPath = "/home/nvidia/ros2_ws/src/yolo_cpp/yolov5s.onnx";
const std::string classNamesPath = "/home/nvidia/ros2_ws/src/yolo_cpp/models/coco.names";
const std::vector<std::string> classNames = utils::loadNames(classNamesPath);
const float confThreshold = 0.3f;
const float iouThreshold = 0.4f;
bool isGPU = false;

YOLODetector detector {nullptr};


class YOLONODE: public rclcpp::Node
{
    public:
       YOLONODE(std::string name)
       : Node(name)
       {
            /*initialize yolov7*/
            const char* modelPath = "/home/nvidia/ros2_ws/src/yolov7_cpp/src/assets/yolop-tiny-640-640.onnx";
            detector = YOLODetector(modelPath, isGPU, cv::Size(640, 640));
            /*create subscriber*/
            auto default_qos = rclcpp::QoS(rclcpp::SystemDefaultsQoS());
            // sub_img = this->create_subscription<sensor_msgs::msg::Image>("/camera/color/image_raw", 1, std::bind(&YOLONODE::imgCallBack, this, _1));
            sub_img = this->create_subscription<sensor_msgs::msg::Image>(
                            "/camera/color/image_raw", default_qos, std::bind(&YOLONODE::imgCallBack, this, _1));
            /*create publisher*/
            pub_objs = this->create_publisher<std_msgs::msg::String>("topic_yolo_objs", 10);
            pub_img = this->create_publisher<sensor_msgs::msg::Image>("res_img", 10);

       }
    
    private:
        cv::Mat doInference(cv::Mat image){
            result = detector.detect(image, confThreshold, iouThreshold);
            utils::visualizeDetection(image, result, classNames);
            return image;
        }

        void imgCallBack(sensor_msgs::msg::Image::SharedPtr msg)
        {
            // if(!rclcpp::ok())
            //     return;
            cv_bridge::CvImagePtr cv_ptr;
            cv_ptr = cv_bridge::toCvCopy(msg, "8UC3");
            cv::Mat color_mat;
            cv_ptr->image.copyTo(color_mat);
            // printf("subscribe image sucessfully!!!");
            /*yolov7 inference*/
            res_img = doInference(color_mat);
            std::string result_string = ",";
            // printf("inference image sucessfully!!!");
            for(int i = 0; i < result.size(); i++)
            {
                result_string += std::to_string(result[i].box.x) + "," + std::to_string(result[i].box.y) + "," + std::to_string(result[i].box.x + result[i].box.width) + "," + std::to_string(result[i].box.y + result[i].box.height);
            }
            std::cout << result_string << std::endl;
            std_msgs::msg::String message;
            message.data = result_string;
            pub_objs->publish(message);
            result_string = ",";

            sensor_msgs::msg::Image msg_img;
            cv_bridge::CvImage cv_img;
            cv_img.encoding = "bgr8";
            cv_img.header.stamp = this->now();
            cv_img.image = color_mat;
            
            cv_img.toImageMsg(msg_img);
            pub_img->publish(msg_img);
        }
        /*define subscription*/
        rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_img;
        /*define publisher*/
        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_objs;
        rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_img;
};



int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<YOLONODE>("yolo_node");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}




1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118

参考
https://github.com/ANI717/ros2_camera_publish/blob/main/ros2_camera_publish/camera_publish_function.py