前言

使用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文件中，如下

<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。编写如下测试文件

#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);
	
	// 关键部分：创建一个unitybridge
	std::shared_ptr<UnityBridge> unity_bridge_ptr_ = UnityBridge::getInstance();
	// 让bridge连接到unity，也就是server，并指定场景
	bool unity_ready_ = unity_bridge_ptr_->connectUnity(UnityScene::INDUSTRIAL);
	
	while(ros::ok()){
		ros::spinOnce();
	}
	
	return 0;
}

注意在编译时cmake文件需要参照之前安装笔记中，“后续使用”部分进行修改。

而后续往场景中添加物体也是依赖unitybridge，示例如下

// 添加无人机
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中。示例代码如下

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

    // intialize quadrotor
    std::shared_ptr<Quadrotor> quad_ptr_= std::make_shared<Quadrotor>();
    QuadState quad_state_;
    quad_state_.setZero();
    quad_ptr_->reset(quad_state_);

    // initialize unity bridge
    std::shared_ptr<UnityBridge> unity_bridge_ptr_ = UnityBridge::getInstance();

    // add quadrotor
    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）渲染下一帧并写回状态。

// Define new quadrotor state
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;

// Set new state
quad_ptr_->setState(quad_state_);

// Render next frame
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侧的动力学控制成为主导。

例如在下面的这个循环示例中，不断地设置无人机的位置然后渲染，让无人机看起来像是在“运动”一样，但实际上并没有任何动力学环节。

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;

  // Keep the quad level while it follows a simple circular path.
  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

#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;

// Initialize gates
std::string object_id = "unity_gate"; // Unique name
std::string prefab_id = "rpg_gate"; // Name of the prefab in the Assets/Resources folder
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)));

// Initialize Unity bridge
std::shared_ptr<UnityBridge> unity_bridge_ptr_;
unity_bridge_ptr_ = UnityBridge::getInstance();

// Add gates
unity_bridge_ptr_->addStaticObject(gate);
bool unity_ready_ = unity_bridge_ptr_->connectUnity(UnityScene::WAREHOUSE);

将示例的代码加入到之前的代码中然后运行

参考内容

Flightmare documentation — Flightmare documentation