【PX4｜mavros】Gazebo 多机仿真｜C++ 类封装｜位置控制

宇宙爆肝锦标赛冠军

个人专栏：PX4

发布时间 2021.08.24阅读数 6118 评论数 5

官方手册参考链接：Gazebo 多机仿真

1. 运行多机仿真

# Clone the PX4/PX4-Autopilot code
cd PX4-Autopilot
git submodule update --init --recursive
# Source your environment（这样设置的环境变量只在当前终端有效）
source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd):$(pwd)/Tools/sitl_gazebo
# 运行启动文件
roslaunch px4 multi_uav_mavros_sitl.launch

# 永久配置环境变量
gedit ~/.bashrc
# 打开文件后在最后添加如下两行：
source ~/PX4-Autopilot/Tools/setup_gazebo.bash ~/PX4-Autopilot ~/PX4-Autopilot/build/px4_sitl_default
ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4-Autopilot:~/PX4-Autopilot/Tools/sitl_gazebo

# 查看话题消息
rostopic list
# 可以看到话题名称都是在最前面加上 uav0 ，uav1，uav2

2. 分析启动文件

该启动文件位于 https://github1s.com/PX4/PX4-Autopilot/blob/master/launch/multi_uav_mavros_sitl.launch

先是声明一些变量并赋值：

<!-- MAVROS posix SITL environment launch script -->
<!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
<!-- vehicle model and world -->
<arg name="est" default="ekf2"/>
<arg name="vehicle" default="iris"/>
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
<!-- gazebo configs -->
<arg name="gui" default="true"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>

接着在 Gazebo 中加载一个空世界：

<!-- Gazebo sim -->
<!-- 在gazebo中加载一个世界 -->
<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)"/>
</include>

设置无人机的 ID 和飞控的 UDP 端口（注意与后面的 mavlink 的 UDP 端口不同）：

<arg name="ID" value="0"/>
<arg name="fcu_url" default="udp://:14540@localhost:14580"/>

运行另一个启动文件 single_vehicle_spawn.launch ，传入一些参数：

<!-- PX4 SITL and vehicle spawn -->
<!-- 从 xacro 创建 urdf 模型, 加载gazebo模型并运行 px4 sitl 应用程序实例 -->
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
  <arg name="x" value="0"/>
  <arg name="y" value="0"/>
  <arg name="z" value="0"/>
  <arg name="R" value="0"/>
  <arg name="P" value="0"/>
  <arg name="Y" value="0"/>
  <arg name="vehicle" value="$(arg vehicle)"/>
  <arg name="mavlink_udp_port" value="14560"/>
  <arg name="mavlink_tcp_port" value="4560"/>
  <arg name="ID" value="$(arg ID)"/>
  <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
  <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
  <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
</include>

运行 mavros 节点：

<!-- MAVROS -->
<!-- 运行mavros节点 -->
<include file="$(find mavros)/launch/px4.launch">
  <arg name="fcu_url" value="$(arg fcu_url)"/>
  <arg name="gcs_url" value=""/>
  <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
  <arg name="tgt_component" value="1"/>
</include>

1 号和 2 号无人机同理，只是编号和端口号存在区别，可以复制以调整为自己需要的飞机架数。

3. C++ 类实现多机仿真

3.1 前言

在实现多无人机仿真时希望能够更加方便的控制无人机，所以使用类来实现。

Visual Stdio 这款智能编译器导致很多人并不知道如何编写复杂的 CMakeLists.txt ，将 cpp 文件链接到 .h 头文件。

首先，类的声明要写在 .h 文件中，其次，类函数的定义要写在 cpp 文件中。那头文件和 cpp 文件又要放在哪个文件夹下？

网上关于 CMakeLists.txt 的讲解大都不直击要点，无法解决我们实际使用中的痛点。

参考 Github 上已有的成熟的 ROS 功能包的实现可以帮助我们理解 CMakeLists.txt 的写法。

3.2 功能包目录结构

zth@ubuntu:~/ws_px4$ tree
.
└── src
    ├── CMakeLists.txt -> /opt/ros/melodic/share/catkin/cmake/toplevel.cmake
    └── offboard
        ├── CMakeLists.txt
        ├── include
        │   └── offboard
        │       └── Uav.h
        ├── launch
        │   └── multi_uav_mavros_sitl.launch
        ├── package.xml
        └── src
            ├── lib
            │   └── Uav.cpp
            ├── uav0.cpp
            └── uav1.cpp

