超维空间S2无人机使用说明书——53、进阶版——添加滤波后使用PID算法进行基于yolo的目标跟踪

引言：为了提高识别效果，采用递推+均值滤波的算法对图像返回的识别准确度和位置信息进行处理，在实际应用过程中有着不错的表现。本小节内容是在52小节的基础上进行优化，可以先参考52小节，在此基础上再参考此处代码即可。由于摄像头没有增加云台，因此无人机的俯仰角度变化会导致识别的位置有抖动，代码中添加了补偿效果，最好还是使用云台，保持稳定的识别效果，可以改善跟随效果

链接: 实机识别跟随效果

注：室外版对GPS和气压计的初始化漂移进行了补偿，可以用于室内，室内版的代码不能用于室外，切忌切忌切忌

室内版代码如下：

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <cmath>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/CommandLong.h>   
#include <string>#define MAX_ERROR 0.20
#define VEL_SET   0.10
#define ALTITUDE  0.40using namespace std;float target_x_angle = 0;
float target_distance = 1500;
float target_hgt = 0;
float linear_x_p = 0.6;
float linear_x_d = 0.22;
float yaw_rate_p = 0.8;
float yaw_rate_d = 0.5;
float x_angle_threshold = 0.1;
float distance_threshold = 300;
float linear_z_p = 0.5;
float linear_z_d = 0.33;
float hgt_threshold = 100;
float max_velocity_z = 0.3;
float max_raw_velocity_x = 2.0;
float max_raw_yaw_rate = 1.0;geometry_msgs::PointStamped object_pos;tf::Quaternion quat; 
double roll, pitch, yaw;
float init_position_x_take_off =0;
float init_position_y_take_off =0;
float init_position_z_take_off =0;
bool  flag_init_position = false; 
nav_msgs::Odometry local_pos;mavros_msgs::State current_state;  
mavros_msgs::PositionTarget setpoint_raw;
//检测到的物体坐标值
string current_frame_id   = "no_object";
double current_position_x = 0;
double current_position_y = 0;
double current_distance   = 0;
double current_position_z = 0;int tmp_flag_frame = 0;//1、订阅无人机状态话题
ros::Subscriber state_sub;//2、订阅无人机实时位置信息
ros::Subscriber local_pos_sub;//3、订阅实时位置信息
ros::Subscriber object_pos_sub;//4、发布无人机多维控制话题
ros::Publisher  mavros_setpoint_pos_pub;//5、请求无人机解锁服务        
ros::ServiceClient arming_client;//6、请求无人机设置飞行模式，本代码请求进入offboard
ros::ServiceClient set_mode_client;bool  flag_no_object = false;
bool  temp_flag_no_object = true;
bool  temp_flag = true;
float current_x_record = 0; 
float current_y_record = 0; 
float current_z_record = 0; bool temp_flag_hgt = true;
bool temp_flag_distance = true;
bool temp_flag_hgt_distance = true;float temp_distance_threshold = 0;
float temp_hgt_threshold      = 0;
void pid_control()
{static float last_error_x_angle = 0;static float last_error_distance = 0;static float last_error_hgt = 0;float x_angle;float distance;float hgt;if(current_position_x == 0 && current_position_y == 0 && current_distance == 0){flag_no_object = true;x_angle  = target_x_angle;distance = target_distance;hgt = target_hgt;}else{flag_no_object = false;x_angle  = current_position_x / current_distance;distance = current_distance;hgt      = current_position_y;}float error_x_angle = x_angle - target_x_angle;float error_distance = distance - target_distance;float error_hgt = hgt - target_hgt;//角度控制if(error_x_angle > -x_angle_threshold && error_x_angle < x_angle_threshold)  {error_x_angle = 0;}//距离控制if(error_distance > -distance_threshold && error_distance < distance_threshold) {error_distance = 0;}//高度控制if (error_hgt > -hgt_threshold && error_hgt < hgt_threshold){error_hgt = 0;}printf("hgt = %f\r\n",hgt);printf("error_hgt = %f\r\n",error_hgt);printf("error_x_angle = %f\r\n",error_x_angle);//距离跟随控制setpoint_raw.velocity.x = error_distance*linear_x_p/1000 + (error_distance - last_error_distance)*linear_x_d/1000;if(setpoint_raw.velocity.x < -max_raw_velocity_x)  {setpoint_raw.velocity.x = -max_raw_velocity_x;}else if(setpoint_raw.velocity.x > max_raw_velocity_x) {setpoint_raw.velocity.x = max_raw_velocity_x;	}//高度跟随控制setpoint_raw.velocity.z = error_hgt * linear_z_p/1000 + (error_hgt - last_error_hgt) * linear_z_d/1000;if (setpoint_raw.velocity.z < -max_velocity_z){setpoint_raw.velocity.z = -max_velocity_z;}else if (setpoint_raw.velocity.z > max_velocity_z){setpoint_raw.velocity.z = max_velocity_z;}//角度跟随控制setpoint_raw.yaw_rate = error_x_angle*yaw_rate_p + (error_x_angle - last_error_x_angle)*yaw_rate_d;if(setpoint_raw.yaw_rate < -max_raw_yaw_rate)  {setpoint_raw.yaw_rate = -max_raw_yaw_rate;}else if(setpoint_raw.yaw_rate > max_raw_yaw_rate) {setpoint_raw.yaw_rate = max_raw_yaw_rate;}if(fabs(setpoint_raw.yaw_rate) < 0.1)  {setpoint_raw.yaw_rate = 0;}//没检测到目标的时候，直接保持原地不动即可，后续可能会改称旋转寻找目标		if(flag_no_object){//hgt_threshold      = temp_hgt_threshold;//distance_threshold = temp_distance_threshold;setpoint_raw.type_mask = /*1 + 2 + 4 + 8 + 16 + 32*/ + 64 + 128 + 256 + 512 + 1024 /*+ 2048*/;			if(temp_flag_no_object){temp_flag_no_object = false;current_x_record = local_pos.pose.pose.position.x;current_y_record = local_pos.pose.pose.position.y;current_z_record = local_pos.pose.pose.position.z;}setpoint_raw.position.x = current_x_record;setpoint_raw.position.y = current_y_record;setpoint_raw.position.z = current_z_record;setpoint_raw.coordinate_frame = 1;}else{temp_flag_no_object = true;if(fabs(error_hgt)<=hgt_threshold){//hgt_threshold = temp_hgt_threshold + 300;//distance_threshold = temp_distance_threshold;if(temp_flag_hgt){temp_flag_hgt = false;					current_z_record = local_pos.pose.pose.position.z;}//error_hgt大于等于0，表明此时无人机在目标物的下方if(error_hgt>=0){setpoint_raw.position.z = current_z_record + 0.4*hgt_threshold*0.001;}else{setpoint_raw.position.z = current_z_record - 0.4*hgt_threshold*0.001;}setpoint_raw.type_mask = 1 + 2 /*+ 4 + 8 + 16 + 32*/ + 64 + 128 + 256 + 512 + 1024 /*+ 2048*/;setpoint_raw.coordinate_frame = 8;}else{//hgt_threshold      = temp_hgt_threshold;//distance_threshold = temp_distance_threshold;temp_flag_hgt = true;setpoint_raw.type_mask = 1 + 2 + 4 /*+ 8 + 16 + 32*/ + 64 + 128 + 256 + 512 + 1024 /*+ 2048*/;setpoint_raw.coordinate_frame = 8;}//if(fabs(error_hgt)<=hgt_threshold)//if(error_x_angle)}		mavros_setpoint_pos_pub.publish(setpoint_raw);last_error_x_angle  = error_x_angle;last_error_distance = error_distance;last_error_hgt      = error_hgt;}void state_cb(const mavros_msgs::State::ConstPtr& msg)
{current_state = *msg;
}void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg)
{local_pos = *msg;if (flag_init_position==false && (local_pos.pose.pose.position.z!=0)){init_position_x_take_off = local_pos.pose.pose.position.x;init_position_y_take_off = local_pos.pose.pose.position.y;init_position_z_take_off = local_pos.pose.pose.position.z;flag_init_position = true;		    }	
}double positon_x_table[5]={};
double positon_y_table[5]={};
double distance_table[5] ={};
int count_positon_x = 0;
int count_positon_y = 0;
int count_distance  = 0;void object_pos_cb(const geometry_msgs::PointStamped::ConstPtr& msg)
{//ROS_INFO("current_position_x = %f");double temp_current_position_x;double temp_current_position_y;double temp_current_distance;int count_target_lost = 0;object_pos = *msg;current_frame_id   = object_pos.header.frame_id; //获取最新的rpy值tf::quaternionMsgToTF(local_pos.pose.pose.orientation, quat);	tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);printf("pitch = %f\r\n",pitch);//此处将距离由单位米改称毫米，方便提高控制精度temp_current_position_x = object_pos.point.x*(-1000);temp_current_position_y = object_pos.point.y*(-1000);temp_current_distance   = object_pos.point.z*(1000)/cos(pitch);printf("temp_current_distance = %f\r\n",temp_current_distance);//获取五次X方向数据positon_x_table[count_positon_x] = temp_current_position_x;count_positon_x++;if(count_positon_x>=5){count_positon_x = 0;} //获取五次Y方向数据positon_y_table[count_positon_y] = temp_current_position_y;count_positon_y++;if(count_positon_y>=5){count_positon_y = 0;}//获取五次距离数据distance_table[count_distance] = temp_current_distance;count_distance++;if(count_distance>=5){count_distance = 0;} double temp_positon_x_table[5]={};double temp_positon_y_table[5]={};double temp_distance_table[5] ={};//遍历数据查找是否有丢失目标的情况，每丢失一次，则计数器+1int temp_i = 0;for(int i=0;i<=4;i++){//所有数据为0，可以判定没有识别到目标if(positon_x_table[i]==0 && positon_y_table[i]==0 && distance_table[i]==0){count_target_lost++;ROS_INFO("count_target_lost = %d",count_target_lost); }else{temp_positon_x_table[temp_i] =  positon_x_table[i];temp_positon_y_table[temp_i] =  positon_y_table[i];temp_distance_table[temp_i]  =  distance_table[i];temp_i++;}}//如果5次里面丢失超过3次，直接判定为识别到目标，可能是其他干扰导致的误识别或者本身就是没有识别到目标if(count_target_lost>3){current_position_x = 0;current_position_y = 0;current_distance   = 0;}//如果认定数组里的数据有3个以上是有效的，那么应该除去最高与最低后采用均值滤波算法else{current_position_x = (temp_positon_x_table[0]+temp_positon_x_table[1]+temp_positon_x_table[2])/3;current_position_y = (temp_positon_y_table[0]+temp_positon_y_table[1]+temp_positon_y_table[2])/3;current_distance   = (temp_distance_table[0] +temp_distance_table[1] +temp_distance_table[2])/3;}if(tmp_flag_frame == 1){pid_control();	 }
}int main(int argc, char *argv[])
{ros::init(argc, argv, "follow_pid");ros::NodeHandle nh;state_sub     = nh.subscribe<mavros_msgs::State>("mavros/state", 100, state_cb);local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 100, local_pos_cb);object_pos_sub = nh.subscribe<geometry_msgs::PointStamped>("object_position", 100, object_pos_cb);mavros_setpoint_pos_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 100);   arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");ros::Rate rate(20.0); ros::param::get("linear_x_p",linear_x_p);ros::param::get("linear_x_d",linear_x_d);ros::param::get("yaw_rate_p",yaw_rate_p);ros::param::get("yaw_rate_d",yaw_rate_d);ros::param::get("target_x_angle", target_x_angle);ros::param::get("target_distance",target_distance);ros::param::get("x_angle_threshold", x_angle_threshold);ros::param::get("distance_threshold", distance_threshold);ros::param::get("linear_z_p", linear_z_d);ros::param::get("target_hgt", target_hgt);ros::param::get("hgt_threshold", hgt_threshold);ros::param::get("max_velocity_z",max_velocity_z);ros::param::get("max_raw_velocity_x",max_raw_velocity_x);ros::param::get("max_raw_yaw_rate",max_raw_yaw_rate);temp_distance_threshold = distance_threshold;temp_hgt_threshold      = hgt_threshold;printf("temp_distance_threshold = %f\r\n",temp_distance_threshold);printf("temp_hgt_threshold = %f\r\n",     temp_hgt_threshold);//等待连接到PX4无人机while(ros::ok() && current_state.connected){ros::spinOnce();rate.sleep();}setpoint_raw.type_mask = /*1 + 2 + 4 + 8 + 16 + 32*/ + 64 + 128 + 256 + 512 /*+ 1024 + 2048*/;setpoint_raw.coordinate_frame = 1;setpoint_raw.position.x = 0;setpoint_raw.position.y = 0;setpoint_raw.position.z = 0 + ALTITUDE;mavros_setpoint_pos_pub.publish(setpoint_raw);for(int i = 100; ros::ok() && i > 0; --i){mavros_setpoint_pos_pub.publish(setpoint_raw);ros::spinOnce();rate.sleep();}//请求offboard模式变量mavros_msgs::SetMode offb_set_mode;offb_set_mode.request.custom_mode = "OFFBOARD";//请求解锁变量mavros_msgs::CommandBool arm_cmd;arm_cmd.request.value = true;ros::Time last_request = ros::Time::now();//请求进入offboard模式并且解锁无人机，15秒后退出，防止重复请求       while(ros::ok()){//请求进入OFFBOARD模式if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ROS_INFO("Offboard enabled");}last_request = ros::Time::now();}else {//请求解锁if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0))){if( arming_client.call(arm_cmd) && arm_cmd.response.success){ROS_INFO("Vehicle armed");}last_request = ros::Time::now();}}if(fabs(local_pos.pose.pose.position.z-ALTITUDE)<0.1){if(ros::Time::now() - last_request > ros::Duration(1.0)){tmp_flag_frame= 1;break;}}mavros_setpoint_pos_pub.publish(setpoint_raw);ros::spinOnce();rate.sleep();}  while(ros::ok()){mavros_setpoint_pos_pub.publish(setpoint_raw);ros::spinOnce();rate.sleep();}}

