8.6_Flightmare仿真器——使用Gazebo动力学

Mian 发布于

前言

在上一节中我们学习了flightmare自带的动力学与飞控的使用，而flightmare还可以使用gazebo作为动力学，即gazebo中为一个仅有无人机的空白世界，用于接收电机转速然后输出无人机位置；而flightmare仅用于视觉渲染，返回无人机当前看到的视觉图像。而在官方给出的演示文档中，也是使用gazebo作为动力学，可以推断官方或许更支持这种方法。

而除了动力学的组成改变之外，本节还希望通过官方的演示文档，能够学习到更标准化的项目组成方式，最终拥有创建自己的项目的能力，毕竟在上一节中所有内容都是放在一个节点内。

注意：本文中部分内容由AI生成，故仅供参考

标准化项目结构

在上一节中，所有的内容都写在一个ros节点中，然后使用一个launch文件启动飞控和节点，但是这样将不同功能的实现放在一个文件中并不好，更好的是根据功能解耦，不同的文件实现不同的功能，且使用类来进行接口标准化，以便于复用调试和迁移等。
所以在这里，首先将仿真器与规划器的拆分开来，其中规划器即上层的导航算法，接收例如rgb，depth，odom等话题，然后输出轨迹点；而仿真器即底层的动力学和视觉计算，则与前者相反，输出rgb，depth，odom，而接收轨迹点等内容。两个部分之间通过ros话题来沟通，使得算法能够脱离底层的环境而存在，更便于迁移到实机或者不同的仿真环境中。

由于解耦的特性，在这一节中主要讨论仿真器的使用，而仿真器又大体可以分为三个部分：无人机，动力学与渲染器。其中无人机包含飞控autopilot以及与动力学的接口rpg_rotors_interface；动力学为环境gazebo与无人机模型；渲染器为flightmare render与其对外接口flight_pilot。大致示意图如下

Gazebo动力学

