Skip to content

Commit dc144ae

Browse files
ZbyLGscZbyLGsc
committed
Release FUEL
1 parent 1742de4 commit dc144ae

File tree

272 files changed

+65176
-2
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

272 files changed

+65176
-2
lines changed

‎.gitignore

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
fuel_planner/.vscode
2+
*.vscode/
3+
*.clang-format

‎LICENSE

+674
Large diffs are not rendered by default.

‎README.md

+48-2
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,53 @@ __Authors__: [Boyu Zhou](http://boyuzhou.net) and [Shaojie Shen](http://uav.ust.
1717

1818
Complete videos: [video1](https://www.youtube.com/watch?v=_dGgZUrWk-8).
1919

20-
The associated paper is submitted to RA-L with ICRA 2021 option. [Preprint](https://arxiv.org/abs/2010.11561) is available.
20+
Please cite our paper if you use this project in your research:
21+
- [__FUEL: Fast UAV Exploration using Incremental Frontier Structure and Hierarchical Planning__](https://arxiv.org/abs/2010.11561), Boyu Zhou, Yichen Zhang, Xinyi Chen, Shaojie Shen, IEEE Robotics and Automation Letters (**RA-L**) with ICRA 2021 option
2122

22-
The code will be made public after publication.
23+
```
24+
@article{zhou2021fuel,
25+
title={FUEL: Fast UAV Exploration Using Incremental Frontier Structure and Hierarchical Planning},
26+
author={Zhou, Boyu and Zhang, Yichen and Chen, Xinyi and Shen, Shaojie},
27+
journal={IEEE Robotics and Automation Letters},
28+
volume={6},
29+
number={2},
30+
pages={779--786},
31+
year={2021},
32+
publisher={IEEE}
33+
}
34+
```
2335

36+
Please kindly star :star: this project if it helps you. We take great efforts to develope and maintain it :grin::grin:.
37+
38+
39+
## Quick Start
40+
41+
This project is mostly based on [Fast-Planner](https://github.com/HKUST-Aerial-Robotics/Fast-Planner).
42+
It has been tested on Ubuntu 16.04(ROS Kinetic) and 18.04(ROS Melodic). Take Ubuntu 18.04 as an example, run the following commands to setup:
43+
44+
```
45+
sudo apt-get install libarmadillo-dev ros-melodic-nlopt
46+
cd ${YOUR_WORKSPACE_PATH}/src
47+
git clone https://github.com/HKUST-Aerial-Robotics/FUEL.git
48+
cd ../
49+
catkin_make
50+
```
51+
52+
After compilation you can start the visualization by:
53+
54+
```
55+
source devel/setup.bash && roslaunch exploration_manager rviz.launch
56+
```
57+
and start a simulation (run in a new terminals):
58+
```
59+
source devel/setup.bash && roslaunch exploration_manager exploration.launch
60+
```
61+
You will find a cluttered scene to be explored and the drone in ```Rviz```. You can trigger the exploration to start by the ```2D Nav Goal``` tool. A sample simulation is showed in the figure. The unknown obstacles are shown in grey, while the frontiers are shown as colorful voxels. The planned and executed trajectories are also displayed.
62+
63+
<p id="demo1" align="center">
64+
<img src="files/5.gif" width = "500" height = "418"/>
65+
</p>
66+
67+
68+
## Acknowledgements
69+
We use **NLopt** for non-linear optimization and use **LKH** for travelling salesman problem.

‎files/5.gif

4.26 MB
Loading

‎fuel_planner/active_perception/CMakeLists.txt

+47
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
cmake_minimum_required(VERSION 2.8.3)
2+
project(active_perception)
3+
4+
set(CMAKE_BUILD_TYPE "Release")
5+
set(CMAKE_CXX_FLAGS "-std=c++11")
6+
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall")
7+
8+
find_package(PCL 1.7 REQUIRED)
9+
find_package(Eigen3 REQUIRED)
10+
find_package(catkin REQUIRED COMPONENTS
11+
roscpp
12+
rospy
13+
std_msgs
14+
visualization_msgs
15+
plan_env
16+
bspline
17+
path_searching
18+
cv_bridge
19+
)
20+
21+
22+
catkin_package(
23+
INCLUDE_DIRS include
24+
LIBRARIES active_perception
25+
CATKIN_DEPENDS plan_env bspline path_searching
26+
DEPENDS system_lib
27+
)
28+
29+
include_directories(
30+
SYSTEM
31+
include
32+
${catkin_INCLUDE_DIRS}
33+
${Eigen3_INCLUDE_DIRS}
34+
${PCL_INCLUDE_DIRS}
35+
)
36+
37+
add_library( active_perception
38+
src/traj_visibility.cpp
39+
src/heading_planner.cpp
40+
src/frontier_finder.cpp
41+
src/graph_node.cpp
42+
src/perception_utils.cpp
43+
)
44+
target_link_libraries( active_perception
45+
${catkin_LIBRARIES}
46+
${PCL_LIBRARIES}
47+
)

‎fuel_planner/active_perception/include/active_perception/frontier_finder.h

+134
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,134 @@
1+
#ifndef _FRONTIER_FINDER_H_
2+
#define _FRONTIER_FINDER_H_
3+
4+
#include <ros/ros.h>
5+
#include <Eigen/Eigen>
6+
#include <memory>
7+
#include <vector>
8+
#include <list>
9+
#include <utility>
10+
11+
using Eigen::Vector3d;
12+
using std::shared_ptr;
13+
using std::unique_ptr;
14+
using std::vector;
15+
using std::list;
16+
using std::pair;
17+
18+
class RayCaster;
19+
20+
namespace fast_planner {
21+
class EDTEnvironment;
22+
class PerceptionUtils;
23+
24+
// Viewpoint to cover a frontier cluster
25+
struct Viewpoint {
26+
// Position and heading
27+
Vector3d pos_;
28+
double yaw_;
29+
// Fraction of the cluster that can be covered
30+
// double fraction_;
31+
int visib_num_;
32+
};
33+
34+
// A frontier cluster, the viewpoints to cover it
35+
struct Frontier {
36+
// Complete voxels belonging to the cluster
37+
vector<Vector3d> cells_;
38+
// down-sampled voxels filtered by voxel grid filter
39+
vector<Vector3d> filtered_cells_;
40+
// Average position of all voxels
41+
Vector3d average_;
42+
// Idx of cluster
43+
int id_;
44+
// Viewpoints that can cover the cluster
45+
vector<Viewpoint> viewpoints_;
46+
// Bounding box of cluster, center & 1/2 side length
47+
Vector3d box_min_, box_max_;
48+
// Path and cost from this cluster to other clusters
49+
list<vector<Vector3d>> paths_;
50+
list<double> costs_;
51+
};
52+
53+
class FrontierFinder {
54+
public:
55+
FrontierFinder(const shared_ptr<EDTEnvironment>& edt, ros::NodeHandle& nh);
56+
~FrontierFinder();
57+
58+
void searchFrontiers();
59+
void computeFrontiersToVisit();
60+
61+
void getFrontiers(vector<vector<Vector3d>>& clusters);
62+
void getDormantFrontiers(vector<vector<Vector3d>>& clusters);
63+
void getFrontierBoxes(vector<pair<Vector3d, Vector3d>>& boxes);
64+
// Get viewpoint with highest coverage for each frontier
65+
void getTopViewpointsInfo(const Vector3d& cur_pos, vector<Vector3d>& points, vector<double>& yaws,
66+
vector<Vector3d>& averages);
67+
// Get several viewpoints for a subset of frontiers
68+
void getViewpointsInfo(const Vector3d& cur_pos, const vector<int>& ids, const int& view_num,
69+
const double& max_decay, vector<vector<Vector3d>>& points,
70+
vector<vector<double>>& yaws);
71+
void updateFrontierCostMatrix();
72+
void getFullCostMatrix(const Vector3d& cur_pos, const Vector3d& cur_vel, const Vector3d cur_yaw,
73+
Eigen::MatrixXd& mat);
74+
void getPathForTour(const Vector3d& pos, const vector<int>& frontier_ids, vector<Vector3d>& path);
75+
76+
void setNextFrontier(const int& id);
77+
bool isFrontierCovered();
78+
void wrapYaw(double& yaw);
79+
80+
shared_ptr<PerceptionUtils> percep_utils_;
81+
82+
private:
83+
void splitLargeFrontiers(list<Frontier>& frontiers);
84+
bool splitHorizontally(const Frontier& frontier, list<Frontier>& splits);
85+
void mergeFrontiers(Frontier& ftr1, const Frontier& ftr2);
86+
bool isFrontierChanged(const Frontier& ft);
87+
bool haveOverlap(const Vector3d& min1, const Vector3d& max1, const Vector3d& min2,
88+
const Vector3d& max2);
89+
void computeFrontierInfo(Frontier& frontier);
90+
void downsample(const vector<Vector3d>& cluster_in, vector<Vector3d>& cluster_out);
91+
void sampleViewpoints(Frontier& frontier);
92+
93+
int countVisibleCells(const Vector3d& pos, const double& yaw, const vector<Vector3d>& cluster);
94+
bool isNearUnknown(const Vector3d& pos);
95+
vector<Eigen::Vector3i> sixNeighbors(const Eigen::Vector3i& voxel);
96+
vector<Eigen::Vector3i> tenNeighbors(const Eigen::Vector3i& voxel);
97+
vector<Eigen::Vector3i> allNeighbors(const Eigen::Vector3i& voxel);
98+
bool isNeighborUnknown(const Eigen::Vector3i& voxel);
99+
void expandFrontier(const Eigen::Vector3i& first /* , const int& depth, const int& parent_id */);
100+
101+
// Wrapper of sdf map
102+
int toadr(const Eigen::Vector3i& idx);
103+
bool knownfree(const Eigen::Vector3i& idx);
104+
bool inmap(const Eigen::Vector3i& idx);
105+
106+
// Deprecated
107+
Eigen::Vector3i searchClearVoxel(const Eigen::Vector3i& pt);
108+
bool isInBoxes(const vector<pair<Vector3d, Vector3d>>& boxes, const Eigen::Vector3i& idx);
109+
bool canBeMerged(const Frontier& ftr1, const Frontier& ftr2);
110+
void findViewpoints(const Vector3d& sample, const Vector3d& ftr_avg, vector<Viewpoint>& vps);
111+
112+
// Data
113+
vector<char> frontier_flag_;
114+
list<Frontier> frontiers_, dormant_frontiers_, tmp_frontiers_;
115+
vector<int> removed_ids_;
116+
list<Frontier>::iterator first_new_ftr_;
117+
Frontier next_frontier_;
118+
119+
// Params
120+
int cluster_min_;
121+
double cluster_size_xy_, cluster_size_z_;
122+
double candidate_rmax_, candidate_rmin_, candidate_dphi_, min_candidate_dist_,
123+
min_candidate_clearance_;
124+
int down_sample_;
125+
double min_view_finish_fraction_, resolution_;
126+
int min_visib_num_, candidate_rnum_;
127+
128+
// Utils
129+
shared_ptr<EDTEnvironment> edt_env_;
130+
unique_ptr<RayCaster> raycaster_;
131+
};
132+
133+
} // namespace fast_planner
134+
#endif

‎fuel_planner/active_perception/include/active_perception/graph_node.h

+86
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,86 @@
1+
#ifndef _GRAPH_NODE_H_
2+
#define _GRAPH_NODE_H_
3+
#include <vector>
4+
#include <unordered_map>
5+
#include <queue>
6+
#include <list>
7+
#include <memory>
8+
#include <iostream>
9+
#include <math.h>
10+
#include <algorithm>
11+
#include <Eigen/Eigen>
12+
13+
using std::list;
14+
using std::queue;
15+
using std::shared_ptr;
16+
using std::unique_ptr;
17+
using std::unordered_map;
18+
using std::vector;
19+
using std::cout;
20+
using Eigen::Vector3d;
21+
using Eigen::Vector3i;
22+
23+
class RayCaster;
24+
25+
namespace fast_planner {
26+
// Basic noded type containing only general artributes required by graph search
27+
class BaseNode {
28+
public:
29+
typedef shared_ptr<BaseNode> Ptr;
30+
BaseNode() {
31+
g_value_ = 1000000;
32+
closed_ = false;
33+
}
34+
~BaseNode() {
35+
}
36+
37+
virtual void print() {
38+
std::cout << "Base node" << std::endl;
39+
}
40+
41+
int id_;
42+
bool closed_;
43+
double g_value_;
44+
};
45+
46+
// Node type for viewpoint refinement
47+
class Astar;
48+
class SDFMap;
49+
class ViewNode : public BaseNode {
50+
public:
51+
typedef shared_ptr<ViewNode> Ptr;
52+
ViewNode(const Vector3d& p, const double& y);
53+
ViewNode() {
54+
}
55+
~ViewNode() {
56+
}
57+
58+
virtual void print() {
59+
std::cout << "View node" << yaw_ << std::endl;
60+
}
61+
62+
void printNeighbors() {
63+
for (auto v : neighbors_)
64+
v->print();
65+
}
66+
67+
double costTo(const ViewNode::Ptr& node);
68+
static double computeCost(const Vector3d& p1, const Vector3d& p2, const double& y1, const double& y2,
69+
const Vector3d& v1, const double& yd1, vector<Vector3d>& path);
70+
// Coarse to fine path searching
71+
static double searchPath(const Vector3d& p1, const Vector3d& p2, vector<Vector3d>& path);
72+
73+
// Data
74+
vector<ViewNode::Ptr> neighbors_;
75+
ViewNode::Ptr parent_;
76+
Vector3d pos_, vel_;
77+
double yaw_, yaw_dot_;
78+
79+
// Parameters shared among nodes
80+
static double vm_, am_, yd_, ydd_, w_dir_;
81+
static shared_ptr<Astar> astar_;
82+
static shared_ptr<RayCaster> caster_;
83+
static shared_ptr<SDFMap> map_;
84+
};
85+
}
86+
#endif

0 commit comments

Comments
 (0)