室外版代码

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <cmath>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/CommandLong.h>   
#include <string>#define MAX_ERROR 0.20
#define VEL_SET   0.10
#define ALTITUDE  1.0using namespace std;float target_x_angle = 0;
float target_distance = 1500;
float target_hgt = 0;
float linear_x_p = 0.6;
float linear_x_d = 0.22;
float yaw_rate_p = 0.8;
float yaw_rate_d = 0.5;
float x_angle_threshold = 0.1;
float distance_threshold = 300;
float linear_z_p = 0.5;
float linear_z_d = 0.33;
float hgt_threshold = 100;
float max_velocity_z = 0.3;
float max_raw_velocity_x = 2.0;
float max_raw_yaw_rate = 1.0;geometry_msgs::PointStamped object_pos;tf::Quaternion quat; 
double roll, pitch, yaw;
float init_position_x_take_off =0;
float init_position_y_take_off =0;
float init_position_z_take_off =0;
bool  flag_init_position = false; 
nav_msgs::Odometry local_pos;mavros_msgs::State current_state;  
mavros_msgs::PositionTarget setpoint_raw;
//检测到的物体坐标值
string current_frame_id   = "no_object";
double current_position_x = 0;
double current_position_y = 0;
double current_distance   = 0;
double current_position_z = 0;int tmp_flag_frame = 0;//1、订阅无人机状态话题
ros::Subscriber state_sub;//2、订阅无人机实时位置信息
ros::Subscriber local_pos_sub;//3、订阅实时位置信息
ros::Subscriber object_pos_sub;//4、发布无人机多维控制话题
ros::Publisher  mavros_setpoint_pos_pub;//5、请求无人机解锁服务        
ros::ServiceClient arming_client;//6、请求无人机设置飞行模式，本代码请求进入offboard
ros::ServiceClient set_mode_client;bool  flag_no_object = false;
bool  temp_flag_no_object = true;
bool  temp_flag = true;
float current_x_record = 0; 
float current_y_record = 0; 
float current_z_record = 0; bool temp_flag_hgt = true;
//bool temp_flag_distance = true;
//bool temp_flag_hgt_distance = true;float temp_distance_threshold = 0;
float temp_hgt_threshold      = 0;
void pid_control()
{static float last_error_x_angle = 0;static float last_error_distance = 0;static float last_error_hgt = 0;float x_angle;float distance;float hgt;if(current_position_x == 0 && current_position_y == 0 && current_distance == 0){flag_no_object = true;x_angle  = target_x_angle;distance = target_distance;hgt = target_hgt;}else{flag_no_object = false;x_angle  = current_position_x / current_distance;distance = current_distance;hgt      = current_position_y;}float error_x_angle = x_angle - target_x_angle;float error_distance = distance - target_distance;float error_hgt = hgt - target_hgt;//角度控制if(error_x_angle > -x_angle_threshold && error_x_angle < x_angle_threshold)  {error_x_angle = 0;}//距离控制if(error_distance > -distance_threshold && error_distance < distance_threshold) {error_distance = 0;}//高度控制if (error_hgt > -hgt_threshold && error_hgt < hgt_threshold){error_hgt = 0;}printf("hgt = %f\r\n",hgt);printf("error_hgt = %f\r\n",error_hgt);printf("error_x_angle = %f\r\n",error_x_angle);//距离跟随控制setpoint_raw.velocity.x = error_distance*linear_x_p/1000 + (error_distance - last_error_distance)*linear_x_d/1000;if(setpoint_raw.velocity.x < -max_raw_velocity_x)  {setpoint_raw.velocity.x = -max_raw_velocity_x;}else if(setpoint_raw.velocity.x > max_raw_velocity_x) {setpoint_raw.velocity.x = max_raw_velocity_x;	}//高度跟随控制setpoint_raw.velocity.z = error_hgt * linear_z_p/1000 + (error_hgt - last_error_hgt) * linear_z_d/1000;if (setpoint_raw.velocity.z < -max_velocity_z){setpoint_raw.velocity.z = -max_velocity_z;}else if (setpoint_raw.velocity.z > max_velocity_z){setpoint_raw.velocity.z = max_velocity_z;}//角度跟随控制setpoint_raw.yaw_rate = error_x_angle*yaw_rate_p + (error_x_angle - last_error_x_angle)*yaw_rate_d;if(setpoint_raw.yaw_rate < -max_raw_yaw_rate)  {setpoint_raw.yaw_rate = -max_raw_yaw_rate;}else if(setpoint_raw.yaw_rate > max_raw_yaw_rate) {setpoint_raw.yaw_rate = max_raw_yaw_rate;}if(fabs(setpoint_raw.yaw_rate) < 0.1)  {setpoint_raw.yaw_rate = 0;}//没检测到目标的时候，直接保持原地不动即可，后续可能会改称旋转寻找目标		if(flag_no_object){printf("no_object\r\n");setpoint_raw.type_mask = /*1 + 2 + 4 + 8 + 16 + 32*/ + 64 + 128 + 256 + 512 + 1024 /*+ 2048*/;			if(temp_flag_no_object){temp_flag_no_object = false;current_x_record = local_pos.pose.pose.position.x;current_y_record = local_pos.pose.pose.position.y;current_z_record = local_pos.pose.pose.position.z;}setpoint_raw.position.x = current_x_record;setpoint_raw.position.y = current_y_record;setpoint_raw.position.z = current_z_record;setpoint_raw.coordinate_frame = 1;}else{printf("yes_object\r\n");temp_flag_no_object = true;if(fabs(error_hgt)<=hgt_threshold){//hgt_threshold = temp_hgt_threshold + 300;//distance_threshold = temp_distance_threshold;if(temp_flag_hgt){temp_flag_hgt = false;					current_z_record = local_pos.pose.pose.position.z;}//error_hgt大于等于0，表明此时无人机在目标物的下方if(error_hgt>0){setpoint_raw.position.z = current_z_record + 0.4*hgt_threshold*0.001;}else{setpoint_raw.position.z = current_z_record - 0.4*hgt_threshold*0.001;}setpoint_raw.type_mask = 1 + 2 /*+ 4 + 8 + 16 + 32*/ + 64 + 128 + 256 + 512 + 1024 /*+ 2048*/;setpoint_raw.coordinate_frame = 8;}else{//hgt_threshold      = temp_hgt_threshold;//distance_threshold = temp_distance_threshold;temp_flag_hgt = true;setpoint_raw.type_mask = 1 + 2 + 4 /*+ 8 + 16 + 32*/ + 64 + 128 + 256 + 512 + 1024 /*+ 2048*/;setpoint_raw.coordinate_frame = 8;}//if(fabs(error_hgt)<=hgt_threshold)//if(error_x_angle)}	printf("error_hgt = %f\r\n",error_hgt);	printf("current_x_record = %f\r\n",current_x_record);printf("current_y_record = %f\r\n",current_y_record);printf("current_z_record = %f\r\n",current_z_record);printf("setpoint_raw.position.x = %f\r\n",setpoint_raw.position.x);		printf("setpoint_raw.position.y = %f\r\n",setpoint_raw.position.y);		printf("setpoint_raw.position.z = %f\r\n",setpoint_raw.position.z);		mavros_setpoint_pos_pub.publish(setpoint_raw);last_error_x_angle  = error_x_angle;last_error_distance = error_distance;last_error_hgt      = error_hgt;}void state_cb(const mavros_msgs::State::ConstPtr& msg)
{current_state = *msg;
}void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg)
{local_pos = *msg;if(flag_init_position==false && (local_pos.pose.pose.position.z!=0)){init_position_x_take_off = local_pos.pose.pose.position.x;init_position_y_take_off = local_pos.pose.pose.position.y;init_position_z_take_off = local_pos.pose.pose.position.z;flag_init_position = true;		    }	
}double positon_x_table[5]={};
double positon_y_table[5]={};
double distance_table[5] ={};
int count_positon_x = 0;
int count_positon_y = 0;
int count_distance  = 0;void object_pos_cb(const geometry_msgs::PointStamped::ConstPtr& msg)
{//ROS_INFO("current_position_x = %f");double temp_current_position_x;double temp_current_position_y;double temp_current_distance;int count_target_lost = 0;object_pos = *msg;current_frame_id   = object_pos.header.frame_id; //获取最新的rpy值tf::quaternionMsgToTF(local_pos.pose.pose.orientation, quat);	tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//printf("pitch = %f\r\n",pitch);//此处将距离由单位米改称毫米，方便提高控制精度temp_current_position_x = object_pos.point.x*(-1000);temp_current_position_y = object_pos.point.y*(-1000);temp_current_distance   = object_pos.point.z*(1000)/cos(pitch);//printf("temp_current_distance = %f\r\n",temp_current_distance);//获取五次X方向数据positon_x_table[count_positon_x] = temp_current_position_x;count_positon_x++;if(count_positon_x>=5){count_positon_x = 0;} //获取五次Y方向数据positon_y_table[count_positon_y] = temp_current_position_y;count_positon_y++;if(count_positon_y>=5){count_positon_y = 0;}//获取五次距离数据distance_table[count_distance] = temp_current_distance;count_distance++;if(count_distance>=5){count_distance = 0;} double temp_positon_x_table[5]={};double temp_positon_y_table[5]={};double temp_distance_table[5] ={};//遍历数据查找是否有丢失目标的情况，每丢失一次，则计数器+1int temp_i = 0;for(int i=0;i<=4;i++){//所有数据为0，可以判定没有识别到目标if(positon_x_table[i]==0 && positon_y_table[i]==0 && distance_table[i]==0){count_target_lost++;ROS_INFO("count_target_lost = %d",count_target_lost); }else{temp_positon_x_table[temp_i] =  positon_x_table[i];temp_positon_y_table[temp_i] =  positon_y_table[i];temp_distance_table[temp_i]  =  distance_table[i];temp_i++;}}//如果5次里面丢失超过3次，直接判定为识别到目标，可能是其他干扰导致的误识别或者本身就是没有识别到目标if(count_target_lost>3){current_position_x = 0;current_position_y = 0;current_distance   = 0;}//如果认定数组里的数据有3个以上是有效的，那么应该除去最高与最低后采用均值滤波算法else{current_position_x = (temp_positon_x_table[0]+temp_positon_x_table[1]+temp_positon_x_table[2])/3;current_position_y = (temp_positon_y_table[0]+temp_positon_y_table[1]+temp_positon_y_table[2])/3;current_distance   = (temp_distance_table[0] +temp_distance_table[1] +temp_distance_table[2])/3;}if(tmp_flag_frame == 1){pid_control();	 }
}int main(int argc, char *argv[])
{ros::init(argc, argv, "follow_pid");ros::NodeHandle nh;state_sub     = nh.subscribe<mavros_msgs::State>("mavros/state", 100, state_cb);local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 100, local_pos_cb);object_pos_sub = nh.subscribe<geometry_msgs::PointStamped>("object_position", 100, object_pos_cb);mavros_setpoint_pos_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 100);   arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");ros::Rate rate(20.0); ros::param::get("linear_x_p",linear_x_p);ros::param::get("linear_x_d",linear_x_d);ros::param::get("yaw_rate_p",yaw_rate_p);ros::param::get("yaw_rate_d",yaw_rate_d);ros::param::get("target_x_angle", target_x_angle);ros::param::get("target_distance",target_distance);ros::param::get("x_angle_threshold", x_angle_threshold);ros::param::get("distance_threshold", distance_threshold);ros::param::get("linear_z_p", linear_z_d);ros::param::get("target_hgt", target_hgt);ros::param::get("hgt_threshold", hgt_threshold);ros::param::get("max_velocity_z",max_velocity_z);ros::param::get("max_raw_velocity_x",max_raw_velocity_x);ros::param::get("max_raw_yaw_rate",max_raw_yaw_rate);temp_distance_threshold = distance_threshold;temp_hgt_threshold      = hgt_threshold;printf("temp_distance_threshold = %f\r\n",temp_distance_threshold);printf("temp_hgt_threshold = %f\r\n",     temp_hgt_threshold);//等待连接到PX4无人机while(ros::ok() && current_state.connected){ros::spinOnce();rate.sleep();}setpoint_raw.type_mask = /*1 + 2 + 4 + 8 + 16 + 32*/ + 64 + 128 + 256 + 512 /*+ 1024 + 2048*/;setpoint_raw.coordinate_frame = 1;//float init_position_x_take_off =0;//float init_position_y_take_off =0;//float init_position_z_take_off =0;setpoint_raw.position.x = init_position_x_take_off + 0;setpoint_raw.position.y = init_position_y_take_off + 0;setpoint_raw.position.z = init_position_z_take_off + ALTITUDE;mavros_setpoint_pos_pub.publish(setpoint_raw);for(int i = 100; ros::ok() && i > 0; --i){mavros_setpoint_pos_pub.publish(setpoint_raw);ros::spinOnce();rate.sleep();}//请求offboard模式变量mavros_msgs::SetMode offb_set_mode;offb_set_mode.request.custom_mode = "OFFBOARD";//请求解锁变量mavros_msgs::CommandBool arm_cmd;arm_cmd.request.value = true;ros::Time last_request = ros::Time::now();//请求进入offboard模式并且解锁无人机，15秒后退出，防止重复请求       while(ros::ok()){//请求进入OFFBOARD模式if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ROS_INFO("Offboard enabled");}last_request = ros::Time::now();}else {//请求解锁if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0))){if( arming_client.call(arm_cmd) && arm_cmd.response.success){ROS_INFO("Vehicle armed");}last_request = ros::Time::now();}}if(fabs(local_pos.pose.pose.position.z-ALTITUDE)<0.1){if(ros::Time::now() - last_request > ros::Duration(1.0)){tmp_flag_frame= 1;break;}}		setpoint_raw.position.x = init_position_x_take_off + 0;setpoint_raw.position.y = init_position_y_take_off + 0;setpoint_raw.position.z = init_position_z_take_off + ALTITUDE;mavros_setpoint_pos_pub.publish(setpoint_raw);ros::spinOnce();rate.sleep();}  while(ros::ok()){mavros_setpoint_pos_pub.publish(setpoint_raw);ros::spinOnce();rate.sleep();}}

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mzph.cn/news/591003.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

