Flightmare

https://flightmare.readthedocs.io/en/latest/
Admin
Flightmare is a flexible, high-fidelity simulator for quadrotor control and perception, providing...
Tokens:39,274
Snippets:296
Trust Score:9.6
Update:4 months ago
Show doc for...
Tokens:
Raw
### Launch Camera Example with ROS

Source: https://flightmare.readthedocs.io/en/latest/tutorials_general/camera.html

Launches the camera example using ROS. This command is used to start the ROS nodes and bring up the simulation environment for camera-related functionalities.

```bash
roslaunch flightros camera.launch

```

--------------------------------

### Run Gazebo Example

Source: https://flightmare.readthedocs.io/en/latest/tutorials_general/pilot.html

Launches the Gazebo simulation environment for the rotors using the provided ROS launch file. This command is used to start the simulation environment.

```Shell
roslaunch flightros rotors_gazebo.launch

```

--------------------------------

### Install Git and Configure User Settings

Source: https://flightmare.readthedocs.io/en/latest/getting_started/quick_start.html

Installs Git and configures global user name, email, and color UI. Essential for version control.

```bash
sudo apt-get install git
git config --global user.name "Your Name Here"
git config --global user.email "Same Email as used for github"
git config --global color.ui true

```

--------------------------------

### Build and Run Flightmare ROS Examples

Source: https://flightmare.readthedocs.io/en/latest/getting_started/quick_start.html

Build the flightros package with samples enabled using catkin build and launch a ROS example for quadrotor simulation with Flightmare rendering.

```bash
# The examples are by default not built.
catkin build flightros -DBUILD_SAMPLES:=ON

# Now you can run any example.
roslaunch flightros rotors_gazebo.launch

```

--------------------------------

### Setup and Solve Motion Planning Problem with OMPL

Source: https://flightmare.readthedocs.io/en/latest/advanced_steps/motion_planning.html

Configures the OMPL state space (SE3) with bounds derived from mesh data, sets a state validity checker, defines random start and goal states, and attempts to solve the planning problem within a time limit.

```C++
void motion_planning::plan() {
    // construct the state space we are planning in
    auto space(std::make_shared<ob::SE3StateSpace>());

    // set the bounds for the R^3 part of SE(3)
    ob::RealVectorBounds bounds(3);

    bounds.setLow(0, min_bounds.x);
    bounds.setLow(1, min_bounds.y);
    bounds.setLow(2, min_bounds.z);
    bounds.setHigh(0, max_bounds.x);
    bounds.setHigh(1, max_bounds.y);
    bounds.setHigh(2, max_bounds.z);

    space->setBounds(bounds);

    // define a simple setup class
    og::SimpleSetup ss(space);

    // set state validity checking for this space
    ss.setStateValidityChecker(
        [](const ob::State *state) { return isStateValid(state); });

    // create a random start state
    ob::ScopedState<> start(space);
    start.random();

    // create a random goal state
    ob::ScopedState<> goal(space);
    goal.random();

    // set the start and goal states
    ss.setStartAndGoalStates(start, goal);

    // this call is optional, but we put it in to get more output information
    ss.setup();
    ss.print();

    // attempt to solve the problem within one second of planning time
    ob::PlannerStatus solved = ss.solve(1.0);

    if (solved) {
        std::cout << "Found solution:" << std::endl;


```

--------------------------------

### Compile flightros examples

Source: https://flightmare.readthedocs.io/en/latest/getting_started/faq.html

Run this command to compile the flightros package and its examples if they have not been compiled yet.

```bash
catkin build flightros -DBUILD_SAMPLES:=ON

```

--------------------------------

### Compile and Install Flightlib

Source: https://flightmare.readthedocs.io/en/latest/getting_started/quick_start.html

Compiles the flightlib C++ components and installs it as a Python package.

```bash
cd flightmare/flightlib
# it first compile the flightlib and then install it as a python package.
pip install .

```

--------------------------------

### Setup and Spawn Camera

