wwcs-1314/src/gimbal_server.cpp

#include <ros/ros.h>
#include <string>
#include <thread>
#include <std_msgs/String.h>
#include <std_srvs/SetBool.h>
#include "gimbal.h"
#include "ViewLink.h"
#include "amov_gimbal.h"
#include "prometheus_gimbal_control/GimbalControl.h"
#include "prometheus_gimbal_control/GimbalState.h"
#include "prometheus_gimbal_control/VisionDiff.h"
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

// amovGimbal::gimbal *gimbal = nullptr;
// 定义一个全局的GimbalBase指针，用于后续对云台的控制
GimbalBase *g_gimbal = nullptr;

// 定义一个全局的GimbalState对象，可能用于发布云台的状态信息，但在这段代码中并未使用
prometheus_gimbal_control::GimbalState g_pub_info;

// 定义一个全局布尔变量，用于控制是否停止追踪（可能是某种目标追踪功能）
bool g_stop_track = false;

// 回调函数，当接收到GimbalControl消息时触发
void ctlCallback(const prometheus_gimbal_control::GimbalControl::ConstPtr &ctl)
{
    // 根据GimbalControl消息中的rpyMode字段判断并执行相应的云台控制操作
    switch (ctl->rpyMode)
    {
    case prometheus_gimbal_control::GimbalControl::manual:
        // 定义两个数组，一个用于存储角度控制（roll, yaw, pitch），一个用于存储速度值
        uint8_t rpy_ctl[3];
        double rpy_vel[3];

        // 从GimbalControl消息中提取roll, yaw, pitch的角度值
        rpy_ctl[0] = ctl->roll;
        rpy_ctl[1] = ctl->yaw;
        rpy_ctl[2] = ctl->pitch;

        // 从GimbalControl消息中提取rValue, pValue, yValue的速度值
        rpy_vel[0] = ctl->rValue;
        rpy_vel[1] = ctl->pValue;
        rpy_vel[2] = ctl->yValue;

        // 调用g_gimbal对象的ctl_gimbal方法，传入角度和速度值来控制云台
        g_gimbal->ctl_gimbal(rpy_ctl, rpy_vel);

        // 打印日志，显示当前处于手动模式，并显示角度和速度值
        ROS_INFO_STREAM("Manual "
                        << "rpy control mode: " << (int)rpy_ctl[0] << " " << (int)rpy_ctl[1] << " " << (int)rpy_ctl[2]
                        << " value: " << rpy_vel[0] << " " << rpy_vel[1] << " " << rpy_vel[2]);

        break;
    case prometheus_gimbal_control::GimbalControl::home:
        g_gimbal->home();
        ROS_INFO_STREAM("Back Home");
        break;
    case prometheus_gimbal_control::GimbalControl::hold:
        g_gimbal->hold();
        ROS_INFO_STREAM("Hold");
        break;
    case prometheus_gimbal_control::GimbalControl::fellow:
        g_gimbal->fellow();
        ROS_INFO_STREAM("Fellow");
        break;
    }
    switch (ctl->focusMode)
    {
    case prometheus_gimbal_control::GimbalControl::focusIn:
        g_gimbal->focus_in();
        ROS_INFO_STREAM("Focus In");
        break;
    case prometheus_gimbal_control::GimbalControl::focusOut:
        g_gimbal->focus_out();
        ROS_INFO_STREAM("Focus Out");
        break;
    case prometheus_gimbal_control::GimbalControl::focusStop:
        g_gimbal->focus_stop();
        ROS_INFO_STREAM("Focus Stop");
        break;
    }
    switch (ctl->zoomMode)
    {
    case prometheus_gimbal_control::GimbalControl::zoomIn:
        g_gimbal->zoom_in();
        ROS_INFO_STREAM("Zoom In");
        break;

    case prometheus_gimbal_control::GimbalControl::zoomOut:
        g_gimbal->zoom_out();
        ROS_INFO_STREAM("Zoom Out");
        break;
    case prometheus_gimbal_control::GimbalControl::zoomStop:
        g_gimbal->zoom_stop();
        ROS_INFO_STREAM("Zoom Stop");
        break;
    }
};

