init

README.md

@@ -0,0 +1,136 @@
+# Teach-Repeat-Replan
+Teach-Repeat-Replan: A Complete and Robust System for Aggressive Flight in Complex Environments
+
+**Teach-Repeat-Replan** is a complete and robust system containing all components for **UAV** aggressive **autonomous flight** in complex environments. It is built upon on the classical robotics teach-and-repeat framework, which is widely adopted in infrastructure inspection, aerial transportation, and search-and-rescue. Our system can capture users' intention of a flight mission, convert an arbitrarily jerky teaching trajectory to a guaranteed smooth and safe repeating trajectory, and generate safe local re-plans to avoid unmapped or moving obstacles on the flight.
+
+**Videos:**
+
+<a href="https://youtu.be/urEC2AAGEDs" target="_blank"><img src="https://img.youtube.com/vi/urEC2AAGEDs/0.jpg" 
+alt="video" width="376" height="240" border="5" /></a>
+<a href="https://youtu.be/Ut8WT0BURrM" target="_blank"><img src="https://img.youtube.com/vi/Ut8WT0BURrM/0.jpg" 
+alt="video" width="376" height="240" border="5" /></a>
+
+**Videos for Mainland China:** [Video1](https://www.bilibili.com/video/av57116775/) [Video2](https://www.bilibili.com/video/av57117018/)
+
+**Authors:**[Fei Gao](https://ustfei.com/), [Boyu Zhou](https://github.com/ZbyLGsc), [Luqi Wang](https://lwangax.wordpress.com) and [Shaojie Shen](http://uav.ust.hk/group/) from the [HUKST Aerial Robotics Group](http://uav.ust.hk/).
+
+Sub-modules integrated in our system include:
+
+**Planning:**     flight corridor generation, global spatial-temporal planning, [local online re-planning]()
+
+**Perception:**   [global deformable surfel mapping](https://github.com/HKUST-Aerial-Robotics/DenseSurfelMapping), [local online ESDF mapping](https://github.com/hlx1996/FIESTA)
+
+**Localization:** [global pose graph optimization, local visual-inertial fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion)
+
+**Controlling:**  [geometric controller on SE(3)](https://ieeexplore.ieee.org/document/5717652)
+
+**Architecture:**
+  <div align=center>
+  <img src="https://github.com/HKUST-Aerial-Robotics/Teach-Repeat-Replan/blob/master/files/sys_architecture.png" width = "767" height = "366">
+  </div>
+
+Our system can be applied to situations where the user has a preferable rough route but isn't able to pilot the drone ideally. For example, for drone racing or aerial filming, a beginner-level pilot is impossible to control the drone to finish the race safely or take an aerial video smoothly unless months of training. With our system, the huamn pilot can virtually control the drone with his/her navie operations, then our system automatically generate a very efficient repeating trajectory and autonomously execute it.
+
+Our system can also be used for normal autonomous navigations, like our previous works in [video1](https://youtu.be/Uh2aKmUzXSg) and [video2](https://youtu.be/Dn6pXL3GqeY). For these applications, drone can autonomously fly in complex environments using only onboard sensing and planning.
+
+**Related Papers**
+
+* **Teach-Repeat-Replan: A Complete and Robust System for Aggressive Flight in Complex Environments**, Fei Gao, Luqi Wang, Boyu Zhou, Luxin Han, Jie Pan, shaojie Shen, submitted to IEEE Transactions on Robotics (T-RO), [arxiv]()
+
+* **Optimal Trajectory Generation for Quadrotor Teach-and-Repeat**, Fei Gao, Luqi Wang, Kaixuan Wang, William Wu, Boyu Zhou, Luxin Han, Shaojie Shen, IEEE Robotics and Automation Letters (RA-L), [pdf](https://ieeexplore.ieee.org/abstract/document/8625495) 
+
+*If you use Teach-Repeat-Replan or its sub-modules for your application or research, please cite our related papers.* [bib](https://github.com/HKUST-Aerial-Robotics/Teach-Repeat-Replan/blob/master/files/bib.txt)
+
+## Simulation or Real-World
+To use the Teach-Repeat-Replan system in real world, you can check this branch **[experiment](https://github.com/HKUST-Aerial-Robotics/Teach-Repeat-Replan/tree/experiment)**. Compared to the master branch, **[experiment](https://github.com/HKUST-Aerial-Robotics/Teach-Repeat-Replan/tree/experiment)** has modified versions of [dense-surfel-mapping](https://github.com/HKUST-Aerial-Robotics/DenseSurfelMapping) and [stereo-VINS](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion). However, to test the proposed system in simulation, the master branch is enough.
+*we will release the source code for experiment soon.*
+
+## 1. Prerequisites
+**1.1**   **Ubuntu** and **ROS**
+
+Our software is developed in Ubuntu 16.04. ROS Kinetic. ROS can be installed here: [ROS Installation](http://wiki.ros.org/ROS/Installation)
+
+**1.2**   **convex solvers**
+
+We use **mosek** for conic programming. To use mosek, you should approve a free academic license [here](https://www.mosek.com/products/academic-licenses/). Then create a folder named 'mosek' in your home directory and put your license in it. All header and library files are already included in this repo, so you don't need to download mosek again. 
+
+We use **OOQP** for quadratic programming. 
+
+1. Get a copy of **MA27** from the [HSL Archive](http://www.hsl.rl.ac.uk/download/MA27/1.0.0/a/). Just select the **Personal Licence (allows use without redistribution)**, then fill the information table. You can download from an e-mail sent to you. Then, un-zip **MA27**, and follow the *README* in it, install it to your ubuntu.
+
+2. Manually un-zip packages *OOQP.zip* in the **installation** folder of this repo and install it follow the document *INSTALL* in **OOQP**, install it to your ubuntu.
+
+**1.3**   **some math tools**
+
+```
+  sudo apt-get install libarmadillo-dev 
+```
+
+```
+  sudo apt-get install glpk-utils libglpk-dev
+```
+
+```
+  sudo apt-get install libcdd-dev
+```
+
+**1.4**   **gcc 7**
+
+The simulator requires C++17, which needs **gcc 7** to compile, which can be installed by:
+```
+  sudo add-apt-repository ppa:ubuntu-toolchain-r/test
+  sudo apt-get update
+  sudo apt-get install gcc-7 g++-7
+```
+Then, 
+
+  for ubuntu 14.04:
+  ```
+    sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-4.8 60 --slave /usr/bin/g++ g++ /usr/bin/g++-4.8
+  ```
+
+  for ubuntu 16.04:
+  ```
+    sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-5 60 --slave /usr/bin/g++ g++ /usr/bin/g++-5
+  ```
+Then,
+
+```
+sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-7 70 --slave /usr/bin/g++ g++ /usr/bin/g++-7
+```
+
+Then, use the following command to change the default compiler (choose 0 as default)
+```
+  sudo update-alternatives --config gcc 
+```
+
+check your current gcc compiler
+```
+  gcc --version  
+```
+
+After the installation, you can switch your default compiler back to **gcc 5**. When you catkin_make, the simulator would automatically select gcc 7, so don't worry about your environment.
+
+ ## 2.GPU Dependency
+ Two packages in this repo, **local_sensing** and **polyhedron_generator** needs CUDA, GPU. 
+
+ **local_sensing** runs in simulation, to mimic the depth measured by onboard stereo cameras. It has a stereo camera model and renders a depth image (in GPU) by back-projecting obstcles surrounding the drone. And **polyhedron_generator** is used to find free convex polyhedrons which form the flight corridor.
+
+ They have been tested under CUDA 9.0, 10.0 on x86 and TX2.
+
+ If you don't have a GPU or don't want to try CUDA, don't worry. 
+ For both of these two packages, GPUs are only used for accelerations but are **not necessary**. Now the released code depends on CUDA but we will release a version which is CUDA free but, of course, run a little bit slower. 
+
+ ## 3.Build on ROS
+  I suggest to create an empty new workspace. Then clone the repository to your workspace and catkin_make. For example:
+```
+  cd ~/your_catkin_ws/src
+  git clone https://github.com/HKUST-Aerial-Robotics/Teach-Repeat-Replan.git
+  cd ../
+  catkin_make
+  source ~/your_catkin_ws/devel/setup.bash
+```
+  ## 4.How to use
+  1.1# Teaching (with joystick)
+
+  1.2# Repeating

files/bib.txt

@@ -0,0 +1,17 @@
+@article{gao2019trr,
+  title={Teach-Repeat-Replan: A Complete and Robust System for Aggressive Flight in Complex Environments},
+  author={Gao, Fei and Wang, Luqi and Zhou, Boyu and Han, Luxin and Pan, Jie and Shen, Shaojie},
+  journal={arXiv preprint arXiv:},
+  year={2019}
+}
+
+@article{gao2019optimal,
+  title={Optimal Trajectory Generation for Quadrotor Teach-and-Repeat},
+  author={Gao, Fei and Wang, Luqi and Wang, Kaixuan and Wu, William and Zhou, Boyu and Han, Luxin and Shen, Shaojie},
+  journal={IEEE Robotics and Automation Letters},
+  volume={4},
+  number={2},
+  pages={1493--1500},
+  year={2019},
+  publisher={IEEE}
+}

TRR_core/global_planner/src/teach_repeat_planner.cpp → global_planner/src/teach_repeat_planner.cpp

@@ -655,7 +655,8 @@ int main(int argc, char** argv)
     _pt_max_x = + _x_size / 2.0; _pt_min_x = - _x_size / 2.0;
     _pt_max_y = + _y_size / 2.0; _pt_min_y = - _y_size / 2.0; 
     _pt_max_z = + _z_size;       _pt_min_z = 0.0;
-
+
+    _resolution = max(0.1, _resolution); // In case a too fine resolution crashes the CUDA code.
     _inv_resolution = 1.0 / _resolution;
     _max_x_id = (int)(_x_size * _inv_resolution);
     _max_y_id = (int)(_y_size * _inv_resolution);

TRR_core/polyhedron_generator/CMakeLists.txt → polyhedron_generator/CMakeLists.txt

@@ -6,8 +6,6 @@ list(APPEND CUDA_NVCC_FLAGS -gencode arch=compute_61,code=sm_61)
 set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS};-O3 -use_fast_math )
 set(CUDA_PROPAGATE_HOST_FLAGS OFF)
 
-#[[set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "-std=c++11 -O3 -Wall -march=native")]]
 set(CMAKE_BUILD_TYPE "Release")
 set(CMAKE_CXX_FLAGS "-std=c++11")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -march=native")
@@ -30,7 +28,6 @@ find_package(OsqpEigen REQUIRED)
 find_package(osqp REQUIRED)
 
 set_directory_properties( PROPERTIES COMPILE_DEFINITIONS "" )
-#set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
 
 catkin_package(
     INCLUDE_DIRS include 

trr_simulation.sh

@@ -1,4 +1,6 @@
+rosrun joy joy_node & sleep 0.1;
 roslaunch cascade_controller simulator.launch drone_number:=$i yaw:=true & sleep 1;
+roslaunch trr_global_planner  map_generator.launch   & sleep 1;
 roslaunch trr_global_planner  global_planner.launch  & sleep 1;
 roslaunch trr_local_replanner local_replanner.launch & sleep 1;
 wait

utils/odom_visualization/CMakeLists.txt

@@ -19,44 +19,20 @@ catkin_package(
   DEPENDS system_lib
 )
 
-
-###########
-## Build ##
-###########
 find_package(Eigen3 REQUIRED)
 include_directories(${EIGEN3_INCLUDE_DIR})
 find_package(Armadillo REQUIRED)
 include_directories(${ARMADILLO_INCLUDE_DIRS})
 
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
 include_directories(include)
 include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
-## Declare a cpp library
-# add_library(irobot_msgs
-#   src/${PROJECT_NAME}/irobot_msgs.cpp
-# )
-
 ## Declare a cpp executable
 add_executable(odom_visualization src/odom_visualization.cpp)
-
-## Add cmake target dependencies of the executable/library
-## as an example, message headers may need to be generated before nodes
-# add_dependencies(multi_map_visualization multi_map_server_messages_cpp)
-
-## Specify libraries to link a library or executable target against
 add_dependencies(${PROJECT_NAME}
-  ${catkin_EXPORTED_TARGETS}
-)
-
+  ${catkin_EXPORTED_TARGETS})
 target_link_libraries(odom_visualization
    ${catkin_LIBRARIES}
-   ${ARMADILLO_LIBRARIES}
-   ##pose_utils
-)
-
-
+   ${ARMADILLO_LIBRARIES})