Source: https://flightmare.readthedocs.io/en/latest/tutorials_general/camera.html

Initializes a Quadrotor and RGBCamera, configures camera properties like FOV, resolution, and relative pose, and adds the camera to the quadrotor. It also resets the quadrotor's state and connects to the Unity simulation environment.

```cpp
// Initialization
std::shared_ptr<Quadrotor> quad_ptr_ = std::make_unique<Quadrotor>();
std::shared_ptr<RGBCamera> rgb_camera_ = std::make_unique<RGBCamera>();

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

// Setup Quad
QuadState quad_state_;
quad_state_.setZero();
quad_ptr_->reset(quad_state_);

// Spawn
std::shared_ptr<UnityBridge> unity_bridge_ptr_ = UnityBridge::getInstance();
unity_bridge_ptr_->addQuadrotor(quad_ptr_);
bool unity_ready_ = unity_bridge_ptr_->connectUnity(UnityScene::WAREHOUSE);

```

--------------------------------

### Unity Connection Setup

Source: https://flightmare.readthedocs.io/en/latest/advanced_steps/motion_planning.html

Initializes and connects to the Unity simulation environment. This function is crucial for enabling visualization and interaction with the simulated world.

```C++
bool motion_planning::setUnity(const bool render) {
  unity_render_ = render;
  // flightmare quadrotor
  quad_ptr_.reset(new Quadrotor());
  rgb_camera_.reset(new RGBCamera());
  quad_state_.attitude = Eigen::Vector3f(0, 0, 0);
  quad_state_.angular_velocity = Eigen::Vector3f(0, 0, 0);
  quad_state_.velocity = Eigen::Vector3f(0, 0, 0);
  quad_state_.position = Eigen::Vector3f(0, 0, 1.5);
  quad_ptr_->set_state(quad_state_);
  rgb_camera_->set_position(Eigen::Vector3d(0, 0, 1.5));
  rgb_camera_->set_pose(quad_state_.get_pose());
  rgb_camera_->set_width(1920);
  rgb_camera_->set_height(1080);
  // Flightmare(Unity3D)
  unity_bridge_ptr_.reset(new UnityBridge());
  unity_bridge_ptr_->connect();
  unity_bridge_ptr_->loadScene(scene_id_, unity_render_);
  unity_bridge_ptr_->getQuadrotorStates(quad_state_);
  unity_ready_ = true;
  return true;
}

bool motion_planning::connectUnity(void) {
  // unity quadrotor
  quad_ptr_.reset(new Quadrotor());
  rgb_camera_.reset(new RGBCamera());
  quad_state_.attitude = Eigen::Vector3f(0, 0, 0);
  quad_state_.angular_velocity = Eigen::Vector3f(0, 0, 0);
  quad_state_.velocity = Eigen::Vector3f(0, 0, 0);
  quad_state_.position = Eigen::Vector3f(0, 0, 1.5);
  quad_ptr_->set_state(quad_state_);
  rgb_camera_->set_position(Eigen::Vector3d(0, 0, 1.5));
  rgb_camera_->set_pose(quad_state_.get_pose());
  rgb_camera_->set_width(1920);
  rgb_camera_->set_height(1080);
  // Flightmare(Unity3D)
  unity_bridge_ptr_.reset(new UnityBridge());
  unity_bridge_ptr_->connect();
  unity_bridge_ptr_->loadScene(scene_id_, unity_render_);
  unity_bridge_ptr_->getQuadrotorStates(quad_state_);
  unity_ready_ = true;
  return true;
}
```

--------------------------------

### Install Catkin Tools

Source: https://flightmare.readthedocs.io/en/latest/getting_started/quick_start.html

Installs pip and the catkin-tools package, which is used for building ROS packages.

```bash
sudo apt-get install python-pip

```

```bash
sudo pip install catkin-tools

```

--------------------------------

### Unity Bridge Setup and Connection

Source: https://flightmare.readthedocs.io/en/latest/advanced_steps/motion_planning.html