LeetCode刷题--- 第 N 个泰波那契数

LeetCode刷题--- 第 N 个泰波那契数

个人主页&#xff1a;元清加油_【C】,【C语言】,【数据结构与算法】-CSDN博客 个人专栏 力扣递归算法题 http://t.csdnimg.cn/yUl2I 【C】 ​​​​​​http://t.csdnimg.cn/6AbpV 数据结构与算法 ​​​http://t.csdnimg.cn/hKh2l 前言&#xff1a;这个专栏主要讲述动…
阅读更多...
男女婚姻配对中若男孩有金钱和长相帅气是容易成功的因素分析

男女婚姻配对中若男孩有金钱和长相帅气是容易成功的因素分析

男女婚姻配对中若男孩有金钱和长相帅气是容易成功的因素分析。 男人想要脱离单身主要考虑哪些因素、提升哪些方面。 首先&#xff0c;你要清楚&#xff0c;女人喜欢什么&#xff0c;你和她能长久在一起的概率是多少&#xff1f; 直白的说&#xff0c;女人&#xff0c;就是喜欢…
阅读更多...
算法通关村第二十关-黄金挑战图的常见算法

算法通关村第二十关-黄金挑战图的常见算法

大家好我是苏麟 , 今天聊聊图的常见算法 . 图里的算法是很多的&#xff0c;这里我们介绍一些常见的图算法。这些算法一般都比较复杂&#xff0c;我们这里介绍这些算法的基本含义&#xff0c;适合面试的时候装*&#xff0c;如果手写&#xff0c;那就不用啦。 图分析算法&#xf…
阅读更多...
【软件体系结构】软件体系结构风格

