PX4无人机-Gazebo仿真实现移动物体的跟踪

已于 2022-10-30 21:49:40 修改
阅读量2.1w 收藏 512
点赞数 60
分类专栏： ROS 文章标签： 目标跟踪
于 2021-10-26 09:57:38 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_44939973/article/details/120965458
版权

原文链接PX4无人机-Gazebo仿真实现移动物体的跟踪末尾有演示视频

这个学期我们有一个智能机器人系统的课设，我们组分配到的题目是《仿真环境下使用无人机及相机跟踪移动物体》，本文主要记录完成该课设的步骤以及内容。我们采用的最终方案是PX4飞控+gazebo仿真+mavros通讯控制，实现了在gazebo环境下无人机跟踪一个移动的小车。本文所使用的是Ubuntu18.04 + melodic。

试验环境介绍

首先要搞懂各个部分的关系1，以及各自的作用，才能对控制无人机有个完整的认识，我在一开始做的时候就花了很多时间都没搞懂PX4到底是个无人机还是个什么东西，mavros又是干什么的。下面我简要介绍一下各个部分的关系，让大家有个大致的了解。

PX4飞控

PX4是一个飞控固件，所谓的飞控固件，就是能够向无人机发出控制命令，控制无人机的位姿、飞行速度以及螺旋桨的转速等等。无人机的运动就需要通过飞控固件发出命令来控制。官网的用户手册在这，推荐看英文版本，中文版本有的地方翻译的实在是太烂了，我看的时候感觉像是机翻的，而且与原文的位置都不太一样。

Gazebo仿真

gazebo仿真就不用多说了吧，在学ros基本的操作的时候就应该接触过gazebo。这就是一个能够模拟现实世界的仿真软件，PX4的源代码里就提供了PX4无人机的gazebo模型，通过launch文件直接运行就能得到一个gazebo下的无人机。

MAVROS通讯

mavros里面有个ros，一看就是和ros相关的。我们看官网的介绍MAVROS – MAVLink extendable communication node for ROS with proxy for Ground Control Station’，这句话的意思是，MAVROS是MAVLink为了让ROS代理控制站的扩展交流节点。首先MAVLink是一个无人机通讯协议，也就是说与无人机交流所发出的信号或数据格式都要符合该协议，与HTTP等协议是一个道理。然后控制站其实是PX4为了控制无人机所开发的一个图形化控制站，可以通过GUI的形式来操作无人机，给一般用户很好的体验。而这里是用来代理控制站，也就是说充当控制站来控制无人机。到这里就很明显了，MAVROS就相当于代码版的控制站，若想要通过ros节点开控制无人机的飞行，那就必须通过mavros这个包，在这个包内包含了控制无人机的消息格式等。

综上所述，整个无人机的控制逻辑就是，通过mavros向PX4飞控发送控制命令，PX4再将命令发送到无人机的各个组件，以控制无人机按照用户的逻辑进行运动。而该无人机就在gazebo中，在gazebo中可以看到无人机的运动情况。

实验过程

PX4无人机的安装

1、安装环境依赖

sudo apt install -y ninja-build exiftool python-argparse python-empy python-toml python-numpy python-yaml python-dev python-pip ninja-build protobuf-compiler libeigen3-dev genromfs xmlstarlet libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev

2、安装python依赖（melodic默认的是python2）

pip install pandas jinja2 pyserial cerberus pyulog numpy toml pyquaternion  -i https://pypi.tuna.tsinghua.edu.cn/simple

我安装的时候pyulog没装上，其他的都装上了，如果你们也有这个问题，就把其他的装上，pyulog后面还有个步骤会自动安装

3、安装ros与gazebo

这两个的安装就不多说了，如果没安装的话可以在网上先安装好这两个再继续操作。

4、安装mavros

sudo apt install ros-melodic-mavros ros-melodic-mavros-extras
wget https://gitee.com/robin_shaun/XTDrone/raw/master/sitl_config/mavros/install_geographiclib_datasets.sh
sudo chmod a+x ./install_geographiclib_datasets.sh
sudo ./install_geographiclib_datasets.sh #这步需要装一段时间,请耐心等待PX4配置

我在安装的时候出现两个问题。

第一个问题是sudo apt install包的时候一直出现404，未找到这个包，解决方案时sudo apt update，将软件源更新一下，很可能是原始的位置已经过期了，需要更新才能找到最新的位置。

第二个问题是最后一步的.sh文件执行太慢了，我挂那两三个小时都没结束，大概是因为被墙了吧。解决办法如下2：

  • 在 /usr/share 下目录新建 GeographicLib 目录。

  • 将 geoids gravity magnetic 三个文件夹拷贝到 /usr/share/GeographicLib 文件夹下面。

上述三个文件夹的链接在这，提取码：dje9

5、PX4安装

