detect_topic

1.接收ros图像

2.yolo识别

3.sort跟踪

 yolo.h

#ifndef YOLO_H
#define YOLO_H

#include 
#include 
#include "NvInfer.h"
#include "NvInferPlugin.h"
#include "NvInferRuntimeCommon.h"
#include "NvOnnxParser.h"

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using nvinfer1::Dims2;
using nvinfer1::Dims3;
using nvinfer1::IBuilder;
using nvinfer1::IBuilderConfig;
using nvinfer1::ICudaEngine;
using nvinfer1::IExecutionContext;
using nvinfer1::IHostMemory;
using nvinfer1::ILogger;
using nvinfer1::INetworkDefinition;
using Severity = nvinfer1::ILogger::Severity;

using cv::Mat;
using std::array;
using std::cout;
using std::endl;
using std::ifstream;
using std::ios;
using std::ofstream;
using std::string;
using std::vector;


class Logger : public ILogger {
 public:
  void log(Severity severity, const char* msg) noexcept override {
    if (severity != Severity::kINFO) {
      std::cout << msg << std::endl;
    }
  }
};



class Yolo {
 public:
  Yolo(char* model_path);
  float letterbox(
      const cv::Mat& image,
      cv::Mat& out_image,
      const cv::Size& new_shape,
      int stride,
      const cv::Scalar& color,
      bool fixed_shape,
      bool scale_up);
  float* blobFromImage(cv::Mat& img);
  void draw_objects(const cv::Mat& img, float* Boxes, int* ClassIndexs, int* BboxNum, float* Scores);
  void Init(char* model_path);
  void Infer(
      int aWidth,
      int aHeight,
      int aChannel,
      unsigned char* aBytes,
      float* Boxes,
      int* ClassIndexs,
      int* BboxNum,
      float* Scores);
  ~Yolo();

 private:
  nvinfer1::ICudaEngine* engine = nullptr;
  nvinfer1::IRuntime* runtime = nullptr;
  nvinfer1::IExecutionContext* context = nullptr;
  cudaStream_t stream = nullptr;
  void* buffs[5];
  int iH, iW, in_size, out_size1, out_size2, out_size3, out_size4;
  Logger gLogger;
};

#endif // YOLO_H

yolo.cpp

#include "yolo.h"

float Yolo::letterbox(
    const cv::Mat& image,
    cv::Mat& out_image,
    const cv::Size& new_shape = cv::Size(640, 640),
    int stride = 32,
    const cv::Scalar& color = cv::Scalar(114, 114, 114),
    bool fixed_shape = false,
    bool scale_up = true) {
  cv::Size shape = image.size();
  float r = std::min(
      (float)new_shape.height / (float)shape.height, (float)new_shape.width / (float)shape.width);
  if (!scale_up) {
    r = std::min(r, 1.0f);
  }

  int newUnpad[2]{
      (int)std::round((float)shape.width * r), (int)std::round((float)shape.height * r)};

  cv::Mat tmp;
  if (shape.width != newUnpad[0] || shape.height != newUnpad[1]) {
    cv::resize(image, tmp, cv::Size(newUnpad[0], newUnpad[1]));
  } else {
    tmp = image.clone();
  }

  float dw = new_shape.width - newUnpad[0];
  float dh = new_shape.height - newUnpad[1];

  if (!fixed_shape) {
    dw = (float)((int)dw % stride);
    dh = (float)((int)dh % stride);
  }

  dw /= 2.0f;
  dh /= 2.0f;

  int top = int(std::round(dh - 0.1f));
  int bottom = int(std::round(dh + 0.1f));
  int left = int(std::round(dw - 0.1f));
  int right = int(std::round(dw + 0.1f));
  cv::copyMakeBorder(tmp, out_image, top, bottom, left, right, cv::BORDER_CONSTANT, color);

  return 1.0f / r;
}

float* Yolo::blobFromImage(cv::Mat& img) {
  float* blob = new float[img.total() * 3];
  int channels = 3;
  int img_h = img.rows;
  int img_w = img.cols;
  for (size_t c = 0; c < channels; c++) {
    for (size_t h = 0; h < img_h; h++) {
      for (size_t w = 0; w < img_w; w++) {
        blob[c * img_w * img_h + h * img_w + w] = (float)img.at(h, w)[c] / 255.0;
      }
    }
  }
  return blob;
}