【软件体系结构】软件体系结构风格

5.1 软件体系结构风格概述 多年来&#xff0c;人们在开发某些类型软件过程中积累起来的组织规则和结构&#xff0c;形成了软件体系结构风格。 软件体系结构风格&#xff0c;是总结人们设计经验而形成结构较为巩固、组织较为统一的形式&#xff0c;是一种适合于多种场合的相似…
阅读更多...
uni-app设置地图显示

uni-app设置地图显示

使用前需到**高德开放平台&#xff08;https://lbs.amap.com/&#xff09;**创建应用并申请Key 登录 高德开放平台&#xff0c;进入“控制台”&#xff0c;如果没有注册账号请先根据页面提示注册账号 打开 “应用管理” -> “我的应用”页面&#xff0c;点击“创建新应用”&…
阅读更多...
2023.12.30力扣每日一题——一周中的第几天

2023.12.30力扣每日一题——一周中的第几天

2023.12.30 题目来源我的题解方法一 模拟常识 题目来源 力扣每日一题&#xff1b;题序&#xff1a;1185 我的题解 方法一 模拟常识 首先想要计算指定日期是周几&#xff0c;需要一个参考日期&#xff0c;这里选择1970年12月31日&#xff0c;查询日历可知该天是星期四&#x…
阅读更多...
poi操作Excel给列设置下拉菜单（数据验证）