// 这个回调函数用于处理视觉差分信息，并根据这些信息控制云台追踪目标。
void trackCallback(const prometheus_gimbal_control::VisionDiff::ConstPtr &cent)
{
    // 如果已经设置了停止追踪的标志，则直接返回，不执行追踪。
    if (g_stop_track)
        return;

    // 定义一个临时数组，用于存储目标中心点的坐标偏移量。
    float tmp_cent[2]{0, 0};
    // 计算目标中心点在图像中的x坐标偏移量。
    tmp_cent[0] = cent->objectX + cent->objectWidth / 2 - cent->frameWidth / 2;
    // 计算目标中心点在图像中的y坐标偏移量。
    tmp_cent[1] = cent->objectY + cent->objectHeight / 2 - cent->frameHeight / 2;
    // 获取云台的滚动模式。
    int roll_mode = cent->ctlMode;
    // 获取PID控制参数：比例系数kp。
    float kp = cent->kp;
    // 获取PID控制参数：积分系数ki。
    float ki = cent->ki;
    // 获取PID控制参数：微分系数kd。
    float kd = cent->kd;

    // 定义静态变量，用于记录是否进行了变焦操作。
    static bool flag_zoom = false;
    // 定义静态变量，用于记录是否进行了保持操作。
    static bool flag_hold = false;

    // 如果检测到目标。
    if (cent->detect != 0)
    {
        // 根据当前大小和最大变焦倍数计算变焦值。
        int zoom_val = cent->currSize ? std::fmax(g_gimbal->zoom_val, 1) : 1;
        // 根据变焦值计算缩放比例。
        float scale = std::fmax(g_gimbal->max_zoom - zoom_val, 1.) / g_gimbal->max_zoom;
        // 使用计算出的参数调用云台追踪函数。
        g_gimbal->tracking(tmp_cent, roll_mode, kp * scale, ki, kd, cent->trackIgnoreError * zoom_val / g_gimbal->max_zoom);
        // 如果设置了自动变焦，并且目标在可忽略误差范围内，则进行自动变焦。
        if (cent->autoZoom && cent->currSize && tmp_cent[0] < cent->trackIgnoreError && tmp_cent[1] < cent->trackIgnoreError)
        {
            g_gimbal->auto_zoom(cent->currSize, zoom_val, cent->maxSize, cent->minSize);
            flag_zoom = true;
        }
        // 设置保持标志为真。
        flag_hold = true;
    }
    else
    {
        // 如果检测不到目标，并且之前进行了变焦操作，则停止变焦。
        if (cent->currSize && flag_zoom)
        {
            g_gimbal->zoom_stop();
            flag_zoom = false;
        }
        // 如果之前进行了保持操作，则调用云台保持函数。
        if (flag_hold)
        {
            g_gimbal->hold();
            flag_hold = false;
        }
    }
}

void currSizeCallback(const prometheus_gimbal_control::VisionDiff::ConstPtr &info)
{
    static bool flag = false;
    int now_flag = 0;
    int zoom_val = std::fmax(g_gimbal->zoom_val, 1);
    if (info->detect != 0 && (std::abs(g_pub_info.imuAngleVel[0]) + std::abs(g_pub_info.imuAngleVel[1]) + std::abs(g_pub_info.imuAngleVel[2])) < 3)
    {
        g_gimbal->auto_zoom(info->currSize, zoom_val, info->maxSize, info->minSize);
        flag = true;
    }
    else
    {
        if (flag)
        {
            g_gimbal->zoom_stop();
            flag = false;
        }
    }
}

// 这个函数是一个服务回调函数，用于处理视频录制的开始和停止请求。
bool videoRecord(std_srvs::SetBool::Request &ref, std_srvs::SetBool::Response &req)
{
    // 如果请求的内容是开始录制视频。
    if (ref.data)
    {
        // 调用云台对象的开始录制视频函数。
        g_gimbal->start_record();
        // 设置响应的成功标志为视频流是否已成功打开。
        req.success = g_gimbal->outvideo.isOpened();
        // 打印信息到ROS日志。
        ROS_INFO_STREAM("Start Video Record ! ");
    }
    else
    {
        // 如果请求的内容是停止录制视频。
        // 如果视频流没有打开，则直接返回失败响应。
        if (!g_gimbal->outvideo.isOpened())
        {
            req.success = false;
            req.message = "video record not is started ! ";
            ROS_WARN_STREAM("video record not is started ! ");
        }
        // 调用云台对象的完成录制视频函数。
        g_gimbal->finish_record();
        // 如果视频流仍然打开，说明停止录制失败。
        if (g_gimbal->outvideo.isOpened())
        {
            req.success = false;
            req.message = "finish video fail ! ";
            ROS_WARN_STREAM("finish video fail ! ");
        }
        else
        {
            // 如果视频流已关闭，说明停止录制成功。
            req.success = true;
            ROS_INFO_STREAM("Finish Video Record ! ");
        }
    }
    // 返回true表示处理成功。
    return true;
}


void picCallback(const std_msgs::String::ConstPtr &info)
{
    ROS_INFO("Get Picture ");
    g_gimbal->get_picture();
}