Manages the connection and rendering process with the Unity simulator. Initializes the Unity bridge and connects to the Unity environment.

```C++
bool motion_planning::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 motion_planning::connectUnity() {
  if (!unity_render_ || unity_bridge_ptr_ == nullptr) return false;
  unity_ready_ = unity_bridge_ptr_->connectUnity(scene_id_);
  return unity_ready_;
}
```

--------------------------------

### Install Flightmare System Packages

Source: https://flightmare.readthedocs.io/en/latest/getting_started/quick_start.html

Installs required system packages including build tools, CMake, and libraries for networking, OpenCV, and Python.

```bash
apt-get update && apt-get install -y --no-install-recommends \
   build-essential \
   cmake \
   libzmqpp-dev \
   libopencv-dev

```

--------------------------------

### Install and Run Flightmare with Pip

Source: https://flightmare.readthedocs.io/en/latest/getting_started/quick_start.html

Install Flightmare using pip and run a drone control example with Unity rendering enabled. Ensure you have activated your conda environment and downloaded the Unity binary.

```python
conda activate ENVNAME
cd /path/to/flightmare/flightrl
pip install .
cd examples
python3 run_drone_control.py --train 0 --render 1

```

--------------------------------

### Connect to Unity Server (Python)

Source: https://flightmare.readthedocs.io/en/latest/first_steps/server_and_client.html

Connects to the Flightmare Unity server using a Python script. This example configures the environment using a YAML file and establishes the connection. Ensure the FLIGHTMARE_PATH environment variable is set.

```python
#!/usr/bin/env python3
from ruamel.yaml import YAML, dump, RoundTripDumper

import os

from rpg_baselines.envs import vec_env_wrapper as wrapper
from flightgym import QuadrotorEnv_v1

# select UnityScene in flightmare/flightlib/configs/vec_env.yaml
cfg = YAML().load(open(os.environ["FLIGHTMARE_PATH"] +
                          "/flightlib/configs/vec_env.yaml", 'r'))

env = wrapper.FlightEnvVec(QuadrotorEnv_v1(dump(cfg, Dumper=RoundTripDumper), False))

env.connectUnity()

```

--------------------------------

### Execute Solution Path

Source: https://flightmare.readthedocs.io/en/latest/advanced_steps/motion_planning.html

Retrieves the solution path from a planner and processes the states. This example shows linear interpolation between states, but suggests using the rpg_quadrotor_control library for more advanced control.

```C++
path_ = ss.getSolutionPath().getStates();

for (auto const &pos : path_) {
  vecs_.push_back(stateToEigen(pos));
}

```

--------------------------------

### Install ROS Dependencies

Source: https://flightmare.readthedocs.io/en/latest/getting_started/quick_start.html

Installs necessary system and ROS dependencies for Flightmare on Ubuntu 20.04. Ensure your protobuf compiler version is 3.0.0.

```bash
sudo apt-get install libgoogle-glog-dev protobuf-compiler ros-$ROS_DISTRO-octomap-msgs ros-$ROS_DISTRO-octomap-ros ros-$ROS_DISTRO-joy python-vcstool

```

--------------------------------

### Launch Flightmare Racing Simulation

Source: https://flightmare.readthedocs.io/en/latest/first_steps/envs_and_navigation.html

Use this ROS launch command to start the Flightmare racing environment simulation.

```bash
roslaunch flightros racing.launch

```

--------------------------------

### Install Flightmare Python Dependencies

Source: https://flightmare.readthedocs.io/en/latest/getting_started/quick_start.html

Installs project dependencies listed in requirements.txt within the activated Conda environment.

```bash
conda activate ENVNAME
cd flightmare/ 
pip install -r requirements.txt

```

--------------------------------

### QuadrotorEnv::getAct (Command)

Source: https://flightmare.readthedocs.io/en/latest/cpp_references/quadrotor_env.html

Gets the next command for the quadrotor.

