Fast, Attemptable Route Planner for Navigation in Known and Unknown Environments

Overview

FAR Planner uses a dynamically updated visibility graph for fast replanning. The planner models the environment with polygons and builds a global visibility graph along with the navigation. The planner is capable of handling both known and unknown environments. In a known environment, paths are planned based on a prior map. In an unknown environment, multiple paths are attempted to guide the vehicle to goal based on the environment observed during the navigation. When dynamic obstacles are present, FAR Planner disconnects visibility edges blocked by the dynamic obstacles and reconnects them after regaining visibility. The software implementation uses two CPU threads - one for dynamically updating the visibility graph using ~20% of the thread and the other for path search that can find a path within 3ms, as evaluated on an i7 computer.

FAR Planner was used by the CMU-OSU Team in attending DARPA Subterranean Challenge. In the final competition which took place in Louisville Mega Cavern, KY, the team's robots conducted the most complete traversing and mapping across the site (26 out of 28 sectors) among all teams, winning a "Most Sectors Explored Award".

A video showing functionalities of FAR Planner is available.

Method

Usage

The repository has been tested in Ubuntu 18.04 with ROS Melodic and Ubuntu 20.04 with ROS Noetic. Follow instructions in Autonomous Exploration Development Environment to setup the development environment. Make sure to checkout the branch that matches the computer setup, compile, and download the simulation environments.

To setup FAR Planner, clone the repository.

git clone https://github.com/MichaelFYang/far_planner

In a terminal, go to the folder and compile.

cd far_planner
catkin_make

To run the code, go to the development environment folder in a terminal, source the ROS workspace, and launch.

source devel/setup.sh
roslaunch vehicle_simulator system_indoor.launch

In another terminal, go to the FAR Planner folder, source the ROS workspace, and launch.

source devel/setup.sh
roslaunch far_planner far_planner.launch

Now, users can send a goal by clicking the 'Goalpoint' button in RVIZ and then clicking a point to set the goal. The vehicle will navigate to the goal and build a visibility graph (in cyan) along the way. Areas covered by the visibility graph become free space. When navigating in free space, the planner uses the built visibility graph, and when navigating in unknown space, the planner attempts to discover a way to the goal. By clicking the 'Reset Visibility Graph' button, the planner will reinitialize the visibility graph. By unchecking the 'Planning Attemptable' checkbox, the planner will first try to find a path through the free space. The path will show in green. If such a path does not exist, the planner will consider unknown space together. The path will show in blue. By unchecking the 'Update Visibility Graph' checkbox, the planner will stop updating the visibility graph.

Indoor

Anytime during the navigation, users can use the control panel to navigate the vehicle by clicking the in the black box. The system will switch to smart joystick mode - the vehicle tries to follow the virtual joystick command and avoid collisions at the same time. To resume FAR planner navigation, click the 'Resume Navigation to Goal' button or use the 'Goalpoint' button to set a new goal. Note that users can use a PS3/4 or Xbox controller instead of the virtual joystick. For more information, please refer to our development environment page.

ControlPanel     PS3 Controller

To launch with a different environment, use the command lines below and replace ' ' with one of the environment names in the development environment, i.e. 'campus', 'indoor', 'garage', 'tunnel', and 'forest'.

roslaunch vehicle_simulator system_
   
    .launch
roslaunch far_planner far_planner.launch

   

To run FAR Planner in a Matterport3D environment, follow instructions on the development environment page to setup the Matterport3D environment. Then, use the command lines below to launch the system and FAR Planner.

roslaunch vehicle_simulator system_matterport.launch
roslaunch far_planner far_planner.launch config:=matterport

Matterport

Configuration

FAR planner settings are kept in default.yaml in the 'src/far_planner/config' folder. For Matterport3D environments, the settings are in matterport.yaml in the same folder.

  • is_static_env (default: true) - set to false if the environment contains dynamic obstacles.

Todo

  • The current implementation does not support multi-floor environments. The environment can be 3D but needs to be single floored. An upgrade is planned for multi-floor environment support.
  • A feature will be added to save the visibility graph to file and load it to the planner as a prior.

Reference

  • F. Yang, C. Cao, H. Zhu, J. Oh, and J. Zhang. FAR Planner: Fast, Attemptable Route Planner using Dynamic Visibility Update. Submitted in 2021.

Author

Fan Yang ([email protected])

Credit

Eigen: a lightweight C++ template library for linear algebra.

You might also like...
Fast and robust certifiable relative pose estimation

Fast and Robust Relative Pose Estimation for Calibrated Cameras This repository contains the code for the relative pose estimation between two central

A light and fast internet speed plugin(DDE).
A light and fast internet speed plugin(DDE).

lfxNet English | 简体中文 | 繁體中文 lfxNet 是一款轻量、快速的实时显示系统资源信息的应用程序。 目录 背景 编译 下载 作者 鸣谢 协议 背景 喜爱 DDE ,为 Deepin 爱好者、也是开发者之一。因习惯其它系统上有一个任务栏网速插件,但在 Deepin/UOS上没有

A fast, distributed, high performance gradient boosting (GBT, GBDT, GBRT, GBM or MART) framework based on decision tree algorithms, used for ranking, classification and many other machine learning tasks.

Light Gradient Boosting Machine LightGBM is a gradient boosting framework that uses tree based learning algorithms. It is designed to be distributed a

ThunderGBM: Fast GBDTs and Random Forests on GPUs
ThunderGBM: Fast GBDTs and Random Forests on GPUs

Documentations | Installation | Parameters | Python (scikit-learn) interface What's new? ThunderGBM won 2019 Best Paper Award from IEEE Transactions o

