d.cpp

基于无人机 ROS（机器人操作系统）的视觉控制程序，利用了 SpireCV SDK 进行目标检测和跟踪。主要功能是从相机获取图像，进行目标检测，如果检测到特定目标，则可以通过鼠标点击开始对该目标进行自动跟踪。

1. 头文件和命名空间

#include <iostream>
#include <string>
// 包含 SpireCV SDK 头文件
#include <sv_world.h>
#include <ros/ros.h>
#include <spirecv_msgs/TargetsInFrame.h>
#include <spirecv_msgs/Target.h>
#include <spirecv_msgs/ROI.h>
#include <spirecv_msgs/Control.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

using namespace std;

包含了标准的 C++ 库，ROS 库，以及 SpireCV SDK 的头文件。
使用 namespace std 方便后续代码的编写。

2. 全局变量和函数声明

// 定义窗口名称
static const std::string RGB_WINDOW = "Image window";
// 框选到的矩形
cv::Rect rect_sel;
// 框选起始点
cv::Point pt_origin;

// 是否得到一个新的框选区域
bool b_renew_ROI = false;
// 是否开始跟踪
bool b_begin_TRACK = false;

cv::Mat ros_img;
bool get_ros_img = false;

int img_width, img_height;

//【发布】检测结果图像
image_transport::Publisher aruco_pub;
ros::Publisher spirecv_msg_pub;

// 实现框选逻辑的回调函数
void onMouse(int event, int x, int y, int, void *);
void remoteMouse(spirecv_msgs::Control ctl_msg);

// 声明了用于计算点是否在矩形内的函数
double getCross(node p1, node p2, node p);

定义了一些全局变量，用于控制程序的状态和存储数据。
声明了鼠标回调函数 onMouse 和远程鼠标回调函数 remoteMouse，用于处理用户的输入。

3. 相机回调函数

void cameraCallback(const sensor_msgs::ImageConstPtr &msg)
{
  cv_bridge::CvImagePtr cam_image;
  try
  {
    cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    cam_image->image.copyTo(ros_img);

    get_ros_img = true;
  }
  catch (cv_bridge::Exception &e)
  {
    ROS_ERROR("cv_bridge exception: %s", e.what());
  }
}

订阅相机话题的回调函数，将 ROS 图像消息转换为 OpenCV 图像。
设置标志 get_ros_img = true，表示已经获取到新的图像。

4. 点和几何计算

struct node
{
  double x, y;
};

node p1, p2, p3, p4;
node p;

double getCross(node p1, node p2, node p)
{
  return (p2.x - p1.x) * (p.y - p1.y) - (p.x - p1.x) * (p2.y - p1.y);
}

定义了一个 node 结构体，表示二维点。
定义了 getCross 函数，用于计算叉积，判断点是否在矩形内。

5. 鼠标回调函数

void onMouse(int event, int x, int y, int, void *)
{
  if (b_clicked)
  {
    // 更新框选区域坐标
    pt_origin.x = -1;
    pt_origin.y = -1;
  }
  // 左键按下
  if (event == cv::EVENT_LBUTTONDOWN)
  {
    detect_tracking = true;
    pt_origin = cv::Point(x, y);
  }
  else if (event == cv::EVENT_RBUTTONDOWN)
  {
    detect_tracking = true;
    b_renew_ROI = false;
    b_begin_TRACK = false;
    b_clicked = true;
  }
}

处理鼠标事件：
- 左键点击时，记录点击位置，并设置 detect_tracking = true，表示重新进入检测模式。
- 右键点击时，退出跟踪模式，重新进入检测模式。

6. 远程鼠标回调函数

void remoteMouse(spirecv_msgs::Control ctl_msg)
{
  if (b_clicked)
  {
    // 更新框选区域坐标
    pt_origin.x = -1;
    pt_origin.y = -1;
  }
  // 左键按下
  if (ctl_msg.mouse == spirecv_msgs::Control::MOUSE_LEFT)
  {
    detect_tracking = true;
    pt_origin = cv::Point(ctl_msg.x * img_width, ctl_msg.y * img_height);
  }
  else if (ctl_msg.mouse == spirecv_msgs::Control::MOUSE_RIGHT)
  {
    detect_tracking = true;
    b_renew_ROI = false;
    b_begin_TRACK = false;
    b_clicked = true;
  }
}