poi操作Excel给列设置下拉菜单（数据验证）

效果图&#xff1a; pom.xml文件增加依赖&#xff1a; <dependency><groupId>org.apache.poi</groupId><artifactId>poi</artifactId><version>4.0.1</version></dependency> 12345Workbook实现类有三个&#xff1a;HSSFWork…
阅读更多...
JavaWeb三层架构

JavaWeb三层架构

分层的目的是为了解耦。解耦就是为了降低代码的耦合度。方便项目后期的维护和升级。 JavaWeb应用程序的架构是一个关键的设计决策&#xff0c;而三层架构&#xff08;MVC模式&#xff09;是一种常见且有效的设计模式&#xff0c;能够清晰地分离不同部分的责任。让我们来深入了…
阅读更多...
JavaWeb——前端之HTMLCSS

JavaWeb——前端之HTMLCSS

学习视频链接&#xff1a;https://www.bilibili.com/video/BV1m84y1w7Tb/?spm_id_from333.999.0.0 一、Web开发 1. 概述 能通过浏览器访问的网站 2. Web网站的开发模式——主流是前后端分离 二、前端Web开发 1. 初识 前端编写的代码通过浏览器进行解析和渲染得到我们看到…
阅读更多...
Redis源码——压缩列表

Redis源码——压缩列表

