Environments and navigation

Flightmare environments

The following environments are available in UnityScene. The environment can easily be used with

// UnityScene::<SCENE_NAME>
SceneID scene_id_{UnityScene::INDUSTRIAL};

instead of using the specific ID.

ID

Environment

Summary

0

INDUSTRIAL

A basic outdoor industrial environment

1

WAREHOUSE

A small indoor warehouse environment

2

GARAGE

A small indoor garage environment

3

NATUREFOREST

A high-quality outdoor forest environment

1 / 4
INDUSTRIAL
2 / 4
WAREHOUSE
3 / 4
GARAGE
4 / 4
NATUREFOREST


Run example

roslaunch flightros racing.launch

Here the full code example

racing.hpp
#pragma once

// ros
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <ros/ros.h>

// standard libraries
#include <assert.h>
#include <Eigen/Dense>
#include <chrono>
#include <cmath>
#include <cstring>
#include <fstream>
#include <iostream>
#include <iterator>
#include <sstream>
#include <thread>
#include <vector>

// flightlib
#include "flightlib/bridges/unity_bridge.hpp"
#include "flightlib/bridges/unity_message_types.hpp"
#include "flightlib/common/quad_state.hpp"
#include "flightlib/common/types.hpp"
#include "flightlib/objects/quadrotor.hpp"
#include "flightlib/objects/static_gate.hpp"
#include "flightlib/sensors/rgb_camera.hpp"

// trajectory
#include <polynomial_trajectories/minimum_snap_trajectories.h>
#include <polynomial_trajectories/polynomial_trajectories_common.h>
#include <polynomial_trajectories/polynomial_trajectory.h>
#include <polynomial_trajectories/polynomial_trajectory_settings.h>
#include <quadrotor_common/trajectory_point.h>

using namespace flightlib;

namespace racing {

class manual_timer {
  std::chrono::high_resolution_clock::time_point t0;
  double timestamp{0.0};

 public:
  void start() { t0 = std::chrono::high_resolution_clock::now(); }
  void stop() {
    timestamp = std::chrono::duration<double>(
                  std::chrono::high_resolution_clock::now() - t0)
                  .count() *
                1000.0;
  }
  const double &get() { return timestamp; }
};

// void setupQuad();
bool setUnity(const bool render);
bool connectUnity(void);

// unity quadrotor
std::shared_ptr<Quadrotor> quad_ptr_;
std::shared_ptr<RGBCamera> rgb_camera_;
QuadState quad_state_;

// Flightmare(Unity3D)
std::shared_ptr<UnityBridge> unity_bridge_ptr_;
SceneID scene_id_{UnityScene::WAREHOUSE};
bool unity_ready_{false};
bool unity_render_{true};
RenderMessage_t unity_output_;
uint16_t receive_id_{0};
}  // namespace racing
racing.cpp
#include "flightros/racing/racing.hpp"

bool racing::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_);
    std::cout << "Unity Bridge is created." << std::endl;
  }
  return true;
}

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


int main(int argc, char *argv[]) {
  // initialize ROS
  ros::init(argc, argv, "flightmare_gates");
  ros::NodeHandle nh("");
  ros::NodeHandle pnh("~");
  ros::Rate(50.0);

  // quad initialization
  racing::quad_ptr_ = std::make_unique<Quadrotor>();
  // add camera
  racing::rgb_camera_ = std::make_unique<RGBCamera>();

  // Flightmare
  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;
  racing::rgb_camera_->setFOV(90);
  racing::rgb_camera_->setWidth(720);
  racing::rgb_camera_->setHeight(480);
  racing::rgb_camera_->setRelPose(B_r_BC, R_BC);
  racing::rgb_camera_->setPostProcesscing(std::vector<bool>{
    false, false, false});  // depth, segmentation, optical flow
  racing::quad_ptr_->addRGBCamera(racing::rgb_camera_);

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

  // Initialize gates
  std::string object_id = "unity_gate";
  std::string prefab_id = "rpg_gate";
  std::shared_ptr<StaticGate> gate_1 =
    std::make_shared<StaticGate>(object_id, prefab_id);
  gate_1->setPosition(Eigen::Vector3f(0, 10, 2.5));
  gate_1->setRotation(
    Quaternion(std::cos(0.5 * M_PI_2), 0.0, 0.0, std::sin(0.5 * M_PI_2)));

  std::string object_id_2 = "unity_gate_2";
  std::shared_ptr<StaticGate> gate_2 =
    std::make_unique<StaticGate>(object_id_2, prefab_id);
  gate_2->setPosition(Eigen::Vector3f(0, -10, 2.5));
  gate_2->setRotation(
    Quaternion(std::cos(0.5 * M_PI_2), 0.0, 0.0, std::sin(0.5 * M_PI_2)));

  // Set unity bridge
  racing::setUnity(racing::unity_render_);

  // Add gates
  racing::unity_bridge_ptr_->addStaticObject(gate_1);
  racing::unity_bridge_ptr_->addStaticObject(gate_2);

  // connect unity
  racing::connectUnity();

  // Define path through gates
  std::vector<Eigen::Vector3d> way_points;
  way_points.push_back(Eigen::Vector3d(0, 10, 2.5));
  way_points.push_back(Eigen::Vector3d(5, 0, 2.5));
  way_points.push_back(Eigen::Vector3d(0, -10, 2.5));
  way_points.push_back(Eigen::Vector3d(-5, 0, 2.5));

  std::size_t num_waypoints = way_points.size();
  Eigen::VectorXd segment_times(num_waypoints);
  segment_times << 10.0, 10.0, 10.0, 10.0;
  Eigen::VectorXd minimization_weights(5);
  minimization_weights << 0.0, 1.0, 1.0, 1.0, 1.0;

  polynomial_trajectories::PolynomialTrajectorySettings trajectory_settings =
    polynomial_trajectories::PolynomialTrajectorySettings(
      way_points, minimization_weights, 7, 4);

  polynomial_trajectories::PolynomialTrajectory trajectory =
    polynomial_trajectories::minimum_snap_trajectories::
      generateMinimumSnapRingTrajectory(segment_times, trajectory_settings,
                                        20.0, 20.0, 6.0);

  // Start racing
  racing::manual_timer timer;
  timer.start();

  while (ros::ok() && racing::unity_render_ && racing::unity_ready_) {
    timer.stop();

    quadrotor_common::TrajectoryPoint desired_pose =
      polynomial_trajectories::getPointFromTrajectory(
        trajectory, ros::Duration(timer.get() / 1000));
    racing::quad_state_.x[QS::POSX] = (Scalar)desired_pose.position.x();
    racing::quad_state_.x[QS::POSY] = (Scalar)desired_pose.position.y();
    racing::quad_state_.x[QS::POSZ] = (Scalar)desired_pose.position.z();
    racing::quad_state_.x[QS::ATTW] = (Scalar)desired_pose.orientation.w();
    racing::quad_state_.x[QS::ATTX] = (Scalar)desired_pose.orientation.x();
    racing::quad_state_.x[QS::ATTY] = (Scalar)desired_pose.orientation.y();
    racing::quad_state_.x[QS::ATTZ] = (Scalar)desired_pose.orientation.z();

    racing::quad_ptr_->setState(racing::quad_state_);

    racing::unity_bridge_ptr_->getRender(0);
    racing::unity_bridge_ptr_->handleOutput();
  }

  return 0;
}