与鼠标回调函数类似，但这是用于接收远程控制指令，通过 ROS 话题接收鼠标事件。

7. 主函数

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "car_detection_with_tracking");
  ros::NodeHandle nh("~");
  // 更新频率为60HZ
  ros::Rate loop_rate(60);
  int uav_id = 1;
  std::string camera_params_yaml = "";
  std::string local_saving_path = "";
  std::string input_image_topic = "";
  std::string output_image_topic = "";

  // 初始化一些参数
  nh.getParam("uav_id", uav_id);
  nh.getParam("camera_parameters", camera_params_yaml);
  nh.getParam("local_saving_path", local_saving_path);
  nh.getParam("input_image_topic", input_image_topic);
  nh.getParam("output_image_topic", output_image_topic);

  // 打印参数信息
  printf("UAV_ID: %d\n", uav_id);
  printf("CAMERA_PARAMS: %s\n", camera_params_yaml.c_str());
  printf("LOCAL_SAVING_PATH: %s\n", local_saving_path.c_str());
  printf("INPUT_IMAGE_TOPIC: %s\n", input_image_topic.c_str());
  printf("OUTPUT_IMAGE_TOPIC: %s\n", output_image_topic.c_str());

  // 初始化图像传输和订阅者、发布者
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber image_subscriber;
  image_subscriber = it.subscribe(input_image_topic, 10, cameraCallback);
  ros::Publisher spirecv_msg_pub = nh.advertise<spirecv_msgs::TargetsInFrame>("/uav" + std::to_string(uav_id) + "/spirecv/car_detection_with_tracking", 1);

  ros::Subscriber spirecv_ctl_sub = nh.subscribe("/uav" + std::to_string(uav_id) + "/spirecv/control", 10, remoteMouse);

  aruco_pub = it.advertise(output_image_topic.c_str(), 1);

  // 定义一个新的窗口，可在上面进行框选操作
  cv::namedWindow(RGB_WINDOW);
  // 设置窗口操作回调函数，该函数实现整个框选逻辑
  cv::setMouseCallback(RGB_WINDOW, onMouse, 0);

  // 实例化框选目标跟踪类和通用目标检测器
  sv::SingleObjectTracker sot;
  sv::CommonObjectDetector cod;

  // 加载相机参数
  sot.loadCameraParams(camera_params_yaml);
  cod.loadCameraParams(camera_params_yaml);

  // 初始化图像尺寸
  img_height = sot.image_height;
  img_width = sot.image_width;

  // 实例化视频流传输器
  sv::VideoStreamer streamer;
  streamer.setup(cv::Size(img_width, img_height), 8554, 2);

  // 主循环
  while (ros::ok())
  {
    if (get_ros_img == true)
    {
      get_ros_img = false;
      img = ros_img.clone();

      if (detect_tracking == true)
      {
        // 如果处于检测模式
        // 实例化目标帧
        sv::TargetsInFrame tgts(frame_id++);

        // 调整图像尺寸
        cv::resize(img, img, cv::Size(cod.image_width, cod.image_height));

        // 初始化跟踪器
        rect_sel = cv::Rect(0, 0, 4, 4);  
        sot.init(img, rect_sel);

        // 执行通用目标检测
        cod.detect(img, tgts);

        // 可视化检测结果
        sv::drawTargetsInFrame(img, tgts);

        // 将检测结果转换为 ROS 消息并发布
        spirecv_msgs::TargetsInFrame ros_tgts;
        // ...（设置 ros_tgts 的各个字段）
        spirecv_msg_pub.publish(ros_tgts);

        // 打印检测结果到控制台
        // ...（打印 tgts 的信息）

        // 检查鼠标点击位置是否在检测到的目标内
        for (int i = 0; i < tgts.targets.size(); i++)
        {
          // 计算目标的四个顶点
          // ...（设置 p1, p2, p3, p4）

          // 判断点击点是否在目标矩形内
          if (getCross(p1, p2, p) * getCross(p3, p4, p) >= 0 && getCross(p2, p3, p) * getCross(p4, p1, p) >= 0)
          {
            b_begin_TRACK = false;
            detect_tracking = false;
            rect_sel = cv::Rect(p1.x, p1.y, tgts.targets[i].w * tgts.width, tgts.targets[i].h * tgts.height);
            b_renew_ROI = true;
            frame_id = 0;
            printf("spirecv mode is tracked!\n");
          }
          else
          {
            printf("spirecv mode is detection!\n");
          }
        }
      }
      else
      {
        // 如果处于跟踪模式
        sv::TargetsInFrame tgts(frame_id++);
        cv::resize(img, img, cv::Size(sot.image_width, sot.image_height));

        if (b_renew_ROI)
        {
          // 初始化跟踪器
          sot.init(img, rect_sel);
          b_renew_ROI = false;
          b_begin_TRACK = true;
        }
        else if (b_begin_TRACK)
        {
          // 继续跟踪
          sot.track(img, tgts);

          // 可视化跟踪结果
          sv::drawTargetsInFrame(img, tgts);

          // 将跟踪结果转换为 ROS 消息并发布
          spirecv_msgs::TargetsInFrame ros_tgts;
          // ...（设置 ros_tgts 的各个字段）
          spirecv_msg_pub.publish(ros_tgts);

          // 打印跟踪结果到控制台
          // ...（打印 tgts 的信息）
        }
      }

      // 流式传输图像
      streamer.stream(img);

      // 显示图像
      cv::imshow(RGB_WINDOW, img);
      cv::waitKey(10);

      // 发布图像消息
      sensor_msgs::ImagePtr det_output_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
      aruco_pub.publish(det_output_msg);
    }

    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}