```APIDOC
## getAct(cmd)

### Description
Gets the next command for the quadrotor.

### Parameters
* **cmd** (Command *) - Required - A pointer to the Command object to be filled.

### Return Type
bool
```

--------------------------------

### Racing Environment Header (racing.hpp)

Source: https://flightmare.readthedocs.io/en/latest/first_steps/envs_and_navigation.html

This C++ header file defines essential components for the racing environment, including ROS integration, Unity bridge setup, quadrotor and camera configurations, and timer utilities.

```cpp
#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

```

--------------------------------

### OMPL SimpleSetup Initialization and Planning

Source: https://flightmare.readthedocs.io/en/latest/advanced_steps/motion_planning.html

Demonstrates the initialization of OMPL's SimpleSetup for SE(3) state space, setting bounds, validity checker, start/goal states, and solving the motion planning problem.

```cpp
void planWithSimpleSetup()
{
    // construct the state space we are planning in
    auto space(std::make_shared<ob::SE3StateSpace>());

    // We then set the bounds for the R3 component of this state space:
    ob::RealVectorBounds bounds(3);
    bounds.setLow(-1);
    bounds.setHigh(1);

    space->setBounds(bounds);

    // Create an instance of ompl::geometric::SimpleSetup. Instances of ompl::base::SpaceInformation , and ompl::base::ProblemDefinition are created internally.
    og::SimpleSetup ss(space);

    // Set the state validity checker
    ss.setStateValidityChecker([](const ob::State *state) { return isStateValid(state); });

    // Create a random start state:
    ob::ScopedState<> start(space);
    start.random();

    // And a random goal state:
    ob::ScopedState<> goal(space);
    goal.random();

    // Set these states as start and goal for SimpleSetup.
    ss.setStartAndGoalStates(start, goal);

    // We can now try to solve the problem. This will also trigger a call to ompl::geometric::SimpleSetup::setup() and create a default instance of a planner, since we have not specified one. Furthermore, ompl::base::Planner::setup() is called, which in turn calls ompl::base::SpaceInformation::setup(). This chain of calls will lead to computation of runtime parameters such as the state validity checking resolution. This call returns a value from ompl::base::PlannerStatus which describes whether a solution has been found within the specified amount of time (in seconds). If this value can be cast to true, a solution was found.
    ob::PlannerStatus solved = ss.solve(1.0);

    if (solved)
    {
        std::cout << "Found solution:" << std::endl;
        // print the path to screen
        ss.simplifySolution();
        ss.getSolutionPath().print(std::cout);
    }
}

```

--------------------------------

### Initialize and Configure Unity Environment

Source: https://flightmare.readthedocs.io/en/latest/first_steps/envs_and_navigation.html

Sets up the Unity bridge, adds a quadrotor, and configures camera parameters. This is essential for rendering and interaction with the Unity environment.

```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;
}

```

--------------------------------

### FlightPilot Class Initialization

Source: https://flightmare.readthedocs.io/en/latest/tutorials_general/pilot.html

Initializes the FlightPilot class, loads parameters, sets up the quadrotor and camera, and connects to the Unity simulation.

```cpp
#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::WAREHOUSE),
    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() {}

```

--------------------------------

### Run Motion Planning Launch File

Source: https://flightmare.readthedocs.io/en/latest/advanced_steps/motion_planning.html

Launches the motion planning simulation using ROS. Ensure ROS is sourced and the flightros package is available.

```bash
roslaunch flightros motion_planning.launch

```

--------------------------------

### Quadrotor Geometric Properties

Source: https://flightmare.readthedocs.io/en/latest/cpp_references/quadrotor.html

Methods to get the size, position, and orientation of the quadrotor.

```APIDOC
## Quadrotor.getSize()

### Description
Gets the size of the quadrotor.

### Return Type
Vector<3>

## Quadrotor.getPosition()

### Description
Gets the current position of the quadrotor.

### Return Type
Vector<3>

## Quadrotor.getQuaternion()

### Description
Gets the current orientation of the quadrotor as a quaternion.

### Return Type
Quaternion
```