这里推荐使用gitee安装，非常感谢XTDrone团队3将代码放在了gitee上，我也使用过github安装，但总是断开连接，无数次重试才完整安装完成。

cd ~
git clone https://gitee.com/robin_shaun/PX4_Firmware
cd PX4_Firmware
git checkout -b xtdrone/dev v1.11.0-beta1
bash ./Tools/setup/ubuntu.sh --no-nuttx --no-sim-tools

将.gitmodules替换为如下内容（在PX4_Firmware文件夹中ctrl+h查看隐藏文件）4

[submodule "mavlink/include/mavlink/v2.0"]
	path = mavlink/include/mavlink/v2.0
	url = https://gitee.com/robin_shaun/c_library_v2.git
	branch = master
[submodule "src/drivers/uavcan/libuavcan"]
	path = src/drivers/uavcan/libuavcan
	url = https://gitee.com/robin_shaun/uavcan.git
	branch = px4
[submodule "Tools/jMAVSim"]
	path = Tools/jMAVSim
	url = https://gitee.com/robin_shaun/jMAVSim.git
	branch = master
[submodule "Tools/sitl_gazebo"]
	path = Tools/sitl_gazebo
	url = https://gitee.com/robin_shaun/sitl_gazebo.git
	branch = master
[submodule "src/lib/matrix"]
	path = src/lib/matrix
	url = https://gitee.com/robin_shaun/Matrix.git
	branch = master
[submodule "src/lib/ecl"]
	path = src/lib/ecl
	url = https://gitee.com/robin_shaun/ecl.git
	branch = master
[submodule "boards/atlflight/cmake_hexagon"]
	path = boards/atlflight/cmake_hexagon
	url = https://gitee.com/robin_shaun/cmake_hexagon.git
	branch = px4
[submodule "src/drivers/gps/devices"]
	path = src/drivers/gps/devices
	url = https://gitee.com/robin_shaun/GpsDrivers.git
	branch = master
[submodule "src/modules/micrortps_bridge/micro-CDR"]
	path = src/modules/micrortps_bridge/micro-CDR
	url = https://gitee.com/robin_shaun/micro-CDR.git
	branch = px4
[submodule "platforms/nuttx/NuttX/nuttx"]
	path = platforms/nuttx/NuttX/nuttx
	url = https://gitee.com/robin_shaun/NuttX.git
	branch = px4_firmware_nuttx-9.1.0+
[submodule "platforms/nuttx/NuttX/apps"]
	path = platforms/nuttx/NuttX/apps
	url = https://gitee.com/robin_shaun/NuttX-apps.git
	branch = px4_firmware_nuttx-9.1.0+
[submodule "platforms/qurt/dspal"]
	path = platforms/qurt/dspal
	url = https://gitee.com/robin_shaun/dspal.git
[submodule "Tools/flightgear_bridge"]
	path = Tools/flightgear_bridge
	url = https://gitee.com/robin_shaun/PX4-FlightGear-Bridge.git
	branch = master 
[submodule "Tools/jsbsim_bridge"]
	path = Tools/jsbsim_bridge
	url = https://gitee.com/robin_shaun/px4-jsbsim-bridge.git
[submodule "src/examples/gyro_fft/CMSIS_5"]
	path = src/examples/gyro_fft/CMSIS_5
	url = https://gitee.com/mirrors/CMSIS_5

再次执行子模块更新指令

git submodule update --init --recursive

编译

make px4_sitl_default gazebo

配置环境变量，注意路径的匹配，你若修改了文件夹名要进行对应的修改，第一个catkin_ws是自己的工作目录。

source ~/catkin_ws/devel/setup.bash
source ~/PX4_Firmware/Tools/setup_gazebo.bash ~/PX4_Firmware/ ~/PX4_Firmware/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4_Firmware
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4_Firmware/Tools/sitl_gazebo

下面我们就可以测试PX4无人机了，执行下面的命令

cd ~/PX4_Firmware
roslaunch px4 mavros_posix_sitl.launch

此时会打开gazebo环境，里面地面上有一个无人机。
gazebo无人机仿真
最后一步，测试无人机通讯

rostopic echo /mavros/state

若显示的消息中出现connected: True,则说明MAVROS与SITL通信成功。到此，无人机的配置就结束了。

移动小车的安装

由于实验要求的是实现移动物体的跟踪，因此我使用了一个可控制的小车来代替移动物体5，通过键盘控制节点可以控制小车在gazebo环境中移动。

本试验使用的是TurtleBot3小车，安装和控制移动都非常方便。

安装小车命令6

sudo apt-get install ros-melodic-turtlebot3-*

通过上述命令就安装好了TurtleBot小车，是不是很方便。