ThunderSVM: A Fast SVM Library on GPUs and CPUs
ThunderSVM: A Fast SVM Library on GPUs and CPUs

What's new We have recently released ThunderGBM, a fast GBDT and Random Forest library on GPUs. add scikit-learn interface, see here Overview The miss

Fast, differentiable sorting and ranking in PyTorch
Fast, differentiable sorting and ranking in PyTorch

Torchsort Fast, differentiable sorting and ranking in PyTorch. Pure PyTorch implementation of Fast Differentiable Sorting and Ranking (Blondel et al.)

The official implementation of our CVPR 2021 paper - Hybrid Rotation Averaging: A Fast and Robust Rotation Averaging Approach

Graph Optimizer This repo contains the official implementation of our CVPR 2021 paper - Hybrid Rotation Averaging: A Fast and Robust Rotation Averagin

FG-Net: Fast Large-Scale LiDAR Point Clouds Understanding Network Leveraging Correlated Feature Mining and Geometric-Aware Modelling
FG-Net: Fast Large-Scale LiDAR Point Clouds Understanding Network Leveraging Correlated Feature Mining and Geometric-Aware Modelling

FG-Net: Fast Large-Scale LiDAR Point Clouds Understanding Network Leveraging Correlated Feature Mining and Geometric-Aware Modelling Comparisons of Running Time of Our Method with SOTA methods RandLA and KPConv:

A fast and modern voxel based raytracing engine

CubiCAD A fast and modern voxel based raytracing engine Currently in heavy development and unusable at its current state. This reposity will hold the

Comments
  • what  means IsAPillarPolygon in the contour_graph.cpp

    what means IsAPillarPolygon in the contour_graph.cpp

    hey my dear author , in the contour_graph.cpp i saw a bool func that IsAPillarPolygon , but i dont know what the func is done what means PillarPolygon literally?

    opened by Leeable 2
  • problem of saving Vsibility Graph

    problem of saving Vsibility Graph

    When I am using the function of reading or saving Visibility Graph There are only a part of Vsibility Graph can be saved. The main path of the vehicle can be saved but the other line is missing.

    The picture of real path which I want to save Screenshot from 2021-11-15 07-53-01

    The picture of path when I am reading Screenshot from 2021-11-15 07-54-40

    question 
    opened by overevil 2
  • How to use far_planner in a known map environment

    How to use far_planner in a known map environment

    Thank you very much for your contribution. I want to save the 3D map in PCD format after building the map. The next time I directly import the map, I can use far_planner for planning only by positioning. How should I achieve this? Is it necessary to use far_planner to run the map and save the visibility graph first?

    opened by W0WSS 0
Owner
Fan Yang
Fan Yang
Code and Data for our CVPR 2021 paper "Structured Scene Memory for Vision-Language Navigation"

SSM-VLN Code and Data for our CVPR 2021 paper "Structured Scene Memory for Vision-Language Navigation". Environment Installation Download Room-to-Room

hanqing 34 Aug 24, 2022
Episodic Transformer (E.T.) is a novel attention-based architecture for vision-and-language navigation.

Episodic Transformer (E.T.) is a novel attention-based architecture for vision-and-language navigation. E.T. is based on a multimodal transformer that encodes language inputs and the full episode history of visual observations and actions.

Alex Pashevich 52 Jul 13, 2022
Navigation-mesh Toolset for Games

Recast & Detour Recast Recast is state of the art navigation mesh construction toolset for games. It is automatic, which means that you can throw any

null 5k Sep 21, 2022
Code for "Photometric Visual-Inertial Navigation with Uncertainty-Aware Ensembles" in TRO 2022

Ensemble Visual-Inertial Odometry (EnVIO) Authors : Jae Hyung Jung, Yeongkwon Choe, and Chan Gook Park 1. Overview This is a ROS package of Ensemble V

Jae Hyung Jung 86 Sep 8, 2022
BLLIP reranking parser (also known as Charniak-Johnson parser, Charniak parser, Brown reranking parser)

BLLIP reranking parser (also known as Charniak-Johnson parser, Charniak parser, Brown reranking parser)

Brown Laboratory for Linguistic Information Processing 214 Aug 15, 2022
A fast, scalable, high performance Gradient Boosting on Decision Trees library, used for ranking, classification, regression and other machine learning tasks for Python, R, Java, C++. Supports computation on CPU and GPU.

Website | Documentation | Tutorials | Installation | Release Notes CatBoost is a machine learning method based on gradient boosting over decision tree

CatBoost 6.7k Sep 22, 2022
CTranslate2 is a fast inference engine for OpenNMT-py and OpenNMT-tf models supporting both CPU and GPU executio

CTranslate2 is a fast inference engine for OpenNMT-py and OpenNMT-tf models supporting both CPU and GPU execution. The goal is to provide comprehensive inference features and be the most efficient and cost-effective solution to deploy standard neural machine translation systems such as Transformer models.

OpenNMT 337 Sep 21, 2022
Fast and robust template matching with majority neighbour similarity and annulus projection transformation

A-MNS_TemplateMatching This is the official code for the PatternRecognition2020 paper: Fast and robust template matching with majority neighbour simil

Layjuns 17 Sep 16, 2022
a fast and user-friendly runtime for transformer inference (Bert, Albert, GPT2, Decoders, etc) on CPU and GPU.

a fast and user-friendly runtime for transformer inference (Bert, Albert, GPT2, Decoders, etc) on CPU and GPU.

Tencent 1.2k Sep 21, 2022
New ultra super robust and fast programming language, fully supportable by G++ and Clang

Cplusplusplus New ultra super robust and fast programming language, fully supportable by G++ and Clang How to use: Just write #include <C+++.h> in you

Vladimir Melnikov 1 Nov 29, 2021