// 这个函数用于连续从云台获取图像帧，并通过图像发布器发布。
void callAll(image_transport::Publisher *image_pub)
{
    cv::Mat frame;
    while (true)
    {
        // 从云台获取一帧图像。
        (*g_gimbal) >> frame;
        // 如果帧不为空，则将其发布。
        if (!frame.empty())
        {
            image_pub->publish(cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg());
        }
        // 等待10毫秒。
        std::this_thread::sleep_for(10ms);
    }
}

// 这个服务回调函数用于启动或停止搜索模式。
bool search(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
{
    // 设置响应的成功标志为云台搜索操作的结果。
    resp.success = g_gimbal->search(req.data);
    return true;
}

// 这个服务回调函数用于切换跟踪模式，以获取真实的角速度或计算出的角速度。
bool switchTrack(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
{
    // 将云台的获取真实角速度标志设置为请求中的数据。
    static_cast<GimbalQ10f *>(g_gimbal)->isGetRealVelocity = req.data;
    resp.success = true;
    // 设置响应消息，表明是返回真实角速度还是计算角速度。
    resp.message = static_cast<GimbalQ10f *>(g_gimbal)->isGetRealVelocity ? "return real angle speed" : "return compute angle speed";
    return true;
}

// 这个服务回调函数用于停止跟踪控制。
bool stopTrack(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
{
    // 设置全局停止跟踪标志为请求中的数据。
    g_stop_track = req.data;
    resp.success = true;
    return true;
}

// 这个函数是设备状态回调函数，用于处理设备的不同状态信息。
int VLK_DevStatusCallback(int iType, const char* szBuffer, int iBufLen, void* pUserParam)
{
    // 根据状态类型处理不同的信息。
    if (VLK_DEV_STATUS_TYPE_MODEL == iType)
    {
        // 如果是设备模型信息，则解析并打印模型代码和名称。
        VLK_DEV_MODEL* pModel = (VLK_DEV_MODEL*)szBuffer;
        std::cout << "model code: " << pModel->cModelCode << ", model name: " << pModel->szModelName << std::endl;
    }
    else if (VLK_DEV_STATUS_TYPE_CONFIG == iType)
    {
        // 如果是设备配置信息，则解析并打印版本号、设备ID和序列号。
        VLK_DEV_CONFIG* pDevConfig = (VLK_DEV_CONFIG*)szBuffer;
        std::cout << "VersionNO: " << pDevConfig->cVersionNO << ", DeviceID: " << pDevConfig->cDeviceID << ", SerialNO: " << pDevConfig->cSerialNO << std::endl;
    }
    else if (VLK_DEV_STATUS_TYPE_TELEMETRY == iType)
    {
        // 如果是设备遥测信息，则解析并打印偏航角、俯仰角、传感器类型和变焦倍数。
        // 为了避免干扰用户输入，注释掉了打印遥测信息的代码。
        // VLK_DEV_TELEMETRY* pTelemetry = (VLK_DEV_TELEMETRY*)szBuffer;
        // cout << "Yaw: " << pTelemetry->dYaw << ", Pitch: " << pTelemetry->dPitch << ", sensor type: " << pTelemetry->emSensorType << ", Zoom mag times: " << pTelemetry->sZoomMagTimes << endl;
    }
    else
    {
        // 如果是未知的状态类型，则打印错误信息。
        std::cout << "error: unknown status type: " << iType << std::endl;
    }

    return 0;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "gimbal");
    ros::NodeHandle n("~");
    image_transport::ImageTransport it(n);
    image_transport::Publisher image_pub;
    cv::Mat frame;
    sensor_msgs::ImagePtr img;
    // rpy控制, 变焦, 放大
    image_pub = it.advertise("image_raw", 1);
    ros::Subscriber sub_ctl = n.subscribe<prometheus_gimbal_control::GimbalControl>("control", 10, ctlCallback);
    ros::Subscriber sub_track = n.subscribe<prometheus_gimbal_control::VisionDiff>("track", 10, trackCallback);
    ros::Subscriber save_pic = n.subscribe<std_msgs::String>("get_pic", 100, picCallback);
    ros::Publisher pub_rpy = n.advertise<prometheus_gimbal_control::GimbalState>("state", 10);
    ros::ServiceServer srv_search = n.advertiseService("search", search);
    ros::ServiceServer srv_video = n.advertiseService("record_video", videoRecord);
    ros::ServiceServer srv_track = n.advertiseService("stop_track", stopTrack);

    ros::Rate rate(20);

    int camera_width, camera_height, uav_id;
    std::string camera_id;
    std::string tty_url, save_path, gimbal_type;
    n.param<std::string>("camera_id", camera_id, "0");
    n.param<int>("uav_id", uav_id, 0); // 无人机的ID
    n.param<std::string>("tty_url", tty_url, "/dev/ttyUSB0");
    n.param<int>("camera_width", camera_width, 1920);
    n.param<int>("camera_height", camera_height, 1080);
    n.param<std::string>("save_path", save_path, "gimbal_video_data");
    n.param<std::string>("gimbal_type", gimbal_type, "at10");

    if (gimbal_type == "q10f")
    {
        g_gimbal = new GimbalQ10f(save_path);
    }
    else if (gimbal_type == "g1")
    {
        g_gimbal = new AmovG1(save_path);
    }
    else if (gimbal_type == "at10")
    {
        g_gimbal = new GimbalAT10(save_path);
    }
    else
    {
        ROS_ERROR_STREAM("NOT SUPPORT " << gimbal_type);
        return -1;
    }

    try
    {
        g_gimbal->open_tty(tty_url);
    }
    catch (...)
    {
        ROS_ERROR_STREAM("Unable to open Serial Port ! ");
        getchar();
        return -1;
    }

    try
    {
        // 以60帧率打开，仅对AMOV G1有效
        g_gimbal->open_camera(camera_id, camera_width, camera_height, 60);
    }
    catch (...)
    {
        ROS_ERROR_STREAM("Unable to open Camera ! ");
        getchar();
        return -1;
    }

    if (g_gimbal->device_name == "q10f" || g_gimbal->device_name == "at10")
    {
        static ros::ServiceServer srv_track_mode = n.advertiseService("track_mode", switchTrack);
        static ros::Subscriber sub_currSize = n.subscribe<prometheus_gimbal_control::VisionDiff>("currSize", 10, currSizeCallback);
        static_cast<GimbalQ10f *>(g_gimbal)->stopAuto();
        static_cast<GimbalAT10 *>(g_gimbal)->stopAuto();
        // initialize sdk
        int iRet = VLK_Init();
        if (VLK_ERROR_NO_ERROR != iRet)
        {
        std::cout << "VLK_Init failed, error: " << iRet << std::endl;
        return -1;
        }
        // register device status callback
        VLK_RegisterDevStatusCB(VLK_DevStatusCallback, NULL);
        // ROS_INFO_STREAM("AT10 !");
        // 5秒后停止 zoom_out()
        static ros::Timer stop_zoom = n.createTimer(
            ros::Duration(5.0), [&](const ros::TimerEvent &)
            { g_gimbal->zoom_out(); },
            true);
    }

    g_gimbal->home();
    g_gimbal->zoom_out();
    ROS_INFO_STREAM("Program Started ! ");
    std::thread spin(callAll, &image_pub);
    if(g_gimbal->device_name == "AT10")
    {
        ROS_INFO_STREAM("at10 !");
    }else if(g_gimbal->device_name == "q10f")
    {
        ROS_INFO_STREAM("q10f !");
    }else if(g_gimbal->device_name == "g1"){
        ROS_INFO_STREAM("g1 !");
        // print sdk version
        std::cout << "ViewLink SDK version: " << GetSDKVersion() << std::endl;
    }else {
        ROS_INFO_STREAM("unknow device!!!");
        std::cout << "device name: " << g_gimbal->device_name << std::endl;
        std::cout << "device type: " << gimbal_type << std::endl;
    }


    g_pub_info.Id = uav_id;
    while (ros::ok())
    {
        ros::spinOnce();
        float info[12];
        g_gimbal->get_info(info);
        g_pub_info.mode = g_gimbal->move_state;
        g_pub_info.feedbackMode = g_gimbal->feedback_state;
        g_pub_info.isRecording = g_gimbal->is_recording;
        g_pub_info.zoomState = g_gimbal->zoom_state;
        g_pub_info.zoomVal = g_gimbal->fzoom_val;
        g_pub_info.imuAngle[0] = info[0];
        g_pub_info.imuAngle[1] = info[1];
        g_pub_info.imuAngle[2] = info[2];
        g_pub_info.rotorAngleTarget[0] = info[3];
        g_pub_info.rotorAngleTarget[1] = info[4];
        g_pub_info.rotorAngleTarget[2] = info[5];
        g_pub_info.rotorAngle[0] = info[6];
        g_pub_info.rotorAngle[1] = info[7];
        g_pub_info.rotorAngle[2] = info[8];
        g_pub_info.imuAngleVel[0] = info[9];
        g_pub_info.imuAngleVel[1] = info[10];
        g_pub_info.imuAngleVel[2] = info[11];
        pub_rpy.publish(g_pub_info);
        rate.sleep();
        // std::cout << "run ........" << std::endl;
    }
    delete g_gimbal;
}