void Yolo::draw_objects(const cv::Mat& img, float* Boxes, int* ClassIndexs, int* BboxNum, float* Scores) {
  for (int j = 0; j < BboxNum[0]; j++) {
    cv::Rect rect(Boxes[j * 4], Boxes[j * 4 + 1], Boxes[j * 4 + 2], Boxes[j * 4 + 3]);
    cv::rectangle(img, rect, cv::Scalar(0x27, 0xC1, 0x36), 2);
    cv::putText(
        img,
        std::to_string(ClassIndexs[j])+":"+std::to_string(Scores[j]),
        cv::Point(rect.x, rect.y - 1),
        cv::FONT_HERSHEY_PLAIN,
        1.2,
        cv::Scalar(0xFF, 0xFF, 0xFF),
        2);
    //cv::imwrite("result.jpg", img);
  }
}

Yolo::Yolo(char* model_path) {
  ifstream ifile(model_path, ios::in | ios::binary);
  if (!ifile) {
    cout << "read serialized file failed\n";
    std::abort();
  }

  ifile.seekg(0, ios::end);
  const int mdsize = ifile.tellg();
  ifile.clear();
  ifile.seekg(0, ios::beg);
  vector buf(mdsize);
  ifile.read(&buf[0], mdsize);
  ifile.close();
  cout << "model size: " << mdsize << endl;

  runtime = nvinfer1::createInferRuntime(gLogger);
  initLibNvInferPlugins(&gLogger, "");
  engine = runtime->deserializeCudaEngine((void*)&buf[0], mdsize, nullptr);
  auto in_dims = engine->getBindingDimensions(engine->getBindingIndex("images"));
  iH = in_dims.d[2];
  iW = in_dims.d[3];
  in_size = 1;
  for (int j = 0; j < in_dims.nbDims; j++) {
    in_size *= in_dims.d[j];
  }
  auto out_dims1 = engine->getBindingDimensions(engine->getBindingIndex("num_dets"));
  out_size1 = 1;
  for (int j = 0; j < out_dims1.nbDims; j++) {
    out_size1 *= out_dims1.d[j];
  }
  auto out_dims2 = engine->getBindingDimensions(engine->getBindingIndex("det_boxes"));
  out_size2 = 1;
  for (int j = 0; j < out_dims2.nbDims; j++) {
    out_size2 *= out_dims2.d[j];
  }
  auto out_dims3 = engine->getBindingDimensions(engine->getBindingIndex("det_scores"));
  out_size3 = 1;
  for (int j = 0; j < out_dims3.nbDims; j++) {
    out_size3 *= out_dims3.d[j];
  }
  auto out_dims4 = engine->getBindingDimensions(engine->getBindingIndex("det_classes"));
  out_size4 = 1;
  for (int j = 0; j < out_dims4.nbDims; j++) {
    out_size4 *= out_dims4.d[j];
  }
  context = engine->createExecutionContext();
  if (!context) {
    cout << "create execution context failed\n";
    std::abort();
  }

  cudaError_t state;
  state = cudaMalloc(&buffs[0], in_size * sizeof(float));
  if (state) {
    cout << "allocate memory failed\n";
    std::abort();
  }
  state = cudaMalloc(&buffs[1], out_size1 * sizeof(int));
  if (state) {
    cout << "allocate memory failed\n";
    std::abort();
  }

  state = cudaMalloc(&buffs[2], out_size2 * sizeof(float));
  if (state) {
    cout << "allocate memory failed\n";
    std::abort();
  }

  state = cudaMalloc(&buffs[3], out_size3 * sizeof(float));
  if (state) {
    cout << "allocate memory failed\n";
    std::abort();
  }

  state = cudaMalloc(&buffs[4], out_size4 * sizeof(int));
  if (state) {
    cout << "allocate memory failed\n";
    std::abort();
  }

  state = cudaStreamCreate(&stream);
  if (state) {
    cout << "create stream failed\n";
    std::abort();
  }
}

