diff --git a/flightlib/.vscode/extensions.json b/flightlib/.vscode/extensions.json
new file mode 100644
index 0000000000..5c2577d0a8
--- /dev/null
+++ b/flightlib/.vscode/extensions.json
@@ -0,0 +1,9 @@
+{
+  "recommendations": [
+    "ms-vscode.cpptools",
+    "ms-vscode.cmake-tools",
+    "matepek.vscode-catch2-test-adapter",
+    "hbenl.vscode-test-explorer",
+    "xaver.clang-format"
+  ]
+}
\ No newline at end of file
diff --git a/flightlib/CMakeLists.txt b/flightlib/CMakeLists.txt
index e80ef787ef..32b3e29a41 100644
--- a/flightlib/CMakeLists.txt
+++ b/flightlib/CMakeLists.txt
@@ -195,9 +195,6 @@ if(NOT FLIGHTLIB_SOURCES)
 else()
   # flightlib
   add_library(${PROJECT_NAME} ${FLIGHTLIB_SOURCES})
-  # target_include_directories(${PROJECT_NAME} PRIVATE
-  #   ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include
-  # )
   target_link_libraries(${PROJECT_NAME} PRIVATE
     ${OpenCV_LIBRARIES}
     yaml-cpp
diff --git a/flightlib/build/.clang-format b/flightlib/build/.clang-format
index 2e16a261bf..1bbeb6c5e8 100644
--- a/flightlib/build/.clang-format
+++ b/flightlib/build/.clang-format
@@ -1,4 +1,4 @@
 ---
 DisableFormat: true
 SortIncludes: false
----
+---
\ No newline at end of file
diff --git a/flightlib/build/.gitignore b/flightlib/build/.gitignore
index eedfbbaa28..caff083b36 100644
--- a/flightlib/build/.gitignore
+++ b/flightlib/build/.gitignore
@@ -3,4 +3,4 @@
 # Except this file
 !.gitignore
 !.clang-format
-!setup.py
+!setup.py
\ No newline at end of file
diff --git a/flightlib/cmake/catkin.cmake b/flightlib/cmake/catkin.cmake
new file mode 100644
index 0000000000..b70795aef4
--- /dev/null
+++ b/flightlib/cmake/catkin.cmake
@@ -0,0 +1,28 @@
+# Setup catkin simple
+find_package(catkin_simple REQUIRED)
+catkin_simple()
+
+add_definitions(-std=c++17)
+
+# Library and Executables
+cs_add_library(${PROJECT_NAME} ${FLIGHTLIB_SOURCES})
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${BLAS_LIBRARIES}
+  ${LAPACK_LIBRARIES}
+  ${LAPACKE_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+  yaml-cpp
+  zmq
+  zmqpp
+)
+
+# Build tests
+if(BUILD_TESTS)
+  catkin_add_gtest(flightlib_tests ${FLIGHTLIB_TEST_SOURCES})
+  target_link_libraries(flightlib_tests ${PROJECT_NAME} gtest gtest_main)
+endif()
+
+# Finish catkin simple
+cs_install()
+cs_export()
\ No newline at end of file
diff --git a/flightlib/include/flightlib/bridges/unity_bridge.hpp b/flightlib/include/flightlib/bridges/unity_bridge.hpp
index a3049b4384..5079603665 100644
--- a/flightlib/include/flightlib/bridges/unity_bridge.hpp
+++ b/flightlib/include/flightlib/bridges/unity_bridge.hpp
@@ -1,6 +1,7 @@
 #pragma once
 
 // std libs
+#include <unistd.h>
 #include <chrono>
 #include <fstream>
 #include <map>
@@ -31,8 +32,7 @@ class UnityBridge {
   ~UnityBridge(){};
 
   // connect function
-  bool initializeConnections(void);
-  bool connectUnity(void);
+  bool connectUnity(const SceneID scene_id);
   bool disconnectUnity(void);
 
   // public get functions
@@ -57,6 +57,8 @@ class UnityBridge {
   };
 
  private:
+  bool initializeConnections(void);
+
   //
   SettingsMessage_t settings_;
   PubMessage_t pub_msg_;
@@ -80,11 +82,8 @@ class UnityBridge {
   int64_t last_download_debug_utime_;
   int64_t u_packet_latency_;
 
-  // connecting symbols
-  bool unity_ready_;
-
-  // auxiliary variables
-  std::vector<uint8_t> input_buffer_;
+  // axuiliary variables
+  const Scalar unity_connection_time_out_{10.0};
+  bool unity_ready_{false};
 };
-
 }  // namespace flightlib
\ No newline at end of file
diff --git a/flightlib/include/flightlib/envs/vec_env.hpp b/flightlib/include/flightlib/envs/vec_env.hpp
index 60e7102c97..d801c1ba98 100644
--- a/flightlib/include/flightlib/envs/vec_env.hpp
+++ b/flightlib/include/flightlib/envs/vec_env.hpp
@@ -80,10 +80,10 @@ class VecEnv {
   std::vector<std::string> extra_info_names_;
 
   // Flightmare(Unity3D)
-  const Scalar unity_connection_time_out_{10.0};  // seconds
-  bool unity_ready_{false}, unity_render_{false}, unity_bridge_created_{false};
-  std::shared_ptr<UnityBridge> unity_bridge_;
+  std::shared_ptr<UnityBridge> unity_bridge_ptr_;
   SceneID scene_id_{UnityScene::WAREHOUSE};
+  bool unity_ready_{false};
+  bool unity_render_{false};
   RenderMessage_t unity_output_;
   uint16_t receive_id_{0};
 
diff --git a/flightlib/include/flightlib/objects/quadrotor.hpp b/flightlib/include/flightlib/objects/quadrotor.hpp
index e5830f075d..f26d27c506 100644
--- a/flightlib/include/flightlib/objects/quadrotor.hpp
+++ b/flightlib/include/flightlib/objects/quadrotor.hpp
@@ -1,6 +1,6 @@
 #pragma once
 
-#include <yaml-cpp/yaml.h>
+#include <stdlib.h>
 
 // flightlib
 #include "flightlib/common/command.hpp"
diff --git a/flightlib/package.xml b/flightlib/package.xml
new file mode 100644
index 0000000000..a837a3a56b
--- /dev/null
+++ b/flightlib/package.xml
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>flightlib</name>
+  <version>0.0.1</version>
+  <description>Flightmare: An Open Flexible Quadrotor</description>
+
+  <maintainer email="song@ifi.uzh.ch">Yunlong Song</maintainer>
+
+  <license>GNU GPL</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>gtest</depend>
+
+</package>
\ No newline at end of file
diff --git a/flightlib/src/bridges/unity_bridge.cpp b/flightlib/src/bridges/unity_bridge.cpp
index 3df02ecd4f..26e5790c68 100644
--- a/flightlib/src/bridges/unity_bridge.cpp
+++ b/flightlib/src/bridges/unity_bridge.cpp
@@ -11,7 +11,10 @@ UnityBridge::UnityBridge()
     last_downloaded_utime_(0),
     last_download_debug_utime_(0),
     u_packet_latency_(0),
-    unity_ready_(false) {}
+    unity_ready_(false) {
+  // initialize connections upon creating unity bridge
+  initializeConnections();
+}
 
 bool UnityBridge::initializeConnections() {
   logger_.info("Initializing ZMQ connection!");
@@ -31,15 +34,34 @@ bool UnityBridge::initializeConnections() {
   return true;
 }
 
-bool UnityBridge::connectUnity() {
+bool UnityBridge::connectUnity(const SceneID scene_id) {
+  Scalar time_out_count = 0;
+  Scalar sleep_useconds = 0.2 * 1e5;
+  setScene(scene_id);
   // try to connect unity
-  if (!unity_ready_) {
+  logger_.info("Trying to Connect Unity.");
+  std::cout << "[";
+  while (!unity_ready_) {
+    // if time out
+    if (time_out_count / 1e6 > unity_connection_time_out_) {
+      std::cout << "]" << std::endl;
+      logger_.warn(
+        "Unity Connection time out! Make sure that Unity Standalone "
+        "or Unity Editor is running the Flightmare.");
+      return false;
+    }
     // initialize Scene settings
     sendInitialSettings();
     // check if setting is done
     unity_ready_ = handleSettings();
+    // sleep
+    usleep(sleep_useconds);
+    // increase time out counter
+    time_out_count += sleep_useconds;
+    // print something
+    std::cout << ".";
+    std::cout.flush();
   }
-
   return unity_ready_;
 }
 
@@ -122,7 +144,10 @@ bool UnityBridge::addQuadrotor(Quadrotor* quad) {
   Vehicle_t vehicle_t;
   // get quadrotor state
   QuadState quad_state;
-  if (!quad->getState(&quad_state)) return false;
+  if (!quad->getState(&quad_state)) {
+    logger_.error("Cannot get Quadrotor state.");
+    return false;
+  }
 
   vehicle_t.ID = std::to_string(settings_.vehicles.size());
   vehicle_t.position = positionRos2Unity(quad_state.p);
diff --git a/flightlib/src/envs/vec_env.cpp b/flightlib/src/envs/vec_env.cpp
index c2b6142b55..17cec56181 100644
--- a/flightlib/src/envs/vec_env.cpp
+++ b/flightlib/src/envs/vec_env.cpp
@@ -23,7 +23,6 @@ VecEnv<EnvBase>::VecEnv(const std::string& cfgs, const bool from_file) {
     // load from a string or dictionary
     cfg_ = YAML::Load(cfgs);
   }
-
   // initialization
   init();
 }
@@ -98,8 +97,8 @@ bool VecEnv<EnvBase>::step(Ref<MatrixRowMajor<>> act, Ref<MatrixRowMajor<>> obs,
   }
 
   if (unity_render_ && unity_ready_) {
-    unity_bridge_->getRender(0);
-    unity_bridge_->handleOutput(unity_output_);
+    unity_bridge_ptr_->getRender(0);
+    unity_bridge_ptr_->handleOutput(unity_output_);
   }
   return true;
 }
@@ -153,7 +152,6 @@ void VecEnv<EnvBase>::perAgentStep(int agent_id, Ref<MatrixRowMajor<>> act,
   done(agent_id) = envs_[agent_id]->isTerminalState(terminal_reward);
 
   envs_[agent_id]->updateExtraInfo();
-
   for (int j = 0; j < extra_info.size(); j++)
     extra_info(agent_id, j) =
       envs_[agent_id]->extra_info_[extra_info_names_[j]];
@@ -167,59 +165,32 @@ void VecEnv<EnvBase>::perAgentStep(int agent_id, Ref<MatrixRowMajor<>> act,
 template<typename EnvBase>
 bool VecEnv<EnvBase>::setUnity(bool render) {
   unity_render_ = render;
-  if (unity_render_ && !unity_bridge_created_) {
-    unity_bridge_ = UnityBridge::getInstance();
-    unity_bridge_->initializeConnections();
-
+  if (unity_render_ && unity_bridge_ptr_ != nullptr) {
+    // create unity bridge
+    unity_bridge_ptr_ = UnityBridge::getInstance();
+    // add objects to Unity
     for (int i = 0; i < num_envs_; i++) {
-      envs_[i]->addObjectsToUnity(unity_bridge_);
+      envs_[i]->addObjectsToUnity(unity_bridge_ptr_);
     }
-
-    connectUnity();
-    //
-    unity_bridge_created_ = true;
-    logger_.info("Flightmare Unity is ON!");
+    logger_.info("Flightmare Bridge is created.");
   }
   return true;
 }
 
-template<typename EnvBase>
-void VecEnv<EnvBase>::isTerminalState(Ref<BoolVector<>> terminal_state) {}
-
 template<typename EnvBase>
 bool VecEnv<EnvBase>::connectUnity(void) {
-  Scalar time_out_count = 0;
-  Scalar sleep_useconds = 0.2 * 1e5;
-  logger_.info("Trying to Connect Unity.");
-  std::cout << "[";
-  while (!unity_ready_) {
-    if (unity_bridge_ != nullptr) {
-      // connect unity
-      unity_bridge_->setScene(scene_id_);
-      unity_ready_ = unity_bridge_->connectUnity();
-    }
-    if (time_out_count / 1e6 > unity_connection_time_out_) {
-      std::cout << "]" << std::endl;
-      logger_.warn(
-        "Unity Connection time out! Make sure that Unity Standalone "
-        "or Unity Editor is running the Flightmare.");
-      return false;
-    }
-    // sleep
-    usleep(sleep_useconds);
-    // increase time out counter
-    time_out_count += sleep_useconds;
-    std::cout << ".";
-    std::cout.flush();
-  }
-  logger_.info("Unity Rendering is connected");
-  return true;
+  if (unity_bridge_ptr_ == nullptr) return false;
+  unity_ready_ = unity_bridge_ptr_->connectUnity(scene_id_);
+  return unity_ready_;
 }
 
+template<typename EnvBase>
+void VecEnv<EnvBase>::isTerminalState(Ref<BoolVector<>> terminal_state) {}
+
 template<typename EnvBase>
 void VecEnv<EnvBase>::disconnectUnity(void) {
-  if (unity_bridge_ != nullptr) {
-    unity_bridge_->disconnectUnity();
+  if (unity_bridge_ptr_ != nullptr) {
+    unity_bridge_ptr_->disconnectUnity();
     unity_ready_ = false;
   } else {
     logger_.warn("Flightmare Unity Bridge is not initialized.");
diff --git a/flightrl/.vscode/settings.json b/flightrl/.vscode/settings.json
new file mode 100644
index 0000000000..52863e7207
--- /dev/null
+++ b/flightrl/.vscode/settings.json
@@ -0,0 +1,9 @@
+{
+  "python.autoComplete.extraPaths": [
+    "/home/sysadmin/Projects/RPG_PendulumGate/catkin_gate/devel/lib/python2.7/dist-packages",
+    "/opt/ros/melodic/lib/python2.7/dist-packages",
+    "/home/sysadmin/GitHub/high_mpc",
+    "/home/sysadmin/GitHub/flightmare/flightrl",
+    "/usr/local/python3"
+  ]
+}
\ No newline at end of file
diff --git a/flightros/.vscode/extensions.json b/flightros/.vscode/extensions.json
index 25097a7680..0aa450144e 100644
--- a/flightros/.vscode/extensions.json
+++ b/flightros/.vscode/extensions.json
@@ -1,7 +1,6 @@
 {
     "recommendations": [
         "ms-vscode.cpptools",
-        "ms-vscode.cmake-tools",
         "matepek.vscode-catch2-test-adapter",
         "hbenl.vscode-test-explorer",
         "xaver.clang-format"
diff --git a/flightros/CMakeLists.txt b/flightros/CMakeLists.txt
index c6d55ed7be..cfc80ed0ac 100644
--- a/flightros/CMakeLists.txt
+++ b/flightros/CMakeLists.txt
@@ -1,4 +1,5 @@
 project(flightros)
+
 cmake_minimum_required(VERSION 3.0.0)
 
 find_package(catkin_simple REQUIRED)
@@ -29,8 +30,13 @@ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DEIGEN_STACK_ALLOCATION_LIMIT=1048576")
 set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} ${CMAKE_CXX_ARCH_FLAGS} -Wall -DNDEBUG")
 set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -Wall -g")
 
-# TODO
+cs_add_library(flight_pilot
+    src/flight_pilot.cpp
+)
+target_link_libraries(flight_pilot
+  ${catkin_LIBRARIES}
+)
 
-# # Finish
-# cs_install()
-# cs_export()
\ No newline at end of file
+# Finish
+cs_install()
+cs_export()
\ No newline at end of file
diff --git a/flightros/dependencies.yaml b/flightros/dependencies.yaml
new file mode 100644
index 0000000000..963c3efaa9
--- /dev/null
+++ b/flightros/dependencies.yaml
@@ -0,0 +1,29 @@
+repositories:
+  catkin_simple:
+    type: git
+    url: git@github.com:catkin/catkin_simple.git
+    version: master
+  eigen_catkin:
+    type: git
+    url: git@github.com:ethz-asl/eigen_catkin.git
+    version: master
+  mav_comm:
+    type: git
+    url: git@github.com:ethz-asl/mav_comm.git
+    version: master
+  rotors_simulator:
+    type: git
+    url: git@github.com:ethz-asl/rotors_simulator.git
+    version: master
+  rpg_quadrotor_common:
+    type: git
+    url: git@github.com:uzh-rpg/rpg_quadrotor_common.git
+    version: master
+  rpg_single_board_io:
+    type: git
+    url: git@github.com:uzh-rpg/rpg_single_board_io.git
+    version: master
+  rpg_quadrotor_control:
+    type: git
+    url: git@github.com:uzh-rpg/rpg_quadrotor_control.git
+    version: master
\ No newline at end of file
diff --git a/flightros/include/flightros/flight_pilot.hpp b/flightros/include/flightros/flight_pilot.hpp
new file mode 100644
index 0000000000..90c05f2eb9
--- /dev/null
+++ b/flightros/include/flightros/flight_pilot.hpp
@@ -0,0 +1,67 @@
+
+#pragma once
+
+// ros
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+
+// rpg quadrotor
+#include <autopilot/autopilot_helper.h>
+#include <autopilot/autopilot_states.h>
+#include <quadrotor_common/parameter_helper.h>
+#include <quadrotor_msgs/AutopilotFeedback.h>
+
+// flightlib
+#include "flightlib/bridges/unity_bridge.hpp"
+#include "flightlib/common/quad_state.hpp"
+#include "flightlib/common/types.hpp"
+#include "flightlib/objects/quadrotor.hpp"
+
+using namespace flightlib;
+
+namespace flightros {
+
+class FlightPilot {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  FlightPilot(const ros::NodeHandle& nh, const ros::NodeHandle& pnh);
+  ~FlightPilot();
+
+  // callbacks
+  void mainLoopCallback(const ros::TimerEvent& event);
+  void poseCallback(const nav_msgs::Odometry::ConstPtr& msg);
+
+  bool setUnity(const bool render);
+  bool connectUnity(void);
+  bool loadParams(void);
+
+ private:
+  // ros nodes
+  ros::NodeHandle nh_;
+  ros::NodeHandle pnh_;
+
+  // publisher
+
+  // subscriber
+  ros::Subscriber sub_state_est_;
+
+  // main loop timer
+  ros::Timer timer_main_loop_;
+
+  // unity quadrotor
+  Quadrotor* quad_;
+  QuadState quad_state_;
+
+  // Flightmare(Unity3D)
+  std::shared_ptr<UnityBridge> unity_bridge_ptr_;
+  SceneID scene_id_{UnityScene::WAREHOUSE};
+  bool unity_ready_{false};
+  bool unity_render_{false};
+  RenderMessage_t unity_output_;
+  uint16_t receive_id_{0};
+
+  // auxiliary variables
+  quadrotor_common::QuadStateEstimate state_est_;
+  Scalar main_loop_freq_{50.0};
+};
+}  // namespace flightros
\ No newline at end of file
diff --git a/flightros/launch/flight_pilot.launch b/flightros/launch/flight_pilot.launch
new file mode 100644
index 0000000000..c84bfd0b35
--- /dev/null
+++ b/flightros/launch/flight_pilot.launch
@@ -0,0 +1,6 @@
+<?xml version="1.0"?>
+<launch>
+
+  <node pkg="flightros" type="flight_pilot_node" name="flight_pilot_node" output="screen">
+  </node>
+</launch>
diff --git a/flightros/launch/rotors_gazebo.launch b/flightros/launch/rotors_gazebo.launch
new file mode 100644
index 0000000000..2e22e316fc
--- /dev/null
+++ b/flightros/launch/rotors_gazebo.launch
@@ -0,0 +1,115 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="quad_name" default="hummingbird"/>
+
+  <arg name="mav_name" default="$(arg quad_name)"/>
+  <arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo"/>
+  <arg name="world_name" default="$(find rotors_gazebo)/worlds/basic.world"/>
+
+  <arg name="paused" value="true"/>
+  <arg name="gui" value="true"/>
+  <arg name="use_mpc" default="false"/>
+  <arg name="use_ground_truth" default="true"/>
+  <arg name="enable_ground_truth" default="true"/>
+  <arg name="enable_command_feedthrough" default="false"/>
+  <arg name="custom_models" default=""/>
+
+  <arg name="enable_logging" default="false"/>
+  <arg name="log_file" default="$(arg mav_name)"/>
+
+  <arg name="x_init" default="0"/>
+  <arg name="y_init" default="0"/>
+
+  <arg name="debug" default="false"/>
+  <arg name="verbose" default="false"/>
+
+  <!-- Gazebo stuff to spawn the world !-->
+  <env name="GAZEBO_MODEL_PATH" 
+      value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models:$(arg custom_models)"/>
+  <env name="GAZEBO_RESOURCE_PATH" 
+      value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(arg world_name)" />
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="paused" value="$(arg paused)" />
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="verbose" value="$(arg verbose)"/>
+  </include>
+
+  <!-- RotorS stuff to spawn the quadrotor !-->
+  <group ns="$(arg mav_name)">
+    <include file="$(find rotors_gazebo)/launch/spawn_mav.launch">
+      <arg name="mav_name" value="$(arg mav_name)" />
+      <arg name="model" value="$(arg model)" />
+      <arg name="enable_logging" value="$(arg enable_logging)" />
+      <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+      <arg name="log_file" value="$(arg log_file)"/>
+      <arg name="x" value="$(arg x_init)" />
+      <arg name="y" value="$(arg y_init)" />
+    </include>
+  </group>
+
+  <!-- RPG stuff !-->
+  <group ns="$(arg quad_name)" >
+
+    <group if="$(arg use_ground_truth)">
+      <node pkg="rpg_rotors_interface" type="rpg_rotors_interface" 
+          name="rpg_rotors_interface" output="screen" >
+        <rosparam file="$(find rpg_rotors_interface)/parameters/rpg_rotors_interface.yaml" />
+        
+        <remap from="odometry" to="ground_truth/odometry" />
+        <remap from="rpg_rotors_interface/arm" to="bridge/arm" />
+      </node>
+
+      <!-- Autopilot -->
+      <node pkg="autopilot" type="autopilot" name="autopilot" output="screen">
+        <rosparam file="$(find state_predictor)/parameters/hummingbird.yaml" />
+        <rosparam file="$(find rpg_rotors_interface)/parameters/position_controller.yaml" />
+        <rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml" />
+        
+        <param name="position_controller/use_rate_mode" value="True" />
+        
+        <param name="velocity_estimate_in_world_frame" value="false" />
+        <param name="state_estimate_timeout" value="0.1" />
+        <param name="control_command_delay" value="0.05" />
+        <param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)" />
+        
+        <remap from="autopilot/state_estimate" to="ground_truth/odometry" />
+      </node>
+    </group>
+    <group if="$(arg use_mpc)">
+      <node pkg="rpg_mpc" type="autopilot_mpc_instance" name="autopilot" output="screen">
+        <rosparam file="$(find state_predictor)/parameters/hummingbird.yaml" />
+        <rosparam file="$(find rpg_mpc)/parameters/default.yaml" />
+        <rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml" />
+
+        <param name="velocity_estimate_in_world_frame" value="false" />
+        <param name="state_estimate_timeout" value="0.1" />
+        <param name="control_command_delay" value="0.05" />
+        <param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)" />
+
+        <remap from="autopilot/state_estimate" to="ground_truth/odometry" />
+      </node>
+    </group>
+
+    <node pkg="flightros" type="flight_pilot_node" name="flight_pilot_node" output="screen">
+      <rosparam file="$(find flightros)/params/default.yaml" /> 
+      <remap from="flight_pilot/state_estimate" to="ground_truth/odometry" />
+    </node>
+
+    <node pkg="joy" type="joy_node" name="joy_node">
+      <param name="autorepeat_rate" value="10"/>
+    </node>
+
+    <node pkg="manual_flight_assistant" type="manual_flight_assistant" 
+        name="manual_flight_assistant" output="screen">
+      <rosparam file="$(find rpg_rotors_interface)/parameters/manual_flight_assistant.yaml"/>
+    </node>
+
+    <node name="rqt_quad_gui" pkg="rqt_gui" type="rqt_gui" 
+        args="-s rqt_quad_gui.basic_flight.BasicFlight --args 
+        --quad_name $(arg quad_name)" output="screen"/>
+
+  </group> 
+
+</launch>
\ No newline at end of file
diff --git a/flightros/package.xml b/flightros/package.xml
index ac16486e1f..63ced44735 100644
--- a/flightros/package.xml
+++ b/flightros/package.xml
@@ -12,5 +12,9 @@
   <buildtool_depend>catkin_simple</buildtool_depend>
 
   <depend>flightlib</depend>
+  <depend>roscpp</depend>
+  <depend>autopilot</depend>
+  <depend>quadrotor_common</depend>
+  <depend>nav_msgs</depend>
 
 </package>
\ No newline at end of file
diff --git a/flightros/params/default.yaml b/flightros/params/default.yaml
new file mode 100644
index 0000000000..f97f5052ea
--- /dev/null
+++ b/flightros/params/default.yaml
@@ -0,0 +1,2 @@
+scene_id: 0
+main_loop_freq: 50.0
\ No newline at end of file
diff --git a/flightros/src/flight_pilot.cpp b/flightros/src/flight_pilot.cpp
new file mode 100644
index 0000000000..5f8375de2b
--- /dev/null
+++ b/flightros/src/flight_pilot.cpp
@@ -0,0 +1,87 @@
+#include "flightros/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_ = new Quadrotor;
+  quad_state_.setZero();
+  quad_->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(true);
+  connectUnity();
+}
+
+FlightPilot::~FlightPilot() {}
+
+void FlightPilot::poseCallback(const nav_msgs::Odometry::ConstPtr &msg) {
+  state_est_ = quadrotor_common::QuadStateEstimate(*msg);
+
+  std::cout << "pose callback" << std::endl;
+
+  quad_state_.p << state_est_.position.cast<Scalar>();
+  quad_->setState(quad_state_);
+
+  if (unity_render_ && unity_ready_) {
+    unity_bridge_ptr_->getRender(0);
+    unity_bridge_ptr_->handleOutput(unity_output_);
+  }
+}
+
+void FlightPilot::mainLoopCallback(const ros::TimerEvent &event) {
+  // empty
+}
+
+bool FlightPilot::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_);
+    ROS_INFO("[%s] Unity Bridge is created.", pnh_.getNamespace().c_str());
+  }
+  return true;
+}
+
+bool FlightPilot::connectUnity() {
+  if (unity_bridge_ptr_ == nullptr) return false;
+  unity_ready_ = unity_bridge_ptr_->connectUnity(scene_id_);
+  return unity_ready_;
+}
+
+bool FlightPilot::loadParams(void) {
+  // load parameters
+  quadrotor_common::getParam("main_loop_freq", main_loop_freq_, pnh_);
+
+  return true;
+}
+
+}  // namespace flightros
\ No newline at end of file
diff --git a/flightros/src/flight_pilot_node.cpp b/flightros/src/flight_pilot_node.cpp
new file mode 100644
index 0000000000..a405c838f5
--- /dev/null
+++ b/flightros/src/flight_pilot_node.cpp
@@ -0,0 +1,13 @@
+#include <ros/ros.h>
+
+#include "flightros/flight_pilot.hpp"
+
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "flight_pilot");
+  flightros::FlightPilot pilot(ros::NodeHandle(), ros::NodeHandle("~"));
+
+  // spin the ros
+  ros::spin();
+  
+  return 0;
+}
\ No newline at end of file