由于该小车有三种形态，所以还需要通过环境变量指定一种形态，否则无法运行，我实验中使用的是 burger形态，其他两种形态你们可以自己去修改，我下面都以burger形态小车来讲解。通过环境变量指定小车有一下两种方式，推荐第二种一劳永逸，但若要修改就需要进入.bashrc文件中修改

export TURTLEBOT3_MODEL=burger							# 每次打开新的终端都要执行
echo "export TURTLEBOT3_MODEL=burger" >> ~/.bashrc		#直接写入环境变量，打开终端每次都会自动执行

下面我们运行小车，测试一下小车控制

roslaunch turtlebot3_gazebo turtlebot3_world.launch

然后运行键盘控制节点

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

若没安装该节点需要先安装，执行下面的命令

sudo apt-get install ros-melodic-teleop-twist-keyboard

键盘控制节点控制小车
通过I L J K ,四个键可以控制小车的运动，详细的运动控制自己看控制台的输出。到此，移动小车的安装就已经完成。

PX4无人机添加摄像头以及配置的修改

通过上面的工作，我们完成了无人机和移动物体的配置，若要完成无人机通过相机追踪小车，那相机怎么能少得了呢？默认的PX4无人机是不带摄像头的，我们需要修改配置文件使其带上一个摄像头。

给PX4添加一个深度摄像机7

cd ~/PX4_Firmware/launch
cp mavros_posix_sitl.launch mavros_posix_sitl_cp.launch		# 不修改原始无人机文件，复制一个副本进行修改
gedit mavros_posix_sitl_cp.launch

我后面的修改操作都是基于副本，先复制一份然后操作副本，以保持源代码的结构

做如下改动

添加

<arg name="my_model" default="iris_downward_depth_camera"/>

修改

<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>

为

<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg my_model).sdf"/>
注意添加的代码需要在修改的上方

添加摄像头就完成了，但是该摄像头默认的分辨率是48 * 64，非常低，飞高一些就看不清地面的小车了，我们还需要修改一下摄像头的分辨率。

cd ~/PX4_Firmware/Tools/sitl_gazebo/models/depth_camera
gedit depth_camera.sdf

将对应部分修改为

<update_rate>10</update_rate>
...
<image>
<format>R8G8B8</format>
<width>400</width>
<height>400</height>
</image>

width与height对应的就是摄像头的分辨率，update_rate是图像的发布频率，由于把像素改高了，怕系统处理速度慢，因此把频率降低一倍。

下面我们来测试一下摄像头是否配置成功。

cd ~/PX4_Firmware
roslaunch px4 mavros_posix_sitl_cp.launch		# 注意我修改的都是副本，不要运行错了

无人机上的深度相机
然后打开rviz，新开一个终端输入

rviz

先添加一个接收图像的窗口
添加一个图像窗口
将图像的话题选择为/camera/rgb/image_raw，另一个对应的是深度图像，可以自己切换看看效果。

选择对应的话题
这个时候image窗口就会显示无人机摄像机拍摄下来的图像，然后在运行无人机的那个终端输入命令commander takeoff，观察该图像窗口，会随着无人机起飞变化。

无人机起飞测试
由于这个地面平坦，将小车放到这个环境中的话，小车会动不了，所以要将仿真环境改一下，改成原始的空仿真环境。

PX4仿真环境的配置文件是/home/ljw/PX4_Firmware/launch/posix_sitl.launch，与之前一样，我们不对原始文件进行修改，我们修改副本。

cd ~/PX4_Firmware/launch
cp posix_sitl.launch posix_sitl_cp.launch
gedit posix_sitl_cp.launch

将

<!-- Gazebo sim -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
	<arg name="gui" value="$(arg gui)"/>
	<arg name="world_name" value="$(arg world)"/>
	<arg name="debug" value="$(arg debug)"/>
	<arg name="verbose" value="$(arg verbose)"/>
	<arg name="paused" value="$(arg paused)"/>
	<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
</include>

修改为

<!-- Gazebo sim -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="gui" value="$(arg gui)"/>
    <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/empty.world"/>
    <arg name="debug" value="$(arg debug)"/>
    <arg name="verbose" value="$(arg verbose)"/>
    <arg name="paused" value="$(arg paused)"/>
    <arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
</include>

也就是将原本的gazebo的.world文件换成turtlebot3小车的empty.world文件，这个world里什么都没有。光这样修改还没有生效，因为我们修改的是副本，原始调用这个文件的文件也需要修改，调用这个文件的文件就是之前的mavros_posix_sitl_cp.launch

gedit mavros_posix_sitl_cp.launch

将

<include file="$(find px4)/launch/posix_sitl.launch">

修改为

<include file="$(find px4)/launch/posix_sitl_cp.launch">

到此，无人机的配置就已经完成，可以再次运行一次无人机，看看环境是否发生变化。

合并无人机和移动小车

在前面的步骤中，我们将无人机和移动小车都准备好了，下面我们就只要将无人机和小车放在同一环境中，就能开始我们的跟踪实验了。