void Yolo::Infer(
    int aWidth,
    int aHeight,
    int aChannel,
    unsigned char* aBytes,
    float* Boxes,
    int* ClassIndexs,
    int* BboxNum,
    float* Scores) {
  cv::Mat img(aHeight, aWidth, CV_MAKETYPE(CV_8U, aChannel), aBytes);
  cv::Mat pr_img;
  float scale = letterbox(img, pr_img, {iW, iH}, 32, {114, 114, 114}, true);
  cv::cvtColor(pr_img, pr_img, cv::COLOR_BGR2RGB);
  float* blob = blobFromImage(pr_img);

  static int* num_dets = new int[out_size1];
  static float* det_boxes = new float[out_size2];
  static float* det_scores = new float[out_size3];
  static int* det_classes = new int[out_size4];

  cudaError_t state =
      cudaMemcpyAsync(buffs[0], &blob[0], in_size * sizeof(float), cudaMemcpyHostToDevice, stream);
  if (state) {
    cout << "transmit to device failed\n";
    std::abort();
  }
  context->enqueueV2(&buffs[0], stream, nullptr);
  state =
      cudaMemcpyAsync(num_dets, buffs[1], out_size1 * sizeof(int), cudaMemcpyDeviceToHost, stream);
  if (state) {
    cout << "transmit to host failed \n";
    std::abort();
  }
  state = cudaMemcpyAsync(
      det_boxes, buffs[2], out_size2 * sizeof(float), cudaMemcpyDeviceToHost, stream);
  if (state) {
    cout << "transmit to host failed \n";
    std::abort();
  }
  state = cudaMemcpyAsync(
      det_scores, buffs[3], out_size3 * sizeof(float), cudaMemcpyDeviceToHost, stream);
  if (state) {
    cout << "transmit to host failed \n";
    std::abort();
  }

  state = cudaMemcpyAsync(
      det_classes, buffs[4], out_size4 * sizeof(int), cudaMemcpyDeviceToHost, stream);
  if (state) {
    cout << "transmit to host failed \n";
    std::abort();
  }
  BboxNum[0] = num_dets[0];
  int img_w = img.cols;
  int img_h = img.rows;
  int x_offset = (iW * scale - img_w) / 2;
  int y_offset = (iH * scale - img_h) / 2;
  for (size_t i = 0; i < num_dets[0]; i++) {
    float x0 = (det_boxes[i * 4]) * scale - x_offset;
    float y0 = (det_boxes[i * 4 + 1]) * scale - y_offset;
    float x1 = (det_boxes[i * 4 + 2]) * scale - x_offset;
    float y1 = (det_boxes[i * 4 + 3]) * scale - y_offset;
    x0 = std::max(std::min(x0, (float)(img_w - 1)), 0.f);
    y0 = std::max(std::min(y0, (float)(img_h - 1)), 0.f);
    x1 = std::max(std::min(x1, (float)(img_w - 1)), 0.f);
    y1 = std::max(std::min(y1, (float)(img_h - 1)), 0.f);
    Boxes[i * 4] = x0;
    Boxes[i * 4 + 1] = y0;
    Boxes[i * 4 + 2] = x1 - x0;
    Boxes[i * 4 + 3] = y1 - y0;
    ClassIndexs[i] = det_classes[i];
    Scores[i] = det_scores[i];
  }
  delete blob;
}

Yolo::~Yolo() {
  cudaStreamSynchronize(stream);
  cudaFree(buffs[0]);
  cudaFree(buffs[1]);
  cudaFree(buffs[2]);
  cudaFree(buffs[3]);
  cudaFree(buffs[4]);
  cudaStreamDestroy(stream);
  context->destroy();
  engine->destroy();
  runtime->destroy();
}

 listenerMat.cpp

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include 
#include 
#include 
#include "utils.h"
#include "kalmanboxtracker.h"
#include "sort.h"
#include 
#include 
#include 

#include "std_msgs/msg/string.hpp"
 
using namespace std::chrono_literals;
using namespace std::chrono_literals;
using std::placeholders::_1;
//using std::placeholders::_1;
using std::placeholders::_2;

#include "yolo.h"