不再使用flightlib作为动力学，而是用gazebo作为动力学，不过飞控仍然是autopilot。而之前我们只使用launch文件来启动飞控，这里还需要启动gazebo等内容。直接给出官方的参考仿真器部分launch文档如下

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
<?xml version="1.0"?>
<launch>
  <arg name="quad_name" default="hummingbird"/>

  <arg name="mav_name" default="$(arg quad_name)"/>
  <arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo"/>
  <arg name="world_name" default="$(find rotors_gazebo)/worlds/basic.world"/>

  <arg name="use_unity_editor" default="false" />
  <arg name="paused" value="true"/>
  <arg name="gui" value="true"/>
  <arg name="use_mpc" default="false"/>
  <arg name="use_ground_truth" default="true"/>
  <arg name="enable_ground_truth" default="true"/>
  <arg name="enable_command_feedthrough" default="false"/>
  <arg name="custom_models" default=""/>

  <arg name="enable_logging" default="false"/>
  <arg name="log_file" default="$(arg mav_name)"/>

  <arg name="x_init" default="0"/>
  <arg name="y_init" default="0"/>

  <arg name="debug" default="false"/>
  <arg name="verbose" default="false"/>

  <param name="use_sim_time" value="true"/>

  <!-- Gazebo stuff to spawn the world !-->
  <env name="GAZEBO_MODEL_PATH"
      value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models:$(arg custom_models)"/>
  <env name="GAZEBO_RESOURCE_PATH"
      value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(arg world_name)" />
    <arg name="debug" value="$(arg debug)" />
    <arg name="paused" value="$(arg paused)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="verbose" value="$(arg verbose)"/>
  </include>

  <!-- RotorS stuff to spawn the quadrotor !-->
  <group ns="$(arg mav_name)">
    <include file="$(find rotors_gazebo)/launch/spawn_mav.launch">
      <arg name="mav_name" value="$(arg mav_name)" />
      <arg name="model" value="$(arg model)" />
      <arg name="enable_logging" value="$(arg enable_logging)" />
      <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
      <arg name="log_file" value="$(arg log_file)"/>
      <arg name="x" value="$(arg x_init)" />
      <arg name="y" value="$(arg y_init)" />
    </include>
  </group>

  <!-- RPG stuff !-->
  <group ns="$(arg quad_name)" >

    <!-- RPG RotorS interface. -->
    <node pkg="rpg_rotors_interface" type="rpg_rotors_interface"
        name="rpg_rotors_interface" output="screen" >
      <rosparam file="$(find rpg_rotors_interface)/parameters/rpg_rotors_interface.yaml" />
      <!-- .. -->
      <remap from="odometry" to="ground_truth/odometry" />
      <remap from="rpg_rotors_interface/arm" to="bridge/arm" />
    </node>

    <!-- RPG Flightmare Unity Render. -->
    <node pkg="flightrender" type="RPG_Flightmare.x86_64" name="rpg_flightmare_render" unless="$(arg use_unity_editor)">
    </node>

    <!-- Autopilot -->
    <node pkg="autopilot" type="autopilot" name="autopilot" output="screen">
      <rosparam file="$(find state_predictor)/parameters/hummingbird.yaml" />
      <rosparam file="$(find rpg_rotors_interface)/parameters/position_controller.yaml" />
      <rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml" />

      <param name="position_controller/use_rate_mode" value="True" />

      <param name="velocity_estimate_in_world_frame" value="false" />
      <param name="state_estimate_timeout" value="0.1" />
      <param name="control_command_delay" value="0.05" />
      <param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)" />

      <remap from="autopilot/state_estimate" to="ground_truth/odometry" />
    </node>

    <group if="$(arg use_mpc)">
      <node pkg="rpg_mpc" type="autopilot_mpc_instance" name="autopilot" output="screen">
        <rosparam file="$(find state_predictor)/parameters/hummingbird.yaml" />
        <rosparam file="$(find rpg_mpc)/parameters/default.yaml" />
        <rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml" />

        <param name="velocity_estimate_in_world_frame" value="false" />
        <param name="state_estimate_timeout" value="0.1" />
        <param name="control_command_delay" value="0.05" />
        <param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)" />

        <remap from="autopilot/state_estimate" to="ground_truth/odometry" />
      </node>
    </group>

    <node pkg="flightros" type="flight_pilot_node" name="flight_pilot_node" output="screen">
      <rosparam file="$(find flightros)/params/default.yaml" />
      <remap from="flight_pilot/state_estimate" to="ground_truth/odometry" />
    </node>

    <node pkg="joy" type="joy_node" name="joy_node">
      <param name="autorepeat_rate" value="10"/>
    </node>

    <node pkg="manual_flight_assistant" type="manual_flight_assistant"
        name="manual_flight_assistant" output="screen">
      <rosparam file="$(find rpg_rotors_interface)/parameters/manual_flight_assistant.yaml"/>
    </node>

    <node name="rqt_quad_gui" pkg="rqt_gui" type="rqt_gui"
        args="-s rqt_quad_gui.basic_flight.BasicFlight --args
        --quad_name $(arg quad_name)" output="screen"/>

  </group>

</launch>

官方文档分析

上述文档虽然看起来很复杂，但是总体功能可以划分为如下四个部分：

  • 启动gazebo并导入无人机

  • 启动unity渲染器

  • 启动飞控到gazebo的中间转换节点rpg_rotors_interface

  • 启动飞控autopilot并决定使用PID还是MPC模式

  • 启动unity渲染器接口flight_pilot

  • 启动手工控制节点joy和输入转换节点manual_flight_assistant

  • 启动gui控制界面
    其中第一部分是与第三部分最为重要，接下来按部分进行分析

Gazebo启动

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
  <env name="GAZEBO_MODEL_PATH"
      value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models:$(arg custom_models)"/>
  <env name="GAZEBO_RESOURCE_PATH"
      value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(arg world_name)" />
    <arg name="debug" value="$(arg debug)" />
    <arg name="paused" value="$(arg paused)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="verbose" value="$(arg verbose)"/>
  </include>
  
<group ns="$(arg mav_name)">
    <include file="$(find rotors_gazebo)/launch/spawn_mav.launch">
      <arg name="mav_name" value="$(arg mav_name)" />
      <arg name="model" value="$(arg model)" />
      <arg name="enable_logging" value="$(arg enable_logging)" />
      <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
      <arg name="log_file" value="$(arg log_file)"/>
      <arg name="x" value="$(arg x_init)" />
      <arg name="y" value="$(arg y_init)" />
    </include>
  </group>