初始化 ROS 节点和参数：获取无人机 ID，相机参数文件路径，本地保存路径，输入和输出图像话题等参数。
订阅和发布：订阅相机图像话题和控制指令话题，发布检测和跟踪结果，以及输出的图像。
窗口和回调函数：创建 OpenCV 窗口，用于显示图像，并设置鼠标回调函数。
加载相机参数：为检测器和跟踪器加载相机参数。
主循环：
- 检查是否有新的图像。
- 如果处于检测模式：
  - 使用通用目标检测器进行目标检测。
  - 可视化检测结果。
  - 将检测结果发布为 ROS 消息。
  - 检查鼠标点击位置，如果点击在目标上，则切换到跟踪模式。
- 如果处于跟踪模式：
  - 使用单目标跟踪器进行跟踪。
  - 可视化跟踪结果。
  - 将跟踪结果发布为 ROS 消息。
  - 如果需要，可以通过右键点击退出跟踪模式。
- 显示图像并发布图像消息。

y.cpp

基于无人机 ROS（机器人操作系统）的目标跟踪程序，使用了 SpireCV 和 Prometheus 的消息类型，实现了对目标的检测和跟踪。通过订阅视觉检测结果，计算控制指令，将其发布给无人机控制模块，从而实现无人机对目标的自主跟踪。

1. 头文件和命名空间

#include <ros/ros.h>
#include <sstream>
#include <Eigen/Eigen>
#include <iostream>
#include <prometheus_msgs/UAVState.h>
#include <prometheus_msgs/UAVCommand.h>
#include <prometheus_msgs/UAVControlState.h>
#include <spirecv_msgs/TargetsInFrame.h>
#include <spirecv_msgs/Target.h>
#include <spirecv_msgs/ROI.h>
// #include <prometheus_msgs/Target.h>
// #include <prometheus_msgs/TargetsInFrame.h>
// #include <prometheus_msgs/ROI.h>
#include <mission_utils.h>
#include "printf_utils.h"

using namespace std;
using namespace Eigen;

#define NODE_NAME "yolov5_tracking"

包含的头文件：
- ROS 相关的头文件，如 ros/ros.h。
- Eigen 库用于线性代数计算。
- Prometheus 和 SpireCV 的自定义消息类型，用于无人机状态和目标信息的通信。
- 一些工具类头文件，如 mission_utils.h 和 printf_utils.h，用于辅助功能。
命名空间：
- 使用了标准命名空间 std 和 Eigen 命名空间 Eigen，方便后续代码编写。
宏定义：
- 定义了节点名称 NODE_NAME，用于 ROS 节点的初始化。

2. 全局变量声明