Yolo *myyolo;
sort *mot_tracker;
 
std::vector colors;

std::string msgstr = "";

cv::Mat detectResultMat = cv::Mat::zeros(1080,720,CV_8UC3);


void filter(float* Boxes,int* ClassIndexs,int* BboxNum,float* Scores,
            float conf_thres, std::vector classes,float *newBoxes,
                    int *newClassIndexs,int *newBboxNum,float *newScores)
{
    int count = 0;
    for (int i =0;iconf_thres){
        std::vector::iterator iter=std::find(classes.begin(),classes.end(),ClassIndexs[i]);
        if ( iter!=classes.end())
        {
          //cout << "Not found" << endl;
          for (int j=0;j<4;j++) {
            newBoxes[count*4+j]=Boxes[i*4+j];
            }

            newClassIndexs[count]=ClassIndexs[i];

            newScores[count]=Scores[i];

            count++;
        }
      }

    }

    newBboxNum[0]=count;
}

// 3.定义节点类；
class MinimalSubscriber : public rclcpp::Node
{
  public:
    MinimalSubscriber(std::string name)
    : Node(name)
    {
      srand(time(0));
      for (size_t i = 0; i < 100; i++)
        colors.push_back(cv::Vec3b(rand() % 255, rand() % 255, rand() % 255));

      mot_tracker = new sort(5, 4, 0.25);

      // 3-1.创建订阅方；
      //subscription_ = this->create_subscription("pubImageTopic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
      subscription_ = this->create_subscription("pubImageTopic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));


       // 3-1.创建发布方；
      msgPublisher_ = this->create_publisher("detectResultMsgtopic", 10);
      // 3-2.创建定时器；
      msgTimer = this->create_wall_timer(50ms, std::bind(&MinimalSubscriber::msg_timer_callback, this));

      //mat topic
      matPublisher_ = this->create_publisher("detectResultMatTopic", 10);
      matTimer = this->create_wall_timer(50ms, std::bind(&MinimalSubscriber::mat_timer_callback, this));
    }
 
  private:
    // 3-2.处理订阅到的消息；

    //rclcpp::Subscription subscription_;
    rclcpp::Subscription::SharedPtr subscription_;

    //void topic_callback(const sensor_msgs::msg::CompressedImage & msg) const

    rclcpp::TimerBase::SharedPtr msgTimer;
    rclcpp::Publisher::SharedPtr msgPublisher_;

    //std_msgs::msg::String detectResultMessage;// = std_msgs::msg::String();
    //detectResultMessage.data  = "null";


    rclcpp::TimerBase::SharedPtr matTimer;
    rclcpp::Publisher::SharedPtr matPublisher_;

    //sensor_msgs::msg::CompressedImage::SharedPtr detect_result_img_compressed_;// = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", des1080).toCompressedImageMsg();



    void msg_timer_callback(){
      // 3-3.组织消息并发布。
      auto detectResultMessage = std_msgs::msg::String();
      detectResultMessage.data = msgstr;
      RCLCPP_INFO(this->get_logger(), "发布的消息：'%s'", detectResultMessage.data.c_str());
      msgPublisher_ ->publish(detectResultMessage);
    }


    void mat_timer_callback(){
      sensor_msgs::msg::CompressedImage::SharedPtr detect_result_img_compressed_ = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", detectResultMat).toCompressedImageMsg();
      //RCLCPP_INFO(this->get_logger(), "发布的消息：检测图片");
      this->matPublisher_ ->publish(*detect_result_img_compressed_);
    }


    
    void topic_callback(const sensor_msgs::msg::CompressedImage::ConstPtr  msg) const
    {

    try
    {
      cv_bridge::CvImagePtr cv_ptr_compressed = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);
      cv::Mat imgCallback = cv_ptr_compressed->image;
      //cv::imshow("imgCallback",imgCallback);
      //cv::waitKey(1);

      cv::Mat des1080;
 
      //cv::resize(imgCallback, des1080, cv::Size(1920, 1080), 0, 0, cv::INTER_NEAREST);

      cv::resize(imgCallback, des1080, cv::Size(1080, 720), 0, 0, cv::INTER_NEAREST);


      //std::cout<<"cv_ptr_compressed: "<image.cols<<" h: "<image.rows<Infer(des1080.cols, des1080.rows, des1080.channels(), des1080.data, Boxes, ClassIndexs, BboxNum,Scores);
      //auto end = std::chrono::system_clock::now();

      //std::cout<(end-start).count()<<"ms"<draw_objects(des1080, Boxes, ClassIndexs, BboxNum, Scores);
      float newBoxes[400];
      int newBboxNum[1]; 
      int newClassIndexs[100]; 
      float newScores[100];
      float conf_thres = 0.4;
      std::vector classes;
      //classes.push_back(1);//识别车
      classes.push_back(0);//识别人

      filter(Boxes, ClassIndexs, BboxNum,Scores,conf_thres,classes,newBoxes,newClassIndexs,newBboxNum,newScores);
     
      std::vector detections;

      for (int i =0; i< newBboxNum[0]; i++){

           cv::Rect rect(newBoxes[i * 4], newBoxes[i * 4 + 1], newBoxes[i * 4 + 2], newBoxes[i * 4 + 3]);
           detections.push_back(rect);
       }

       std::vector> trackers = mot_tracker->update(detections);
       //qDebug()<detectResultMessage.data  = std::to_string(84);
          //msgstr = "sddh15151515";
          //std::string str
          msgstr = sendstr;

       }
       else{
          //msgstr = "null";
          msgstr = sendstr;
       }



      cv::imshow("des1080",des1080);
      cv::waitKey(1);


      detectResultMat = des1080;

      //auto end = std::chrono::system_clock::now();

      //std::cout<(end-start).count()<<"ms"<(str.c_str());
  myyolo = new Yolo(chr);
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc, argv);
  // 4.调用spin函数，并传入节点对象指针。
  auto node = std::make_shared("detect");
  rclcpp::spin(node);
  //rclcpp::spin(std::make_shared());
  // 5.释放资源；
  rclcpp::shutdown();
  return 0;
}

