diff --git a/.clang-format b/.clang-format
new file mode 100644
index 0000000..fe04676
--- /dev/null
+++ b/.clang-format
@@ -0,0 +1,31 @@
+---
+BasedOnStyle: Google
+UseTab: ForIndentation
+TabWidth: 4
+ColumnLimit: 150
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+IndentWidth: 4
+Language: Cpp
+MaxEmptyLinesToKeep: 2
+NamespaceIndentation: None
+SortUsingDeclarations: true
+SpaceAfterTemplateKeyword: false
+SpaceInEmptyParentheses: false
+Standard: Cpp11
+#圆括号之后,多行内容,进行对齐
+AlignAfterOpenBracket: Align
+#连续赋值时,对齐所有等号
+AlignConsecutiveAssignments: true
+#连续声明时,对齐所有声明的变量名
+#AlignConsecutiveDeclarations: true
+AllowShortLoopsOnASingleLine: false
+#对齐连续的尾随的注释
+AlignTrailingComments: true
+#将简单的语句块放到一个单行
+AllowShortBlocksOnASingleLine: false
+#if (a) return;放单行 属性:Never、WithoutElse没有else的可以放单行、OnlyFirstIf只有第一个if放单行、AllIfsAndElse总是把简短的if, else if和else语句放在同一行。
+AllowShortIfStatementsOnASingleLine: false
+---
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..032ced8
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,33 @@
+# Python
+dist
+*.egg-info
+*.so
+__pycache__
+
+# Pycharm
+.idea/
+
+# VScode
+.vscode/
+.vscode/*
+**/.vscode/*
+!.vscode/extensions.json
+
+# General
+_build/
+.sass-cache
+_posts
+_site
+
+# saved
+run/saved/*
+!run/saved/YOPO_1
+run/yopo_trt.pth
+externals/
+
+#logs
+run/utils/*.csv
+
+# dataset
+/run/yopo_sim/*
+
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000..2d773b1
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,25 @@
+MIT License
+
+Copyright (c) 2024, TJU-Aerial-Robotics
+Tianjin University, China
+
+This work is developed based on Flightmare Simulator.
+The original LICENSE can be found in the LICENSE_FLIGHTMARE file.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/LICENSE_FLIGHTMARE b/LICENSE_FLIGHTMARE
new file mode 100644
index 0000000..512533b
--- /dev/null
+++ b/LICENSE_FLIGHTMARE
@@ -0,0 +1,22 @@
+MIT License
+
+Copyright (c) 2020 Robotics and Perception Group,
+University of Zurich, Switzerland
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
\ No newline at end of file
diff --git a/README.md b/README.md
index 45b5727..4d3d23c 100644
--- a/README.md
+++ b/README.md
@@ -1,9 +1,226 @@
-# YOPO
-### Code for Paper: You Only Plan Once: A Learning-based One-stage Planner with Guidance Learning
-More details can be found in the video on [YouTube](https://youtu.be/m7u1MYIuIn4) or [bilibili](https://www.bilibili.com/video/BV15M4m1d7j5/).
-**The code is to be released...**
+# You Only Plan Once
-A simple demonstration with random goals clicked in rviz at the flight speed of 4m/s (the video is 3x speed-up and the map is unavailable).
+Paper: [You Only Plan Once: A Learning-Based One-Stage Planner With Guidance Learning](https://ieeexplore.ieee.org/document/10528860)
-
+Video of this paper can be found: [YouTube](https://youtu.be/m7u1MYIuIn4), [bilibili](https://www.bilibili.com/video/BV15M4m1d7j5)
+
+Some realworld experiment: [YouTube](https://youtu.be/LHvtbKmTwvE), [bilibili](https://www.bilibili.com/video/BV1jBpve5EkP)
+
+## Introduction:
+We proposed a learning-based planner for autonomous navigation in obstacle-dense environments which intergrats (i) perception and mapping, (ii) front-end path searching, and (iii) back-end optimization of classical methods into a single network.
+
+Considering the multi-modal nature of the navigation problem and to avoid local minima around initial values, our approach adopts a set of motion primitives as anchor to cover the searching space, and predicts the offsets and scores of primitives for further improvement (like the one-stage object detector YOLO).
+
+Compared to giving expert demonstrations for imitation in imitation learning or exploring by trial-and-error in reinforcement learning, we directly back-propagate the numerical gradient (e.g. from ESDF) to the weights of neural network in the training process, which is realistic, accurate, and timely.
+
+
+
+  |
+  |
+  |
+
+
+ | primitive anchors |
+ predicted traj and scores |
+ learning method |
+
+
+
+## Acknowledgements:
+
+This project is developed based on the open-source simulator [Flightmare](https://github.com/uzh-rpg/flightmare) and the gradient computation is modified from [grad_traj_optimization](https://github.com/HKUST-Aerial-Robotics/grad_traj_optimization), thanks for their excellent work!
+
+## Installation
+
+The project was tested with Ubuntu 20.04 and Jetson Orin/Xavier NX.
+
+**1. Flightmare Dependencies**
+
+```
+sudo apt-get update && apt-get install -y --no-install-recommends \
+ build-essential \
+ cmake \
+ libzmqpp-dev \
+ libopencv-dev \
+ libpcl-dev
+```
+
+**2. Add sourcing of your catkin workspace as FLIGHTMARE_PATH environment variable:**
+```
+# modify "~/YOPO" to your path
+echo "export FLIGHTMARE_PATH=~/YOPO" >> ~/.bashrc
+source ~/.bashrc
+```
+**3. Unity:**
+
+Download the Flightmare Standalone uploaded by [uzh-rpg
+/agile_autonomy](https://zenodo.org/records/5517791/files/standalone.tar), extract it and put in the `flightrender` folder.
+It should looks like:
+```
+flightrender/
+├── CMakeLists.txt
+└── RPG_Flightmare/
+ ├── flightmare.x86_64
+ └── ...
+```
+
+**4. Create a conda virtual environment.**
+```
+conda create --name yopo python=3.8
+conda activate yopo
+
+conda install pytorch torchvision torchaudio pytorch-cuda=11.8 -c pytorch -c nvidia
+pip install opencv-python
+pip install gym stable-baselines3==1.5.0
+pip install scipy scikit-build ruamel-yaml==0.17.21 numpy==1.22.3 tensorboard==2.9.1 empy catkin_pkg
+```
+**5. build the flightlib**
+```
+conda activate yopo
+cd YOPO/flightlib/build
+cmake ..
+make -j8
+pip install .
+```
+**6. Some issues may arise when we test on different devices.**
+
+6.1. No module named 'flightpolicy'
+
+```
+# modify "~/YOPO/flightpolicy" to your path
+echo "export PYTHONPATH=$PYTHONPATH:~/YOPO" >> ~/.bashrc
+source ~/.bashrc
+```
+6.2. No module named 'flightgym'
+```
+cd YOPO/flightlib/build
+pip install -e .
+```
+
+## Train the Policy
+**1. Data Collection:** For efficiency, we proactively collect dataset (images and states) by randomly initializing the drone's state (position and orientation). We randomly sample multiple velocities and accelerations for each image during the training process. The distribution of random sampled velocity is as `/docs/distribution_of_sampled_velocity.png`. It may take nearly 1 hour for collection with default dataset size but you only need to collect once. The data will be saved at `run/yopo_sim`.
+```
+cd ~/YOPO/run
+conda activate yopo
+python data_collection_simulation.py
+```
+
+**2. Training:**
+```
+cd ~/YOPO/run
+conda activate yopo
+python run_yopo.py --train=1
+```
+It may take 2-3 hours to traing with default dataset size and training epoch. If everything goes well, the training log is as follows:
+
+
+
+
+
+Besides, you can refer to [quadrotor_env.yaml](./flightlib/configs/quadrotor_env.yaml), [vec_env.yaml](./flightlib/config/vec_env.yaml) and [traj_opt.yaml](./flightlib/config/traj_opt.yaml) for some modifications of quadrotor, environment, and trajectory optimization.
+
+## Test the Policy
+
+You can test the policy using pre-trained weights we provide at `run/saved/YOPO_1/Policy/epoch0_iter0.pth`. (For example, to use the weights of `YOPO_1/Policy/epoch2_iter3.pth`, you should use `--trial=1 --epoch=2 --iter=3`)
+
+**1. Test without dynamics model and controller (simple but not recommended).**
+
+
+```
+cd ~/YOPO/run
+conda activate yopo
+python run_yopo.py --train=0 --render=1 --trial=1 --epoch=0 --iter=0
+```
+
+It will take a while for unity setup, and then you will see:
+
+
+
+
+**2. Test with dynamics model and controller (recommended).**
+
+**Prapare:** We did not use the built-in dynamics of Flightmare; instead, we used a ROS-based simulator and controller from [Fast Planner](https://github.com/HKUST-Aerial-Robotics/Fast-Planner). For your convenience, we have extracted only the relevant sections from the project, which you can refer to [UAV_Simulator](https://github.com/TJU-Aerial-Robotics/UAV_Simulator) for installation.
+
+Besides, we recommend using tmux & tmuxinator for terminal management.
+
+**2.1** Start the simulation environment with Unity and the ROS interface (It will take some time to load Unity and randomly generate a new environment)
+
+```
+cd ~/YOPO/flightrender/RPG_Flightmare
+./flightmare.x86_64
+```
+
+```
+cd ~/YOPO/flightlib/build
+./flightros_node
+```
+
+Besides, you can refer to [quadrotor_ros.yaml](./flightlib/configs/quadrotor_ros.yaml) for some modifications.
+
+**2.2** Start the dynamics simulation and controller of UAV
+```
+cd ~/UAV_Simulator
+source devel/setup.bash
+roslaunch so3_quadrotor_simulator simulator.launch
+```
+
+**2.3** Start the YOPO inference and the planner (the implementation of yopo_planner_node will be moved to test_yopo_ros.py in the future)
+
+```
+cd ~/YOPO/run
+conda activate yopo
+python test_yopo_ros.py --trial=1 --epoch=0 --iter=0
+```
+
+```
+cd ~/YOPO/flightlib/build
+./yopo_planner_node
+```
+
+**2.4** Visualization: start the RVIZ and publish the map.
+Then you can click the `2D Nav Goal` on RVIZ as the goal at will, just like the following GIF.
+```
+cd ~/YOPO/
+rviz -d yopo.rviz
+```
+
+```
+cd ~/YOPO/flightlib/build
+./map_visual_node
+```
+
+
+
+
+
+
+## TensorRT Deployment
+We highly recommend using TensorRT for acceleration when flying in real world. It only takes 1ms for inference on NVIDIA Orin NX.
+
+**1. Prepera:**
+```
+conda activate yopo
+pip install -U nvidia-tensorrt --index-url https://pypi.ngc.nvidia.com
+
+git clone https://github.com/NVIDIA-AI-IOT/torch2trt
+cd torch2trt
+python setup.py install
+```
+**2. PyTorch model to TensorRT model**
+```
+cd ~/YOPO/
+conda activate yopo
+python yopo_trt_transfer.py --trial=1 --epoch=0 --iter=0
+```
+**3. TensorRT Inference**
+```
+cd ~/YOPO/
+conda activate yopo
+python test_yopo_ros.py --use_tensorrt=True
+```
+
+## Finally
+We are still working on improving and refactoring the code to improve the readability, reliability, and efficiency. For any technical issues, please feel free to contact me (lqzx1998@tju.edu.cn) 😀 We are very open and enjoy collaboration!
+
+If you find this work useful or interesting, please kindly give us a star ⭐; If our repository supports your academic projects, please cite our paper. Thank you!
\ No newline at end of file
diff --git a/click_in_rviz.gif b/docs/click_in_rviz.gif
old mode 100644
new mode 100755
similarity index 100%
rename from click_in_rviz.gif
rename to docs/click_in_rviz.gif
diff --git a/docs/depth_egb_stereo_image.png b/docs/depth_egb_stereo_image.png
new file mode 100644
index 0000000..cfe49e7
Binary files /dev/null and b/docs/depth_egb_stereo_image.png differ
diff --git a/docs/distribution_of_sampled_velocity.png b/docs/distribution_of_sampled_velocity.png
new file mode 100644
index 0000000..eb81830
Binary files /dev/null and b/docs/distribution_of_sampled_velocity.png differ
diff --git a/docs/executed_trajectory.png b/docs/executed_trajectory.png
new file mode 100644
index 0000000..61f7b45
Binary files /dev/null and b/docs/executed_trajectory.png differ
diff --git a/docs/predicted_trajectories.png b/docs/predicted_trajectories.png
new file mode 100644
index 0000000..f384ac0
Binary files /dev/null and b/docs/predicted_trajectories.png differ
diff --git a/docs/primitive_trajectories.png b/docs/primitive_trajectories.png
new file mode 100644
index 0000000..958f959
Binary files /dev/null and b/docs/primitive_trajectories.png differ
diff --git a/docs/proposed_guidance_learning.png b/docs/proposed_guidance_learning.png
new file mode 100755
index 0000000..2577072
Binary files /dev/null and b/docs/proposed_guidance_learning.png differ
diff --git a/docs/simple_demo.gif b/docs/simple_demo.gif
new file mode 100755
index 0000000..3d8ad55
Binary files /dev/null and b/docs/simple_demo.gif differ
diff --git a/docs/train_log.png b/docs/train_log.png
new file mode 100644
index 0000000..7afb83e
Binary files /dev/null and b/docs/train_log.png differ
diff --git a/flightlib/CMakeLists.txt b/flightlib/CMakeLists.txt
new file mode 100644
index 0000000..6a7a0a5
--- /dev/null
+++ b/flightlib/CMakeLists.txt
@@ -0,0 +1,321 @@
+cmake_minimum_required(VERSION 3.0)
+
+project(flightlib VERSION 0.1.0)
+
+message(STATUS "====================== !Flightmare! ======================")
+
+# ###############################################################################
+# Options
+# ###############################################################################
+option(ENABLE_FAST "Build with optimizations for speed" ON)
+option(ENABLE_BLAS "Build using BLAS and LAPACK libraries" OFF)
+option(ENABLE_PARALLEL "Build using openmp parallelization" ON)
+option(EIGEN_FROM_SYSTTEM "Use the system-provided Eigen" ON)
+set(
+ EIGEN_ALTERNATIVE "" CACHE STRING
+ "Path to alternative Eigen, autodownload if blank"
+)
+
+# ###############################################################################
+# Finding Dependencies
+# ###############################################################################
+
+# Eigen3
+message(STATUS "======> Setup Dependencies")
+
+if(EIGEN_FROM_SYSTTEM)
+ find_package(Eigen3 3.3.4 QUIET)
+
+ if(EIGEN3_FOUND)
+ message(STATUS "Using system provided Eigen.")
+ message(${EIGEN3_INCLUDE_DIR})
+ else()
+ message(STATUS "No sufficient Eigen version (3.3.4) found.")
+ message(STATUS "Restoring to download Eigen sources.")
+ include(cmake/eigen.cmake)
+ endif()
+elseif(EIGEN_ALTERNATIVE STREQUAL "")
+ include(cmake/eigen.cmake)
+else()
+ set(EIGEN_INCLUDE_DIR ${EIGEN_ALTERNATIVE})
+endif()
+
+message(STATUS "Eigen3 include dir: ${EIGEN3_INCLUDE_DIR}")
+
+# PCL (Many VTK errors are reported, but do not affect compilation)
+find_package(PCL 1.10 REQUIRED)
+message(STATUS "Using system provided PCL.")
+message(${PCL_INCLUDE_DIRS})
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+# Including dependencies
+find_package(OpenCV REQUIRED)
+include_directories(${OpenCV_INCLUDE_DIRS})
+
+# Including dependencies
+include(cmake/pybind11.cmake)
+include(cmake/yaml.cmake)
+
+# Including OpenMP & CUDA
+find_package(OpenMP REQUIRED)
+find_package(CUDA REQUIRED)
+
+if(ENABLE_BLAS)
+ set(BLA_VENDOR "Generic")
+ find_package(BLAS REQUIRED)
+
+ if(BLAS_FOUND)
+ message(STATUS "Found BLAS: ${BLAS_LIBRARIES}")
+ else()
+ message(ERROR "Could not enable BLAS because BLAS was not found")
+ endif()
+
+ find_package(LAPACK REQUIRED)
+
+ if(LAPACK_FOUND)
+ message(STATUS "Found Lapack: ${LAPACK_LIBRARIES}")
+ else()
+ message(ERROR "Could not enable LAPACK because LAPACK was not found")
+ endif()
+endif()
+
+# Check for ccache
+if(NOT DEFINED CATKIN_DEVEL_PREFIX)
+ find_program(CCACHE_PROGRAM ccache)
+
+ if(CCACHE_PROGRAM)
+ set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "${CCACHE_PROGRAM}")
+ else()
+ message(INFO "Build time could be improved with ccache!")
+ message(INFO "sudo apt install ccache")
+ endif()
+endif()
+
+# ###############################################################################
+# Setup Compilation Flag
+# ###############################################################################
+message(STATUS "======> Setup Compilation ")
+
+add_definitions(-DEIGEN_STACK_ALLOCATION_LIMIT=1048576)
+include_directories(${EIGEN_INCLUDE_DIR} "tests")
+include_directories(${OpenCV_INCLUDE_DIRS})
+
+# Set default build type
+if(NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt)
+ if(NOT CMAKE_BUILD_TYPE)
+ set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE)
+ endif()
+endif()
+
+cuda_select_nvcc_arch_flags(CUDA_ARCH_FLAGS)
+message("CUDA Architecture: ${CUDA_ARCH_FLAGS}")
+
+if(${CMAKE_BUILD_TYPE} STREQUAL Release)
+ SET(CUDA_NVCC_FLAGS "${CUDA_ARCH_FLAGS};-O3;-use_fast_math" CACHE STRING "nvcc flags" FORCE)
+ message("build CUDA with -O3")
+else()
+ message("build CUDA with Debug")
+ SET(CUDA_NVCC_FLAGS "-g ;-G ;${CUDA_ARCH_FLAGS}" CACHE STRING "nvcc flags" FORCE)
+ SET(CUDA_VERBOSE_BUILD ON CACHE BOOL "nvcc verbose" FORCE)
+endif()
+
+# Add c++ flags
+set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -fPIC -Wall -DNDEBUG -fopenmp")
+set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -fPIC -Wall -g -fopenmp")
+set(CMAKE_CXX_STANDARD 17)
+
+# Architectural flags
+if("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "armv7l")
+ message(STATUS "Using ARMv7 optimized flags!")
+ set(CMAKE_CXX_ARCH_FLAGS " -Wno-psabi -march=armv7-a -mfpu=neon -mfloat-abi=hard -funsafe-math-optimizations")
+elseif("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
+ message(STATUS "Using ARM aarch64 optimized flags!")
+ set(CMAKE_CXX_ARCH_FLAGS " -Wno-psabi -march=armv8-a+crypto -mcpu=cortex-a57+crypto")
+else()
+ set(CMAKE_CXX_ARCH_FLAGS " -march=native")
+endif()
+
+# Optimized flags
+if(ENABLE_FAST)
+ message(STATUS "Enabling fast optimization flags!")
+ set(CMAKE_CXX_FAST_FLAGS " -Ofast")
+else()
+ set(CMAKE_CXX_FAST_FLAGS " -O0")
+endif()
+
+# BLAS Flags
+if(BLAS_FOUND AND LAPACK_FOUND)
+ message(STATUS "Enabling BLAS and LAPACK")
+ set(CMAKE_CXX_BLAS_FLAGS " -DEIGEN_USE_BLAS -DEIGEN_USE_LAPACK -DEIGEN_USE_LAPACKE")
+else()
+ set(CMAKE_CXX_BLAS_FLAGS "")
+endif()
+
+# Summarize Flags
+set(CMAKE_CXX_FLAGS_RELEASE
+ "${CMAKE_CXX_FLAGS_RELEASE} ${CMAKE_CXX_FAST_FLAGS} ${CMAKE_CXX_ARCH_FLAGS} ${CMAKE_CXX_PAR_FLAGS}")
+string(REPLACE "-DNDEBUG" "" CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO}")
+message(STATUS "The activated CXX RELEASE configuration is:\n ${CMAKE_CXX_FLAGS_RELEASE}")
+message(STATUS "The activated CXX DEBUG configuration is:\n ${CMAKE_CXX_FLAGS_DEBUG}")
+
+# ###############################################################################
+# Specify Build Resources
+# ###############################################################################
+message(STATUS "======> Setup Build ")
+
+add_subdirectory(${PROJECT_SOURCE_DIR}/third_party/arc_utilities)
+add_subdirectory(${PROJECT_SOURCE_DIR}/third_party/sdf_tools)
+
+# Create file lists for flightlib source
+file(GLOB_RECURSE FLIGHTLIB_SOURCES
+ src/bridges/*.cpp
+ src/dynamics/*.cpp
+ src/objects/*.cpp
+ src/sensors/*.cpp
+ src/sensors/*.cu
+ src/envs/*.cpp
+ src/common/*.cpp
+ src/grad_traj_optimization/*.cpp
+)
+
+# Create file lists for flightlib_gym source
+file(GLOB_RECURSE FLIGHTLIB_GYM_SOURCES
+ src/wrapper/*.cpp
+)
+
+# ###############################################################################
+# Optional Catkin Build
+# ###############################################################################
+if(DEFINED CATKIN_DEVEL_PREFIX)
+ message(STATUS "======> Building with -- catkin -- ")
+ include(cmake/catkin.cmake)
+ return()
+endif()
+
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ sdf_tools
+ pcl_ros
+ pcl_conversions
+ sensor_msgs
+)
+
+# ###############################################################################
+# Setup Build
+# ###############################################################################
+
+# Library and Executables
+include_directories(include)
+include_directories(${PROJECT_SOURCE_DIR}/third_party/sdf_tools/include)
+include_directories(${PROJECT_SOURCE_DIR}/third_party/arc_utilities/include)
+
+# 1. The Training Lib
+if(NOT FLIGHTLIB_SOURCES)
+ set(LIBRARY_NAME)
+else()
+ # flightlib -CUDA
+ cuda_add_library(${PROJECT_NAME} ${FLIGHTLIB_SOURCES})
+ target_link_libraries(${PROJECT_NAME} # PRIVATE
+ ${OpenCV_LIBRARIES}
+ ${PCL_LIBRARIES}
+ ${CUDA_curand_LIBRARY}
+ ${CUDA_cublas_LIBRARY}
+ ${CUDA_LIBRARIES}
+ ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PROJECT_SOURCE_DIR}
+ yaml-cpp
+ zmq
+ zmqpp
+ stdc++fs)
+ set(LIBRARY_NAME ${PROJECT_NAME})
+ set_target_properties(${PROJECT_NAME} PROPERTIES POSITION_INDEPENDENT_CODE TRUE)
+endif()
+
+if(FLIGHTLIB_GYM_SOURCES)
+ # flightlib_gym (python3 binding with Pybind11)
+ pybind11_add_module(flightgym MODULE
+ ${FLIGHTLIB_GYM_SOURCES})
+
+ if(EIGEN3_FOUND)
+ target_include_directories(flightgym PRIVATE
+
+ # ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include
+ ${PYBIND11_INCLUDE_DIR}
+ ${PROJECT_SOURCE_DIR}/include
+ ${EIGEN3_INCLUDE_DIR})
+ else()
+ target_include_directories(flightgym PRIVATE
+
+ # ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include
+ ${PYBIND11_INCLUDE_DIR}
+ ${PROJECT_SOURCE_DIR}/externals/eigen/eigen3 # pybind11 use #include , however, flightmare use #include
+ ${PROJECT_SOURCE_DIR}/include)
+ endif()
+
+ target_link_libraries(flightgym PRIVATE ${LIBRARY_NAME})
+endif()
+
+if(ENABLE_BLAS AND BLAS_FOUND AND LAPACK_FOUND)
+ message(STATUS "Linking standard BLAS ${BLAS_LIBRARIES}")
+ target_link_libraries(${LIBRARY_NAME}
+ ${BLAS_LIBRARIES}
+ ${LAPACK_LIBRARIES}
+ ${LAPACKE_LIBRARIES}
+ )
+endif()
+
+# Build Other ROS PKG
+find_package(catkin REQUIRED COMPONENTS
+ tf
+ nav_msgs
+ cv_bridge
+)
+
+include_directories(
+ ${catkin_INCLUDE_DIRS}
+ ${PROJECT_SOURCE_DIR}/include
+)
+
+# 2. ROS Simulation Node
+add_executable(flightros_node
+ src/ros_nodes/flight_pilot.cpp
+ src/ros_nodes/flight_pilot_node.cpp
+)
+
+target_link_libraries(flightros_node
+ ${LIBRARY_NAME}
+ ${catkin_LIBRARIES}
+ ${OpenCV_LIBRARIES}
+ zmqpp
+ stdc++fs
+)
+
+# 3. The Planner
+add_executable(yopo_planner_node
+ src/ros_nodes/yopo_planner_node.cpp
+)
+target_link_libraries(yopo_planner_node
+ ${LIBRARY_NAME}
+ ${catkin_LIBRARIES}
+)
+
+# 4. Tool: Trajectory Evaluation
+add_executable(traj_eval_node
+ src/ros_nodes/traj_eval_node.cpp
+)
+target_link_libraries(traj_eval_node
+ ${LIBRARY_NAME}
+ ${catkin_LIBRARIES}
+)
+
+# 5. Tool: Map Visualization
+add_executable(map_visual_node
+ src/ros_nodes/map_visual_node.cpp
+)
+target_link_libraries(map_visual_node
+ ${LIBRARY_NAME}
+ ${catkin_LIBRARIES}
+)
+
+message(STATUS "================ !Done. No more nightmare! ================")
diff --git a/flightlib/build/.clang-format b/flightlib/build/.clang-format
new file mode 100644
index 0000000..1bbeb6c
--- /dev/null
+++ b/flightlib/build/.clang-format
@@ -0,0 +1,4 @@
+---
+DisableFormat: true
+SortIncludes: false
+---
\ No newline at end of file
diff --git a/flightlib/build/.gitignore b/flightlib/build/.gitignore
new file mode 100644
index 0000000..eedfbba
--- /dev/null
+++ b/flightlib/build/.gitignore
@@ -0,0 +1,6 @@
+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore
+!.clang-format
+!setup.py
diff --git a/flightlib/build/setup.py b/flightlib/build/setup.py
new file mode 100644
index 0000000..d9b98a8
--- /dev/null
+++ b/flightlib/build/setup.py
@@ -0,0 +1,15 @@
+from setuptools import setup, Extension
+
+#
+setup(name='flightgym',
+ version='0.0.1',
+ author="Junjie Lu / Yunlong Song",
+ author_email='lqzx1998@tju.edu.cn / song@ifi.uzh.ch',
+ description="Flightmare: A Quadrotor Simulator",
+ long_description='This project is modified based on Flightmare by Yunlong Song, Thanks for his excellent work!',
+ packages=[''],
+ package_dir={'': './'},
+ package_data={'': ['flightgym.cpython-36m-x86_64-linux-gnu.so']},
+ zip_fase=True,
+ url=None,
+)
diff --git a/flightlib/cmake/catkin.cmake b/flightlib/cmake/catkin.cmake
new file mode 100644
index 0000000..463b3a1
--- /dev/null
+++ b/flightlib/cmake/catkin.cmake
@@ -0,0 +1,29 @@
+# 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()
diff --git a/flightlib/cmake/eigen.cmake b/flightlib/cmake/eigen.cmake
new file mode 100644
index 0000000..e17cb3e
--- /dev/null
+++ b/flightlib/cmake/eigen.cmake
@@ -0,0 +1,25 @@
+# Download and unpack eigen at configure time
+message(STATUS "Getting Eigen...")
+
+configure_file(
+ cmake/eigen_download.cmake
+ ${PROJECT_SOURCE_DIR}/externals/eigen/CMakeLists.txt)
+
+execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
+ RESULT_VARIABLE result
+ WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/eigen
+ OUTPUT_QUIET)
+if(result)
+ message(FATAL_ERROR "Download of Eigen failed: ${result}")
+endif()
+execute_process(COMMAND ${CMAKE_COMMAND} --build .
+ RESULT_VARIABLE result
+ WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/eigen
+ OUTPUT_QUIET)
+if(result)
+ message(FATAL_ERROR "Build step for eigen failed: ${result}")
+endif()
+
+message(STATUS "Eigen downloaded!")
+
+set(EIGEN_INCLUDE_DIR ${PROJECT_SOURCE_DIR}/externals/eigen)
\ No newline at end of file
diff --git a/flightlib/cmake/eigen_download.cmake b/flightlib/cmake/eigen_download.cmake
new file mode 100644
index 0000000..11404ce
--- /dev/null
+++ b/flightlib/cmake/eigen_download.cmake
@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.0.0)
+
+project(eigen-external)
+
+include(ExternalProject)
+ExternalProject_Add(eigen
+ GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git
+ GIT_TAG 3.3.4
+ SOURCE_DIR "${PROJECT_SOURCE_DIR}/externals/eigen/eigen3"
+ CONFIGURE_COMMAND ""
+ BUILD_COMMAND ""
+ INSTALL_COMMAND ""
+ TEST_COMMAND ""
+ UPDATE_DISCONNECTED ON
+)
\ No newline at end of file
diff --git a/flightlib/cmake/gtest.cmake b/flightlib/cmake/gtest.cmake
new file mode 100644
index 0000000..98ee7bc
--- /dev/null
+++ b/flightlib/cmake/gtest.cmake
@@ -0,0 +1,37 @@
+# Download and unpack googletest at configure time
+message(STATUS "Getting gtests...")
+
+configure_file(cmake/gtest_download.cmake ${PROJECT_SOURCE_DIR}/externals/googletest-download/CMakeLists.txt)
+execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
+ RESULT_VARIABLE result
+ WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/googletest-download
+ OUTPUT_QUIET)
+if(result)
+ message(FATAL_ERROR "CMake step for googletest failed: ${result}")
+endif()
+execute_process(COMMAND ${CMAKE_COMMAND} --build .
+ RESULT_VARIABLE result
+ WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/googletest-download
+ OUTPUT_QUIET)
+if(result)
+ message(FATAL_ERROR "Build step for googletest failed: ${result}")
+endif()
+
+message(STATUS "gtests downloaded!")
+
+# Prevent overriding the parent project's compiler/linker
+# settings on Windows
+set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+
+# Add googletest directly to our build. This defines
+# the gtest and gtest_main targets.
+add_subdirectory(${PROJECT_SOURCE_DIR}/externals/googletest-src
+ ${PROJECT_SOURCE_DIR}/externals/googletest-build
+ EXCLUDE_FROM_ALL)
+
+# The gtest/gtest_main targets carry header search path
+# dependencies automatically when using CMake 2.8.11 or
+# later. Otherwise we have to add them here ourselves.
+if (CMAKE_VERSION VERSION_LESS 2.8.11)
+ include_directories("${gtest_SOURCE_DIR}/include")
+endif()
\ No newline at end of file
diff --git a/flightlib/cmake/gtest_download.cmake b/flightlib/cmake/gtest_download.cmake
new file mode 100644
index 0000000..fdeded5
--- /dev/null
+++ b/flightlib/cmake/gtest_download.cmake
@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.0.0)
+
+project(googletest-download NONE)
+
+include(ExternalProject)
+ExternalProject_Add(googletest
+ GIT_REPOSITORY https://github.com/google/googletest.git
+ GIT_TAG main
+ SOURCE_DIR "${PROJECT_SOURCE_DIR}/externals/googletest-src"
+ BINARY_DIR "${PROJECT_SOURCE_DIR}/externals/googletest-build"
+ CONFIGURE_COMMAND ""
+ BUILD_COMMAND ""
+ INSTALL_COMMAND ""
+ TEST_COMMAND ""
+ UPDATE_DISCONNECTED ON
+)
diff --git a/flightlib/cmake/pybind11.cmake b/flightlib/cmake/pybind11.cmake
new file mode 100644
index 0000000..f4c56cd
--- /dev/null
+++ b/flightlib/cmake/pybind11.cmake
@@ -0,0 +1,33 @@
+# Download and unpack pybind11 at configure time
+message(STATUS "Getting Pybind11...")
+
+# set(PYBIND11_PYTHON_VERSION 3.6)
+set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION_STRING})
+
+configure_file(
+ cmake/pybind11_download.cmake
+ ${PROJECT_SOURCE_DIR}/externals/pybind11-download/CMakeLists.txt)
+
+execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
+ RESULT_VARIABLE result
+ WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/pybind11-download
+ OUTPUT_QUIET)
+if(result)
+ message(FATAL_ERROR "Cmake Step for Pybind11 failed: ${result}")
+endif()
+
+execute_process(COMMAND ${CMAKE_COMMAND} --build .
+ RESULT_VARIABLE result
+ WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/pybind11-download
+ OUTPUT_QUIET)
+if(result)
+ message(FATAL_ERROR "Build step for eigen failed: ${result}")
+endif()
+
+message(STATUS "Pybind11 downloaded!")
+
+set(PYBIND11_INCLUDE_DIR ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include)
+add_subdirectory(${PROJECT_SOURCE_DIR}/externals/pybind11-src
+ EXCLUDE_FROM_ALL)
+
+include_directories(SYSTEM "${PROJECT_SOURCE_DIR}/externals/pybind11-src/include")
\ No newline at end of file
diff --git a/flightlib/cmake/pybind11_download.cmake b/flightlib/cmake/pybind11_download.cmake
new file mode 100644
index 0000000..d81e578
--- /dev/null
+++ b/flightlib/cmake/pybind11_download.cmake
@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.0.0)
+
+project(pybind11-download)
+
+include(ExternalProject)
+ExternalProject_Add(pybind11
+ GIT_REPOSITORY https://cf.ghproxy.cc/https://github.com/pybind/pybind11
+ GIT_TAG master
+ SOURCE_DIR "${PROJECT_SOURCE_DIR}/externals/pybind11-src"
+ BINARY_DIR "${PROJECT_SOURCE_DIR}/externals/pybind11-bin"
+ CONFIGURE_COMMAND ""
+ CMAKE_ARGS ""
+ BUILD_COMMAND ""
+ INSTALL_COMMAND ""
+ TEST_COMMAND ""
+ UPDATE_DISCONNECTED ON
+)
\ No newline at end of file
diff --git a/flightlib/cmake/yaml.cmake b/flightlib/cmake/yaml.cmake
new file mode 100644
index 0000000..8d1fdea
--- /dev/null
+++ b/flightlib/cmake/yaml.cmake
@@ -0,0 +1,33 @@
+# Download and unpack eigen at configure time
+message(STATUS "Getting yaml-cpp...")
+
+configure_file(
+ cmake/yaml_download.cmake
+ ${PROJECT_SOURCE_DIR}/externals/yaml-download/CMakeLists.txt)
+
+execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
+ RESULT_VARIABLE result
+ WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/yaml-download
+ OUTPUT_QUIET
+ ERROR_QUIET)
+if(result)
+ message(FATAL_ERROR "CMake step for yaml-cpp failed: ${result}")
+endif()
+execute_process(COMMAND ${CMAKE_COMMAND} --build .
+ RESULT_VARIABLE result
+ WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/yaml-download
+ OUTPUT_QUIET
+ ERROR_QUIET)
+if(result)
+ message(FATAL_ERROR "Build step for yaml failed: ${result}")
+endif()
+
+message(STATUS "Yaml downloaded!")
+
+add_subdirectory(${PROJECT_SOURCE_DIR}/externals/yaml-src
+ ${PROJECT_SOURCE_DIR}/externals/yaml-build
+ EXCLUDE_FROM_ALL)
+target_compile_options(yaml-cpp PUBLIC -fPIC -w)
+
+include_directories(SYSTEM "${PROJECT_SOURCE_DIR}/externals/yaml-src/include")
+link_directories("${PROJECT_SOURCE_DIR}/externals/yaml-build")
\ No newline at end of file
diff --git a/flightlib/cmake/yaml_download.cmake b/flightlib/cmake/yaml_download.cmake
new file mode 100644
index 0000000..8e75664
--- /dev/null
+++ b/flightlib/cmake/yaml_download.cmake
@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.0.0)
+
+project(yaml-download)
+
+include(ExternalProject)
+ExternalProject_Add(yaml
+ GIT_REPOSITORY https://cf.ghproxy.cc/https://github.com/jbeder/yaml-cpp
+ GIT_TAG master
+ SOURCE_DIR "${PROJECT_SOURCE_DIR}/externals/yaml-src"
+ BINARY_DIR "${PROJECT_SOURCE_DIR}/externals/yaml-bin"
+ CONFIGURE_COMMAND ""
+ CMAKE_ARGS "-DBUILD_TESTING=OFF -DYAML_CPP_INSTALL=OFF"
+ CMAKE_CACHE_ARGS -DBUILD_TESTING:BOOL=OFF -DYAML_CPP_INSTALLC:BOOL=ON
+ BUILD_COMMAND ""
+ INSTALL_COMMAND ""
+ TEST_COMMAND ""
+ UPDATE_DISCONNECTED ON
+)
\ No newline at end of file
diff --git a/flightlib/configs/quadrotor_env.yaml b/flightlib/configs/quadrotor_env.yaml
new file mode 100755
index 0000000..740c339
--- /dev/null
+++ b/flightlib/configs/quadrotor_env.yaml
@@ -0,0 +1,26 @@
+quadrotor_env:
+ collect_data: yes # yes in Data Collection (random init state); no in Imitation Learning and Testing
+ bounding_box: [60.0, 60.0, 2.0] # spawn quadrotor within this bounding box
+ bounding_box_origin: [-10, 20, 2.5]
+ sim_dt: 0.1 # sim_dt in imitation learning and testing
+
+data_collection:
+ roll_var: 0.01
+ pitch_var: 0.01
+
+rgb_camera_left:
+ on: yes
+ t_BC: [0.0, 0.0, 0.1] # translational vector of the camera with repect to the body frame
+ r_BC: [0.0, 0.0, -90] # rotational angle (roll, pitch, yaw) of the camera in degree.
+ width: 160
+ height: 90
+ fov: 90.0 # Horizontal FOV
+ enable_depth: yes
+ enable_segmentation: no # not used
+ enable_opticalflow: no # not used
+
+# Enable Stereo depth when rgb_camera_right on.
+rgb_camera_right:
+ on: no
+ t_BC: [0.0, -0.2, 0.1] # translational vector of the camera with repect to the body frame
+ r_BC: [0.0, 0.0, -90] # rotational angle (roll, pitch, yaw) of the camera in degree.
diff --git a/flightlib/configs/quadrotor_ros.yaml b/flightlib/configs/quadrotor_ros.yaml
new file mode 100644
index 0000000..ec9c838
--- /dev/null
+++ b/flightlib/configs/quadrotor_ros.yaml
@@ -0,0 +1,30 @@
+main_loop_freq: 30
+unity_render: yes
+scene_id: 4 # 0 wasteland, 1 japanese street, 4 emptyforest in standalone (SR)
+odom_topic: /juliett/ground_truth/odom
+quad_size: 0.1
+ply_path: "/flightrender/RPG_Flightmare/pointcloud_data/"
+
+rgb_camera_left:
+ on: yes
+ t_BC: [0.0, 0.0, 0.1] # translational vector of the camera with repect to the body frame
+ r_BC: [0.0, -5.0, -90] # rotational angle (roll, pitch, yaw) of the camera in degree.
+ width: 160
+ height: 90
+ fov: 90.0 # Horizontal FOV
+ enable_depth: yes
+ enable_segmentation: no
+ enable_opticalflow: no
+
+# enable stereo depth when rgb_camera_right on (If use, please use larger resolution (e.g., 640x360)).
+rgb_camera_right:
+ on: no
+ t_BC: [0.0, -0.2, 0.1] # translational vector of the camera with repect to the body frame
+ r_BC: [0.0, -5.0, -90] # rotational angle (roll, pitch, yaw) of the camera in degree.
+
+unity:
+ spawn_trees: true
+ avg_tree_spacing: 4.0
+ bounding_box: [80.0, 80.0, 11.0] # spawn objects within this bounding box
+ bounding_box_origin: [-10, 20, 2.5]
+ pointcloud_resolution: 0.2
\ No newline at end of file
diff --git a/flightlib/configs/traj_opt.yaml b/flightlib/configs/traj_opt.yaml
new file mode 100644
index 0000000..4bbb76f
--- /dev/null
+++ b/flightlib/configs/traj_opt.yaml
@@ -0,0 +1,64 @@
+vel_max: 6.0
+# segment_time = 2 * radio / vel_max
+
+# IMPORTANT PARAM: weight of penalties (6m/s)
+ws: 0.00004
+wc: 0.001
+wl: 0.00
+wg: 0.0002
+
+#ws: 0.00004
+#wc: 0.001
+#wl: 0.02
+#wg: 0.0001
+
+# trajectory and primitive parma
+horizon_num: 5
+vertical_num: 3
+horizon_camera_fov: 90.0
+vertical_camera_fov: 60.0
+horizon_anchor_fov: 30
+vertical_anchor_fov: 30
+goal_length: 10
+radio_range: 4.0 # planning horizon: 2 * radio_range
+vel_fov: 90.0 # not use currently
+radio_num: 1 # 1 just ok
+vel_num: 1 # 1 just ok
+vel_prefile: 0.0 # 0 just ok
+
+# For data efficiency, we randomly sample multiple vel and acc for each depth image with the following the distribution.
+# values at normalized speed (actual speed can be denormalized by multiplying v_multiple)
+# 单位数据倍数: v_multiple = 0.5 * v_max = radio / time
+# v数据的均值: v_mean = v_multiple * v_mean_unit
+# v数据的方差: v_var = v_multiple^2 * v_var_unit
+# a数据的均值: v_mean = v_multiple^2 * a_mean_unit
+# a数据的方差: v_var = v_multiple^4 * a_var_unit
+vx_mean_unit: 1.5
+vy_mean_unit: 0.0
+vz_mean_unit: 0.0
+vx_var_unit: 0.15
+vy_var_unit: 0.45
+vz_var_unit: 0.1
+ax_mean_unit: 0.0
+ay_mean_unit: 0.0
+az_mean_unit: 0.0
+ax_var_unit: 0.0278
+ay_var_unit: 0.05
+az_var_unit: 0.0278
+
+# penalties
+alpha: 10.0
+d0: 1.2
+r: 0.6
+
+alphav: 2.0
+v0: 3.5
+rv: 1.5
+
+alphaa: 2.0
+a0: 3.5
+ra: 1.5
+
+# deprecated weight
+wv: 0.0
+wa: 0.0
diff --git a/flightlib/configs/vec_env.yaml b/flightlib/configs/vec_env.yaml
new file mode 100644
index 0000000..6e29305
--- /dev/null
+++ b/flightlib/configs/vec_env.yaml
@@ -0,0 +1,15 @@
+env:
+ seed: 1
+ scene_id: 4 # 0 wasteland, 1 japanese street, 4 emptyforest in SR standalone
+ num_envs: 16 # Important: same to batch size!
+ num_threads: 16
+ dataset_path: "/run/yopo_sim/"
+ ply_path: "/run/yopo_sim/"
+
+unity:
+ spawn_trees: true
+ avg_tree_spacing: 4
+ # larger than the position to generate the drone to ensure the completeness of the point cloud in edge.
+ bounding_box: [80.0, 80.0, 11] # spawn objects within this bounding box
+ bounding_box_origin: [-10, 20, 2.5]
+ pointcloud_resolution: 0.2
diff --git a/flightlib/include/flightlib/bridges/unity_bridge.hpp b/flightlib/include/flightlib/bridges/unity_bridge.hpp
new file mode 100644
index 0000000..0e5720a
--- /dev/null
+++ b/flightlib/include/flightlib/bridges/unity_bridge.hpp
@@ -0,0 +1,115 @@
+//
+// This bridge was originally from FlightGoggles.
+// We made several changes on top of it.
+//
+#pragma once
+
+// std libs
+#include
+#include
+#include
+
+#include
+#include
+#include