压缩列表ziplist本质上就是一个字节数组&#xff0c;是Redis为了节约内存而设计的一种线性数据结构&#xff0c;可以包含多个元素&#xff0c;每个元素可以是一个字节数组或一个整数。Redis的有序集合、散列和列表都直接或者间接使用了压缩列表。当有序集合或散列表的元素个数比…
阅读更多...
项目经验简单总结

项目经验简单总结

引擎 unity 2020 语言 C# lua python(用于工具链) java (用于SDK对接) js&#xff08;PC WEB SDK对接&#xff09; 编辑器 VS VSCODE IDEA eclipse 项目开发模块规划分 主项目工程&#xff0c;UI资源项目工程&#xff0c;模型场景资源项目工程 主项目工程&#xff1a;所有的…
阅读更多...
qt图像绘制QPainter

qt图像绘制QPainter

QPainter 以下是一些常用的 Qt::PenStyle 枚举值&#xff1a; Qt::NoPen&#xff1a;无线条。Qt::SolidLine&#xff1a;实线。Qt::DashLine&#xff1a;虚线&#xff0c;由短划线组成。Qt::DotLine&#xff1a;点线&#xff0c;由点组成。Qt::DashDotLine&#xff1a;点划线&…
阅读更多...
炫酷鼠标悬停随机渐变文本动画效果

