基于ROS搭建简易软件框架实现ROV水下目标跟踪（十四完结）–目标跟踪模块

  模块十分简单，可以介绍的内容很少。包括两个部分：计算目标物中心距图片中心的偏差，对应cabin_vision/object_deviation；PID跟踪控制，对应cabin_behaviors/pid_tracking。 一、偏差计算 输入输出：  
  监听topic：/darknet_ros/bounding_boxes，格式darknet_ros_msgs::BoundingBoxes，目标框图顶点在图片中像素点的位置； 发布topic：/cabin_vision/deviation，格式geometry_msgs::PointStamped，目标中心像素点与图片中心像素点偏差。 直接看代码吧，object_deviation.cpp。这部分我写了个python版本的object_deviation.py，有兴趣的话可以看看。 首先是读取设定的摄像头分辨率。  

//Load the width and heigth of the camera image
string properties_file;
nh.param("properties_file", properties_file, std::string(""));
if(properties_file.size() != 0){
    if(access(properties_file.c_str(),F_OK) == -1){
        std::cout<<""<<std::endl;
        std::cout<<RED<<"\""<<properties_file<<"\""<<" does not exist!!!!"<<std::endl;
        std::cout<<""<<std::endl;
        ros::shutdown();
    }
    else{
        YAML::Node properties;
        properties = YAML::LoadFile(properties_file);
        image_width = properties["image_width"].as<int>();
        image_height = properties["image_height"].as<int>();
    }
}

  再者为收到一个目标数据后，计算偏差。  

void ObjectDeviation::BoundingBoxSub(const darknet_ros_msgs::BoundingBoxes msg){
    deviation_msg.header.stamp = ros::Time::now();
    deviation_msg.point.x = (msg.bounding_boxes[0].xmin + msg.bounding_boxes[0].xmax) / 2.0 - image_width / 2.0;
    deviation_msg.point.y = (msg.bounding_boxes[0].ymin + msg.bounding_boxes[0].ymax) / 2.0 - image_height / 2.0;
    deviation_pub.publish(deviation_msg);
}

    二、PID跟踪控制 输入输出：  
  监听topic：/cabin_vision/deviation，格式geometry_msgs::PointStamped，目标中心像素点与图片中心像素点偏差。 发布topic：/command/netLoad，格式cabin_msgs::netLoad，作用于机器人上的力及转矩，直接作为基础运动模块的输入； 直接看代码cabin_behaviors/pid_tracking.cpp。对于偏差的处理，如下图：  
  设定了一个区域，当机器人中心位于该区域时，即偏差量小于safe_area_x或safe_area_y时，PID控制器在相应方向输出的控制量为0。PID控制器随手写了一个简易的，对应代码：  

double PIDTracking::PIDController(double err, double pid_p, double pid_i, double pid_d, double last_err, double *integral, double safe_area){
    double u = 0.0;
    //double t = *integral;
    if(abs(err) > safe_area){
        *integral += err;
        //t += err;
        //u = pid_p * err + pid_i * t + pid_d * (err - last_err);
        u = pid_p * err + pid_i * (*integral) + pid_d * (err - last_err);
        last_err = err;
        //*integral = t;
    }
    return u;
}

  剩下的是ros下借助rqt动态调参工具的配置，包括pid参数，安全区域大小，跟踪模式开关：  

//Callback for dynamic reconfigure
void PIDTracking::DynamicReconfigCallback(cabin_behaviors::PIDTrackingConfig &config, uint32_t levels){
    pid_p_mz = config.pid_p_mz;
    pid_i_mz = config.pid_i_mz;
    pid_d_mz = config.pid_d_mz;
    pid_p_z = config.pid_p_z;
    pid_i_z = config.pid_i_z;
    pid_d_z = config.pid_d_z;
    safe_area_x = config.safe_area_x;
    safe_area_y = config.safe_area_y;
 
    tracking_switch = config.tracking_switch;
}

    实际在调试时没有太追求效果，只用了比例控制，即将积分i和微分d的参数都设为0。   自此，整个demo软件框架所有模块基本都介绍完毕。