通过阅读turtlebot3的启动文件/opt/ros/melodic/share/turtlebot3_gazebo/turtlebot3_empty_world.launch，可以看到启动小车的代码，我们将该代码添加到启动无人机的文件中，就能同时启动小车和无人机。

cd ~/PX4_Firmware/launch
gedit posix_sitl_cp.launch

添加如下代码

<!-- car model and parameter -->
    <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
    <arg name="x_pos" default="1.0"/>
    <arg name="y_pos" default="1.0"/>
    <arg name="z_pos" default="0.0"/>
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_burger.urdf.xacro" />

<!-- gazebo car model -->
<node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf" args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />

我们让小车出生在1 1位置，无人机默认出生在0 0 位置。为了让小车更容易被无人机识别，我们将小车全身改成黑色。

roscd turtlebot3_description
cd urdf
sudo gedit turtlebot3_burger.gazebo.xacro

注意选择自己的小车文件进行修改，我这里修改的是burger小车的。

将文件中所有<material>Gazebo/xxx</material>中的xxx全部改成Black。

到此，无人机与移动小车全部配置完毕执行，我们测试一下。

cd ~/PX4_Firmware
roslaunch px4 mavros_posix_sitl_cp.launch

无人机与小车
我们可以看到中间一个无人机和一个黑乎乎的小车，周围的环境已经变成了空的了。然后启动键盘控制节点可以控制小车的运动

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

控制无人机运动

通过阅读PX4官网的一个控制实例8，我们可以大致了解无人机运动的控制流程。我将实例代码复制过来并且添加注释，让大家对无人机控制代码有个理解。

/*
头文件，包括常见的geometry_msgs和mavros通信需要的mavros_msgs，添加上就行
*/
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>


/*
current_state表示的是无人机的状态，在主函数中订阅了对应的话题，这个状态就会不断更新，表示无人机当前的状态。state_cb就是对应的回调函数，会不断的执行，更新状态。现在先不用管为什么需要这个状态，后面的代码会解释。
*/
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;
    
	// 订阅无人机的状态
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    
    /* 
    发布一个geometry_msgs::PoseStamped的消息，需要知道的是，这个消息是控制无人机的一种方式，将指定坐标包裹进这个消息，然后发布出去，无人机就能自动飞行到指定的坐标地点
    */ 
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    
    /*
    无人机有一个锁，如果不解锁，无人机虽然接受了命令但是不会动被锁住了，只有解锁了才能对无人机进行控制，下面这个服务调用就是用来请求解锁无人机。上面的current_state就包含了无人机是否解锁的信息，若没解锁就需要解锁，否则就不用，其用途在这就体现出来
    */
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    
    /*
    无人机飞行有很多种模式，如果需要用代码操控无人机，我们就需要切换到OFFBOARD模式。上面的current_state也包含了无人机当前的飞行模式，若不是OFFBOARD就需要切换到该模式。下面的这个服务调用就是用来请求切换无人机飞行模式。
    */
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    /*
    在OFFBOARD模式下，需要以>=2HZ的频率向无人机发送消息，否则无人机会回退到OFFBOARD模式之前所在的模式，因此这里的rate需要设置的比2大就行
    */
    ros::Rate rate(20.0);
	
    // wait for FCU connection
    /*
    等待无人机与控制站连接（代码的方式就是代理），只有连接了才能发送消息
    */
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    /*
    pose就是坐标，本实例是让无人机在2m处悬空，因此z设置为2，z表示的就是高度
    */
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    // 下面这个感觉有没有都无所谓
    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    // 请求的切换模式的消息，设置为OFFBOARD
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    // 请求解锁的消息，arm表示解锁，设置为true，disarm是上锁
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    // 记录上次请求的时间
    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        // 如果无人机模式不是OFFBOARD并且离上次操作时间大于5秒就发送请求切换，这里的5s是为了演示清楚设置的延时
        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 {
            // 如果当前未解锁且与请求时间大于5s，就发送请求解锁
            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();
            }
        }
		
        // 不断发送位置消息，但是只有解锁后才能真正开始运动，如果不发送就会退出OFFBOARD模式，因为请求发送速度要>=2HZ
        local_pos_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

通过官网的代码就能知道控制无人机飞行的一般流程，只要将核心代码逻辑修改一下，就能实现无人机对小车的跟踪了。可以先将上面的代码运行一次，观察一下无人机的运动。

总体流程为：

1、通过launch文件启动无人机的gazebo环境

2、新建一个工作空间，包含的依赖有roscpp、std_msgs、geometry_msgs、mavros、cv_bridge、image_transport、sensor_msgs

其中后面几个是为了后续图像处理用的，这里一并导入

3、创建一个cpp文件，将上述代码复制进去

4、修改CMakeLists

5、rosrun该节点