3.3 类头文件

// 文件名：Uav.h
#pragma once

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

class Uav
{
public:
    Uav(int id);
    void state_cb(const mavros_msgs::State::ConstPtr& msg);
    void wait_for_FCU();
    void send_setpoints();
    void set_mode();
    void set_arm_cmd();
    void init();
    void enable_offboard();
    void enable_armed();
    void fly_2_target(double x, double y, double z);


    ros::Subscriber state_sub;
    ros::Publisher local_pos_pub;
    ros::ServiceClient arming_client;
    ros::ServiceClient set_mode_client;

    mavros_msgs::State current_state;
    geometry_msgs::PoseStamped pose;
    mavros_msgs::SetMode offb_set_mode;
    mavros_msgs::CommandBool arm_cmd;

    ros::NodeHandle nh;

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


private:

};

3.3 类定义文件

// 文件名：Uav.cpp
#include <offboard/Uav.h>
#include <string>

Uav::Uav(int id=0):nh(), rate(20.0)
{
    std::string name = "uav" + std::to_string(id);

    state_sub = nh.subscribe<mavros_msgs::State>
            (name + "/mavros/state", 10, &Uav::state_cb, this);
    local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            (name + "/mavros/setpoint_position/local", 10);
    arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            (name + "/mavros/cmd/arming");
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            (name + "/mavros/set_mode");

    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;
}

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

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

//send a few setpoints before starting
void Uav::send_setpoints()
{
    for(int i = 100; ros::ok() && i > 0; --i)
    {
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }
}

void Uav::set_mode()
{
    offb_set_mode.request.custom_mode = "OFFBOARD";
}

void Uav::set_arm_cmd()
{
    arm_cmd.request.value = true;
}

void Uav::init()
{
    wait_for_FCU();
    send_setpoints();
    set_mode();
    set_arm_cmd();
}

void Uav::enable_offboard()
{
    if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
    {
        ROS_INFO("Offboard enabled");
    }
}

void Uav::enable_armed()
{
    if( arming_client.call(arm_cmd) && arm_cmd.response.success)
    {
        ROS_INFO("Vehicle armed");
    }
}

void Uav::fly_2_target(double x, double y, double z)
{
    pose.pose.position.x = x;
    pose.pose.position.y = y;
    pose.pose.position.z = z;

    local_pos_pub.publish(pose);
}

3.4 节点文件

节点文件中包含主函数。

