add RotorS interface.

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"
+  ]
+}

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

flightlib/build/.clang-format

@@ -1,4 +1,4 @@
 ---
 DisableFormat: true
 SortIncludes: false
----
+---

flightlib/build/.gitignore

@@ -3,4 +3,4 @@
 # Except this file
 !.gitignore
 !.clang-format
-!setup.py
+!setup.py

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

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

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

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"

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="[email protected]">Yunlong Song</maintainer>
+
+  <license>GNU GPL</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>gtest</depend>
+
+</package>

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

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.");

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"
+  ]
+}

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"

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()
+# Finish
+cs_install()
+cs_export()

flightros/dependencies.yaml

@@ -0,0 +1,29 @@
+repositories:
+  catkin_simple:
+    type: git
+    url: [email protected]:catkin/catkin_simple.git
+    version: master
+  eigen_catkin:
+    type: git
+    url: [email protected]:ethz-asl/eigen_catkin.git
+    version: master
+  mav_comm:
+    type: git
+    url: [email protected]:ethz-asl/mav_comm.git
+    version: master
+  rotors_simulator:
+    type: git
+    url: [email protected]:ethz-asl/rotors_simulator.git
+    version: master
+  rpg_quadrotor_common:
+    type: git
+    url: [email protected]:uzh-rpg/rpg_quadrotor_common.git
+    version: master
+  rpg_single_board_io:
+    type: git
+    url: [email protected]:uzh-rpg/rpg_single_board_io.git
+    version: master
+  rpg_quadrotor_control:
+    type: git
+    url: [email protected]:uzh-rpg/rpg_quadrotor_control.git
+    version: master