在无人机解锁后就能看到无人机飞到了空中2m处

控制无人机跟踪运动小车

通过上述无人机悬空的代码我们已经了解了控制无人机飞行的代码流程：首先定义好需要发送与接收的话题消息，并且定义好各个请求，然后将无人机切换到OFFBOARD模式，接着解锁无人机，同时需要一直给无人机发送运动控制的消息，包括位置控制或速度控制，并且频率要大于2HZ。通过以上流程框架，我们就能设计一个自动跟踪移动小车的代码。

通过网上查阅资料看到，控制无人机运动不仅可以通过发送位置消息9，还可以像控制小乌龟一样，发送速度消息10，这就为跟踪小车的提供了方案。我设计的跟踪思路：

之前案例订阅和发布的话题就不用多说了，全部都要订阅，然后需要额外订阅的是无人机发送的图像，之前设置了10HZ，也就是一秒钟无人机会发送十帧图像过来，在该订阅的回调函数内，对图像进行处理，检查图像内是否有小车，如果有的话，就通过比较小车像素点的位置和图像中心像素点的位置，来判断方位，并相应的设置速度。图像处理回调函数如下：

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/Altitude.h>


#include "sensor_msgs/Image.h"
#include "cv_bridge/cv_bridge.h"

#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
// 一些全局变量

// 悬空高度（追踪小车的高度）
const double h = 4;
// 调整高度的速度（上升或下降）
const double hv = 0.1;

// 控制无人机的速度
geometry_msgs::Twist velocity;

// 无人机当前的高度
double curH;

// 无人机是否已经稳定在空中的标志
bool start = false;

void doImg(const sensor_msgs::Image::ConstPtr &msg) {
    
    if(!start) return;
    
    // 将无人机发布的图像先转化为灰度图，再进行二值化，就能得到黑白图像，若小车出现，那么在图像内有黑色的像素，否则图像全是白色像素，这也是我将小车改成黑色的原因，若改成其它颜色就不好进行分离
    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
	cv::Mat img = cv_ptr -> image;
    cv::Mat gray, bin;
    cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
    cv::threshold(gray, bin, 127, 255, cv::THRESH_BINARY);
    
    // 获得图像的宽和高
    static int row = bin.rows, col = bin.cols;
    // 图像中心点的位置，我们假设图像中心点的位置就是无人机的位置，这样就能很方便的发布速度来控制无人机
    static double centX = row / 2, centY = col / 2;
    
    // x y用来记录小车在该帧图像出现的位置
    int x, y;
    // 是否找到小车的标记
    bool findCar = false;
    
    // 遍历图像，若图像内有黑色像素则代表发现了小车，记录下此时的x y位置
    for(int i = 0; i < row; i++) {
        for(int j = 0; j < col; j++) {
            uchar point = bin.at<uchar>(i, j);
            if(point == 0) {
                findCar = true;
                x = i, y = j;
                break;
            }
        }
        if(findCar) break;
    }
    
    // 记录最后一次找到小车的时间
    static ros::Time last_find_time = ros::Time::now();
    if(findCar) {
        ROS_INFO("找到目标位置, x = %d, y = %d", x, y);
        // 将小车（所在像素点）相对无人机（图像中心像素点）的位置归一化到0 ~ 1之间，并以此作为控制无人机的速度，小车离无人机越远，则无人机的速度越大，否则无人机的速度越小
        double vx = abs(centX - x) / centX;
        double vy = abs(centY - y) / centY;
        
        // 经测试，无人机发送的图像的垂直方向是无人机的x方向，图像的水平方向是无人机的y方向
        // 因此，若小车（像素位置）在无人机（像素位置）上方，需要发送一个正的x方向速度，否则要发送一个负方向的速度
        if(x < centX) velocity.linear.x = vx;
        else velocity.linear.x = -vx;
        
		// y方向同理
        if(y < centY) velocity.linear.y = vy;
        else velocity.linear.y = -vy;

        // 若不给无人机发送z方向的速度，无人机会时上时下，因此通过下面这个代码控制无人机高度，若低于一定高度，就发布z方向的速度，若高于某个高度，就发送一个-z方向的速度，让无人机下降
        if(curH < h - 0.5) velocity.linear.z = hv;
        else if(curH < h + 0.5) velocity.linear.z = 0;
        else velocity.linear.z = (curH - h) * -hv;
        ROS_INFO("发布速度 x : %f, y : %f, z : %f", velocity.linear.x, velocity.linear.y, velocity.linear.z);
        // 记录无人机最后一次发现小车的时间，后面有用
        last_find_time = ros::Time::now();
    } else {
        ros::Time now = ros::Time::now();
        velocity.linear.x = 0;
        velocity.linear.y = 0;
        // 无人机丢失目标五秒内，什么都不操作
        if(now - last_find_time < ros::Duration(5)) {
            ROS_INFO("没有找到目标...");
        } else {
            // 无人机丢失目标五秒后，开始向上飞行（扩大视野）来搜寻小车，搜寻的最高高度是无人机跟踪小车高度的两倍，这也是前面代码中控制无人机下降的原因，若无人机在升空过程中发现目标小车，会立刻下降跟踪小车
            if(curH < 2 * h - 1) {
                ROS_INFO("上升高度寻找，当前高度为：%.2f", curH);
                velocity.linear.z = hv;
            } else {
                if(curH > 2 * h + 1) velocity.linear.z = -hv;
                else velocity.linear.z = 0;
                ROS_INFO("目标丢失。。。");
            }
        }
    }
}