炫酷鼠标悬停随机渐变文本动画效果

如图所示&#xff0c;这是一个很炫酷的鼠标悬停动画效果&#xff0c;卡片的文字随着鼠标的移动不断变化着&#xff0c;且文字的颜色伴随着渐变色跟随鼠标移动&#xff0c;中心部分是突出的LOGO效果&#xff0c;整个交互效果十分引人注目。原效果来源于 evervault.com/customers…
阅读更多...
Spring Boot实战：深入理解@Service与@Mapper注解

Spring Boot实战：深入理解@Service与@Mapper注解

1. Service 注解 Service 是Spring框架提供的一个注解&#xff0c;用于标记类为业务逻辑层的组件。当类上标注了Service注解后&#xff0c;Spring容器会自动扫描并创建该类的一个实例&#xff08;即Bean&#xff09;&#xff0c;这样我们就可以在其他地方通过自动装配&#xf…
阅读更多...
Head First Design Patterns - 装饰者模式

Head First Design Patterns - 装饰者模式

什么是装饰者模式 装饰者模式动态地将额外责任附加到对象上。对于拓展功能&#xff0c;装饰者提供子类化的弹性替代方案。 --《Head First Design Patterns》中的定义 为什么会有装饰者模式 根据上述定义&#xff0c;简单来说&#xff0c;装饰者模式就是对原有的类&#xff0c…
阅读更多...
源码解析：mybatis调用链之XMLStatementBuilder解析解析sql语句节点