prometheus_msgs::UAVState g_UAVState;
Eigen::Vector3f g_drone_pos;
prometheus_msgs::UAVControlState g_uavcontrol_state;
spirecv_msgs::Target g_Detection_raw;
prometheus_msgs::UAVCommand g_command_now;
Eigen::Vector3f pos_body_frame;
Eigen::Vector3f pos_body_enu_frame;
float kpx_track, kpy_track, kpz_track;
bool is_detected = false;
int num_count_vision_lost = 0;
int num_count_vision_regain = 0;
int g_uav_id;
int Thres_vision = 0;
Eigen::Vector3f camera_offset;
float distance_to_setpoint;
Eigen::Vector3f tracking_delta;
Eigen::Vector3d car_pose_;
int car_id;
spirecv_msgs::TargetsInFrame det;
std::ostringstream info;

无人机状态相关变量：
- g_UAVState：无人机的当前状态，包括位置、姿态等。
- g_drone_pos：无人机的位置向量。
控制相关变量：
- g_uavcontrol_state：无人机的控制状态，表示当前无人机的控制模式。
- g_command_now：将要发送给无人机的控制命令。
目标检测相关变量：
- g_Detection_raw：最新的目标检测结果。
- is_detected：标志是否检测到目标。
控制参数：
- kpx_track, kpy_track, kpz_track：控制器的比例增益，用于计算控制指令。
- tracking_delta：无人机与目标之间的期望距离偏移。
其他变量：
- num_count_vision_lost、num_count_vision_regain：用于计数视觉丢失和重新检测到的次数。
- camera_offset：相机相对于无人机中心的偏移量。
- Thres_vision：视觉丢失计数器的阈值。
- det：存储目标检测的消息。
- info：用于格式化输出信息的字符串流。

3. 无人机状态回调函数

void droneStateCb(const prometheus_msgs::UAVState::ConstPtr &msg)
{
    g_UAVState = *msg;
    g_drone_pos[0] = g_UAVState.position[0];
    g_drone_pos[1] = g_UAVState.position[1];
    g_drone_pos[2] = g_UAVState.position[2];
}

功能：订阅无人机的状态信息，更新全局变量 g_UAVState 和无人机位置 g_drone_pos。
参数：msg，来自 /prometheus/state 话题的消息，包含无人机的状态信息。

4. 视觉检测回调函数

void VisionCb(const spirecv_msgs::TargetsInFrame::ConstPtr &msg)
{
    det = *msg;
    car_id = det.frame_id;
    g_Detection_raw.mode = false;
    for(auto &tar : msg->targets)
    {
        // if(!tar.mode)
        //     continue;
        g_Detection_raw = tar;
        // car_pose_[0] = tar.px;
        // car_id = tar.tracked_id;

        // cout << "132456 is :" << car_pose_[0] << endl;
        // break;
    }
}

功能：订阅来自视觉检测节点的目标信息，更新全局变量 g_Detection_raw。
参数：msg，来自 /spirecv/car_detection_with_tracking 话题的消息，包含检测到的目标列表。
逻辑：
- 将接收到的消息赋值给全局变量 det。
- 提取 frame_id，存储到 car_id。
- 初始化 g_Detection_raw.mode 为 false，表示当前未检测到目标。
- 遍历 msg->targets，如果找到目标，则将其赋值给 g_Detection_raw。
- （注释掉的代码表明，可能曾经有过滤 tar.mode 的逻辑。）

5. 主函数