--------------------------------

### Create Catkin Workspace

Source: https://flightmare.readthedocs.io/en/latest/getting_started/quick_start.html

Initializes a new catkin workspace and configures it for building with Release type.

```bash
cd
mkdir -p catkin_ws/src
cd catkin_ws
catkin config --init --mkdirs --extend /opt/ros/$ROS_DISTRO --merge-devel --cmake-args -DCMAKE_BUILD_TYPE=Release

```

--------------------------------

### Quadrotor State and Dynamics Retrieval

Source: https://flightmare.readthedocs.io/en/latest/cpp_references/quadrotor.html

Methods to get the current state, motor thrusts, motor speeds, and dynamics of the quadrotor.

```APIDOC
## Quadrotor.getState(QuadState* const state_)

### Description
Retrieves the current state of the quadrotor.

### Parameters
* **state** (QuadState* const) - Pointer to store the quadrotor's state.

### Return Type
bool

## Quadrotor.getMotorThrusts(Ref <Vector<4>> motor_thrusts_)

### Description
Retrieves the current motor thrusts of the quadrotor.

### Parameters
* **motor_thrusts** (Ref <Vector<4>>) - Reference to store the motor thrusts.

### Return Type
bool

## Quadrotor.getMotorOmega(Ref <Vector<4>> motor_omega_)

### Description
Retrieves the current motor angular velocities (omega) of the quadrotor.

### Parameters
* **motor_omega** (Ref <Vector<4>>) - Reference to store the motor angular velocities.

### Return Type
bool

## Quadrotor.getDynamics(QuadrotorDynamics* const dynamics_)

### Description
Retrieves the quadrotor's dynamics parameters.

### Parameters
* **dynamics** (QuadrotorDynamics* const) - Pointer to store the quadrotor dynamics.

### Return Type
bool

## Quadrotor.getDynamics()

### Description
Retrieves the quadrotor's dynamics parameters.

### Return Type
const QuadrotorDynamics&
```

--------------------------------

### Main Function for Flightmare Camera ROS Node

Source: https://flightmare.readthedocs.io/en/latest/tutorials_general/camera.html

Initializes ROS, sets up publishers for camera data, configures a quadrotor with an RGB camera, connects to Unity, and enters a loop to continuously render and publish camera feeds. This is the main entry point for the camera node.

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

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

  // quad initialization
  camera::quad_ptr_ = std::make_unique<Quadrotor>();
  // add mono camera
  camera::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;
  camera::rgb_camera_->setFOV(90);
  camera::rgb_camera_->setWidth(720);
  camera::rgb_camera_->setHeight(480);
  camera::rgb_camera_->setRelPose(B_r_BC, R_BC);
  camera::rgb_camera_->setPostProcesscing(
    std::vector<bool>{true, true, true});  // depth, segmentation, optical flow
  camera::quad_ptr_->addRGBCamera(camera::rgb_camera_);


  // // initialization
  camera::quad_state_.setZero();

  camera::quad_ptr_->reset(camera::quad_state_);

  // connect unity
  camera::setUnity(camera::unity_render_);
  camera::connectUnity();

  while (ros::ok() && camera::unity_render_ && camera::unity_ready_) {
    camera::quad_state_.x[QS::POSX] = (Scalar)0;
    camera::quad_state_.x[QS::POSY] = (Scalar)0;
    camera::quad_state_.x[QS::POSZ] = (Scalar)0;
    camera::quad_state_.x[QS::ATTW] = (Scalar)0;
    camera::quad_state_.x[QS::ATTX] = (Scalar)0;
    camera::quad_state_.x[QS::ATTY] = (Scalar)0;
    camera::quad_state_.x[QS::ATTZ] = (Scalar)0;

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

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

    cv::Mat img;

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

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

    camera::rgb_camera_->getSegmentation(img);
    sensor_msgs::ImagePtr segmentation_msg = 
      cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
    segmentation_msg->header.stamp.fromNSec(camera::counter);
    camera::segmentation_pub_.publish(segmentation_msg);

    camera::rgb_camera_->getOpticalFlow(img);
    sensor_msgs::ImagePtr opticflow_msg = 
      cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
    opticflow_msg->header.stamp.fromNSec(camera::counter);
    camera::opticalflow_pub_.publish(opticflow_msg);

    camera::counter++;
  }

  return 0;
}
```

--------------------------------

### Get Render Frame (C++)

Source: https://flightmare.readthedocs.io/en/latest/first_steps/server_and_client.html

Retrieves the render frame from the simulation for a specific quadrotor (index 0). Must be called after handleOutput.

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

```

