前言
使用ROS+Flightmare的通信结构
ROS继续负责算法的控制,而Flightmare则取代Gazebo负责进行仿真
核心区别。该工作流程中,客户端通过UnityBridge向服务端注册actor并持续更新状态
flightmare基础概念
server与client
类比于roscore与rosnode,flightmare设计中存在server与client。其中将unity渲染引擎称为server,而自己的程序则称为client,client使用提供的UnityBridge(底层为ZMQ消息通信库)与server进行通信,具体表示为用UnityBridge的addQuadrotor()、connectUnity()、getRender()等接口
故在启动程序之前,需要先启动unity引擎(server),就是之前下载的二进制文件,通常是写在launch文件中,如下
1 2 3 4
| <launch> <node pkg="flightrender" type="RPG_Flightmare.x86_64" name="rpg_flightmare_render" /> <node pkg="offb" type="offb_node" name="offb_node" output="screen" /> </launch>
|
或者直接运行rosrun flightrender RPG_Flightmare.x86_64来启动

注意,一个程序可以既是ros node,又是flightmare client,并不冲突。
在启动server之后,便可以尝试在client即自定义文件中连接到server。编写如下测试文件
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21
| #include "flightlib/bridges/unity_bridge.hpp" #include <ros/ros.h>
using namespace flightmare;
int main(int argc, char **argv){ ros::init(arc, argv, "test"); ros::NodeHandle nh; ros::Rate rate(20.0); std::shared_ptr<UnityBridge> unity_bridge_ptr_ = UnityBridge::getInstance(); bool unity_ready_ = unity_bridge_ptr_->connectUnity(UnityScene::INDUSTRIAL); while(ros::ok()){ ros::spinOnce(); } return 0; }
|
注意在编译时cmake文件需要参照之前安装笔记中,“后续使用”部分进行修改。
而后续往场景中添加物体也是依赖unitybridge,示例如下
1 2 3 4 5 6
| unity_bridge_ptr_->addQuadrotor(std::shared_ptr<Quadrotor> quad);
unity_bridge_ptr_->addCamera(std::shared_ptr<UnityCamera> unity_camera);
unity_bridge_ptr_->addStaticObject(std::shared_ptr<StaticObject> static_object);
|
物体object
在能够打开并连接到server后,接下来引入几个新的概念:actor 、object、 prefeb
actor:任何在运行时通过client注册到server中的物体。例如在gazebo中,物体是通过launch时加载到场景中,在flightmare中物体是在引擎运行时注册到场景中的,例如无人机。
object:可以视为actor的一种特例,通常指场景种的静态物体
prefeb:这是unity中的概念,意为已经做好的“模板”,具有一定的属性和动画。因为虽然client注册了物体,但是并没有决定这个物体长什么样,需要指定prefeb_id来告诉server需要用哪个prefeb实例化。
向场景中加入无人机
首先要创建一个Quad,然后设定初始状态,最后通过addQuadrotor加入到server中。示例代码如下
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
| #include "flightlib/bridges/unity_bridge.hpp" #include "flightlib/common/quad_state.hpp" #include "flightlib/common/types.hpp" #include "flightlib/objects/quadrotor.hpp"
#include <ros/ros.h>
using namespace flightlib;
int main(int argc, char **argv) { ros::init(argc, argv, "offb_node"); ros::NodeHandle nh; ros::Rate rate(20.0);
std::shared_ptr<Quadrotor> quad_ptr_= std::make_shared<Quadrotor>(); QuadState quad_state_; quad_state_.setZero(); quad_ptr_->reset(quad_state_);
std::shared_ptr<UnityBridge> unity_bridge_ptr_ = UnityBridge::getInstance();
unity_bridge_ptr_->addQuadrotor(quad_ptr_); bool unity_ready_ = unity_bridge_ptr_->connectUnity(UnityScene::INDUSTRIAL);
while(ros::ok()) { ros::spinOnce(); }
return 0; }
|
其中reset仅用于初始化无人机位置,后续则使用setState来设定无人机位置。运行结果如图所示