int main(int argc, char **argv)
{
    ros::init(argc, argv, "yolov5_tracking");
    ros::NodeHandle nh;

    ros::Rate rate(25);

    //视觉丢失次数阈值
    nh.param<int>("Thres_vision", Thres_vision, 10);

    //追踪的前后间隔
    nh.param<float>("tracking_delta_x", tracking_delta[0], 2);
    nh.param<float>("tracking_delta_y", tracking_delta[1], 0.0);
    nh.param<float>("tracking_delta_z", tracking_delta[2], 2);

    nh.param<float>("camera_offset_x", camera_offset[0], 0.0);
    nh.param<float>("camera_offset_y", camera_offset[1], 0.0);
    nh.param<float>("camera_offset_z", camera_offset[2], 0.0);

    //追踪控制参数
    nh.param<float>("kpx_track", kpx_track, 0.1);
    nh.param<float>("kpy_track", kpy_track, 0.1);
    nh.param<float>("kpz_track", kpz_track, 0.1);

    nh.param<int>("uav_id", g_uav_id, 1);

    // 订阅无人机状态、视觉检测结果、无人机控制状态
    ros::Subscriber curr_pos_sub = nh.subscribe<prometheus_msgs::UAVState>("/uav" + std::to_string(g_uav_id) + "/prometheus/state", 10, droneStateCb);
    ros::Subscriber vision_sub = nh.subscribe<spirecv_msgs::TargetsInFrame>("/uav" + std::to_string(g_uav_id) + "/spirecv/car_detection_with_tracking", 10, VisionCb);
    ros::Subscriber uav_control_state_sub = nh.subscribe<prometheus_msgs::UAVControlState>(
        "/uav" + std::to_string(g_uav_id) + "/prometheus/control_state", 10,
        [&](const prometheus_msgs::UAVControlState::ConstPtr &msg) -> void
        {
            g_uavcontrol_state = *msg;
        });

    // 发布控制指令
    ros::Publisher command_pub = nh.advertise<prometheus_msgs::UAVCommand>("/uav" + std::to_string(g_uav_id) + "/prometheus/command", 10);

    while (ros::ok())
    {
        ros::spinOnce();

        // 检查无人机是否处于 COMMAND_CONTROL 模式
        if (g_uavcontrol_state.control_state != prometheus_msgs::UAVControlState::COMMAND_CONTROL)
        {
            PCOUT(-1, WHITE, "Waiting for enter COMMAND_CONTROL state");
        }

        // 如果未检测到目标
        if (!g_Detection_raw.mode)
        {
            g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Current_Pos_Hover;
            PCOUT(-1, GREEN, "Waiting for click target!");
        }
        else
        {
            // 计算无人机与目标的距离
            distance_to_setpoint = pos_body_frame.norm();

            // 设置无人机的控制指令为移动模式
            g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Move;
            g_command_now.Move_mode = prometheus_msgs::UAVCommand::XYZ_VEL_BODY;

            // 根据目标的位置计算速度指令
            g_command_now.velocity_ref[0] = 0.5 * (g_Detection_raw.pz - 2.0);
            g_command_now.velocity_ref[1] = -0.8 * g_Detection_raw.px;

            // 根据目标的视线角调整垂直速度
            if (g_Detection_raw.los_ay <= -20)
            {
                g_command_now.velocity_ref[2] = 0.8 * g_Detection_raw.py;
            }
            else if (g_Detection_raw.los_ay >= -15)
            {
                g_command_now.velocity_ref[2] = -0.8 * g_Detection_raw.py;
            }
            else
            {
                g_command_now.velocity_ref[2] = 0;
            }

            g_command_now.yaw_ref = 0;
            PCOUT(-1, GREEN, "target tracking!");
        }

        // 更新控制指令的时间戳和命令 ID
        g_command_now.header.stamp = ros::Time::now();
        g_command_now.Command_ID = g_command_now.Command_ID + 1;

        // 在初始阶段，先悬停一段时间
        if (g_command_now.Command_ID < 10)
            g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Init_Pos_Hover;

        // 发布控制指令
        command_pub.publish(g_command_now);

        rate.sleep();
    }

    return 0;
}

节点初始化：使用 ros::init 初始化节点，节点名称为 "yolov5_tracking"。
参数获取：
- 使用 nh.param 获取参数服务器上的参数，如视觉丢失阈值、追踪控制参数、无人机 ID 等。
话题订阅和发布：
- 订阅无人机状态、视觉检测结果、无人机控制状态。
- 发布控制指令到 /prometheus/command。
主循环：
- 使用 ros::spinOnce() 处理回调函数。
- 检查无人机是否处于 COMMAND_CONTROL 模式，如果不是，等待进入该模式。
- 根据是否检测到目标，决定无人机的控制指令：
  - 未检测到目标：发送悬停指令 Current_Pos_Hover，等待目标出现。
  - 检测到目标：计算无人机需要的速度指令，跟踪目标。
    - 水平速度根据目标的位置 g_Detection_raw.px 和 g_Detection_raw.pz 计算。
    - 垂直速度根据目标的视线角 g_Detection_raw.los_ay 进行调整。
- 更新控制指令的时间戳和命令 ID。
- 在初始阶段（命令 ID 小于 10），发送初始化悬停指令 Init_Pos_Hover。
- 发布控制指令 g_command_now。