源码解析：mybatis调用链之XMLStatementBuilder解析解析sql语句节点

该过程由XMLMapperBuilder的configurationElement方法触发&#xff1a; XMLMapperBuilder private void configurationElement(XNode context) {try {//获取mapper节点的namespace属性String namespace context.getStringAttribute("namespace");if (namespace n…
阅读更多...
Verilog视频信号图形显示 FPGA(iCE40)

Verilog视频信号图形显示 FPGA(iCE40)

您需要一块带视频输出的 FPGA 板。 我们将在 640x480 下工作&#xff0c;几乎任何视频输出都可以在此像素工作。 它有助于轻松地对 FPGA 板进行编程并相当熟悉 Verilog。 如果您没有开发板&#xff0c;请不要担心&#xff0c;您可以使用 Verilator 模拟器。 材料 Lattice iCE…
阅读更多...
绝地求生：PUBG到底怎么穿？

绝地求生：PUBG到底怎么穿？

闲游盒突然就认为这条裤子很不错&#xff0c;褶皱设计&#xff0c;版型&#xff0c;和其他饰品的搭配能力方面&#xff0c;私以为比柏林裤和悲喜白色长裤的可搭配能力要强一些。可能是因为看起来更日常化&#xff0c;在我进行尝试后发现柏林裤版型在穿鞋的时候会露出脚脖子&…
阅读更多...
目标检测-Owo Stage-YOLOv2

目标检测-Owo Stage-YOLOv2

文章目录 前言一、YOLOv2的网络结构和流程二、YOLOv2的创新点预处理网络结构训练 总结 前言 根据前文目标检测-One Stage-YOLOv1可以看出YOLOv1的主要缺点是&#xff1a; 和Fast-CNN相比&#xff0c;速度快&#xff0c;但精度下降。&#xff08;边框回归不加限制&#xff09;…
阅读更多...
2023年最新版的linux运维面试题（四）

2023年最新版的linux运维面试题（四）

作者简介&#xff1a;一名云计算网络运维人员、每天分享网络与运维的技术与干货。 公众号&#xff1a;网络豆云计算学堂 座右铭&#xff1a;低头赶路&#xff0c;敬事如仪 个人主页&#xff1a; 网络豆的主页​​​​​ 写在前面 大家好&#xff0c;我是网络豆&#xff0…
阅读更多...
最新文章

Copyright @ 2022~2023