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

别人家的孩子380

已于 2022-12-12 00:26:41 修改

阅读量1.5k

点赞数 2

文章标签：无人机 ubuntu c++ 计算机视觉 opencv

于 2022-12-12 00:22:11 首次发布

本文链接：https://blog.csdn.net/qq_60183616/article/details/128280479

版权

首先这算是我参加的比赛中算是赛前准备比较成体系的一个比赛吧，所以就想趁着还记得写一点心得，这个比赛是吉林省机器人大赛的一个分赛道，智能无人机仿真挑战赛，我的队伍也是在这个比赛中以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);//dyx
    ros::Subscriber lidar_sub = nh.subscribe<sensor_msgs::LaserScan>("/lidar2Dscan", 1000, lidar_cb);//dyx
    ros::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 arm
    int 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 takeoff
    int 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 frame
        Command_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 np

class 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 = False

    def 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 = face
                cv2.rectangle(cv_image, (x, y), (x+w, y+h), (255,255,0), 2)
            self.faceFlag = True
	    
        
        cv2.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()

识别效果