--------------------------------

### Publish RGB Image using ROS

Source: https://flightmare.readthedocs.io/en/latest/tutorials_general/camera.html

Initializes ROS and sets up a publisher to broadcast RGB images. This is useful for visualizing camera data in tools like RViz. Requires a valid 'img' cv::Mat object.

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

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

sensor_msgs::ImagePtr rgb_msg =
    cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
  rgb_msg->header.stamp.fromNSec(0);

rgb_pub_.publish(rgb_msg);

```

--------------------------------

### Spawn RGB Camera with Post-Processing

Source: https://flightmare.readthedocs.io/en/latest/first_steps/sensors_and_data.html

Initializes an RGB camera and attaches it to a quadrotor. Enables depth, segmentation, and optical flow post-processing layers.

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

```

--------------------------------

### Connect to Unity

Source: https://flightmare.readthedocs.io/en/latest/python_references/flight_env_vec.html

Establishes a connection to the Flightmare simulation. This must be called before interacting with the environment.

```python
connectUnity(_self_)

```

--------------------------------

### Build Flightmare Workspace

Source: https://flightmare.readthedocs.io/en/latest/getting_started/quick_start.html

Builds the catkin workspace, compiling Flightmare and its dependencies.

```bash
catkin build

```

--------------------------------

### Get Point from Trajectory and Update Quadrotor State

Source: https://flightmare.readthedocs.io/en/latest/first_steps/envs_and_navigation.html

Retrieves a specific point from a generated trajectory based on elapsed time and updates the quadrotor's state accordingly. This loop continuously updates the quadrotor's pose and renders the scene.

```cpp
manual_timer timer;
timer.start();

while (ros::ok()) {
  timer.stop();

  quadrotor_common::TrajectoryPoint desired_pose =
    polynomial_trajectories::getPointFromTrajectory(
      trajectory, ros::Duration(timer.get() / 1000));

  // Set pose
  quad_state_.x[QS::POSX] = (Scalar)desired_pose.position.x();
  quad_state_.x[QS::POSY] = (Scalar)desired_pose.position.y();
  quad_state_.x[QS::POSZ] = (Scalar)desired_pose.position.z();
  quad_state_.x[QS::ATTW] = (Scalar)desired_pose.orientation.w();
  quad_state_.x[QS::ATTX] = (Scalar)desired_pose.orientation.x();
  quad_state_.x[QS::ATTY] = (Scalar)desired_pose.orientation.y();
  quad_state_.x[QS::ATTZ] = (Scalar)desired_pose.orientation.z();

  quad_ptr_->setState(quad_state_);

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

```

--------------------------------

### Step Simulation and Send Actions (Python)

Source: https://flightmare.readthedocs.io/en/latest/first_steps/server_and_client.html

Advances the Flightmare simulation by one step and sends actions to the server. Requires action and send_id parameters.

```python
env.stepUnity(action, send_id)

```

--------------------------------

### Step Environment with Unity Rendering

Source: https://flightmare.readthedocs.io/en/latest/python_references/flight_env_vec.html

Simulates one step in the environment with thrusts and renders the simulation in Unity. Returns observations, reward, done status, and info. Requires a frame ID.

```python
stepUnity(_self_ , _action_ , _send_id_)

```

--------------------------------

