前言

接续前文，继续学习flightmare中的基础使用方法

flightmare基础概念

Environments and navigation

预设环境

flightmare提供了预设的环境，例如之前使用的bool unity_ready = unity_bridge_ptr->connectUnity(UnityScene::INDUSTRIAL);

路径点

在官方文档中的这一部分使用 rpg_quadrotor_control来生成轨迹并取轨迹点，对于基础思想的学习过于复杂，直接参考上一节中的代码

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();
}

即在循环中使用setState，然后进行渲染。注意，此处仍然没有使用飞控相关的内容，都是直接设置无人机位置。

Sensors and data

主要是使用相机

添加相机

要使用相机，首先要创建一个相机对象并添加到无人机上；而不能像gazebo中直接导入一个有相机的模型

// 创建一个相机对象
std::shared_ptr<RGBCamera> rgb_camera = std::make_shared<RGBCamera>();
// 配置相机的参数
rgb_camera->setWidth(640);
rgb_camera->setHeight(360);
rgb_camera->setFOV(90.0);
rgb_camera->setRelPose(
	Vector<3>(0.2, 0.0, 0.1),
	Matrix<3, 3>::Identity());
// 将相机挂载到无人机上
quad_ptr->addRGBCamera(rgb_camera);
// 再将挂载了相机的无人机注册到unitybridge

接收图像

回顾之前见过的handleOutput，其作用是读取渲染结果，返回输出给各传感器。而在这之后，还需要将数据从传感器的缓存队列中读出来

unity_bridge_ptr_->getRender(0);
unity_bridge_ptr_->handleOutput();

cv::Mat img;

rgb_camera_->getRGBImage(img); // CV_U8C3
rgb_camera_->getDepthMap(img); // CV_32FC1

其中cv::Mat是opencv的通用图像容器，可以存储多种格式的图像。其次，虽然叫做rgb_camera，但是也可以读取到深度图像

发布话题

在前面将图像数据读取出来之后，需要手动发布为ros话题（上述操作在gazebo中都是自动完成的）。

// initialize ROS
ros::init(argc, argv, "flightmare_rviz");
ros::NodeHandle pnh("~");
ros::Rate(50.0);

// initialize publishers
image_transport::ImageTransport it(pnh);
rgb_pub_ = it.advertise("/rgb", 1);
depth_pub_ = it.advertise("/depth", 1);

// while循环中
unity_bridge_ptr_->getRender(0);
unity_bridge_ptr_->handleOutput();

int frame_id = 0;
cv::Mat img;

rgb_camera_->getRGBImage(img);
sensor_msgs::ImagePtr rgb_msg =
  cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
rgb_msg->header.stamp.fromNSec(frame_id);
rgb_pub_.publish(rgb_msg);

rgb_camera_->getDepthMap(img);
sensor_msgs::ImagePtr depth_msg =
  cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
depth_msg->header.stamp.fromNSec(frame_id);
depth_pub_.publish(depth_msg);

这里出现了一个之前在gazebo中没有见过的结构image_transport::ImageTransport，是专门负责ROS图像消息的发布和订阅的封装。而sensor_msgs的作用则是专门用于ROS的消息传递格式。

综合实验

将上面的相机相关内容结合起来，并将渲染画面、接受图像和发布话题放入到循环中，最后可以使用rqt_image_view来观察结果

FrameID frame_id = 0;
int log_counter = 0;

while (ros::ok()) {
  // Keep sending the current state to Unity and receive sensor outputs.
  unity_bridge_ptr->getRender(frame_id);
  unity_bridge_ptr->handleOutput();

  // Read the latest RGB image returned by Unity and publish it as a ROS image.
  cv::Mat rgb_image;
  if (rgb_camera->getRGBImage(rgb_image)) {
    sensor_msgs::ImagePtr rgb_msg =
      cv_bridge::CvImage(std_msgs::Header(), "bgr8", rgb_image).toImageMsg();
    rgb_msg->header.stamp = ros::Time::now();
    rgb_pub.publish(rgb_msg);
  }

  // Depth is returned as a single-channel float image in meters.
  cv::Mat depth_image;
  if (rgb_camera->getDepthMap(depth_image)) {
    sensor_msgs::ImagePtr depth_msg =
      cv_bridge::CvImage(std_msgs::Header(), "32FC1", depth_image)
        .toImageMsg();
    depth_msg->header.stamp = ros::Time::now();
    depth_pub.publish(depth_msg);
  }

  if (++log_counter % 30 == 0) {
    ROS_INFO_STREAM("publishing camera data on /rgb and /depth");
  }

  ++frame_id;
  ros::spinOnce();
  rate.sleep();
}

最终实验结果如图所示

观察上面的代码其实可以发现，相较于gazebo在flightmare中缺少了许多自动完成的中间环节，例如需要手动要求引擎渲染图像、在无人机模型上加入相机、将相机图像转发为ROS话题等。这些需要手动完成的部分虽然增加了程序的复杂度与上手的难度，但是也对应得增加了可操作性。同时因为可解耦的特性，即使没有图像渲染的部分，整个仿真也可以正常运行。而去掉这些内容后，仿真文件的一般结构与gazebo中文件的结构也非常相近。但是由于多了一部分与仿真环节进行连接的部分，仿真环节中运行的代码与实机运行的代码将会存在区别。

Pointcloud

和在gazebo中使用点云的方式不同（由摄像头动态生成），flightmare中点云是“一次性”导出的，例如在程序一开始一次性生成整个场景的点云地图，然后无人机在飞行时根据需要取用，而不是动态生成

总结

flightmare的基础使用方式还是很简单的，首先是可以作为单独的功能包进行安装，其次使用编译好的二进制仿真文件启动，最后在代码中使用“桥”来与仿真文件联系即可。

参考内容

Environments and navigation — Flightmare documentation
Sensors and data — Flightmare documentation
Point Cloud — Flightmare documentation