设置无人机状态
通过以下代码来控制无人机的位置。有三个关键的部分,(1)设定位姿(2)发布状态(3)渲染下一帧并写回状态。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
| quad_state_.x[QS::POSX] = (Scalar)position.x; quad_state_.x[QS::POSY] = (Scalar)position.y; quad_state_.x[QS::POSZ] = (Scalar)position.z; quad_state_.x[QS::ATTW] = (Scalar)orientation.w; quad_state_.x[QS::ATTX] = (Scalar)orientation.x; quad_state_.x[QS::ATTY] = (Scalar)orientation.y; quad_state_.x[QS::ATTZ] = (Scalar)orientation.z;
quad_ptr_->setState(quad_state_);
unity_bridge_ptr_->getRender(0); unity_bridge_ptr_->handleOutput();
|
这里getRender和handleOutput相较于之前的gazebo仿真,是一个新的且较难理解的概念,此处先作比较简要的解释。首先setState并不是发送给飞控的目标点,而是直接设定的无人机的位置,例如设置z=2并不会让无人机平滑地飞到2m高度,而是直接设定无人机此时在2m高度,无人机“瞬移”到该位置。然后getRender和handleOutput通常成对出现,其中getRender是让仿真器去渲染出这一帧的画面,handleOutput则是返回如摄像头等传感器数据。这样显式地控制渲染与返回内容的节奏,而不是根据unity内部自动的渲染节奏,实现了动力学和渲染的解耦,例如可以让控制保持在100hz,而画面渲染在30hz,让client侧的动力学控制成为主导。
例如在下面的这个循环示例中,不断地设置无人机的位置然后渲染,让无人机看起来像是在“运动”一样,但实际上并没有任何动力学环节。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21
| while (ros::ok()) { const double t = (ros::Time::now() - start_time).toSec();
quad_state.x[QS::POSX] = 2.0 * std::cos(0.5 * t); quad_state.x[QS::POSY] = 2.0 * std::sin(0.5 * t); quad_state.x[QS::POSZ] = 1.5;
quad_state.x[QS::ATTW] = 1.0; quad_state.x[QS::ATTX] = 0.0; quad_state.x[QS::ATTY] = 0.0; quad_state.x[QS::ATTZ] = 0.0;
quad_ptr->setState(quad_state);
unity_bridge_ptr->getRender(frame_id);
++frame_id; ros::spinOnce(); rate.sleep(); }
|
加入障碍物
在场景中加入障碍物/object的方法与加入无人机稍有不同。(1)使用的库文件不一样(2)使用object id与prefab id(3)使用eigen数组作为位置(4)使用addStateObject
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
| #include <Eigen/Dense>
#include "flightlib/bridges/unity_bridge.hpp" #include "flightlib/bridges/unity_message_types.hpp" #include "flightlib/common/types.hpp" #include "flightlib/objects/static_gate.hpp"
using namespace flightlib;
std::string object_id = "unity_gate"; std::string prefab_id = "rpg_gate"; std::shared_ptr<StaticGate> gate = std::make_shared<StaticGate>(object_id, prefab_id); gate->setPosition(Eigen::Vector3f(0, 10, 2.5)); gate->setRotation( Quaternion(std::cos(0.5 * M_PI_2), 0.0, 0.0, std::sin(0.5 * M_PI_2)));
std::shared_ptr<UnityBridge> unity_bridge_ptr_; unity_bridge_ptr_ = UnityBridge::getInstance();
unity_bridge_ptr_->addStaticObject(gate); bool unity_ready_ = unity_bridge_ptr_->connectUnity(UnityScene::WAREHOUSE);
|
将示例的代码加入到之前的代码中然后运行

参考内容
Flightmare documentation — Flightmare documentation