### Quadrotor State and Command Setting

Source: https://flightmare.readthedocs.io/en/latest/cpp_references/quadrotor.html

Methods for setting the quadrotor's state and command.

```APIDOC
## Quadrotor.setState(const QuadState& state_)

### Description
Sets the quadrotor's state to the provided QuadState.

### Parameters
* **state** (const QuadState &) - The desired quadrotor state.

### Return Type
bool

## Quadrotor.setCommand(const Command& cmd_)

### Description
Sets the command for the quadrotor.

### Parameters
* **cmd** (const Command &) - The command to be set for the quadrotor.

### Return Type
bool
```

--------------------------------

### Camera Class Methods for Unity Integration

Source: https://flightmare.readthedocs.io/en/latest/tutorials_general/camera.html

These methods handle the connection and rendering logic with the Unity simulation environment. `setUnity` initializes the Unity bridge if rendering is enabled and the bridge is not yet created. `connectUnity` establishes the connection to Unity, returning true if successful.

```cpp
#include "flightros/camera/camera.hpp"

bool camera::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 camera::connectUnity() {
  if (!unity_render_ || unity_bridge_ptr_ == nullptr) return false;
  unity_ready_ = unity_bridge_ptr_->connectUnity(scene_id_);
  return unity_ready_;
}
```

--------------------------------

### Publish Camera Data via ROS

Source: https://flightmare.readthedocs.io/en/latest/first_steps/sensors_and_data.html

Initializes ROS publishers and publishes camera sensor data (RGB, depth, segmentation, optical flow) as ROS Image messages.

```cpp
// 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);
segmentation_pub_ = it.advertise("/segmentation", 1);
opticalflow_pub_ = it.advertise("/opticalflow", 1);

// ...

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_std_msgs::Header(), "bgr8", img).toImageMsg();
depth_msg->header.stamp.fromNSec(frame_id);
depth_pub_.publish(depth_msg);

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

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

```

--------------------------------

### Connect to Unity Server (C++)

Source: https://flightmare.readthedocs.io/en/latest/first_steps/server_and_client.html

Establishes a connection to the Flightmare Unity server using the UnityBridge. Ensure the server is running before executing. Connects to default ports 10253 and 10254 with a 10-second timeout.

```cpp
#include "flightlib/bridges/unity_bridge.hpp"

std::shared_ptr<UnityBridge> unity_bridge_ptr_ = UnityBridge::getInstance();
bool unity_ready_ = unity_bridge_ptr_->connectUnity(UnityScene::INDUSTRIAL);

```

--------------------------------

### Read Point Cloud and Populate KD-Tree

Source: https://flightmare.readthedocs.io/en/latest/advanced_steps/motion_planning.html

Loads a point cloud from a .ply file using tinyply.h, parses its header, and populates an Eigen matrix and a KD-Search Tree for efficient state validation. Includes error handling for file operations and tinyply parsing.

