2022年吉林省智能无人机仿真挑战赛参赛心得（ROS+gazebo）

本文主要是介绍2022年吉林省智能无人机仿真挑战赛参赛心得（ROS+gazebo），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

比赛介绍

知识储备

示例代码

飞机飞行

人脸识别

提交代码

首先这算是我参加的比赛中算是赛前准备比较成体系的一个比赛吧，所以就想趁着还记得写一点心得，这个比赛是吉林省机器人大赛的一个分赛道，智能无人机仿真挑战赛，我的队伍也是在这个比赛中以1分28秒的成绩获得了吉林省一等奖季军的成绩。

比赛介绍

该比赛主要就是在ubuntu系统下使用ros对gazebo环境下的仿真无人机进行操控，按照题目给出的得分点得分，得分相同的队伍按照完成时间排序，比赛使用的虚拟机和仿真环境比赛组委会是会提供的。

知识储备

做这个比赛还是需要一些知识储备的，首先要对ubuntu下的ros有所了解，关于一些ros的基础操作是要学会的，例如功能包和工作空间的创建，话题的创建和收发，对于功能包的编译等，这里比较推荐古月居的教程，真的很细致；对于所掌握的语言，一般就是C++和python，其实并不需要我们很是精通，无人机仿真赛官方提供了很多的示例代码，我们只需要在示例代码的基础上进行修改其实就足够我进行比赛了

示例代码

这里主要介绍一下官方给我们提供的文件的结构讲解，主要能够让飞机飞行的代码都存在Home文件夹中的sim_ws这个工作空间之中

在该文件夹下，其中主要的代码存放在px4_com这个功能包之中

在该目录下的文件都很重要：

config文件夹中主要储存着yaml参数列表文件，方便对无人机进行调参

launch文件夹中主要储存着编译后的可执行文件

scripts文件夹中主要储存python文件，python文件是可以直接执行的，不需要编译，但是如果需要调用外部库文件，也是需要编写launch文件的

src中主要储存的就是主要控制飞行的C++代码

飞机飞行

在src文件夹中的Application文件夹中存在着有关飞机飞行重要文件

这些文件就是我们主要需要修改的文件，文件中的代码都有详细的注释，代码并不是特别难，不需要读的特别明白，只要会修改实现自己想要的功能即可，修稿之后编译执行即可检验效果，很方便易用

我所修改的文件是4x4ABCdemo.cpp，部分修改代码如下

