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

已于 2022-10-30 21:49:40 修改
阅读量2.4w 收藏 582
点赞数 67
分类专栏： 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: 速度控制 ↩︎

解矣。
关注
专栏目录
gazebo与PX4联合仿真
m0_49586319的博客
08-11 967
PX4和gazebo联合仿真
无人机仿真—PX4编译，gazebo仿真及简单off board控制模式下无人机起飞
LI2582957686的博客
03-18 2635
无人机PX4编译offboard模式简单起飞
【PX4-AutoPilot教程-TIPS】PX4飞控Gazebo仿真去除asphalt沥青地面
最新发布
在后厂村路努力科研搬砖的无人机控制方向练习生一名～
09-10 416
在使用PX4飞控进行仿真测试时，默认的Gazebo仿真环境通常会包含一个沥青地面（asphalt plane）。然而，在某些特定测试场景下，我们可能希望去除这一默认地面，以便测试无人机在不同环境下的表现，或者为了减少仿真中不必要的资源消耗。本文将介绍如何在Gazebo仿真中去除默认的沥青地面。
px4+gazebo无人机仿真，定点起飞，y=x²轨迹飞行
qq_36144028的博客
06-27 3388
如果启动之后，3号终端一直跳offboard enabled但是始终不见Vehicle armed，而且gazebo中无人机也没有反应，可能是PX4版本太新了(13.0)，解决办法是安装1.12.3。稍微等待一会，3号终端如下图所示，正常情况下能看到gazebo中的无人机升高到2m位置。将其剪切到.bashrc的最下方，保存关闭，然后终端进行刷新。其中eDP-1是显示屏名字，替换为自己的显示屏名字。之后问题解决，重新上面的启动步骤。到此前期准备已经完毕，可以启动。启动QGC，等待QGC连接成功。
px4的gazebo仿真教程
11-01
px4的gazebo仿真教程，详细介绍了单机的gazebo仿真和多机仿真的实现过程，适用于想使用ros进行多节点多机仿真的学习
使用Gazebo进行PX4的仿真
qq_73407651的博客
07-08 619
该文总结了其他多篇文章的方法，做了一个简单的记录，免得以后想要再做的时候又忘了怎么做了。这里主要针对Gazebo做工作，mavros这东西的安装不再详述，别的文章写的很清楚。先打开.bashrc文件，在底部添加一行：export PATH="$PATH: ~/.local/bin"这会编译一个名为px4_sitl_default的编译文件，并且用于gazebo。运行之后就会有报错numpy版本问题，先别急，让bug飞！上面是1.12.3的，需要其他版本的自行更改。有数据，在变化，那就差不多对的了。
基于PX4、ROS和Gazebo的无人机通用仿真平台
04-14
XTDrone项目中，ROS作为桥梁，将PX4的数据与Gazebo仿真环境连接起来，实现了无人机的虚拟飞行。 3. Gazebo：Gazebo是一款开源的3D仿真器，常用于机器人研究。它能模拟真实的物理环境，包括重力、摩擦力和碰撞检测等...
px4无人机-gazebo仿真实现移动物体的跟踪
10-31
综上所述，通过在Gazebo仿真环境中使用PX4无人机，可以实现对移动物体的跟踪。通过获取无人机和移动物体的位置信息，并根据位置信息计算出无人机的飞行动作，以实现跟踪移动物体的功能。同时，还可以添加一些算法来...
PX4 gazebo仿真 2023.4.13更新
private_Jack的博客
12-22 965
PX4配置gazebo仿真环境
Ubuntu 18.04 PX4 Gazebo仿真
RENyunfan
07-11 611
PX4 Gazebo仿真 1202年来看最好安装的版本还是melodic和PX4 clone https://github.com/PX4/PX4-Autopilot.git --recursive 如果因为网络问题没有一次下载完成，就可以进入目录里 git submodule update --init --recursive 随后 make px4_sitl gazebo 如果出现小飞机就成功了！ 如果不成功可以 make submodulesclean git submo
【PX4-GAZEBO半实物仿真平台——添加三轴可控云台完成视频采集并编码后发送到机载端】
Vesan的博客
07-31 655
在为无人机添加三轴云台控制模型时，可以先从PX4的模型库中找相应的载荷模型，由于PX4代码中没有对于的三轴云台模型，但有一款名为Typhoon H480 (Hexrotor)的六旋翼的sitl软件在环仿真的模型。此时我们去查看这个模型的SDF文件，可以看到这个SDF文件包括了无人机机体和载荷，是写在了一起，与以往的无人机和载荷分开写不太一致。由于之前的脚本的sitl软件在环仿真的，半实物hitl是由真实硬件飞控为载体，因此不同之处还是有很多的，这点可以对照着sitl的代码逻辑去看。实现对三轴云台的控制。
手把手教PX4新版本固件+gazebo+ros melodic无人机仿真添加单目相机
Trd_string的博客
04-07 1801
这里简单科普一下我们接下来要用的vim的用法，vim打开文件之后是不能直接进行编辑的，需要先按i键进入insert模式，之后再编辑，编辑完内容之后，按esc退出insert模式，再按：(shift+;画线的是要更改的，更改的原来的语句我把它注释在上一行了，意思就是你刚打开这个文件它应该显示的是画线上一行的语句（不带注释），你可以把它删掉或注释掉。编辑完毕保存退出即可，此时我们的无人机上已经添加了相机了，但是默认朝向是朝前的，这时我们需要更改一下朝向。圈出来的是要新加进去的，直接把我圈出来的加进去就可以了；
PX4+Gazebo仿真：通过键盘控制无人机飞行
lxxwkan的博客
08-01 844
实验环境：Ubuntu20.04，ROS noetic。
ROS系统下gazebo环境中，无人机结合SiamCar算法进行物体跟踪及轨迹评估的实现
fRmfNCtMavQj的博客
05-20 1402
本文将基于ROS系统中的gazebo仿真环境，利用四旋翼无人机和小车作为目标，结合SiamCar算法完成对物体的跟踪，并给出轨迹对比图等评估指标。通过在rviz上显示小车和无人机的运动轨迹，可以观察到跟踪效果，并保存小车和无人机运动过程中的坐标位置。同样地，在rviz上显示小车和无人机的运动轨迹，并保存坐标位置，以便后续的轨迹对比和评估。ROS系统下gazebo环境中，无人机结合目标跟踪算法（SiamCar），完成对物体的跟踪（可以是小车或者其他的），然后给出轨迹对比图等评估指标。
ROS&Gazebo自定义多旋翼无人机仿真——环境搭建和模型建立
qq_30858991的博客
09-28 5316
在ubuntu平台使用ROS、Gazebo搭建自定义多旋翼无人机可视化仿真环境，并使用自定义的六旋翼无人机模型，进行SITL软件在环仿真。无人机:自定义六旋翼固件：APM/PX4。
基于ROS的PX4+Gazebo仿真——PX4一键起飞及飞行控制
热门推荐
m0_64730542的博客
05-11 1万+
一键起飞和飞行控制
PX4 + Gazebo：多机仿真测试，Offboard模式控制
lxxwkan的博客
01-05 2950
多机飞行文件，这里需要对里边的内容修改。根据上边提示修改好自己的launch文件，进入自己的代码目录，进行编译（如果自己单机能跑，这一步可以省略）Source 环境（这一步需要大家自己看一下文件目录里的位置，每个版本可能存在一定的差异，更换到自己路径即可）没有什么问题的话应该是可以看到Gazebo里出现了四架飞机，然后初始位置和我们设置的相同。
（五）PX4+Gazebo硬件在环（HIL）仿真配置
拾柒.17
04-07 1581
本文介绍了Ubuntu 20.04下Gazebo+PX4的硬件在环（HIL）仿真流程。
评论 73
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值