// 文件名：Uav0.cpp
#include <offboard/Uav.h>

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

    Uav uav0(0);
    uav0.init();

    ros::Time last_request = ros::Time::now();

    while(ros::ok())
    {
        if( uav0.current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            uav0.enable_offboard();
            last_request = ros::Time::now();
        } 
        else 
        {
            if( !uav0.current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
            {
                uav0.enable_armed();
                last_request = ros::Time::now();
            }
        }

        uav0.fly_2_target(4, -4, 6);

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

    return 0;
}

// 文件名：Uav1.cpp
#include <offboard/Uav.h>

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

    Uav uav0(1);
    uav0.init();

    ros::Time last_request = ros::Time::now();

    while(ros::ok())
    {
        if( uav0.current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            uav0.enable_offboard();
            last_request = ros::Time::now();
        } 
        else 
        {
            if( !uav0.current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
            {
                uav0.enable_armed();
                last_request = ros::Time::now();
            }
        }

        uav0.fly_2_target(4, 4, 6);

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

    return 0;
}

3.5 CMakeList.txt


cmake_minimum_required(VERSION 3.0.2)
project(offboard)

find_package(catkin REQUIRED COMPONENTS
  mavros_msgs
  roscpp
)

catkin_package(
  # INCLUDE_DIRS include
  # LIBRARIES offboard
  # CATKIN_DEPENDS mavros_msgs roscpp
  # DEPENDS system_lib
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_library(offboard
  src/lib/Uav.cpp
)

# uav0
add_executable(uav0 
  src/uav0.cpp
)
add_dependencies(uav0
  offboard
)
target_link_libraries(uav0
   offboard
  ${catkin_LIBRARIES}
)

# uav1
add_executable(uav1
  src/uav1.cpp
)
add_dependencies(uav1
  offboard
)
target_link_libraries(uav1
   offboard
  ${catkin_LIBRARIES}
)

3.6 launch 文件

<!-- 文件名：multi_uav_mavros_sitl.launch -->
<?xml version="1.0"?>
<launch>
    <!-- MAVROS posix SITL environment launch script -->
    <!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
    <!-- vehicle model and world -->
    <arg name="est" default="ekf2"/>
    <arg name="vehicle" default="iris"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
    <!-- gazebo configs -->
    <arg name="gui" default="true"/>
    <arg name="debug" default="false"/>
    <arg name="verbose" default="false"/>
    <arg name="paused" default="false"/>
    <!-- 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)"/>
    </include>
    <!-- UAV0 -->
    <group ns="uav0">
        <!-- MAVROS and vehicle configs -->
        <arg name="ID" value="0"/>
        <arg name="fcu_url" default="udp://:14540@localhost:14580"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
            <arg name="x" value="0"/>
            <arg name="y" value="0"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="$(arg vehicle)"/>
            <arg name="mavlink_udp_port" value="14560"/>
            <arg name="mavlink_tcp_port" value="4560"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
        </include>
        <!-- MAVROS -->
        <include file="$(find mavros)/launch/px4.launch">
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group>
    <!-- UAV1 -->
    <group ns="uav1">
        <!-- MAVROS and vehicle configs -->
        <arg name="ID" value="1"/>
        <arg name="fcu_url" default="udp://:14541@localhost:14581"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
            <arg name="x" value="1"/>
            <arg name="y" value="0"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="$(arg vehicle)"/>
            <arg name="mavlink_udp_port" value="14561"/>
            <arg name="mavlink_tcp_port" value="4561"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
        </include>
        <!-- MAVROS -->
        <include file="$(find mavros)/launch/px4.launch">
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group>
    <node pkg="offboard" type="uav0" name="uav0" output="screen"/>
    <node pkg="offboard" type="uav1" name="uav1" output="screen"/>
</launch>

3.7 测试效果

catkin_make # 编译工作空间

roslaunch offboard multi_uav_mavros_sitl.launch # 运行启动文件

通过 QGC 可以监控无人机：

可查看当前正在运行的节点：

rosnode list

ROS gazebo PX4 mavros uav

原创文章作者：宇宙爆肝锦标赛冠军。如若转载，请注明出处：古月居 https://www.guyuehome.com/34953

打赏 2

上一篇：【工业机器人｜知识要点（三）】离线编程 & 智能感知与自动编程

下一篇：【工业机器人｜知识点（四）】搬运机器人

【PX4｜mavros】Gazebo 多机仿真｜C++ 类封装｜位置控制

宇宙爆肝锦标赛冠军

1. 运行多机仿真

2. 分析启动文件

3. C++ 类实现多机仿真

3.1 前言

3.2 功能包目录结构

3.3 类头文件

3.3 类定义文件

3.4 节点文件

3.5 CMakeList.txt

3.6 launch 文件

3.7 测试效果

为你推荐

四足机器人优化方法初探：最小二乘法的基本理解

【Matlab控制系统仿真（5）】bode图的串联超前校正

机械臂开发之正运动学

多传感器融合定位理论基础（十二）：滑动窗口原理及应用

结构光综述

(16) 主成分分析 Principal Component Analysis - PRML && CS229

评论（5）

关于作者

宇宙爆肝锦标赛冠军

80

0

8

5

【三维轨迹规划与控制系统设计（二）】伺服驱动器与变频器的基本原理与接线&基于Qt的上位机软件设计（上）

【PCL点云库教程（一）】PCL的安装和预备知识

精选【基于 QT5 的 ROS2 GUI 开发教程（一）】话题消息的发布和订阅

相关推荐

语音信号处理 | 使用短时能量和谱质心特征进行端点检测

用scikit-learn和pandas学习线性回归

基于Gazebo的四足机器人步态规划仿真

RViz2可视化传感器数据系列教程之二：在RViz2中显示激光雷达点云数据

Python版msckf算法使用测试

鲁棒控制理论（十二）无人机纵向控制

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

TA的专栏

边缘计算

ChatGPT-for-Robotics

3D视觉

Pybind11

mmdetection

实例分割

Webots

移动机器人

coppeliaSim

PX4

目标检测

Coppeliasim｜轨迹规划

图像与点云融合教程

三维轨迹规划与控制系统

PCL

工业机器人

基于 QT5 的 ROS2 GUI 开发教程

Unity-Robotic-Hub 入门教程

专栏导航

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板