这一部分基本不需要进行修改，可以直接复制到自己的启动文件中

飞控与渲染启动

1
2
3
4
5
6
7
8
<!-- RPG RotorS interface. -->
    <node pkg="rpg_rotors_interface" type="rpg_rotors_interface"
        name="rpg_rotors_interface" output="screen" >
      <rosparam file="$(find rpg_rotors_interface)/parameters/rpg_rotors_interface.yaml" />
      <!-- .. -->
      <remap from="odometry" to="ground_truth/odometry" />
      <remap from="rpg_rotors_interface/arm" to="bridge/arm" />
    </node>

飞控到Gazebo的中间转换，负责将飞控输出的姿态/角速度/推力转换为gazebo能够理解的电机转速。这里要注意对于odometry的remap，修改为自己的odometry相关话题

remap与group

  1. remap的作用是将节点中所使用的topic/service修改为launch文件中指定的名字，这样在不用修改源码的情况下，更换输入/输出的topic
  2. group的作用主要有两个：给一组节点统一加上namespace；根据条件启用/禁用一组节点
    ①统一namespace
    1
    2
    3
    4
    5
    ><arg name="quad_name" default="hummingbird"/>
    ><group ns="$(arg quad_name)">
       <node pkg="autopilot" type="autopilot" name="autopilot"/>
       <node pkg="joy" type="joy_node" name="joy_node"/>
     </group>
    那么节点名会变成类似：
    /hummingbird/autopilot
    /hummingbird/joy_node
    topic 也通常会带上 namespace，例如：
    /hummingbird/autopilot/velocity_command
    /hummingbird/joy
    ②条件启动
    比如：
    1
    2
    3
    ><group if="$(arg use_mpc)">
       <node pkg="rpg_mpc" type="autopilot_mpc_instance" name="autopilot"/>
     </group>
    意思是：
    只有 use_mpc 为 true 时，才启动这个 group 里面的节点。
1
2
3
<!-- RPG Flightmare Unity Render. -->
<node pkg="flightrender" type="RPG_Flightmare.x86_64" name="rpg_flightmare_render" unless="$(arg use_unity_editor)">
</node>

启动渲染器，flightmare的基本使用方法，注意二进制文件的名称要与使用一致

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
<!-- Autopilot -->
<node pkg="autopilot" type="autopilot" name="autopilot" output="screen">
  <rosparam file="$(find state_predictor)/parameters/hummingbird.yaml" />
  <rosparam file="$(find rpg_rotors_interface)/parameters/position_controller.yaml" />
  <rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml" />

  <param name="position_controller/use_rate_mode" value="True" />

  <param name="velocity_estimate_in_world_frame" value="false" />
  <param name="state_estimate_timeout" value="0.1" />
  <param name="control_command_delay" value="0.05" />
  <param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)" />

  <remap from="autopilot/state_estimate" to="ground_truth/odometry" />
</node>

启动飞控，和上一节中的内容相同，使用默认的配置文件，可以直接复制。然后注意autopilot/state_estimate也remap到之前的odometry话题中

1
2
3
4
5
6
7
8
9
10
11
12
13
14
<group if="$(arg use_mpc)">
  <node pkg="rpg_mpc" type="autopilot_mpc_instance" name="autopilot" output="screen">
    <rosparam file="$(find state_predictor)/parameters/hummingbird.yaml" />
    <rosparam file="$(find rpg_mpc)/parameters/default.yaml" />
    <rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml" />

    <param name="velocity_estimate_in_world_frame" value="false" />
    <param name="state_estimate_timeout" value="0.1" />
    <param name="control_command_delay" value="0.05" />
    <param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)" />

    <remap from="autopilot/state_estimate" to="ground_truth/odometry" />
  </node>
</group>

是否使用mpc，通过参数控制，如果不启动则使用pid

PID与MPC
简单地说两者都是从期望位姿+odom计算出目标姿态/角速度/推力的方法。而PID计算量小，更直接，但能力更弱；而MPC更复杂，但是考虑未来，效果更好