int main(int argc, char **argv)
{ros::init(argc, argv, "collision_avoidance");ros::NodeHandle nh("~");image_transport::ImageTransport it(nh);// 频率 [20Hz]ros::Rate rate(20.0);//【订阅】Lidar数据//ros::Subscriber lidar_sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 1000, lidar_cb);//dyxros::Subscriber lidar_sub = nh.subscribe<sensor_msgs::LaserScan>("/lidar2Dscan", 1000, lidar_cb);//dyxros::Subscriber face_sub = nh.subscribe<std_msgs::String>("/face_detected", 1000,see_man);//【订阅】无人机当前位置 坐标系 NED系//ros::Subscriber position_sub = nh.subscribe<geometry_msgs::Pose>("/drone/pos", 100, pos_cb);ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 100, pos_cb);  //dyx// 【发布】发送给position_control.cpp的命令ros::Publisher command_pub = nh.advertise<px4_command::command>("/px4/command", 10);//读取参数表中的参数nh.param<float>("R_outside", R_outside, 2);nh.param<float>("R_inside", R_inside, 1);nh.param<float>("p_xy", p_xy, 0.5);nh.param<float>("vel_track_max", vel_track_max, 0.5);nh.param<float>("p_R", p_R, 0.0);nh.param<float>("p_r", p_r, 0.0);nh.param<float>("vel_collision_max", vel_collision_max, 0.0);nh.param<float>("vel_sp_max", vel_sp_max, 0.0);nh.param<int>("range_min", range_min, 0.0);nh.param<int>("range_max", range_max, 0.0);nh.param<float>("A_x", A_x, 0.0);nh.param<float>("A_y", A_y, 0.0);nh.param<float>("B_x", B_x, 0.0);nh.param<float>("B_y", B_y, 0.0);nh.param<float>("C_x", C_x, 0.0);nh.param<float>("C_y", C_y, 0.0);nh.param<float>("D_x", D_x, 0.0);nh.param<float>("D_y", D_y, 0.0);    nh.param<float>("E_x", E_x, 0.0);nh.param<float>("E_y", E_y, 0.0);nh.param<float>("F_x", F_x, 0.0);nh.param<float>("F_y", F_y, 0.0);nh.param<float>("G_x", G_x, 0.0);nh.param<float>("G_y", G_y, 0.0);//获取设定的起飞高度//nh.getParam("/px4_pos_controller/Takeoff_height",fly_height);//check armint Arm_flag;cout<<"Input 1 for going:"<<endl;cin >> Arm_flag;if(Arm_flag == 1){Command_now.command = Arm;command_pub.publish(Command_now);}else return -1;ros::Duration(1).sleep();//check takeoffint Take_off_flag;//cout<<"Whether choose to Takeoff? 1 for Takeoff, 0 for quit"<<endl;//cin >> Take_off_flag;Take_off_flag=1;ros::Duration(1).sleep();if(Take_off_flag == 1){Command_now.command = Takeoff;command_pub.publish(Command_now);}else return -1;//打印现实检查参数printf_param();int check_flag;//输入1,继续，其他，退出程序，检查设定的参数是否正确//cout << "Please check the parameter and setting，1 for go on， else for quit: "<<endl;//cin >> check_flag;check_flag=1;ros::Duration(10).sleep();if(check_flag != 1){return -1;}//cout<<"Input 1:"<<endl;//cin >> ABC;ABC=1;ros::Duration(1).sleep();//初值vel_track[0]= 0;vel_track[1]= 0;vel_collision[0]= 0;vel_collision[1]= 0;vel_sp_body[0]= 0;vel_sp_body[1]= 0;vel_sp_ENU[0]= 0;vel_sp_ENU[1]= 0;//四向最小距离 初值flag_land = 0;flag_hold = 0;//输出指令初始化int comid = 1;//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Main Loop<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<while(ros::ok()){//回调一次 更新传感器状态//1. 更新雷达点云数据，存储在Laser中,并计算四向最小距离ros::spinOnce();/**************************dyx****************************************///模式2策略：ABC三点坐标已知，每次飞往ABC三点与返回原点时设定标志。if(ABC==1){if(!reach_ABC_flag[0])  //飞到A点，标记1，{collision_avoidance(A_x,A_y);float abs_distance;abs_distance = sqrt((pos_drone.pose.position.x - A_x) * (pos_drone.pose.position.x -A_x) + (pos_drone.pose.position.y - A_y) * (pos_drone.pose.position.y - A_y));cout <<"----------------------------------GO A-------------------------------" <<endl;if(abs_distance < 0.5 ){reach_ABC_flag[0]=true;ABC=2;}}}else if(ABC==2){if(!reach_ABC_flag[1]){collision_avoidance(B_x,B_y);float abs_distance;abs_distance = sqrt((pos_drone.pose.position.x - B_x) * (pos_drone.pose.position.x -B_x) + (pos_drone.pose.position.y - B_y) * (pos_drone.pose.position.y - B_y));cout <<"----------------------------------GO B-------------------------------" <<endl;if(abs_distance < 0.5 ){reach_ABC_flag[1]=true;ABC=3;}}}        else if(ABC==3){if(!reach_ABC_flag[2]){collision_avoidance(C_x,C_y);float abs_distance;abs_distance = sqrt((pos_drone.pose.position.x - C_x) * (pos_drone.pose.position.x -C_x) + (pos_drone.pose.position.y - C_y) * (pos_drone.pose.position.y - C_y));cout <<"----------------------------------GO C-------------------------------" <<endl;if(abs_distance < 0.5 ){reach_ABC_flag[2]=true;ABC=4;}}}else if(ABC==4){if(!reach_ABC_flag[3]){collision_avoidance(D_x,D_y);float abs_distance;abs_distance = sqrt((pos_drone.pose.position.x - D_x) * (pos_drone.pose.position.x -D_x) + (pos_drone.pose.position.y - D_y) * (pos_drone.pose.position.y - D_y));cout <<"----------------------------------GO D-------------------------------" <<endl;if(abs_distance < 0.4 ){reach_ABC_flag[3]=true;ABC=5;}}}else if(ABC==0){cout <<"----------------------------------Land-------------------------------" <<endl;flag_land=1;}else if(flag_see_man==1){ABC=0;}else if(flag_see_man==0){       p_xy =0.5;vel_track_max=0.5;if(ABC==5){vel_sp_ENU[0]=0;collision_avoidance(E_x,E_y);float abs_distance;abs_distance = sqrt((pos_drone.pose.position.x - E_x) * (pos_drone.pose.position.x -E_x) + (pos_drone.pose.position.y - E_y) * (pos_drone.pose.position.y - E_y));cout <<"----------------------------------SEEING RIGNT-------------------------------"<< abs_distance <<endl;if(abs_distance < 0.6 ){ABC=6;flag_count++;}}if(ABC==6){     collision_avoidance(F_x,F_y);float abs_distance;abs_distance = sqrt((pos_drone.pose.position.x - F_x) * (pos_drone.pose.position.x -F_x) + (pos_drone.pose.position.y - F_y) * (pos_drone.pose.position.y - F_y));cout <<"----------------------------------SEEING LEFT-------------------------------"<< abs_distance <<endl;if(abs_distance < 0.4 ){ABC=5;flag_count++;}}if(flag_count==3){fly_height=1.2;}if(flag_count==4){ABC=0;}}//启用ENU下的指令Command_now.command = Move_ENU;     //机体系下移动Command_now.comid = comid;comid++;Command_now.sub_mode = 2; // xy 速度控制模式 z 位置控制模式Command_now.vel_sp[0] =  vel_sp_ENU[0];Command_now.vel_sp[1] =  vel_sp_ENU[1];  //ENU frameCommand_now.pos_sp[2] =  fly_height;Command_now.yaw_sp = 0 ;if(flag_land == 1) Command_now.command = Land;command_pub.publish(Command_now);//打印printf();rate.sleep();}return 0;}

人脸识别

关于人脸识别部分官方并没有提供更多的示例，我们需要自己编写opencv的相关代码，网上的代码也比较多，这是我们所用的代码做一个参考

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from std_msgs.msg import String
import numpy as npclass cv:def __init__(self):    # 创建cv_bridge，声明图像的发布者和订阅者cascade_1 = rospy.get_param("~cascade_1", "")self.face_detector = cv2.CascadeClassifier(cascade_1)self.land = rospy.Publisher("face_detected", String, queue_size=10)self.bridge = CvBridge()self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.callback)self.faceFlag = Falsedef callback(self,data):# 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式try:cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")except CvBridgeError as e:print e# 显示Opencv格式的图像#cv_image=cv2.flip(cv_image,0)#cv_image=cv2.flip(cv_image,1)gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)faces = self.face_detector.detectMultiScale(gray, 1.2, 6)if len(faces)>0:for face in faces: x, y, w, h = facecv2.rectangle(cv_image, (x, y), (x+w, y+h), (255,255,0), 2)self.faceFlag = Truecv2.imshow("Image window", cv_image)if self.faceFlag:self.land.publish("T")cv2.waitKey(0)else:cv2.waitKey(30)if __name__ == '__main__':try:# 初始化ros节点rospy.init_node("cv_bridge_test")rospy.loginfo("Starting cv_bridge_test node")cv()rospy.spin()except KeyboardInterrupt:print "Shutting down cv_bridge_test node."cv2.destroyAllWindows()

识别效果