package.xml

  detect_topic
  0.0.0
  TODO: Package description
  tuya
  TODO: License declaration

  ament_cmake

  rclcpp
  std_msgs

  ament_lint_auto
  ament_lint_common

  
    ament_cmake
  

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(detect_topic)


add_definitions(-std=c++14)

# Default to C++14
#if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
#endif()


if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

find_package(cv_bridge REQUIRED)
find_package(image_transport)
find_package(sensor_msgs REQUIRED)

find_package(CUDA REQUIRED)

include_directories(${CUDA_INCLUDE_DIRS})

include_directories(/usr/local/cuda/include)
link_directories(/usr/local/cuda/lib64)
# tensorrt
include_directories(/usr/include/aarch64-linux-gnu/)
link_directories(/usr/lib/aarch64-linux-gnu/)

# tensorrt
#include_directories(/home/tuya/TensorRT-8.6.1.6/targets/x86_64-linux-gnu/include/)
#link_directories(/home/tuya/TensorRT-8.6.1.6/targets/x86_64-linux-gnu/lib/)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall -Ofast -Wfatal-errors -D_MWAITXINTRIN_H_INCLUDED")

add_executable(detect
  src/listenerMat.cpp
  src/yolo.h 
  src/yolo.cpp
  src/hungarian.h
  src/hungarian.cpp
  src/kalmanboxtracker.h
  src/kalmanboxtracker.cpp
  src/sort.h
  src/sort.cpp
  src/utils.h
  src/utils.cpp
)

find_package(OpenCV REQUIRED)
include_directories( ${OpenCV_INCLUDE_DIRS})
target_link_libraries(detect ${OpenCV_LIBS})

# ros
target_link_libraries(detect
  ${rclcpp_LIBRARIES} 
)

target_link_libraries(detect nvinfer)
target_link_libraries(detect nvinfer_plugin)
target_link_libraries(detect cudart)

ament_target_dependencies(detect
  rclcpp
  std_msgs
  sensor_msgs 
  cv_bridge 
  OpenCV 
  image_transport
)
 


# 安装可执行文件
install(TARGETS detect
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

SORT代码

sort demo_涂鸦c的博客-CSDN博客