上面的回调函数完成了对无人机追踪小车速度的控制，其运行逻辑是：若无人机发现了小车，就通过小车相对无人机的方位，发送x y方向的速度，否则如果丢失的话，在五秒内不进行任何操作，超过五秒后，开始提升无人机的高度扩大视野来寻找小车，最大高度是跟踪小车高度的两倍，一旦发现小车立刻下降并跟踪小车。

上面只是一个回调函数，还要主函数来控制无人机。

void do_H(const mavros_msgs::Altitude::ConstPtr& msg) {
    curH = msg->local;
}

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;
    setlocale(LC_ALL, "");

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::Publisher local_vec_pub = nh.advertise<geometry_msgs::Twist>
            ("/mavros/setpoint_velocity/cmd_vel_unstamped", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");
    ros::Subscriber img_sub = nh.subscribe<sensor_msgs::Image>("/camera/rgb/image_raw", 10, doImg);

    ros::Subscriber height_sub = nh.subscribe<mavros_msgs::Altitude>
            ("/mavros/altitude", 10, do_H);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = h;

    velocity.linear.x = 0;
    velocity.linear.y = 0;
    velocity.linear.z = 0;

    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();

    bool takeoff = false;

    while(ros::ok()){
        if(!takeoff) {
            if( current_state.mode != "OFFBOARD" &&
                (ros::Time::now() - last_request > ros::Duration(2.0))){
                if( set_mode_client.call(offb_set_mode) &&
                    offb_set_mode.response.mode_sent){
                    ROS_INFO("Offboard enabled");
                }
                last_request = ros::Time::now();
            }

            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(2.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }

            if( current_state.armed && 
                (ros::Time::now() - last_request > ros::Duration(5.0))) {
                    takeoff = true;
                    ROS_INFO("Vehicle stabled");
                    start = true;
                    ROS_INFO("开始追踪...");
                    last_request = ros::Time::now();
                }

            local_pos_pub.publish(pose);

        } else {
            local_vec_pub.publish(velocity);
        }

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

上述代码的逻辑比较好理解，我就不加注释，流程是：先通过位置控制无人机，让无人机在高度h处稳定悬空，当无人机稳定悬停在空中时，将控制无人机的方式改为速度控制，也就是通过发送速度来控制无人机。

为了方便讲解，将代码分成了两部分贴，将两部分合起来放在一个cpp里，就能正常执行。代码中无人机是否稳定悬空是通过一个时间延迟实现的，假定无人机能在五秒内悬停在指定点，五秒前都是通过位置控制无人机，五秒后就一直通过速度控制无人机。

到此，无人机跟踪运动小车的整个实验就完成了。

参考

  1. APM,PX4,GAZEBO,MAVLINK,MAVROS,ROS之间的关系以及科研设备选型 ↩︎

  2. 执行 install_geographiclib_datasets.sh 错误！ ↩︎

  3. XTDrone仿真平台基础配置 ↩︎

  4. Ubuntu18.04下基于ROS和PX4的无人机仿真平台的基础配置搭建 ↩︎

  5. 在gazebo中导入移动小车+二维码 ↩︎

  6. ROS-melodic学习turtlebot3笔记＜一功能包导入与测试＞ ↩︎

  7. PX4+gazebo仿真给无人机添加摄像头 ↩︎

  8. MAVROS Offboard control example ↩︎

  9. 使用ROS节点控制PX4——位置控制 ↩︎

  10. PX4学习笔记3: 速度控制 ↩︎

解矣。
关注
  • 60
    点赞
  • 踩
  • 512
    收藏
    觉得还不错? 一键收藏
  • 66
    评论
专栏目录
无人机学习之gazebo仿真文件
yyl80的博客
08-20 2965
《一》仿真文件 SDF文件、xacro文件（gazebo可以显示）、urdf文件（在rviz显示）。三个文件都可以加载dae等三位模型文件。 《二》model模型文件 database.config数据坤的元数据 xxx.config模型的元数据 xxx.sdf模型的SDF文件描述 plugin文件夹 《三》模型的.config 的内容 name:名字 version：版本 sdf:描述模型的SD...
《XTDrone无人机仿真开发》，13节，每节课两小时到三小时 与文档同款01-1预习-无人机与XTDrone简介
03-11
ROS+PX4+gazebo+mavros-国内顶级无人机仿真平台-XTDrone搭建教程。 XTDrone代码:XTDrone: 基于PX4、ROS和Gazebo的无人机通用仿真平台 (gitee.com) XTDrone使用文档:仿真平台基础配置 (yuque.com) 看readme.txt 01_1预习-无人机与XTDrone简介 02_2预习-搭建XTDrone 03_3无人机动态模型与仿真实现 04_4无人机传感器模型与仿真实现 05_5飞行控制器与PX4的基本使用 06_6XTDrone基础配置 07_7视觉SLAM 08_8视觉SLAM 09_9激光SLAM 10_10运动规划 11_11目标检测与跟踪 12_12无人机精准降落 13_13无人机集群编队
ROS+PX4无人机室内多点飞行代码
08-10
整体流程如下： 无人机起飞后请求进入offboard模式，紧接着请求解锁，解锁后飞行至0.3米高，紧接着逆时针飞行边长为0.5米的正方形，每个边长飞行8秒钟。 完成正方形后自动进入降落模式，全程无需手动。 已经在实体无人机上测试过多次。 该程序的对比官方程序要实用的多，程序中添加了模式切换判断，成功以后不会重复切换，遥控器可以直接进行接管控制，安全性比起官方提供程序要高得多，强烈建议新手或者刚接触不久的朋友采用这个功能包。
开源飞控之王PX4
u013558123的博客
03-25 830
PX4作为开源飞控的代表，凭借其卓越的性能和广泛的应用场景，赢得了众多无人机爱好者和开发者的青睐。相信在未来的发展中，PX4将继续引领无人机飞行控制技术的前沿，为无人机应用带来更多的可能性。
无人机仿真-PX4固件编译、3D仿真环境gazebo及offboard模式下控制无人机运动
习惯积淀的博客
03-23 4237
无人机仿真-PX4固件编译、3D仿真环境gazebo及offboard模式控制无人机运动 这篇博客讲的是如何使用仿真环境进行无人机的仿真和控制，如果还没搭建好仿真环境，可以参考我的上一篇博客，欢迎点赞和留言???? PX4固件的编译、上传以及载入gazebo仿真器 在安装完环境后，就可以对PX4固件进行编译了 cd /Firmware #进入你的机器上下载PX4固件的目录 make px4_sit...
（学习笔记）机器人自主导航从零开始第四步———Rviz、Gazebo、Meshlab的安装
qq_59475883的博客
02-21 4074
一、Rviz的安装 Rviz是一个
基于PX4、ROS和Gazebo的无人机通用仿真平台
04-14
基于PX4、ROS和Gazebo的无人机通用仿真平台
无人机仿真—PX4编译，gazebo仿真及简单off board控制模式下无人机起飞
LI2582957686的博客
03-18 2165
无人机PX4编译offboard模式简单起飞
PX gazebo仿真提示找不到px4， mavlink_sitl_gazebo等软件包
理想主义者的胜利的博客
05-17 1171
能够正确执行make px4_sitl_default gazebo，但不能正确执行roslaunch px4 posix_sitl.launch。例如提示找不到px4，mavlink_sitl_gazebo等软件包。
【ROS&GAZEBO】多旋翼无人机仿真（三）——自定义多旋翼模型
Reed Liao
03-03 7959
【ROS&GAZEBO】多旋翼无人机仿真（一）——搭建仿真环境 【ROS&GAZEBO】多旋翼无人机仿真（二）——基于rotors的仿真 【ROS&GAZEBO】多旋翼无人机仿真（三）——自定义多旋翼模型 上一篇介绍了如何用rotors进行仿真，这一篇继续介绍在ROS&GAZEBO中如何自定义多旋翼模型。 搭建模型 在GAZEBO搭建模型第一步是建一个模型，可以在GAZEBO中直接设计模型，也可以通过导入STL文件建立模型，这里我选择后者，用SolidWorks设计了一个四旋
四旋翼无人机仿真之hector_quadrotor（ROS+Gazebo）（二）键盘teleop_twist_keyboard控制无人机飞行
weixin_48719406的博客
02-17 2530
四旋翼无人机仿真之hector_quadrotor（二）键盘teleop_twist_keyboard控制无人机飞行
PX4源码解读
09-30
对开源飞控自己的理解，适合新入门阅读，有很高的借鉴意义！
mavros_controllers:使用启用了PX4的车辆的mavros进行的侵略性轨迹跟踪
05-03
mavros_controllers 在脱机模式下使用软件包控制MAV的控制器。 概述 该存储库包含用于使用mavros软件包控制MAV的控制器。 存储库中包含以下软件包 geometric_controller：基于几何控制的轨迹跟踪控制器 controller_msgs：定制消息定义 trajectory_publisher：节点将设定点发布为运动原语/轨迹的状态，以供控制器遵循。 入门 安装PX4 SITL（仅用于模拟） 遵循所示的说明，要检查是否正确设置了必要的环境，可以使用以下命令运行gazebo SITL cd < Firmware> DONT_RUN=1 make px4_sitl_default gazebo 要获取PX4环境的源代码，请运行以下命令 cd < Firmware> source ~ /catkin_ws/d
PX4无人机控制文件，offb_node.zip
10-06
运行px4，文件1是从一个点到另一个点，文件2是输入多个点让飞机在gazebo里仿真飞行，参考博客：https://mp.csdn.net/mp_blog/creation/editor/120622663
Simulink程序：使用ROS2控制无人机沿自定义圆形轨迹飞行并在Gazebo中可视化
02-22
文章【PX4&Simulink&Gazebo联合仿真】在Simulink中使用ROS2控制无人机沿自定义圆形轨迹飞行并在Gazebo中可视化对应的Simulink源程序。 https://blog.csdn.net/weixin_45031928/article/details/136215025 microRTPS桥接工具由运行在PX4上的客户端和运行在计算机上的服务端组成，它们进行通信以提供uORB和ROS2话题格式之间的双向数据交换和话题转换。使得可以创建直接与PX4的uORB话题接口的ROS2订阅服务器或发布服务器节点。这样在Ubuntu中就生成了一个可以调用uORB话题接口的ROS2节点，这个节点可以和运行在同一局域网下的Matlab/Simulink上的ROS2节点进行通信，以实现PX4&Simulink&Gazebo联合仿真。
基于PX4ROS和Gazebo的无人机通用仿真平台
04-11
通过结合PX4飞控系统、ROS机器人操作系统和Gazebo仿真环境，XTDrone能够模拟多种飞行器和无人系统的行为，从而帮助开发人员验证他们的算法和控制方案。在XTDrone上验证过的算法经过充分测试和验证，可以确保其在真实...
基于PX4、ROS和Gazebo的无人机通用仿真平台源码.zip
最新发布
04-23
基于PX4、ROS和Gazebo的无人机通用仿真平台源码 XTDrone是基于PX4、ROS与Gazebo的无人机通用仿真平台。支持多旋翼飞行器（包含四轴和六轴）、固定翼飞行器、复合翼飞行器（包含quadplane，tailsitter和tiltrotor）与...
px4四旋翼无人机控制和编队飞行控制gazebo软件在环仿真
08-03
px4四旋翼无人机控制和编队飞行控制gazebo软件在环仿真
ROS-noetic + MAVROS + PX4-1.12.3+gazebo多机协同仿真
09-01
本文件所有代码已经调试好，默认使用者的Ubuntu已经装好了PX4-Autopilot，使用方法： 1.首先source一下本工作空间：source ~/multi_uav_test202206/devel/setup.bash 2.打开QGC，没安装就自己下载； 2.打开终端输入...
px4无人机-gazebo仿真实现移动物体的跟踪
10-31
PX4无人机以及Gazebo仿真环境是目前广泛使用的无人机软件和仿真平台，能够帮助开发者进行无人机飞行控制算法的开发和测试。在使用PX4和Gazebo进行仿真时，可以通过编写代码实现无人机跟踪移动物体的功能。 首先，需要在Gazebo仿真环境中创建一个场景，并将无人机和移动物体添加到场景中。可以使用Gazebo自带的模型库或者自定义模型来创建无人机和移动物体。然后，将无人机与Gazebo和PX4进行连接，以便将无人机的状态信息传输到Gazebo仿真环境中。 其次，需要编写代码来实现无人机对移动物体的跟踪。首先，需要获取无人机和移动物体的位置信息。可以通过Gazebo提供的API或者PX4提供的功能来获取无人机和移动物体的位置。然后，根据无人机和移动物体的位置信息，计算出无人机需要采取的飞行姿态和动作来实现跟踪移动物体的功能。最后，通过控制无人机的舵面、电机等飞行控制设备，实现无人机的飞行动作。 在跟踪移动物体的过程中，还可以添加一些算法来提高跟踪的准确性和稳定性。例如，可以使用视觉传感器来实时识别和跟踪移动物体，通过视觉算法和控制算法来控制无人机的飞行动作。另外，还可以使用数据融合算法，将无人机的惯性传感器数据和视觉传感器数据进行融合，提高跟踪的鲁棒性和精确性。 综上所述，通过在Gazebo仿真环境中使用PX4无人机，可以实现对移动物体的跟踪。通过获取无人机和移动物体的位置信息，并根据位置信息计算出无人机的飞行动作，以实现跟踪移动物体的功能。同时，还可以添加一些算法来提高跟踪的准确性和稳定性。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 66
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值