渲染器接口flight_pilot启动

与上一节中所有内容都在一个节点内不同，更加标准化的方法是将规划算法的部分与unity_bridge部分解耦，以便后续随时更换算法。

1
2
3
4
<node pkg="flightros" type="flight_pilot_node" name="flight_pilot_node" output="screen">
    <rosparam file="$(find flightros)/params/default.yaml" />
    <remap from="flight_pilot/state_estimate" to="ground_truth/odometry" />
</node>

而官方提供了一个可供参考的接口类flight_pilot，其相关内容在/flightros下，共三个文件flight_pilot.hpp声明类结构，flight_pilot.cpp声明类内容，flight_pilot_node.cpp入口函数实例化类并使用。这里主要分析flight_pilot.cpp如下。同时注意到在launch文件中，这里也有一个需要remap的部分

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
#include "flightros/pilot/flight_pilot.hpp"

namespace flightros {

FlightPilot::FlightPilot(const ros::NodeHandle &nh, const ros::NodeHandle &pnh)
  : nh_(nh),
    pnh_(pnh),
    scene_id_(UnityScene::INDUSTRIAL),
    unity_ready_(false),
    unity_render_(false),
    receive_id_(0),
    main_loop_freq_(50.0) {
  // load parameters
  if (!loadParams()) {
    ROS_WARN("[%s] Could not load all parameters.",
             pnh_.getNamespace().c_str());
  } else {
    ROS_INFO("[%s] Loaded all parameters.", pnh_.getNamespace().c_str());
  }

  // quad initialization
  quad_ptr_ = std::make_shared<Quadrotor>();

  // add mono camera
  rgb_camera_ = std::make_shared<RGBCamera>();
  Vector<3> B_r_BC(0.0, 0.0, 0.3);
  Matrix<3, 3> R_BC = Quaternion(1.0, 0.0, 0.0, 0.0).toRotationMatrix();
  std::cout << R_BC << std::endl;
  rgb_camera_->setFOV(90);
  rgb_camera_->setWidth(720);
  rgb_camera_->setHeight(480);
  rgb_camera_->setRelPose(B_r_BC, R_BC);
  quad_ptr_->addRGBCamera(rgb_camera_);

  // initialization
  quad_state_.setZero();
  quad_ptr_->reset(quad_state_);


  // initialize subscriber call backs
  sub_state_est_ = nh_.subscribe("flight_pilot/state_estimate", 1,
                                 &FlightPilot::poseCallback, this);

  timer_main_loop_ = nh_.createTimer(ros::Rate(main_loop_freq_),
                                     &FlightPilot::mainLoopCallback, this);


  // wait until the gazebo and unity are loaded
  ros::Duration(5.0).sleep();

  // connect unity
  setUnity(unity_render_);
  connectUnity();
}

FlightPilot::~FlightPilot() {}

void FlightPilot::poseCallback(const nav_msgs::Odometry::ConstPtr &msg) {
  quad_state_.x[QS::POSX] = (Scalar)msg->pose.pose.position.x;
  quad_state_.x[QS::POSY] = (Scalar)msg->pose.pose.position.y;
  quad_state_.x[QS::POSZ] = (Scalar)msg->pose.pose.position.z;
  quad_state_.x[QS::ATTW] = (Scalar)msg->pose.pose.orientation.w;
  quad_state_.x[QS::ATTX] = (Scalar)msg->pose.pose.orientation.x;
  quad_state_.x[QS::ATTY] = (Scalar)msg->pose.pose.orientation.y;
  quad_state_.x[QS::ATTZ] = (Scalar)msg->pose.pose.orientation.z;
  //
  quad_ptr_->setState(quad_state_);

  if (unity_render_ && unity_ready_) {
    unity_bridge_ptr_->getRender(0);
    unity_bridge_ptr_->handleOutput();

    if (quad_ptr_->getCollision()) {
      // collision happened
      ROS_INFO("COLLISION");
    }
  }
}

void FlightPilot::mainLoopCallback(const ros::TimerEvent &event) {
  // empty
}

bool FlightPilot::setUnity(const bool render) {
  unity_render_ = render;
  if (unity_render_ && unity_bridge_ptr_ == nullptr) {
    // create unity bridge
    unity_bridge_ptr_ = UnityBridge::getInstance();
    unity_bridge_ptr_->addQuadrotor(quad_ptr_);
    ROS_INFO("[%s] Unity Bridge is created.", pnh_.getNamespace().c_str());
  }
  return true;
}

bool FlightPilot::connectUnity() {
  if (!unity_render_ || unity_bridge_ptr_ == nullptr) return false;
  unity_ready_ = unity_bridge_ptr_->connectUnity(scene_id_);
  return unity_ready_;
}

bool FlightPilot::loadParams(void) {
  // load parameters
  quadrotor_common::getParam("main_loop_freq", main_loop_freq_, pnh_);
  quadrotor_common::getParam("unity_render", unity_render_, pnh_);

  return true;
}

}  // namespace flightros