```C++
std::vector<motion_planning::float3> motion_planning::readPointCloud() {
std::unique_ptr<std::istream> file_stream;
std::vector<uint8_t> byte_buffer;
std::string filepath =
  std::experimental::filesystem::path(__FILE__).parent_path().string() +
  "/data/point_cloud.ply";
try {
  file_stream.reset(new std::ifstream(filepath, std::ios::binary));

  if (!file_stream || file_stream->fail())
    throw std::runtime_error("file_stream failed to open " + filepath);

  file_stream->seekg(0, std::ios::end);
  const float size_mb = file_stream->tellg() * float(1e-6);
  file_stream->seekg(0, std::ios::beg);

  PlyFile file;
  file.parse_header(*file_stream);

  std::cout << "\t[ply_header] Type: "
            << (file.is_binary_file() ? "binary" : "ascii") << std::endl;
  for (const auto &c : file.get_comments())
    std::cout << "\t[ply_header] Comment: " << c << std::endl;
  for (const auto &c : file.get_info())
    std::cout << "\t[ply_header] Info: " << c << std::endl;

  for (const auto &e : file.get_elements()) {
    std::cout << "\t[ply_header] element: " << e.name << " (" << e.size << ")"
              << std::endl;
    for (const auto &p : e.properties) {
      std::cout << "\t[ply_header] \tproperty: " << p.name
                << " (type=" << tinyply::PropertyTable[p.propertyType].str
                << ")";
      if (p.isList)
        std::cout << " (list_type=" << tinyply::PropertyTable[p.listType].str
                  << ")";
      std::cout << std::endl;
    }
  }

  // Because most people have their own mesh types, tinyply treats parsed data
  // as structured/typed byte buffers. See examples below on how to marry your
  // own application-specific data structures with this one.
  std::shared_ptr<PlyData> vertices, normals, colors, texcoords, faces,
    tripstrip;

  // The header information can be used to programmatically extract properties
  // on elements known to exist in the header prior to reading the data. For
  // brevity of this sample, properties like vertex position are hard-coded:
  try {
    vertices =
      file.request_properties_from_element("vertex", {"x", "y", "z"});
  } catch (const std::exception &e) {
    std::cerr << "tinyply exception: " << e.what() << std::endl;
  }

  manual_timer read_timer;

  read_timer.start();
  file.read(*file_stream);
  read_timer.stop();

  const float parsing_time = read_timer.get() / 1000.f;
  std::cout << "\tparsing " << size_mb << "mb in " << parsing_time
            << " seconds [" << (size_mb / parsing_time) << " MBps]"
            << std::endl;

  if (vertices)
    std::cout << "\tRead " << vertices->count << " total vertices "
              << std::endl;


  const size_t numVerticesBytes = vertices->buffer.size_bytes();
  std::vector<float3> verts(vertices->count);
  std::memcpy(verts.data(), vertices->buffer.get(), numVerticesBytes);

  int idx = 0;
  for (const auto &point_tinyply : verts) {
    if (idx == 0) {
      points_ = Eigen::Vector3d(static_cast<double>(point_tinyply.x),
                                static_cast<double>(point_tinyply.y),
                                static_cast<double>(point_tinyply.z));
    } else {
      points_.conservativeResize(points_.rows(), points_.cols() + 1);
      points_.col(points_.cols() - 1) =
        Eigen::Vector3d(static_cast<double>(point_tinyply.x),
                        static_cast<double>(point_tinyply.y),
                        static_cast<double>(point_tinyply.z));
    }
    idx += 1;
  }
  kd_tree_.SetMatrixData(points_);

  return verts;
} catch (const std::exception &e) {
  std::cerr << "Caught tinyply exception: " << e.what() << std::endl;
}
return verts;
}
```

--------------------------------

### Step Environment with Actions

Source: https://flightmare.readthedocs.io/en/latest/python_references/flight_env_vec.html

Simulates one step in the environment using the provided propeller thrusts. Returns observations, reward, done status, and info. Note: stepUnity() is not yet implemented.

```python
step(_self_ , _action_)

```

--------------------------------

### Flightmare Camera Header File

Source: https://flightmare.readthedocs.io/en/latest/tutorials_general/camera.html

This C++ header file includes necessary libraries and defines global variables for camera operations within the Flightmare simulation. It sets up ROS publishers and Flightmare-specific components.

```cpp
#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 <cmath>
#include <cstring>
#include <experimental/filesystem>
#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/sensors/rgb_camera.hpp"

using namespace flightlib;

namespace camera {

// publisher
image_transport::Publisher rgb_pub_;
image_transport::Publisher depth_pub_;
image_transport::Publisher segmentation_pub_;
image_transport::Publisher opticalflow_pub_;
int counter = 0;

// 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 camera

```

--------------------------------

### Parameter Loading

Source: https://flightmare.readthedocs.io/en/latest/tutorials_general/pilot.html

Loads configuration parameters for the FlightPilot from the ROS parameter server.

```cpp
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

```
Tokens:
Raw