该接口的写法基本可以进行复用，其中主要需要自己修改的部分为构造函数以及加载参数的loadParams。然后入口部分如下

1
2
3
4
5
6
7
8
9
10
11
12
13
#include <ros/ros.h>

#include "flightros/pilot/flight_pilot.hpp"

int main(int argc, char** argv) {
  ros::init(argc, argv, "flight_pilot");
  flightros::FlightPilot pilot(ros::NodeHandle(), ros::NodeHandle("~"));

  // spin the ros
  ros::spin();

  return 0;
}

手动控制与GUI启动

1
2
3
4
5
6
7
8
9
10
11
12
<node pkg="joy" type="joy_node" name="joy_node">
     <param name="autorepeat_rate" value="10"/>
   </node>

   <node pkg="manual_flight_assistant" type="manual_flight_assistant"
       name="manual_flight_assistant" output="screen">
     <rosparam file="$(find rpg_rotors_interface)/parameters/manual_flight_assistant.yaml"/>
   </node>

   <node name="rqt_quad_gui" pkg="rqt_gui" type="rqt_gui"
       args="-s rqt_quad_gui.basic_flight.BasicFlight --args
       --quad_name $(arg quad_name)" output="screen"/>

其中joy包用于接收手柄等控制器的输入，而manual_flight_assistant则用于将控制器输入转换为飞控能懂的形式，rqt_quad_gui用于gui控制无人机。这三者在自己使用时完全可以删掉，唯一需要注意的是，飞控虽然在接收到控制指令时会自动切换模式，但仍然需要手动输入指令切换到start模式，通常有两种方式，一就是使用gui提供的按钮，二是命令行输入。如果不使用gui，通常可以用一个shell文件来启动规划器的launch文件，并在其开头加入如下两行内容：

1
2
3
4
rostopic pub /"${quad_name}"/bridge/arm std_msgs/Bool "data: true" -1;
rostopic pub /"${quad_name}"/autopilot/start std_msgs/Empty "{}" -1;

sleep 2

其意义分别为允许rotors_interface输出电机转速，让autopilot切换到start模式

总结

虽然官方给出的参考文档与标准流程是基于gazebo的，但是并不表示其轻量化的动力学是无意义的，在后续基于学习的方法中，为了提升效率往往也还是会使用轻量化动力学。

参考内容

flightmare/flightros/launch/pilot/rotors_gazebo.launch at master · uzh-rpg/flightmare
flightmare/flightros/src/pilot/flight_pilot.cpp at master · uzh-rpg/flightmare

Mian

Mian

每天成长一点

25
2
30
目录
  1. 1. 前言
  2. 2. 标准化项目结构
  3. 3. Gazebo动力学
    1. 3.1. 官方文档分析
      1. 3.1.1. Gazebo启动
      2. 3.1.2. 飞控与渲染启动
      3. 3.1.3. 渲染器接口flight_pilot启动
      4. 3.1.4. 手动控制与GUI启动
  4. 4. 总结
  5. 5. 参考内容
CATEGORIES
TAGS
AprilTag CPP FlightMare Gazebo IntelRealsenseD435 MAVROS NUC13 Python ROS1 Ubuntu20-04 Unity astar catkin cmake docker drone octomap px4 rosbag vins vitfly 仿真 动力学 参数服务器 实机 建图 模块化 视觉定位 路径规划 飞控