├── doc └── HAP_paper.pdf ├── planner ├── srv │ ├── get_path.srv │ ├── get_nearest.srv │ └── get_state.srv ├── msg │ ├── Graph.msg │ ├── Edge.msg │ └── Vertex.msg ├── src │ ├── graph_planner │ │ ├── graph_planner_node.cpp │ │ ├── graph_visualization_node.cpp │ │ └── graph_planner.cpp │ ├── sea_planner_node.cpp │ ├── kd_search │ │ └── kd_search.cpp │ ├── utility.cpp │ ├── rtrrtp.cpp │ └── rtrrt.cpp ├── launch │ └── sea_planner.launch ├── install │ └── hap.rosinstall ├── include │ ├── kd_search │ │ ├── kd_search.h │ │ └── util.h │ ├── graph_planner │ │ ├── graph_base.h │ │ └── graph_planner.h │ ├── sea_planner │ │ ├── rtrrt_base.h │ │ ├── rtrrtp.h │ │ ├── rtrrt.h │ │ └── utility.h │ └── termcolor.hpp ├── package.xml ├── config │ ├── params.yaml │ ├── planning copy.yaml │ ├── planning_default.yaml │ └── planning_under_full_grid.yaml └── CMakeLists.txt ├── README.md ├── .gitignore └── LICENSE /doc/HAP_paper.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdwyc/history-aware-planner/HEAD/doc/HAP_paper.pdf -------------------------------------------------------------------------------- /planner/srv/get_path.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point target_point 2 | --- 3 | nav_msgs/Path global_path -------------------------------------------------------------------------------- /planner/msg/Graph.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # Construct graph 4 | Vertex[] vertices 5 | 6 | # Vsulization 7 | Edge[] edges -------------------------------------------------------------------------------- /planner/srv/get_nearest.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point query_point 2 | --- 3 | geometry_msgs/Point nearest_vertex 4 | float64 distance 5 | -------------------------------------------------------------------------------- /planner/msg/Edge.msg: -------------------------------------------------------------------------------- 1 | # Relative two vertices 2 | Vertex vertex_1 # id correapond to the sequence of Vertex msg 3 | Vertex vertex_2 4 | 5 | -------------------------------------------------------------------------------- /planner/msg/Vertex.msg: -------------------------------------------------------------------------------- 1 | # Unique identifier 2 | int32 vertex_id 3 | 4 | # Physical location 5 | geometry_msgs/Point location 6 | 7 | # Information gain for goal reselection 8 | float32 information_gain 9 | -------------------------------------------------------------------------------- /planner/srv/get_state.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | time stamp 4 | string state 5 | # State Lists: ACTIVE -> Planning 6 | # WAITING -> Waiting for goal 7 | float32 estimated_distance # The length of reference line -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # History-Aware Planning for Risk-free Autonomous Navigation on Unknown Uneven Terrain [ICRA2024] 2 | ## ⭐Note 3 | The code is released, the running guidance is coming soon. You can find the paper preprint version in [here](./doc/HAP_paper.pdf)! 4 | 5 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /planner/src/graph_planner/graph_planner_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "sea_planner/utility.h" 4 | #include "graph_planner/graph_planner.h" 5 | 6 | 7 | int main(int argc, char **argv) 8 | { 9 | ros::init(argc, argv, "graph_planner_node"); 10 | 11 | ros::NodeHandle nh; 12 | // Create planner Object 13 | ParamLoader param_loader; 14 | graph_planner planner; 15 | std::cout << termcolor::bold << termcolor::green 16 | << "Graph Planner Node Launched!" 17 | << termcolor::reset << std::endl; 18 | ros::MultiThreadedSpinner threadNum(2); 19 | ros::spin(threadNum); 20 | return 0; 21 | } 22 | -------------------------------------------------------------------------------- /planner/src/sea_planner_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | // #include "sea_planner/rtrrt.h" 4 | // #include "sea_planner/rtrrt_base.h" 5 | #include "sea_planner/rtrrtp.h" 6 | 7 | 8 | int main(int argc, char **argv) 9 | { 10 | ros::init(argc, argv, "sea_planner_node"); 11 | 12 | ros::NodeHandle nh; 13 | // Create planner Object 14 | ParamLoader param_loader; 15 | rtrrtp planner; 16 | std::cout << termcolor::bold << termcolor::green 17 | << "Planner Node Launched!" 18 | << termcolor::reset << std::endl; 19 | // ros::MultiThreadedSpinner threadNum(2); 20 | // ros::spin(threadNum); 21 | ros::spin(); 22 | return 0; 23 | } 24 | -------------------------------------------------------------------------------- /planner/launch/sea_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /planner/install/hap.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: trajectory_tool 3 | uri: https://github.com/sdwyc/trajectory_tool.git 4 | version: main 5 | 6 | - git: 7 | local-name: scout_simulator 8 | uri: https://github.com/sdwyc/Scout_simulation_env.git 9 | version: main 10 | 11 | - git: 12 | local-name: scout_simulator 13 | uri: https://github.com/sdwyc/Scout_simulation_env.git 14 | version: main 15 | 16 | - git: 17 | local-name: sim_environment_arranger 18 | uri: https://github.com/sdwyc/sim_environment_arranger.git 19 | version: main 20 | 21 | - git: 22 | local-name: grid_map 23 | uri: https://github.com/ANYbotics/grid_map.git 24 | version: 2.1.0 25 | 26 | - git: 27 | local-name: kdtree 28 | uri: https://github.com/sdwyc/kdtree.git 29 | version: main 30 | 31 | - git: 32 | local-name: elevation_mapping 33 | uri: https://github.com/ANYbotics/elevation_mapping.git 34 | version: 0.6.0 35 | 36 | 37 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Neo 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /planner/include/kd_search/kd_search.h: -------------------------------------------------------------------------------- 1 | #ifndef KD_SEARCH_H 2 | #define KD_SEARCH_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "util.h" 14 | #include "nanoflann.hpp" 15 | 16 | using namespace nanoflann; 17 | typedef KDTreeSingleIndexDynamicAdaptor>,KDTreePt::PointCloud,3> DynamicTree; 18 | 19 | class nanoKDTree{ 20 | private: 21 | DynamicTree* kdtree_; 22 | int dim_, max_leaf_; 23 | std::shared_ptr> cloud_; 24 | public: 25 | nanoKDTree(int dim, int max_leaf, std::shared_ptr> cloud); 26 | ~nanoKDTree(){}; 27 | void AddPoint(size_t start_ind, size_t end_ind); 28 | void RemovePoint(size_t removeInd); 29 | void RadiusSearch(float query_pt[3], float radius, vector& indices, vector& dists); 30 | void NearestSearch(float query_pt[3], size_t num_results, vector& indices, vector& dists); 31 | }; 32 | 33 | #endif // KD_SEARCH_H -------------------------------------------------------------------------------- /planner/include/kd_search/util.h: -------------------------------------------------------------------------------- 1 | #ifndef UTIL_H 2 | #define UTIL_H 3 | 4 | #include 5 | using namespace std; 6 | 7 | namespace KDTreePt{ 8 | 9 | template 10 | struct Point{ 11 | num_t x, y, z; 12 | 13 | Point(num_t x, num_t y, num_t z) : x(x), y(y), z(z) {} 14 | bool operator == (const Point & pt){ 15 | float dis_sq = (this->x-pt.x)*(this->x-pt.x)+(this->y-pt.y)*(this->y-pt.y)+(this->z-pt.z)*(this->z-pt.z); 16 | if (dis_sq < 1.0e-4) 17 | { 18 | return true; 19 | } 20 | return false; 21 | } 22 | }; 23 | 24 | // Define basic tree data type 25 | template 26 | class PointCloud{ 27 | public: 28 | std::vector> points; 29 | 30 | size_t kdtree_get_point_index(const Point& pt) 31 | { 32 | return std::find(points.begin(), points.end(), pt)-points.begin(); 33 | } 34 | 35 | inline size_t kdtree_get_point_count() const 36 | { 37 | return points.size(); 38 | } 39 | 40 | inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const 41 | { 42 | if (dim == 0) 43 | return points[idx].x; 44 | else if (dim == 1) 45 | return points[idx].y; 46 | else if (dim == 2) 47 | return points[idx].z; 48 | return 0.0; 49 | } 50 | 51 | template 52 | bool kdtree_get_bbox(BBOX &) const 53 | { 54 | return false; 55 | } 56 | 57 | }; 58 | 59 | } 60 | 61 | 62 | #endif // UTIL_H -------------------------------------------------------------------------------- /planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sea_planner 4 | 1.0.0 5 | The sea_planner package 6 | 7 | wyc 8 | 9 | 10 | TODO 11 | catkin 12 | std_msgs 13 | geometry_msgs 14 | nav_msgs 15 | grid_map_core 16 | grid_map_msgs 17 | grid_map_pcl 18 | grid_map_ros 19 | roscpp 20 | rospy 21 | messgage_generation 22 | geometry_msgs 23 | nav_msgs 24 | grid_map_core 25 | grid_map_msgs 26 | grid_map_pcl 27 | grid_map_ros 28 | roscpp 29 | rospy 30 | std_msgs 31 | geometry_msgs 32 | nav_msgs 33 | grid_map_core 34 | grid_map_msgs 35 | grid_map_pcl 36 | grid_map_ros 37 | roscpp 38 | rospy 39 | message_runtime 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /planner/src/kd_search/kd_search.cpp: -------------------------------------------------------------------------------- 1 | #include "kd_search/kd_search.h" 2 | #include "kd_search/util.h" 3 | #include "kd_search/nanoflann.hpp" 4 | 5 | using namespace std; 6 | 7 | nanoKDTree::nanoKDTree(int dim, int max_leaf, std::shared_ptr> cloud): 8 | cloud_(cloud), dim_(dim), max_leaf_(max_leaf) 9 | { 10 | // Intial KDTree 11 | DynamicTree* Dykdtree_ = new DynamicTree(dim_, *cloud_, {10}); 12 | kdtree_ = Dykdtree_; 13 | } 14 | 15 | void nanoKDTree::AddPoint(size_t start_ind, size_t end_ind){ 16 | kdtree_->addPoints(start_ind, end_ind); 17 | } 18 | 19 | void nanoKDTree::RemovePoint(size_t removeInd){ 20 | kdtree_->removePoint(removeInd); 21 | } 22 | 23 | void nanoKDTree::RadiusSearch(float query_pt[3], float radius, vector& indices, vector& dists){ 24 | radius *= radius; // Search based on distance_square 25 | vector> indices_dists; 26 | nanoflann::RadiusResultSet resultSet(radius, indices_dists); 27 | kdtree_->findNeighbors(resultSet, query_pt); 28 | // // Sort by dists(small -> large) 29 | for(size_t i=0; i& indices, vector& dists){ 36 | // knn search 37 | size_t ret_index[num_results]; 38 | float out_dist_sqr[num_results]; 39 | nanoflann::KNNResultSet resultSet(num_results); 40 | resultSet.init(ret_index, out_dist_sqr); 41 | kdtree_->findNeighbors(resultSet, query_pt); 42 | // output 43 | for (size_t i = 0; i < resultSet.size(); ++i) 44 | { 45 | indices.push_back(ret_index[i]); 46 | dists.push_back(out_dist_sqr[i]); 47 | } 48 | } -------------------------------------------------------------------------------- /planner/config/params.yaml: -------------------------------------------------------------------------------- 1 | seap: 2 | # Frames 3 | robotFrame: "base_link" # Robot chassis frame_id 4 | globalFrame: "map" # Global frame_id 5 | 6 | RobotHeight: 0.11 # Robot Height(meters) 7 | 8 | epsilon: 0.5 # rtrrt grow rate 9 | minNodeDistance: 0.3 # The minimum distance between nodes 10 | neiborRangeThre: 1.2 # nodes less than this value are considered neibor nodes 11 | minTreeNodeNum: 100 # The minimum number of tree nodes need by navigation 12 | minTreeEdgeNum: 200 # The minimum number of tree edges need by navigation 13 | maxNodesNum: 100000 # The maximum node number of whole tree 14 | alpha: 0.1 # rtrrt sample rate threshold for robot->goal 15 | beta: 1.4 # rtrrt sample rate threshold for random sample 16 | Gamma: 1.0 # rtrrt sample rate threshold for sensor range sample 17 | allowRewiringFromRand: 2.0 # Single rewiring-from-rand step time constrain 18 | allowRewiringFromRoot: 4.0 # Single rewiring-from-root step time constrain 19 | gradientDiffThre: 5.0 # Maximum gradient threshold for edge, degree form 20 | targetTolerance: 0.4 # The tolerance of consider node as target node 21 | goalTolerance: 1.0 # The tolerance of arriving goal 22 | waypointTolerance: 1.0 # The tolerance of arriving waypoint 23 | changRootTolerance: 1.0 # The tolerance of arriving waypoint 24 | informationRadius: 1.0 # Check node information gain 25 | informationThre: 0.3 # Check node information gain 26 | frontierRadius: 1.0 # Check node's surrounding whether exist NaN grid 27 | frontierDisThre: 3.0 # The leaf nodes less than this value aren't considered as frontier 28 | exploredAreaRadius: 0.8 # The leaf nodes' circle check radius for avoiding explored area 29 | exploredAreaThre: 8 # The leaf nodes' circle number threshold of existed nodes 30 | findNearNodeThre: 2.0 # The distance threshold to final goal in order to select navigation type 31 | obstacleSectorNumThre: 2 # If one node orientation_array element:1) greater than this value, considered obstacle node 32 | inflationRadius: 1.2 # The inflation radius for obstacle node 33 | maxPenalty: 10.0 # The maximum of Penalty factor 34 | regressionFactor: 5 # Control the Penalty's slope -------------------------------------------------------------------------------- /planner/config/planning copy.yaml: -------------------------------------------------------------------------------- 1 | seap: 2 | # Frames 3 | robotFrame: "base_link" # Robot chassis frame_id 4 | globalFrame: "map" # Global frame_id 5 | 6 | RobotHeight: 0.11 # Robot Height(meters) 7 | 8 | epsilon: 0.5 # rtrrt grow rate 9 | minNodeDistance: 0.3 # The minimum distance between nodes 10 | neiborRangeThre: 0.6 # nodes less than this value are considered neibor nodes 11 | minTreeNodeNum: 100 # The minimum number of tree nodes need by navigation 12 | minTreeEdgeNum: 300 # The minimum number of tree edges need by navigation 13 | maxNodesNum: 100000 # The maximum node number of whole tree 14 | alpha: 0.1 # rtrrt sample rate threshold for robot->goal 15 | beta: 1.4 # rtrrt sample rate threshold for random sample 16 | Gamma: 0.5 # rtrrt sample rate threshold for sensor range sample 17 | allowRewiringFromRand: 2.0 # Single rewiring-from-rand step time constrain 18 | allowRewiringFromRoot: 4.0 # Single rewiring-from-root step time constrain 19 | gradientDiffThre: 15.0 # Maximum gradient threshold for edge, degree form 20 | targetTolerance: 0.4 # The tolerance of consider node as target node 21 | goalTolerance: 0.5 # The tolerance of arriving goal 22 | waypointTolerance: 0.5 # The tolerance of arriving waypoint 23 | changRootTolerance: 0.8 # The tolerance of arriving waypoint 24 | informationRadius: 1.0 # Check node information gain 25 | informationThre: 0.3 # Check node information gain 26 | frontierRadius: 1.0 # Check node's surrounding whether exist NaN grid 27 | frontierDisThre: 3.0 # The leaf nodes less than this value aren't considered as frontier 28 | exploredAreaRadius: 0.8 # The leaf nodes' circle check radius for avoiding explored area 29 | exploredAreaThre: 15 # The leaf nodes' circle number threshold of existed nodes 30 | findNearNodeThre: 2.0 # The distance threshold to final goal in order to select navigation type 31 | obstacleSectorNumThre: 2 # If one node orientation_array element:1) greater than this value, considered obstacle node 32 | inflationRadius: 0.8 # The inflation radius for obstacle node 33 | maxPenalty: 10.0 # The maximum of Penalty factor 34 | regressionFactor: 5 # Control the Penalty's slope -------------------------------------------------------------------------------- /planner/config/planning_default.yaml: -------------------------------------------------------------------------------- 1 | seap: 2 | # Frames 3 | robotFrame: "base_link" # Robot chassis frame_id 4 | globalFrame: "map" # Global frame_id 5 | 6 | RobotHeight: 0.11 # Robot Height(meters) 7 | 8 | epsilon: 0.5 # rtrr grow rate 9 | minNodeDistance: 0.3 # The minimum distance between nodes 10 | neiborRangeThre: 0.9 # nodes less than this value are considered neibor nodes 11 | minTreeNodeNum: 400 # The minimum number of tree nodes need by navigation 12 | minTreeEdgeNum: 400 # The minimum number of tree edges need by navigation 13 | maxNodesNum: 100000 # The maximum node number of whole tree 14 | alpha: 0.1 # rtrrt sample rate threshold for robot->goal 15 | beta: 1.4 # rtrrt sample rate threshold for random sample 16 | Gamma: 0.5 # rtrrt sample rate threshold for sensor range sample 17 | allowRewiringFromRand: 2.0 # Single rewiring-from-rand step time constrain 18 | allowRewiringFromRoot: 4.0 # Single rewiring-from-root step time constrain 19 | gradientDiffThre: 13.0 # Maximum gradient threshold for edge, degree form 20 | targetTolerance: 0.4 # The tolerance of consider node as target node 21 | goalTolerance: 0.5 # The tolerance of arriving goal 22 | waypointTolerance: 0.5 # The tolerance of arriving waypoint 23 | changRootTolerance: 0.8 # The tolerance of arriving waypoint 24 | informationRadius: 1.0 # Check node information gain 25 | informationThre: 0.3 # Check node information gain 26 | frontierRadius: 1.0 # Check node's surrounding whether exist NaN grid 27 | frontierDisThre: 3.0 # The leaf nodes less than this value aren't considered as frontier 28 | exploredAreaRadius: 0.8 # The leaf nodes' circle check radius for avoiding explored area 29 | exploredAreaThre: 15 # The leaf nodes' circle number threshold of existed nodes 30 | findNearNodeThre: 2.0 # The distance threshold to final goal in order to select navigation type 31 | obstacleSectorNumThre: 2 # If one node orientation_array element:1) greater than this value, considered obstacle node 32 | inflationRadius: 0.8 # The inflation radius for obstacle node 33 | maxPenalty: 10.0 # The maximum of Penalty factor 34 | regressionFactor: 5 # Control the Penalty's slope -------------------------------------------------------------------------------- /planner/config/planning_under_full_grid.yaml: -------------------------------------------------------------------------------- 1 | seap: 2 | # Frames 3 | robotFrame: "base_link" # Robot chassis frame_id 4 | globalFrame: "map" # Global frame_id 5 | 6 | RobotHeight: 0.11 # Robot Height(meters) 7 | 8 | epsilon: 0.5 # rtrr grow rate 9 | minNodeDistance: 0.2 # The minimum distance between nodes 10 | neiborRangeThre: 0.9 # nodes less than this value are considered neibor nodes 11 | minTreeNodeNum: 400 # The minimum number of tree nodes need by navigation 12 | minTreeEdgeNum: 400 # The minimum number of tree edges need by navigation 13 | maxNodesNum: 100000 # The maximum node number of whole tree 14 | alpha: 0.1 # rtrrt sample rate threshold for robot->goal 15 | beta: 1.4 # rtrrt sample rate threshold for random sample 16 | Gamma: 0.5 # rtrrt sample rate threshold for sensor range sample 17 | allowRewiringFromRand: 2.0 # Single rewiring-from-rand step time constrain 18 | allowRewiringFromRoot: 4.0 # Single rewiring-from-root step time constrain 19 | gradientDiffThre: 20.0 # Maximum gradient threshold for edge, degree form 20 | targetTolerance: 0.4 # The tolerance of consider node as target node 21 | goalTolerance: 0.5 # The tolerance of arriving goal 22 | waypointTolerance: 0.5 # The tolerance of arriving waypoint 23 | changRootTolerance: 0.8 # The tolerance of arriving waypoint 24 | informationRadius: 1.0 # Check node information gain 25 | informationThre: 0.3 # Check node information gain 26 | frontierRadius: 1.0 # Check node's surrounding whether exist NaN grid 27 | frontierDisThre: 3.0 # The leaf nodes less than this value aren't considered as frontier 28 | exploredAreaRadius: 0.8 # The leaf nodes' circle check radius for avoiding explored area 29 | exploredAreaThre: 15 # The leaf nodes' circle number threshold of existed nodes 30 | findNearNodeThre: 2.0 # The distance threshold to final goal in order to select navigation type 31 | obstacleSectorNumThre: 4 # If one node orientation_array element:1) greater than this value, considered obstacle node 32 | inflationRadius: 0.8 # The inflation radius for obstacle node 33 | maxPenalty: 10.0 # The maximum of Penalty factor 34 | regressionFactor: 5 # Control the Penalty's slope -------------------------------------------------------------------------------- /planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sea_planner) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++14") 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | std_msgs 9 | geometry_msgs 10 | nav_msgs 11 | grid_map_core 12 | grid_map_msgs 13 | grid_map_pcl 14 | grid_map_ros 15 | pcl_ros 16 | roscpp 17 | rospy 18 | message_generation 19 | ) 20 | 21 | find_package(PCL REQUIRED QUIET) 22 | 23 | add_message_files( 24 | FILES 25 | Edge.msg 26 | Graph.msg 27 | Vertex.msg 28 | ) 29 | 30 | ## Generate services in the 'srv' folder 31 | add_service_files( 32 | FILES 33 | get_nearest.srv 34 | get_state.srv 35 | get_path.srv 36 | ) 37 | 38 | ## Generate actions in the 'action' folder 39 | # add_action_files( 40 | # FILES 41 | # Action1.action 42 | # Action2.action 43 | # ) 44 | 45 | ## Generate added messages and services with any dependencies listed here 46 | generate_messages( 47 | DEPENDENCIES 48 | std_msgs geometry_msgs nav_msgs 49 | ) 50 | 51 | catkin_package( 52 | INCLUDE_DIRS include 53 | DEPENDS PCL 54 | LIBRARIES sea_planner 55 | CATKIN_DEPENDS geometry_msgs nav_msgs grid_map_core grid_map_msgs grid_map_pcl grid_map_ros roscpp rospy message_runtime 56 | ) 57 | 58 | include_directories( 59 | include 60 | ${catkin_INCLUDE_DIRS} 61 | ${PCL_INCLUDE_DIRS} 62 | ) 63 | 64 | link_directories( 65 | include 66 | ${PCL_LIBRARY_DIRS} 67 | ) 68 | 69 | add_executable(${PROJECT_NAME}_node 70 | src/sea_planner_node.cpp 71 | src/rtrrt.cpp 72 | src/rtrrtp.cpp 73 | src/utility.cpp 74 | src/kd_search/kd_search.cpp) 75 | add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_gencpp) 76 | target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 77 | 78 | add_executable(graph_planner_node 79 | src/graph_planner/graph_planner_node.cpp 80 | src/graph_planner/graph_planner.cpp 81 | src/utility.cpp 82 | src/kd_search/kd_search.cpp) 83 | add_dependencies(graph_planner_node ${PROJECT_NAME}_gencpp) 84 | target_link_libraries(graph_planner_node ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 85 | 86 | add_executable(graph_visualization_node src/graph_planner/graph_visualization_node.cpp) 87 | add_dependencies(graph_visualization_node ${PROJECT_NAME}_gencpp) 88 | target_link_libraries(graph_visualization_node ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 89 | 90 | -------------------------------------------------------------------------------- /planner/src/utility.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "grid_map_ros/grid_map_ros.hpp" 7 | #include "grid_map_msgs/GridMap.h" 8 | #include "grid_map_core/grid_map_core.hpp" 9 | #include "sea_planner/utility.h" 10 | 11 | 12 | float util::distance(float x1, float x2, float y1, float y2){ 13 | return sqrt(SQ(x1-x2)+SQ(y1-y2)); 14 | } 15 | 16 | float util::getElevation(float x, float y, grid_map::GridMap elevation_map, string layer){ 17 | Position p{x, y}; 18 | if(!elevation_map.isInside(p)){ 19 | return NAN; 20 | } 21 | return elevation_map.atPosition(layer, p); 22 | } 23 | 24 | float util::gradient(float distance, float z_diff){ 25 | return atan2(fabs(z_diff), distance); 26 | } 27 | 28 | float util::costFunc(float sub_distance, float distance_heuristic , float sub_gradient, float total_distance, float total_gradient){ 29 | return Eta * (0.5*((sub_distance+distance_heuristic)/total_distance) + 0.5*(sub_gradient/total_gradient)); 30 | } 31 | 32 | float util::getInfoGain(float node_x, float node_y, grid_map::GridMap elevation_map, string layer){ 33 | float infomation_gain = 0; 34 | float resolution = elevation_map.getResolution(); 35 | Position center(node_x, node_y); 36 | double radius = informationRadius; 37 | float circleArea = M_PI * SQ(radius); 38 | int totalCellNum = round(circleArea / SQ(resolution)); // all cells contained by circle 39 | float availableCellNum = 0.0; // Iteratable cell numbers, float type is to get float num result 40 | float unknownCellNum = 0.0; 41 | for (grid_map::CircleIterator iterator(elevation_map, center, radius); 42 | !iterator.isPastEnd(); ++iterator) { 43 | availableCellNum++; 44 | if(isnan(elevation_map.at(layer, *iterator))){ 45 | unknownCellNum ++; 46 | } 47 | } 48 | unknownCellNum += (totalCellNum-availableCellNum); 49 | infomation_gain = unknownCellNum/totalCellNum>1? 1 : unknownCellNum/totalCellNum; 50 | return infomation_gain; 51 | } 52 | 53 | int util::getOrientation(const int arr[8]){ 54 | int num = 0; 55 | for(int i=0; i<8; i++){ 56 | if(arr[i] == 1){ 57 | num++; 58 | } 59 | } 60 | return num; 61 | } 62 | 63 | float util::getPenalty(const float min_distance_to_obs){ 64 | float scale = 100.0; 65 | float x = min_distance_to_obs * (2*scale / inflationRadius) - scale; 66 | return (maxPenalty-1.0) * (1.0 / (1+regressionFactor*exp(min_distance_to_obs))) +1.0; 67 | } -------------------------------------------------------------------------------- /planner/include/graph_planner/graph_base.h: -------------------------------------------------------------------------------- 1 | #ifndef GRAPH_BASE_H_ 2 | #define GRAPH_BASE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include "sea_planner/utility.h" 8 | 9 | namespace graph_ns{ 10 | // enum NodeState{ 11 | // ROOT=0, 12 | // NORMAL=1, 13 | // LEAF=2, 14 | // OBSTACLE=3, 15 | // }; 16 | 17 | // Graph Vertex 18 | struct Vertex{ 19 | using Ptr = shared_ptr; 20 | size_t vertex_id; // Unique ID 21 | 22 | geometry_msgs::Point location; // Vertex position 23 | 24 | map> vertex_edge_list; // Relative connection with vertices and edges 25 | 26 | float information_gain; // information gain for reselection 27 | 28 | PointFrontier pcl_pt; // PCL point for calculating frontier 29 | 30 | Vertex(){} 31 | // Init Func 32 | Vertex(sea_planner::Vertex vertex_msg) 33 | { 34 | location.x = vertex_msg.location.x; 35 | location.y = vertex_msg.location.y; 36 | location.z = vertex_msg.location.z; 37 | information_gain = vertex_msg.information_gain; 38 | } 39 | 40 | Vertex(float x_, float y_,float z_, float info_gain_=0.0) 41 | { 42 | location.x = x_; 43 | location.y = y_; 44 | location.z = z_; 45 | information_gain = info_gain_; 46 | pcl_pt.x = x_; 47 | pcl_pt.x = y_; 48 | pcl_pt.x = z_; 49 | pcl_pt.intensity = info_gain_; 50 | } 51 | 52 | }; 53 | // Graph Edge 54 | struct Edge{ 55 | using Ptr = shared_ptr; 56 | 57 | std::shared_ptr Vertex_1; // Relative vertex 58 | std::shared_ptr Vertex_2; 59 | float length; // The length of edge 60 | float elevation_diff; // The difference of Z value 61 | float gradient; // The gradient of Edge, rad form 62 | 63 | Edge(){}; 64 | // Init 65 | Edge(std::shared_ptr Vertex_1_, std::shared_ptr Vertex_2_){ 66 | Vertex_1 = Vertex_1_; 67 | Vertex_2 = Vertex_2_; 68 | length = util::distance(Vertex_1->location.x, Vertex_2->location.x, 69 | Vertex_1->location.y, Vertex_2->location.y); 70 | 71 | } 72 | bool operator == (const Edge & edge){ 73 | if (this->Vertex_1 == edge.Vertex_1 && 74 | this->Vertex_2 == edge.Vertex_2) 75 | { 76 | return true; 77 | } 78 | return false; 79 | } 80 | }; 81 | 82 | typedef struct Vertex Vertex; 83 | typedef struct Edge Edge; 84 | } 85 | 86 | #endif // GRAPH_BASE_H_ -------------------------------------------------------------------------------- /planner/src/graph_planner/graph_visualization_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "sea_planner/utility.h" 4 | #include "sea_planner/Graph.h" 5 | #include "sea_planner/Vertex.h" 6 | #include "sea_planner/Edge.h" 7 | #include 8 | #include 9 | 10 | ros::Publisher *pub_edge_marker; 11 | sea_planner::Graph edge_set; 12 | 13 | void edgeHandler(const sea_planner::GraphConstPtr &edges){ 14 | if(!edges->edges.empty()){ 15 | edge_set.edges.clear(); 16 | edge_set = *edges; 17 | } 18 | } 19 | 20 | int main(int argc, char **argv) 21 | { 22 | ros::init(argc, argv, "graph_visualization_node"); 23 | ros::NodeHandle nh; 24 | 25 | // Publisher and Subscriber definition 26 | ros::Publisher pub_edge_marker_ = nh.advertise("graph_edge_marker", 10); 27 | pub_edge_marker = &pub_edge_marker_; 28 | ros::Publisher pub_vertices_marker_ = nh.advertise("graph_vertices_marker", 10); 29 | ros::Subscriber sub_edge_ = nh.subscribe("graph_edge", 10, edgeHandler); 30 | 31 | ros::Rate r(10); 32 | while(ros::ok()){ 33 | ros::spinOnce(); 34 | visualization_msgs::Marker edge_marker, node_marker; 35 | // Marker init 36 | edge_marker.header.frame_id = edge_set.header.frame_id; 37 | edge_marker.header.stamp = ros::Time::now(); 38 | edge_marker.type = edge_marker.LINE_LIST; 39 | edge_marker.action = edge_marker.ADD; 40 | edge_marker.lifetime = ros::Duration(); 41 | edge_marker.color.a = 1.0; 42 | edge_marker.color.r = 0.0 / 255.0; 43 | edge_marker.color.g = 255.0 / 255.0; 44 | edge_marker.color.b = 255.0 / 255.0; 45 | edge_marker.scale.x = 0.015; 46 | edge_marker.scale.y = 0.015; 47 | edge_marker.scale.z = 0.015; 48 | edge_marker.pose.orientation.w = 1; 49 | edge_marker.points.clear(); 50 | 51 | node_marker.header.frame_id = edge_set.header.frame_id; 52 | node_marker.header.stamp = ros::Time::now(); 53 | node_marker.type = node_marker.SPHERE_LIST; 54 | node_marker.action = node_marker.ADD; 55 | node_marker.lifetime = ros::Duration(); 56 | node_marker.color.a = 1.0; 57 | node_marker.color.r = 245.0/255.0; 58 | node_marker.color.g = 121.0/255.0; 59 | node_marker.color.b = 0.0/255.0; 60 | node_marker.scale.x = 0.08; 61 | node_marker.scale.y = 0.08; 62 | node_marker.scale.z = 0.08; 63 | node_marker.pose.orientation.w = 1; 64 | node_marker.points.clear(); 65 | 66 | // Add edge data 67 | for(int i=0; ipublish(edge_marker); 75 | pub_vertices_marker_.publish(node_marker); 76 | r.sleep(); 77 | } 78 | return 0; 79 | } 80 | -------------------------------------------------------------------------------- /planner/include/sea_planner/rtrrt_base.h: -------------------------------------------------------------------------------- 1 | #ifndef RTRRT_BASE_H_ 2 | #define RTRRT_BASE_H_ 3 | 4 | #include 5 | #include 6 | #include "utility.h" 7 | 8 | namespace rtrrt_ns{ 9 | enum NodeState{ 10 | ROOT=0, 11 | NORMAL=1, 12 | LEAF=2, 13 | OBSTACLE=3, 14 | }; 15 | 16 | // RTRRT node 17 | struct Node{ 18 | using Ptr = shared_ptr; 19 | 20 | size_t node_id; // Node unique id 21 | geometry_msgs::Point state; // Node position 22 | 23 | Node::Ptr parent=NULL, prevParent=NULL; // Parent node 24 | std::shared_ptr endEdge; // The Edge connected with parant node 25 | std::list> startEdges; // The Edge connected with child node 26 | std::list children; // Child nodes 27 | int obstacle_orientation[8] = {0, 0, 0, 0, 0, 0, 0, 0}; // This array is used to certify obstacle node, if element num(=1) > threshold, 28 | // this node is considered as obstacle 29 | 30 | float disToRoot; // The sum of branch length from root to this node in tree 31 | float gradientToRoot; // The sum of gradient from root to this node in tree 32 | float penaltyFactor = 1.0; 33 | NodeState nodeState; // Node state: ROOT, NORMAL, LEAF, OBSTACLE 34 | PointFrontier pcl_pt; // PCL point for calculating frontier 35 | 36 | Node(){} 37 | // Init Func 38 | Node(float x_, float y_,float z_, 39 | float disToStart_, float gradientToRoot_, 40 | Node::Ptr parent_ = NULL, NodeState nodeState_= NodeState::NORMAL, 41 | float info_gain_ = 0.0) 42 | { 43 | state.x = x_; 44 | state.y = y_; 45 | state.z = z_; 46 | disToRoot = disToStart_; 47 | gradientToRoot = gradientToRoot_; 48 | parent = parent_; 49 | nodeState = nodeState_; 50 | pcl_pt.x = x_; 51 | pcl_pt.y = y_; 52 | pcl_pt.z = z_; 53 | pcl_pt.intensity = info_gain_; 54 | } 55 | 56 | }; 57 | // RTRRT edge 58 | struct Edge{ 59 | using Ptr = shared_ptr; 60 | std::shared_ptr fromNode; // Parent Node 61 | std::shared_ptr toNode; // Child Node 62 | float length; // The length of edge 63 | float elevation_diff; // The difference of Z value 64 | float gradient; // The gradient of Edge, rad form 65 | 66 | Edge(){}; 67 | // Init 68 | Edge(std::shared_ptr fromNode_, std::shared_ptr toNode_){ 69 | fromNode = fromNode_; 70 | toNode = toNode_; 71 | length = util::distance(fromNode_->state.x, toNode_->state.x, 72 | fromNode_->state.y, toNode_->state.y); 73 | 74 | } 75 | bool operator == (const Edge & edge){ 76 | if (this->fromNode == edge.fromNode && 77 | this->toNode == edge.toNode) 78 | { 79 | return true; 80 | } 81 | return false; 82 | } 83 | }; 84 | 85 | typedef struct Node Node; 86 | typedef struct Edge Edge; 87 | // typedef Node_Ptr Node::Ptr; 88 | // typedef Edge_Ptr Edge::Ptr; 89 | 90 | 91 | // inline bool rtrrt_ns::Edge::operator==(const rtrrt_ns::Edge& edge) const 92 | // { 93 | // if (this->fromNode == edge.fromNode && 94 | // this->toNode == edge.toNode) 95 | // { 96 | // return true; 97 | // } 98 | // return false; 99 | // } 100 | 101 | } 102 | 103 | #endif // RTRRT_BASE_H_ -------------------------------------------------------------------------------- /planner/include/sea_planner/rtrrtp.h: -------------------------------------------------------------------------------- 1 | #ifndef RTRRTP_H_ 2 | #define RTRRTP_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "sea_planner/rtrrt.h" 9 | #include "sea_planner/get_state.h" 10 | #include "sea_planner/Graph.h" 11 | #include "sea_planner/Vertex.h" 12 | #include "sea_planner/Edge.h" 13 | #include "sea_planner/get_path.h" 14 | 15 | using namespace std; 16 | using namespace util; 17 | 18 | enum PlannerState{ 19 | ACTIVE=0, 20 | RESELECTION=1, 21 | WAITING=2, 22 | }; 23 | 24 | class rtrrtp : public ParamLoader 25 | { 26 | private: 27 | ros::NodeHandle nh; 28 | // ROS publisher 29 | string global_frame; 30 | string base_frame; 31 | ros::Publisher pub_rtrrt_path; 32 | ros::Publisher pub_selected_branch; 33 | ros::Publisher pub_ref_line; 34 | ros::Publisher pub_target_marker; 35 | ros::Publisher pub_goal_marker; 36 | ros::Publisher pub_fake_path; 37 | ros::Publisher pub_waypoint_marker; 38 | ros::Publisher pub_waypoint; 39 | ros::Publisher pub_global_frontier; 40 | ros::Publisher pub_graph_vertices; 41 | 42 | // ROS subscriber 43 | ros::Subscriber sub_goal; 44 | ros::Subscriber sub_robotpose; 45 | 46 | // ROS Service 47 | ros::ServiceServer planningTriger; 48 | ros::ServiceServer stopServer; 49 | ros::ServiceServer resetServer; 50 | ros::ServiceServer getStateServ; 51 | ros::ServiceClient reselectionTri; 52 | 53 | ros::Timer poseUpdater; 54 | ros::Timer visualizer; 55 | ros::Time last_time; 56 | std::shared_ptr rtRRT; // Launch rtrrt 57 | Pose robot_pose; 58 | nav_msgs::Path rtrrt_path; 59 | Nodes selectedBranch_nodes; 60 | std_srvs::Empty general_req; 61 | PlannerState cur_state; 62 | geometry_msgs::PoseStamped final_goal; 63 | geometry_msgs::PoseStamped subgoal; 64 | geometry_msgs::PointStamped way_point; 65 | std::mutex mtx; 66 | vector closeList; // Store visited frontier 67 | rtrrt_ns::Node::Ptr target_node; // Taget node distance to goal or subgoal less than threshold 68 | unordered_map globalFrontier; // Global frontier for replanning 69 | unordered_map localFrontier; // local frontier within the horizon 70 | vector Path_node; // Experiment need 71 | 72 | bool tagetFound; // The flag of finding target node in tree 73 | bool newGoalReceived; // The flag of receiving a new goal 74 | bool updatedPose; // The flag of updating robot pose using listening TF 75 | bool useFinalGoal; // The flag of using final goal as target 76 | bool useSubGoal; // The flag of using subgoal as target 77 | bool HasNewVertices; // The flage of publish new vertices to grpah_planner 78 | 79 | public: 80 | rtrrtp(); 81 | ~rtrrtp(); 82 | void init(); // Planner Initialization 83 | void searchPath(); // Searching path on the tree, for finding a shortest and static path 84 | void executeCycle(); // Planner steer robot to arrive final goal 85 | void publishPath(); // Publish rtrrt path(robot_pose -> target_goal) 86 | void publishWaypoint(const nav_msgs::Path &target_path); // Publish waypoint 87 | void publishNewVertices(); // Publish new graph vertices 88 | void selectNavType(); // Using subgoal or final goal? 89 | void validateFrontier(); // Validate all global frontiers 90 | void visualizationCB(); // Timer: Publish markers 91 | void updateRobotPoseCB(const geometry_msgs::PoseWithCovarianceStampedConstPtr &new_pose); // Update robot pose through listening TF 92 | void ExecuteThread(); // Multithread for execute plan 93 | void stayCurrentPose(); // When changing the goal, the robot should stay current pose 94 | void resetResource(); // Reset all vars and tree for resetting planner 95 | bool getStateHandler(sea_planner::get_state::Request& req, 96 | sea_planner::get_state::Response& state_res); // Get current planner state 97 | bool resetPlanner(std_srvs::Empty::Request& req, 98 | std_srvs::Empty::Response& res); // Reset planner for re-planning 99 | bool stopPlanner(std_srvs::Empty::Request& req, 100 | std_srvs::Empty::Response& res); // Stopping planner when robot arrives goal and waits next goal 101 | void goalHandler(const geometry_msgs::PoseStampedConstPtr & goal_point); // Goal callback func 102 | }; 103 | 104 | #endif // RTRRTP_H_ -------------------------------------------------------------------------------- /planner/include/graph_planner/graph_planner.h: -------------------------------------------------------------------------------- 1 | #ifndef GRAPH_PLANNER_H_ 2 | #define GRAPH_PLANNER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "sea_planner/Vertex.h" 23 | #include "sea_planner/Edge.h" 24 | #include "sea_planner/Graph.h" 25 | #include "sea_planner/get_path.h" 26 | #include "graph_base.h" 27 | #include "kd_search/kd_search.h" 28 | 29 | using namespace std; 30 | using namespace util; 31 | using namespace termcolor; 32 | 33 | typedef vector Vertices; 34 | typedef vector Edges; 35 | 36 | class graph_planner : public ParamLoader 37 | { 38 | private: 39 | // ROS parameter 40 | ros::NodeHandle nh; 41 | string global_frame; 42 | string base_frame; 43 | // ROS publisher for visualization 44 | ros::Publisher pub_root_marker; 45 | ros::Publisher pub_frontier_node; 46 | ros::Publisher pub_subgoal_marker; 47 | ros::Publisher pub_path; 48 | ros::Publisher pub_edge; 49 | ros::Publisher pub_vertices; 50 | ros::Subscriber sub_goal; 51 | ros::Subscriber sub_robotpose; 52 | ros::ServiceServer planningFinder; 53 | ros::ServiceServer nearestFinder; 54 | 55 | ros::Timer poseUpdater; 56 | 57 | std::vector rawVertices; 58 | ros::Publisher treePub; 59 | std::mt19937_64 gen_; 60 | std::uniform_real_distribution uniform_rand_; 61 | std::uniform_real_distribution uniform_rand1_; 62 | unordered_map id_vertex; 63 | float path_length; 64 | float map_x; 65 | float map_y; 66 | float map_resolution; 67 | bool receivedNewVertices; 68 | bool receivedGridMap; 69 | bool updatedPose; 70 | bool homeSaved; 71 | // KDTree 72 | //! for vertex_id sorting 73 | std::shared_ptr> kdtree_pts; // Total point list (include removed point) 74 | std::shared_ptr kdtree_; 75 | 76 | pcl::PointCloud::Ptr graphVertices; 77 | pcl::PointCloud::Ptr treeNodes; 78 | 79 | public: 80 | Vertices node_set; 81 | Edges edge_set; 82 | graph_ns::Vertex::Ptr origin; 83 | geometry_msgs::Pose goal; 84 | geometry_msgs::Point *current_target; // Current target node in the tree 85 | graph_ns::Vertex::Ptr current_target_node; // Current target node in the tree 86 | util::Pose robot_pose; 87 | bool newGoalReceived; 88 | unordered_map GlobalFrontier; // Frontier nodes and its information gain 89 | grid_map::GridMap full_grid_map; 90 | // ROS subscriber 91 | ros::Subscriber sub_grid_map; 92 | ros::Subscriber sub_new_vertices; 93 | ros::Subscriber sub_local_frontier; 94 | 95 | graph_planner(); 96 | ~graph_planner(); 97 | void init(); // Initialization 98 | void clearAll(); // Clear whole graph data and cache 99 | bool removeInvlidVertex(Vertices new_vertices); // Check new node whether has edge connection? if not, remove it 100 | bool checkEdge(graph_ns::Edge edge, graph_ns::Vertex::Ptr v1, graph_ns::Vertex::Ptr v2); // Check edge is valid? or not? 101 | bool addEdge(graph_ns::Vertex::Ptr x_new, graph_ns::Vertex::Ptr x_parent); // Add node, edge 102 | bool addVertex(vector raw_vertices, Vertices& new_vertices); // Add new vertices to kdtree 103 | void executeGraphUpdate(); // Execute one step for updating tree 104 | void findPath(); // Find global path with given two points 105 | graph_ns::Vertex::Ptr findTarget(); // Find the best target node near the goal 106 | void updateGoal(geometry_msgs::PoseStamped goal_point); // Update Goal position from external input 107 | void getFrontierVertex(); // Get frontier node from all rtrrt leaf nodes 108 | void getNeiborVertex(float query_pt[3], float radius, vector& neibor_vertices); // Get neibor nodes set of given point 109 | void publishEdges(); // Publisher all edge for Visulization 110 | graph_ns::Vertex::Ptr getNearestVertex(float query_pt[3]); // Get nearest node of given point 111 | Vertices findBestPath(graph_ns::Vertex::Ptr x_start, graph_ns::Vertex::Ptr x_end); // Find the Dijistra best path from start to end 112 | void gridMapHandler(const grid_map_msgs::GridMapConstPtr &raw_map); // Grid map callback func 113 | void newVerticesHandler(const sea_planner::GraphConstPtr &vertices); // New vertices callback func 114 | void goalHandler(const visualization_msgs::MarkerConstPtr &goal_mark); // Final goal callback func 115 | void updateRobotPoseHandle(const geometry_msgs::PoseWithCovarianceStampedConstPtr &new_pose); // Update robot pose through listening TF 116 | void treeNodesHandler(const sensor_msgs::PointCloud2ConstPtr &node_msgs); // Received local frontiers from rtrrt 117 | graph_ns::Vertex::Ptr getNearestVertex(float point_x, float point_y); // Find the nearest vertex in the graph 118 | bool findPath(sea_planner::get_path::Request& req_target, 119 | sea_planner::get_path::Response& res_path); // Find a best path through graph 120 | bool getNearest(sea_planner::get_path::Request& req_target, 121 | sea_planner::get_path::Response& res_path); // Find a best path through graph 122 | 123 | }; 124 | 125 | #endif // GRAPH_PLANNER_H_ -------------------------------------------------------------------------------- /planner/include/sea_planner/rtrrt.h: -------------------------------------------------------------------------------- 1 | #ifndef RTRRT_H_ 2 | #define RTRRT_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include "rtrrt_base.h" 22 | #include "kd_search/kd_search.h" 23 | 24 | using namespace std; 25 | using namespace util; 26 | using namespace rtrrt_ns; 27 | using namespace termcolor; 28 | 29 | typedef pair NodePair; 30 | typedef unordered_map Tree; 31 | typedef vector Nodes; 32 | typedef list NodesList; 33 | typedef list Edges; 34 | 35 | class rtrrt : public ParamLoader 36 | { 37 | private: 38 | // ROS parameter 39 | ros::NodeHandle nh; 40 | string global_frame; 41 | string base_frame; 42 | // ROS publisher for visualization 43 | ros::Publisher pub_rtrrt_edge; 44 | ros::Publisher pub_rtrrt_node; 45 | ros::Publisher pub_removed_node; 46 | ros::Publisher pub_removed_edge; 47 | ros::Publisher pub_root_marker; 48 | ros::Publisher pub_leaf_node; 49 | ros::Publisher pub_frontier_node; 50 | ros::Publisher pub_goal_marker; 51 | ros::Publisher pub_subgoal_marker; 52 | ros::Publisher pub_path; 53 | ros::Publisher pub_range_map; 54 | ros::Publisher pub_sample_node; 55 | ros::Publisher pub_obstacle_node; 56 | ros::Publisher treePub; 57 | ros::Publisher treeNodesPub; 58 | // ROS subscriber 59 | ros::Subscriber sub_graph_vertice; 60 | ros::Subscriber sub_robot_pose; 61 | 62 | ros::Timer TimerVis; 63 | ros::Time last_time; 64 | 65 | rtrrt_ns::Node sampleNode; 66 | rtrrt_ns::Node newNode; 67 | queue q_rewire_from_root; // Rewiring-from-rand queue 68 | queue q_rewire_from_rand; // Rewiring-from-root queue 69 | std::mt19937_64 gen_; 70 | std::uniform_real_distribution uniform_rand_; 71 | std::uniform_real_distribution uniform_rand1_; 72 | float path_length; 73 | grid_map::GridMap sensor_range_map; 74 | float map_x; 75 | float map_y; 76 | float map_resolution; 77 | bool receivedGridMap; 78 | bool updatedPose; 79 | // KDTree 80 | //! for vertex_id sorting 81 | unordered_map id_nodes; // id-node pair 82 | std::shared_ptr> kdtree_pts; // Total point list (include removed point) 83 | std::shared_ptr kdtree_; 84 | 85 | unordered_map obs_id_nodes; // id-node pair for obstable nodes 86 | std::shared_ptr> obstacle_pts; // Total point list (include removed point) 87 | std::shared_ptr obstacleKdtree_; 88 | 89 | pcl::KdTreeFLANN::Ptr pcl_kdtree; 90 | 91 | int skipCountRewiring = 3; // Skip certain num update step to control rewiring-from-root freq 92 | std::mutex mtx; 93 | 94 | pcl::PointCloud::Ptr graphVertices; 95 | pcl::PointCloud::Ptr treeNodes; 96 | 97 | 98 | public: 99 | Tree tree; 100 | NodesList node_set; 101 | Nodes last_removed_nodes; 102 | Edges last_removed_edges; 103 | Nodes obstacle_node_set; 104 | Edges edge_set; 105 | rtrrt_ns::Node::Ptr root; 106 | NodesList leaf_nodes; 107 | geometry_msgs::Pose goal; 108 | geometry_msgs::Point *current_target; // Current target node in the tree 109 | rtrrt_ns::Node::Ptr current_target_node; // Current target node in the tree 110 | util::Pose robot_pose; 111 | bool newGoalReceived; 112 | unordered_map frontierNodes; // Frontier nodes and its information gain 113 | grid_map::GridMap full_grid_map; 114 | // ROS subscriber 115 | ros::Subscriber sub_grid_map; 116 | 117 | rtrrt(); 118 | ~rtrrt(); 119 | void init(); // Initialization 120 | void clearAll(); // Clear whole tree data and cache 121 | rtrrt_ns::Node sample(); // Sample point using rrt, informed rrt, random approached within space 122 | bool checkNewNode(rtrrt_ns::Node x_new); // Check new node is valid? or not? 123 | bool checkEdge(rtrrt_ns::Edge edge); // Check edge is valid? or not? 124 | rtrrt_ns::Node steer(rtrrt_ns::Node x_s); // Steer from parent node to sample, and prolong a certain length 125 | bool addNodeEdge(rtrrt_ns::Node x_new, rtrrt_ns::Node::Ptr x_parent); // Add node, edge, and N-E pair to current tree 126 | void reselectParent(rtrrt_ns::Node::Ptr x_new); // Reselect parent node within a certain range 127 | void rewireRandomNode(); // Rewire the branch at one of node in tree 128 | void rewireFromRoot(); // Rewire the whole tree 129 | void executeTreeUpdate(); // Execute one step for updating tree 130 | void updateRobotPose(const geometry_msgs::PoseWithCovarianceStampedConstPtr &new_pose); // Update robot pose through listening TF 131 | void publishTree(); // Vistualize tree in rviz 132 | rtrrt_ns::Node::Ptr findTarget(); // Find the best target node near the goal 133 | void changeRoot(rtrrt_ns::Node::Ptr new_node); // Change current root to new node 134 | void pruneTree(rtrrt_ns::Node::Ptr new_center); // Prune relevant edges and node as robot movement 135 | void updateGoal(geometry_msgs::PoseStamped goal_point); // Update Goal position from external input 136 | void getFrontierNode(); // Get frontier node from all rtrrt leaf nodes 137 | Nodes getNeiborNodes(float point_x, float point_y, float point_z, float range); // Get neibor nodes set of given point 138 | rtrrt_ns::Node::Ptr getNearestNode(float point_x, float point_y); // Get nearest node of given point 139 | rtrrt_ns::Node::Ptr getObstacleNode(float point_x, float point_y); // Get nearest obstacle node of given point 140 | void updateOrientationArray(rtrrt_ns::Node new_node, rtrrt_ns::Node::Ptr rrt_node); // Update the value of node's orientation array 141 | void publishRemovedItem(); // Publish last removed node and edges 142 | void gridMapHandler(const grid_map_msgs::GridMapConstPtr &raw_map); // Grid map callback func 143 | void graphVerticesHandler(const sensor_msgs::PointCloud2ConstPtr &vertices_msgs); // Received graph vertices from graph_planner 144 | void visualizationCb(); // Timer callback for visualize the whole tree(node + edge) 145 | void visualizeSample(rtrrt_ns::Node sample_node); // Visualization sample point pose 146 | 147 | }; 148 | 149 | #endif // RTRRT_H_ -------------------------------------------------------------------------------- /planner/include/sea_planner/utility.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILITY_H_ 2 | #define UTILITY_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "grid_map_ros/grid_map_ros.hpp" 28 | #include "grid_map_msgs/GridMap.h" 29 | #include "termcolor.hpp" 30 | 31 | #define SQ(x) ((x) * (x)) 32 | #define TAN60 1.73205081 33 | 34 | using namespace grid_map; 35 | using namespace std; 36 | 37 | typedef pcl::PointXYZI PointFrontier; 38 | 39 | const float inf = std::numeric_limits::infinity(); 40 | const int INF = std::numeric_limits::max(); 41 | 42 | const float Eta = 100.0; // Cost value magnification 43 | const float informationRadius = 1.0; // Check node information gain 44 | const float inflationRadius = 0.8; // The inflation radius for obstacle node 45 | const float maxPenalty = 10.0; // The maximum of Penalty factor 46 | const float regressionFactor = 5.0; // Control the penalty's slope 47 | 48 | class ParamLoader 49 | { 50 | public: 51 | string prefix = "seap"; 52 | ros::NodeHandle nh; 53 | 54 | string baseFrame; 55 | string globalFrame; 56 | 57 | float robotHeight; 58 | 59 | float epsilon; // rtrrt grow rate 60 | float minNodeDistance; // The minimum distance between nodes 61 | float neiborRangeThre; // nodes less than this value are considered neibor nodes 62 | int minTreeNodeNum; // The minimum number of tree nodes need by navigation 63 | int minTreeEdgeNum; // The minimum number of tree edges need by navigation 64 | int maxNodesNum; // The maximum node number of whole tree 65 | float alpha; // rtrrt sample rate threshold for robot->goal 66 | float beta; // rtrrt sample rate threshold for random sample 67 | float Gamma; // rtrrt sample rate threshold for sensor range sample 68 | float allowRewiringFromRand; // Single rewiring-from-rand step time constrain 69 | float allowRewiringFromRoot; // Single rewiring-from-root step time constrain 70 | float gradientDiffThre; // Maximum gradient threshold for edge, rad form 71 | float targetTolerance; // The tolerance of consider node as target node 72 | float goalTolerance; // The tolerance of arriving goal 73 | float waypointTolerance; // The tolerance of arriving waypoint 74 | float changRootTolerance; // The tolerance of arriving waypoint 75 | float informationThre; // Check node information gain 76 | float frontierRadius; // Check node's surrounding whether exist NaN grid 77 | float frontierDisThre; // The leaf nodes less than this value aren't considered as frontier 78 | float exploredAreaRadius; // The leaf nodes' circle check radius for avoiding explored area 79 | int exploredAreaThre; // The leaf nodes' circle number threshold of existed nodes 80 | float findNearNodeThre; // The distance threshold to final goal in order to select navigation type 81 | int obstacleSectorNumThre; // If one node orientation_array element(=1) greater than this value, considered obstacle node 82 | 83 | // Load Parameters 84 | ParamLoader() 85 | { 86 | // Frames 87 | nh.param(prefix+"/robotFrame", baseFrame, "base_link"); 88 | nh.param(prefix+"/globalFrame", globalFrame, "map"); 89 | 90 | nh.param(prefix+"/RobotHeight", robotHeight, 0.11); 91 | 92 | // nh.param(prefix+"/Eta", Eta, 100.0); 93 | nh.param(prefix+"/epsilon", epsilon, 0.8); 94 | nh.param(prefix+"/minNodeDistance", minNodeDistance, 0.2); 95 | nh.param(prefix+"/neiborRangeThre", neiborRangeThre, 1.2); 96 | nh.param(prefix+"/alpha", alpha, 0.1); 97 | nh.param(prefix+"/beta", beta, 1.4); 98 | nh.param(prefix+"/Gamma", Gamma, 0.5); 99 | nh.param(prefix+"/allowRewiringFromRand", allowRewiringFromRand, 2.0); 100 | nh.param(prefix+"/allowRewiringFromRoot", allowRewiringFromRoot, 4.0); 101 | nh.param(prefix+"/gradientDiffThre", gradientDiffThre, 7.0); 102 | gradientDiffThre *= M_PI/180; 103 | 104 | nh.param(prefix+"/targetTolerance", targetTolerance, 0.4); 105 | nh.param(prefix+"/goalTolerance", goalTolerance, 0.5); 106 | nh.param(prefix+"/waypointTolerance", waypointTolerance, 0.4); 107 | nh.param(prefix+"/changRootTolerance", changRootTolerance, 0.5); 108 | nh.param(prefix+"/informationThre", informationThre, 0.3); 109 | nh.param(prefix+"/frontierRadius", frontierRadius, 1.0); 110 | nh.param(prefix+"/frontierDisThre", frontierDisThre, 3.0); 111 | nh.param(prefix+"/exploredAreaRadius", exploredAreaRadius, 0.8); 112 | nh.param(prefix+"/findNearNodeThre", findNearNodeThre, 2.0); 113 | nh.param(prefix+"/minTreeNodeNum", minTreeNodeNum, 3600); 114 | nh.param(prefix+"/minTreeEdgeNum", minTreeEdgeNum, 500); 115 | nh.param(prefix+"/maxNodesNum", maxNodesNum, 100000); 116 | nh.param(prefix+"/exploredAreaThre", exploredAreaThre, 12); 117 | nh.param(prefix+"/obstacleSectorNumThre", obstacleSectorNumThre, 2); 118 | 119 | usleep(100); // Wait 100ms for loading 120 | } 121 | }; 122 | 123 | namespace util{ 124 | // Pose in 3d space 125 | struct Pose 126 | { 127 | float x; 128 | float y; 129 | float z; 130 | float roll; 131 | float pitch; 132 | float yaw; 133 | }; 134 | 135 | // Get Euclidean Distance of two points 136 | float distance(float x1, float x2, float y1, float y2); 137 | 138 | // Get Z value of specific position 139 | float getElevation(float x, float y, grid_map::GridMap elevation_map, string layer="elevation"); 140 | 141 | // Get gradient of two point 142 | float gradient(float distance, float z_diff); 143 | 144 | // Get cost value of given information 145 | float costFunc(float sub_distance, float distance_heuristic , float sub_gradient, float total_distance, float total_gradient); 146 | 147 | // Get leaf node information gain so that consider whether is frontier node 148 | float getInfoGain(float node_x, float node_y, grid_map::GridMap elevation_map, string layer="elevation"); 149 | 150 | // Get rrt node obstacle orientation array element(==1) 151 | int getOrientation(const int arr[8]); 152 | 153 | // Get node penalty factor val using sigmoid function 154 | float getPenalty(const float min_distance_to_obs); 155 | 156 | template 157 | sensor_msgs::PointCloud2 publishCloud(const ros::Publisher& thisPub, const T& thisCloud, ros::Time thisStamp, std::string thisFrame) 158 | { 159 | sensor_msgs::PointCloud2 tempCloud; 160 | pcl::toROSMsg(*thisCloud, tempCloud); 161 | tempCloud.header.stamp = thisStamp; 162 | tempCloud.header.frame_id = thisFrame; 163 | if (thisPub.getNumSubscribers() != 0) 164 | thisPub.publish(tempCloud); 165 | return tempCloud; 166 | } 167 | 168 | } 169 | 170 | #endif // UTILITY_H_ 171 | -------------------------------------------------------------------------------- /planner/include/termcolor.hpp: -------------------------------------------------------------------------------- 1 | //! 2 | //! termcolor 3 | //! ~~~~~~~~~ 4 | //! 5 | //! termcolor is a header-only c++ library for printing colored messages 6 | //! to the terminal. Written just for fun with a help of the Force. 7 | //! 8 | //! :copyright: (c) 2013 by Ihor Kalnytskyi 9 | //! :license: BSD, see LICENSE for details 10 | //! 11 | 12 | #ifndef TERMCOLOR_HPP_ 13 | #define TERMCOLOR_HPP_ 14 | 15 | // the following snippet of code detects the current OS and 16 | // defines the appropriate macro that is used to wrap some 17 | // platform specific things 18 | #if defined(_WIN32) || defined(_WIN64) 19 | #define TERMCOLOR_OS_WINDOWS 20 | #elif defined(__APPLE__) 21 | #define TERMCOLOR_OS_MACOS 22 | #elif defined(__unix__) || defined(__unix) 23 | #define TERMCOLOR_OS_LINUX 24 | #else 25 | #error unsupported platform 26 | #endif 27 | 28 | // This headers provides the `isatty()`/`fileno()` functions, 29 | // which are used for testing whether a standart stream refers 30 | // to the terminal. As for Windows, we also need WinApi funcs 31 | // for changing colors attributes of the terminal. 32 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 33 | #include 34 | #elif defined(TERMCOLOR_OS_WINDOWS) 35 | #include 36 | #include 37 | #endif 38 | 39 | #include 40 | #include 41 | 42 | namespace termcolor 43 | { 44 | // Forward declaration of the `_internal` namespace. 45 | // All comments are below. 46 | namespace _internal 47 | { 48 | // An index to be used to access a private storage of I/O streams. See 49 | // colorize / nocolorize I/O manipulators for details. 50 | static int colorize_index = std::ios_base::xalloc(); 51 | 52 | inline FILE* get_standard_stream(const std::ostream& stream); 53 | inline bool is_colorized(std::ostream& stream); 54 | inline bool is_atty(const std::ostream& stream); 55 | 56 | #if defined(TERMCOLOR_OS_WINDOWS) 57 | inline void win_change_attributes(std::ostream& stream, int foreground, int background = -1); 58 | #endif 59 | } 60 | 61 | inline std::ostream& colorize(std::ostream& stream) 62 | { 63 | stream.iword(_internal::colorize_index) = 1L; 64 | return stream; 65 | } 66 | 67 | inline std::ostream& nocolorize(std::ostream& stream) 68 | { 69 | stream.iword(_internal::colorize_index) = 0L; 70 | return stream; 71 | } 72 | 73 | inline std::ostream& reset(std::ostream& stream) 74 | { 75 | if (_internal::is_colorized(stream)) 76 | { 77 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 78 | stream << "\033[00m"; 79 | #elif defined(TERMCOLOR_OS_WINDOWS) 80 | _internal::win_change_attributes(stream, -1, -1); 81 | #endif 82 | } 83 | return stream; 84 | } 85 | 86 | inline std::ostream& bold(std::ostream& stream) 87 | { 88 | if (_internal::is_colorized(stream)) 89 | { 90 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 91 | stream << "\033[1m"; 92 | #elif defined(TERMCOLOR_OS_WINDOWS) 93 | #endif 94 | } 95 | return stream; 96 | } 97 | 98 | inline std::ostream& dark(std::ostream& stream) 99 | { 100 | if (_internal::is_colorized(stream)) 101 | { 102 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 103 | stream << "\033[2m"; 104 | #elif defined(TERMCOLOR_OS_WINDOWS) 105 | #endif 106 | } 107 | return stream; 108 | } 109 | 110 | inline std::ostream& underline(std::ostream& stream) 111 | { 112 | if (_internal::is_colorized(stream)) 113 | { 114 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 115 | stream << "\033[4m"; 116 | #elif defined(TERMCOLOR_OS_WINDOWS) 117 | #endif 118 | } 119 | return stream; 120 | } 121 | 122 | inline std::ostream& blink(std::ostream& stream) 123 | { 124 | if (_internal::is_colorized(stream)) 125 | { 126 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 127 | stream << "\033[5m"; 128 | #elif defined(TERMCOLOR_OS_WINDOWS) 129 | #endif 130 | } 131 | return stream; 132 | } 133 | 134 | inline std::ostream& reverse(std::ostream& stream) 135 | { 136 | if (_internal::is_colorized(stream)) 137 | { 138 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 139 | stream << "\033[7m"; 140 | #elif defined(TERMCOLOR_OS_WINDOWS) 141 | #endif 142 | } 143 | return stream; 144 | } 145 | 146 | inline std::ostream& concealed(std::ostream& stream) 147 | { 148 | if (_internal::is_colorized(stream)) 149 | { 150 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 151 | stream << "\033[8m"; 152 | #elif defined(TERMCOLOR_OS_WINDOWS) 153 | #endif 154 | } 155 | return stream; 156 | } 157 | 158 | inline std::ostream& grey(std::ostream& stream) 159 | { 160 | if (_internal::is_colorized(stream)) 161 | { 162 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 163 | stream << "\033[30m"; 164 | #elif defined(TERMCOLOR_OS_WINDOWS) 165 | _internal::win_change_attributes(stream, 166 | 0 // grey (black) 167 | ); 168 | #endif 169 | } 170 | return stream; 171 | } 172 | 173 | inline std::ostream& red(std::ostream& stream) 174 | { 175 | if (_internal::is_colorized(stream)) 176 | { 177 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 178 | stream << "\033[31m"; 179 | #elif defined(TERMCOLOR_OS_WINDOWS) 180 | _internal::win_change_attributes(stream, FOREGROUND_RED); 181 | #endif 182 | } 183 | return stream; 184 | } 185 | 186 | inline std::ostream& green(std::ostream& stream) 187 | { 188 | if (_internal::is_colorized(stream)) 189 | { 190 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 191 | stream << "\033[32m"; 192 | #elif defined(TERMCOLOR_OS_WINDOWS) 193 | _internal::win_change_attributes(stream, FOREGROUND_GREEN); 194 | #endif 195 | } 196 | return stream; 197 | } 198 | 199 | inline std::ostream& yellow(std::ostream& stream) 200 | { 201 | if (_internal::is_colorized(stream)) 202 | { 203 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 204 | stream << "\033[33m"; 205 | #elif defined(TERMCOLOR_OS_WINDOWS) 206 | _internal::win_change_attributes(stream, FOREGROUND_GREEN | FOREGROUND_RED); 207 | #endif 208 | } 209 | return stream; 210 | } 211 | 212 | inline std::ostream& blue(std::ostream& stream) 213 | { 214 | if (_internal::is_colorized(stream)) 215 | { 216 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 217 | stream << "\033[34m"; 218 | #elif defined(TERMCOLOR_OS_WINDOWS) 219 | _internal::win_change_attributes(stream, FOREGROUND_BLUE); 220 | #endif 221 | } 222 | return stream; 223 | } 224 | 225 | inline std::ostream& magenta(std::ostream& stream) 226 | { 227 | if (_internal::is_colorized(stream)) 228 | { 229 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 230 | stream << "\033[35m"; 231 | #elif defined(TERMCOLOR_OS_WINDOWS) 232 | _internal::win_change_attributes(stream, FOREGROUND_BLUE | FOREGROUND_RED); 233 | #endif 234 | } 235 | return stream; 236 | } 237 | 238 | inline std::ostream& cyan(std::ostream& stream) 239 | { 240 | if (_internal::is_colorized(stream)) 241 | { 242 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 243 | stream << "\033[36m"; 244 | #elif defined(TERMCOLOR_OS_WINDOWS) 245 | _internal::win_change_attributes(stream, FOREGROUND_BLUE | FOREGROUND_GREEN); 246 | #endif 247 | } 248 | return stream; 249 | } 250 | 251 | inline std::ostream& white(std::ostream& stream) 252 | { 253 | if (_internal::is_colorized(stream)) 254 | { 255 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 256 | stream << "\033[37m"; 257 | #elif defined(TERMCOLOR_OS_WINDOWS) 258 | _internal::win_change_attributes(stream, FOREGROUND_BLUE | FOREGROUND_GREEN | FOREGROUND_RED); 259 | #endif 260 | } 261 | return stream; 262 | } 263 | 264 | inline std::ostream& on_grey(std::ostream& stream) 265 | { 266 | if (_internal::is_colorized(stream)) 267 | { 268 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 269 | stream << "\033[40m"; 270 | #elif defined(TERMCOLOR_OS_WINDOWS) 271 | _internal::win_change_attributes(stream, -1, 272 | 0 // grey (black) 273 | ); 274 | #endif 275 | } 276 | return stream; 277 | } 278 | 279 | inline std::ostream& on_red(std::ostream& stream) 280 | { 281 | if (_internal::is_colorized(stream)) 282 | { 283 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 284 | stream << "\033[41m"; 285 | #elif defined(TERMCOLOR_OS_WINDOWS) 286 | _internal::win_change_attributes(stream, -1, BACKGROUND_RED); 287 | #endif 288 | } 289 | return stream; 290 | } 291 | 292 | inline std::ostream& on_green(std::ostream& stream) 293 | { 294 | if (_internal::is_colorized(stream)) 295 | { 296 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 297 | stream << "\033[42m"; 298 | #elif defined(TERMCOLOR_OS_WINDOWS) 299 | _internal::win_change_attributes(stream, -1, BACKGROUND_GREEN); 300 | #endif 301 | } 302 | return stream; 303 | } 304 | 305 | inline std::ostream& on_yellow(std::ostream& stream) 306 | { 307 | if (_internal::is_colorized(stream)) 308 | { 309 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 310 | stream << "\033[43m"; 311 | #elif defined(TERMCOLOR_OS_WINDOWS) 312 | _internal::win_change_attributes(stream, -1, BACKGROUND_GREEN | BACKGROUND_RED); 313 | #endif 314 | } 315 | return stream; 316 | } 317 | 318 | inline std::ostream& on_blue(std::ostream& stream) 319 | { 320 | if (_internal::is_colorized(stream)) 321 | { 322 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 323 | stream << "\033[44m"; 324 | #elif defined(TERMCOLOR_OS_WINDOWS) 325 | _internal::win_change_attributes(stream, -1, BACKGROUND_BLUE); 326 | #endif 327 | } 328 | return stream; 329 | } 330 | 331 | inline std::ostream& on_magenta(std::ostream& stream) 332 | { 333 | if (_internal::is_colorized(stream)) 334 | { 335 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 336 | stream << "\033[45m"; 337 | #elif defined(TERMCOLOR_OS_WINDOWS) 338 | _internal::win_change_attributes(stream, -1, BACKGROUND_BLUE | BACKGROUND_RED); 339 | #endif 340 | } 341 | return stream; 342 | } 343 | 344 | inline std::ostream& on_cyan(std::ostream& stream) 345 | { 346 | if (_internal::is_colorized(stream)) 347 | { 348 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 349 | stream << "\033[46m"; 350 | #elif defined(TERMCOLOR_OS_WINDOWS) 351 | _internal::win_change_attributes(stream, -1, BACKGROUND_GREEN | BACKGROUND_BLUE); 352 | #endif 353 | } 354 | return stream; 355 | } 356 | 357 | inline std::ostream& on_white(std::ostream& stream) 358 | { 359 | if (_internal::is_colorized(stream)) 360 | { 361 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 362 | stream << "\033[47m"; 363 | #elif defined(TERMCOLOR_OS_WINDOWS) 364 | _internal::win_change_attributes(stream, -1, BACKGROUND_GREEN | BACKGROUND_BLUE | BACKGROUND_RED); 365 | #endif 366 | } 367 | 368 | return stream; 369 | } 370 | 371 | //! Since C++ hasn't a way to hide something in the header from 372 | //! the outer access, I have to introduce this namespace which 373 | //! is used for internal purpose and should't be access from 374 | //! the user code. 375 | namespace _internal 376 | { 377 | //! Since C++ hasn't a true way to extract stream handler 378 | //! from the a given `std::ostream` object, I have to write 379 | //! this kind of hack. 380 | inline FILE* get_standard_stream(const std::ostream& stream) 381 | { 382 | if (&stream == &std::cout) 383 | return stdout; 384 | else if ((&stream == &std::cerr) || (&stream == &std::clog)) 385 | return stderr; 386 | 387 | return 0; 388 | } 389 | 390 | // Say whether a given stream should be colorized or not. It's always 391 | // true for ATTY streams and may be true for streams marked with 392 | // colorize flag. 393 | inline bool is_colorized(std::ostream& stream) 394 | { 395 | return is_atty(stream) || static_cast(stream.iword(colorize_index)); 396 | } 397 | 398 | //! Test whether a given `std::ostream` object refers to 399 | //! a terminal. 400 | inline bool is_atty(const std::ostream& stream) 401 | { 402 | FILE* std_stream = get_standard_stream(stream); 403 | 404 | // Unfortunately, fileno() ends with segmentation fault 405 | // if invalid file descriptor is passed. So we need to 406 | // handle this case gracefully and assume it's not a tty 407 | // if standard stream is not detected, and 0 is returned. 408 | if (!std_stream) 409 | return false; 410 | 411 | #if defined(TERMCOLOR_OS_MACOS) || defined(TERMCOLOR_OS_LINUX) 412 | return ::isatty(fileno(std_stream)); 413 | #elif defined(TERMCOLOR_OS_WINDOWS) 414 | return ::_isatty(_fileno(std_stream)); 415 | #endif 416 | } 417 | 418 | #if defined(TERMCOLOR_OS_WINDOWS) 419 | //! Change Windows Terminal colors attribute. If some 420 | //! parameter is `-1` then attribute won't changed. 421 | inline void win_change_attributes(std::ostream& stream, int foreground, int background) 422 | { 423 | // yeah, i know.. it's ugly, it's windows. 424 | static WORD defaultAttributes = 0; 425 | 426 | // Windows doesn't have ANSI escape sequences and so we use special 427 | // API to change Terminal output color. That means we can't 428 | // manipulate colors by means of "std::stringstream" and hence 429 | // should do nothing in this case. 430 | if (!_internal::is_atty(stream)) 431 | return; 432 | 433 | // get terminal handle 434 | HANDLE hTerminal = INVALID_HANDLE_VALUE; 435 | if (&stream == &std::cout) 436 | hTerminal = GetStdHandle(STD_OUTPUT_HANDLE); 437 | else if (&stream == &std::cerr) 438 | hTerminal = GetStdHandle(STD_ERROR_HANDLE); 439 | 440 | // save default terminal attributes if it unsaved 441 | if (!defaultAttributes) 442 | { 443 | CONSOLE_SCREEN_BUFFER_INFO info; 444 | if (!GetConsoleScreenBufferInfo(hTerminal, &info)) 445 | return; 446 | defaultAttributes = info.wAttributes; 447 | } 448 | 449 | // restore all default settings 450 | if (foreground == -1 && background == -1) 451 | { 452 | SetConsoleTextAttribute(hTerminal, defaultAttributes); 453 | return; 454 | } 455 | 456 | // get current settings 457 | CONSOLE_SCREEN_BUFFER_INFO info; 458 | if (!GetConsoleScreenBufferInfo(hTerminal, &info)) 459 | return; 460 | 461 | if (foreground != -1) 462 | { 463 | info.wAttributes &= ~(info.wAttributes & 0x0F); 464 | info.wAttributes |= static_cast(foreground); 465 | } 466 | 467 | if (background != -1) 468 | { 469 | info.wAttributes &= ~(info.wAttributes & 0xF0); 470 | info.wAttributes |= static_cast(background); 471 | } 472 | 473 | SetConsoleTextAttribute(hTerminal, info.wAttributes); 474 | } 475 | #endif // TERMCOLOR_OS_WINDOWS 476 | 477 | } // namespace _internal 478 | 479 | } // namespace termcolor 480 | 481 | #undef TERMCOLOR_OS_WINDOWS 482 | #undef TERMCOLOR_OS_MACOS 483 | #undef TERMCOLOR_OS_LINUX 484 | 485 | #endif // TERMCOLOR_HPP_ -------------------------------------------------------------------------------- /planner/src/graph_planner/graph_planner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "graph_planner/graph_planner.h" 3 | 4 | graph_planner::graph_planner(): 5 | global_frame(ParamLoader::globalFrame), base_frame(ParamLoader::baseFrame), 6 | receivedGridMap(false), updatedPose(false), receivedNewVertices(false), 7 | homeSaved(false), 8 | kdtree_pts(NULL), kdtree_(NULL), 9 | robot_pose({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) 10 | { 11 | init(); 12 | ROS_INFO("Global Planner initialization accomplished"); 13 | } 14 | 15 | graph_planner::~graph_planner(){} 16 | 17 | void graph_planner::init(){ 18 | // KDtree init 19 | kdtree_pts = make_shared>(); 20 | kdtree_ = make_shared(3, 10, kdtree_pts); 21 | 22 | graphVertices.reset(new pcl::PointCloud()); 23 | treeNodes.reset(new pcl::PointCloud()); 24 | 25 | // Publisher init 26 | pub_root_marker = nh.advertise("/graph_root", 10); 27 | pub_frontier_node = nh.advertise("/global_frontier", 10); 28 | pub_path = nh.advertise("/reselection_path",10); 29 | pub_subgoal_marker = nh.advertise("/subgoal_marker",10); 30 | pub_vertices = nh.advertise("graph_vertices", 10); 31 | pub_edge = nh.advertise("/graph_edge",10); 32 | // Subscriber init 33 | sub_grid_map = nh.subscribe("/elevation_mapping/elevation_map", 1, &graph_planner::gridMapHandler, this); 34 | sub_goal = nh.subscribe("/goal_marker", 1, &graph_planner::goalHandler, this); 35 | sub_new_vertices = nh.subscribe("/new_vertices", 1, &graph_planner::newVerticesHandler, this); 36 | sub_local_frontier = nh.subscribe("/tree_nodes", 1, &graph_planner::treeNodesHandler, this); 37 | sub_robotpose = nh.subscribe("/base_link_pose", 1, &graph_planner::updateRobotPoseHandle, this); 38 | // Timer init 39 | ros::Rate Rater(15); 40 | // poseUpdater = nh.createTimer(Rater, &graph_planner::updateRobotPoseHandle, this); 41 | 42 | // Service init 43 | planningFinder = nh.advertiseService("global_graph/get_global_path", &graph_planner::findPath, this); 44 | nearestFinder = nh.advertiseService("global_graph/get_nearest_info", &graph_planner::getNearest, this); 45 | 46 | } 47 | 48 | void graph_planner::publishEdges(){ 49 | if(!edge_set.empty() && pub_edge.getNumSubscribers()!=0){ 50 | vector edge_set_; 51 | vector vertices_set_; 52 | for(int i=0; iVertex_1->location; 55 | v2.location = edge_set[i]->Vertex_2->location; 56 | sea_planner::Edge e_; 57 | e_.vertex_1 = v1; 58 | e_.vertex_2 = v2; 59 | edge_set_.push_back(e_); 60 | } 61 | for(int j=0; jlocation; 64 | vertices_set_.push_back(v); 65 | } 66 | sea_planner::Graph edge_; 67 | edge_.header.frame_id = global_frame; 68 | edge_.header.stamp = ros::Time::now(); 69 | edge_.edges = edge_set_; 70 | edge_.vertices = vertices_set_; 71 | pub_edge.publish(edge_); 72 | } 73 | } 74 | 75 | void graph_planner::executeGraphUpdate(){ 76 | // TODO saveHome 77 | // receied new vertices 78 | if(receivedNewVertices && !rawVertices.empty()){ 79 | // Iterate all vertices, add to G one by one 80 | if(node_set.empty()){ 81 | // The first iteration, new vertex is origin 82 | // Add to kdtree for search 83 | KDTreePt::Point pt(rawVertices[0].location.x, rawVertices[0].location.y, 0.0); 84 | kdtree_pts->points.push_back(pt); 85 | kdtree_->AddPoint(kdtree_pts->points.size()-1, kdtree_pts->points.size()-1); 86 | // Graph initialization, add origin node 87 | graph_ns::Vertex::Ptr origin_vertex = make_shared(); 88 | origin_vertex->vertex_id = kdtree_pts->points.size(); 89 | origin_vertex->location.x = rawVertices[0].location.x; 90 | origin_vertex->location.y = rawVertices[0].location.y; 91 | origin_vertex->location.z = rawVertices[0].location.z; 92 | origin_vertex->information_gain = rawVertices[0].information_gain; 93 | node_set.push_back(origin_vertex); 94 | origin = origin_vertex; 95 | // Add to PCL pointcloud 96 | origin_vertex->pcl_pt.x = origin_vertex->location.x; 97 | origin_vertex->pcl_pt.y = origin_vertex->location.y; 98 | origin_vertex->pcl_pt.z = 0.0; 99 | origin_vertex->pcl_pt.intensity = origin_vertex->information_gain; 100 | graphVertices->points.push_back(origin_vertex->pcl_pt); 101 | return; 102 | } 103 | 104 | // Add graph vertices 105 | Vertices new_vertices; 106 | std::cout << "Before add vertex " << std::endl; 107 | addVertex(rawVertices, new_vertices); 108 | std::cout << "node_set: " << node_set.size() << std::endl; 109 | 110 | // Add edge for each vertex 111 | if(!new_vertices.empty()){ 112 | for(auto it=new_vertices.begin(); it!=new_vertices.end(); it++){ 113 | // Find Neibor 114 | vector neibor_set; 115 | float query_pt[3] = {(*it)->location.x, (*it)->location.y, 0.0}; 116 | getNeiborVertex(query_pt, neiborRangeThre, neibor_set); 117 | // std::cout << "neibor_set: " << neibor_set.size() << std::endl; 118 | // Add and Check vertex & edge 119 | for(int i=0; i& neibor_vertices){ 140 | vector indices; 141 | vector dists_sq; 142 | // Radius search by kd-tree 143 | kdtree_->RadiusSearch(query_pt, radius, indices, dists_sq); 144 | // indices index sequence is same to Vertices set 145 | for(int i=0; i end 150 | Vertices graph_planner::findBestPath(graph_ns::Vertex::Ptr x_start, graph_ns::Vertex::Ptr x_end){ 151 | // Start point != end 152 | Vertices result_path; 153 | if(x_start == x_end){ 154 | printf("Start point and end points are same! No executable path\n"); 155 | return result_path; 156 | } 157 | if(x_start->vertex_edge_list.find(x_end->vertex_id) != x_start->vertex_edge_list.end()){ 158 | result_path.push_back(x_start); 159 | result_path.push_back(x_end); 160 | return result_path; 161 | } 162 | // Cost to the vertex and corresponding vertex 163 | typedef pair cost_pair; 164 | 165 | // Priority queue of vertices 166 | priority_queue, greater> cost_q; 167 | 168 | // Vector of distances 169 | vector dist(node_set.size(), INFINITY); 170 | 171 | // Vector of backpointers 172 | vector backpointers(node_set.size(), std::numeric_limits::max()); 173 | 174 | // Add start point to queue 175 | cost_q.push(make_pair(0, x_start)); 176 | auto iter1 = find(node_set.begin(), node_set.end(), x_start); 177 | dist[iter1-node_set.begin()] = 0; 178 | 179 | // Find path 180 | while(!cost_q.empty()){ 181 | // Pop top element 182 | graph_ns::Vertex::Ptr u = cost_q.top().second; 183 | auto iter_u = find(node_set.begin(), node_set.end(), u); 184 | size_t ind_u = iter_u-node_set.begin(); 185 | cost_q.pop(); 186 | 187 | // Check all neibor nodes 188 | for(auto it=u->vertex_edge_list.begin(); it!=u->vertex_edge_list.end(); it++){ 189 | // Get corresponding edge and cost 190 | graph_ns::Edge::Ptr e = (*it).second; 191 | graph_ns::Vertex::Ptr v = e->Vertex_1==u? e->Vertex_2 : e->Vertex_1; 192 | auto iter_v = find(node_set.begin(), node_set.end(), v); 193 | size_t ind_v = iter_v-node_set.begin(); 194 | float traversal_cost = e->length + fabs(e->elevation_diff); 195 | 196 | // If there is a shorter path to node through u 197 | if(dist[ind_v] > dist[ind_u]+traversal_cost){ 198 | // Updating v 199 | dist[ind_v] = dist[ind_u]+traversal_cost; 200 | cost_q.push(make_pair(dist[ind_v], v)); 201 | backpointers[ind_v] = ind_u; 202 | } 203 | } 204 | 205 | // Arrve end point, Stop 206 | if (u == x_end){ 207 | break; 208 | } 209 | } 210 | 211 | // Retrace to best path 212 | auto iter_end = find(node_set.begin(), node_set.end(), x_end); 213 | size_t current = iter_end-node_set.begin(); 214 | // if(backpointers[current] == INF){ 215 | if(dist[current] == INFINITY){ 216 | // No feasible Path 217 | result_path.clear(); 218 | return result_path; 219 | } 220 | else{ 221 | // Path found 222 | while(current != std::numeric_limits::max()){ 223 | result_path.insert(result_path.begin(), node_set[current]); 224 | current = backpointers[current]; 225 | } 226 | return result_path; 227 | } 228 | } 229 | 230 | bool graph_planner::addVertex(vector raw_vertices, Vertices& new_vertices){ 231 | 232 | for(int i=0; i< raw_vertices.size(); i++){ 233 | float query_pt[3] = {raw_vertices[i].location.x, raw_vertices[i].location.y, 0.0}; 234 | vector indices; 235 | vector dists_sq; 236 | kdtree_->NearestSearch(query_pt, 1, indices, dists_sq); 237 | // if(dists_sq[0] < 1.0e-4){ 238 | if(dists_sq[0] < minNodeDistance/4){ 239 | // if(dists_sq[0] < minNodeDistance){ 240 | // Avoid too close vertex 241 | // if(dists_sq[0] < 1.0e-4){ 242 | // // Update corresponding vertex information gain 243 | // graph_ns::Vertex* v = id_vertex.at(indices[0]); 244 | // v->information_gain = raw_vertices[i].information_gain; 245 | // // TODO Check this correct 246 | // } 247 | continue; 248 | } 249 | // Add to Kdtree 250 | KDTreePt::Point pt(raw_vertices[i].location.x, raw_vertices[i].location.y, 0.0); 251 | kdtree_pts->points.push_back(pt); 252 | kdtree_->AddPoint(kdtree_pts->points.size()-1, kdtree_pts->points.size()-1); 253 | // Add vertices to node set 254 | graph_ns::Vertex::Ptr new_v = make_shared(); 255 | new_v->location.x = raw_vertices[i].location.x; 256 | new_v->location.y = raw_vertices[i].location.y; 257 | new_v->location.z = raw_vertices[i].location.z; 258 | new_v->information_gain = raw_vertices[i].information_gain; 259 | new_v->vertex_id = kdtree_pts->points.size(); 260 | node_set.push_back(new_v); 261 | new_vertices.push_back(new_v); 262 | id_vertex.insert(make_pair(kdtree_pts->points.size()-1, new_v)); 263 | 264 | new_v->pcl_pt.x = raw_vertices[i].location.x; 265 | new_v->pcl_pt.y = raw_vertices[i].location.y; 266 | new_v->pcl_pt.z = 0.0; 267 | new_v->pcl_pt.intensity = raw_vertices[i].information_gain; 268 | 269 | // If new_v is global frontier, add to unique set 270 | if(new_v->information_gain != 0 ){ 271 | GlobalFrontier.insert(make_pair(new_v, new_v->information_gain)); 272 | } 273 | } 274 | return true; 275 | } 276 | 277 | bool graph_planner::addEdge(graph_ns::Vertex::Ptr x_new, graph_ns::Vertex::Ptr x_parent){ 278 | bool outcome = false; // Whether or not add node-edge successfully 279 | graph_ns::Edge::Ptr parent_to_new = std::make_shared(); 280 | parent_to_new->elevation_diff = x_new->location.z - x_parent->location.z; 281 | float dis_new_parent = distance(x_new->location.x, x_parent->location.x, x_new->location.y, x_parent->location.y); 282 | parent_to_new->length = dis_new_parent; 283 | parent_to_new->gradient = util::gradient(dis_new_parent, parent_to_new->elevation_diff); 284 | 285 | // Check edge quality and repeat 286 | if(checkEdge(*parent_to_new, x_parent, x_new)){ 287 | // Check pass, add edge 288 | // Edge setting 289 | parent_to_new->Vertex_1 = x_parent; 290 | parent_to_new->Vertex_2 = x_new; 291 | x_new->vertex_edge_list.insert(make_pair(x_parent->vertex_id, parent_to_new)); 292 | x_parent->vertex_edge_list.insert(make_pair(x_new->vertex_id, parent_to_new)); 293 | edge_set.push_back(parent_to_new); 294 | outcome = true; 295 | } 296 | else parent_to_new = NULL; 297 | return outcome; 298 | } 299 | 300 | bool graph_planner::removeInvlidVertex(Vertices new_vertices){ 301 | // Iterate all new vertices 302 | int count = 0; 303 | for(int i=0; ivertex_edge_list.empty()){ 306 | // Remove it from kdtree 307 | KDTreePt::Point pt(new_vertices[i]->location.x, new_vertices[i]->location.y, 0.0); 308 | size_t ind = kdtree_pts->kdtree_get_point_index(pt); 309 | kdtree_->RemovePoint(ind); 310 | // Remove it from node_set 311 | auto iter = find(node_set.begin(), node_set.end(), new_vertices[i]); 312 | node_set.erase(iter); 313 | count++; 314 | } 315 | else{ 316 | graphVertices->points.push_back(new_vertices[i]->pcl_pt); 317 | } 318 | } 319 | if(count){ 320 | // printf("Removed %d invalid graph vertices!\n", count); 321 | } 322 | return true; 323 | } 324 | 325 | bool graph_planner::checkEdge(graph_ns::Edge edge, graph_ns::Vertex::Ptr v1, graph_ns::Vertex::Ptr v2){ 326 | // Check repeat 327 | auto v1_res = v1->vertex_edge_list.find(v2->vertex_id); 328 | auto v2_res = v2->vertex_edge_list.find(v1->vertex_id); 329 | if(v1_res != v1->vertex_edge_list.end() || v2_res != v2->vertex_edge_list.end()){ 330 | // Repeat edge 331 | return false; 332 | } 333 | 334 | // Check gradient 335 | if(fabs(edge.gradient) > gradientDiffThre){ 336 | return false; 337 | } 338 | // Avoid edge is too short 339 | float d = distance(v1->location.x, v2->location.x, v1->location.y, v2->location.y); 340 | if(d>neiborRangeThre || dpose.pose, transform); 351 | double roll, pitch, yaw; 352 | tf::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw); 353 | robot_pose.x = transform.getOrigin().x(); 354 | robot_pose.y = transform.getOrigin().y(); 355 | robot_pose.z = transform.getOrigin().z()-0.11; 356 | robot_pose.roll = roll; 357 | robot_pose.pitch = pitch; 358 | robot_pose.yaw = yaw; 359 | updatedPose = true; 360 | // tf::StampedTransform transform; 361 | // tf::TransformListener listener; 362 | // try 363 | // { 364 | // listener.waitForTransform("map","base_link",ros::Time::now(),ros::Duration(0.5)); 365 | // listener.lookupTransform("map","base_link",ros::Time(0),transform); 366 | // double roll, pitch, yaw; 367 | // tf::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw); 368 | // robot_pose.x = transform.getOrigin().x(); 369 | // robot_pose.y = transform.getOrigin().y(); 370 | // robot_pose.z = transform.getOrigin().z()-0.11; 371 | // robot_pose.roll = roll; 372 | // robot_pose.pitch = pitch; 373 | // robot_pose.yaw = yaw; 374 | // updatedPose = true; 375 | // } 376 | // catch(const std::exception& e) 377 | // { 378 | // std::cerr << e.what() << '\n'; 379 | // } 380 | } 381 | 382 | void graph_planner::gridMapHandler(const grid_map_msgs::GridMapConstPtr &raw_map){ 383 | GridMapRosConverter::fromMessage(*raw_map, full_grid_map); 384 | map_x = raw_map->info.length_x; 385 | map_y = raw_map->info.length_y; 386 | map_resolution = raw_map->info.resolution; 387 | receivedGridMap = true; 388 | } 389 | 390 | void graph_planner::newVerticesHandler(const sea_planner::GraphConstPtr &vertices){ 391 | rawVertices.clear(); 392 | // save new vertices 393 | // std::cout << "vertices: " << vertices->vertices.size() << std::endl; 394 | rawVertices = vertices->vertices; 395 | receivedNewVertices = true; 396 | executeGraphUpdate(); 397 | } 398 | 399 | void graph_planner::goalHandler(const visualization_msgs::MarkerConstPtr &goal_mark){ 400 | newGoalReceived = true; 401 | goal = goal_mark->pose; 402 | } 403 | 404 | void graph_planner::treeNodesHandler(const sensor_msgs::PointCloud2ConstPtr &node_msgs){ 405 | treeNodes->points.clear(); 406 | pcl::fromROSMsg(*node_msgs, *treeNodes); 407 | } 408 | 409 | graph_ns::Vertex::Ptr graph_planner::getNearestVertex(float point_x, float point_y){ 410 | float query_pt[3]={point_x, point_y, 0.0}; 411 | vector indices; 412 | vector dists_sq; 413 | kdtree_->NearestSearch(query_pt, 1, indices, dists_sq); 414 | if(indices.empty()){ 415 | return NULL; 416 | } 417 | 418 | graph_ns::Vertex::Ptr nearestVertex = id_vertex.at(indices[0]); 419 | return nearestVertex; 420 | } 421 | 422 | bool graph_planner::findPath(sea_planner::get_path::Request& req_target, 423 | sea_planner::get_path::Response& res_path){ 424 | printf("11111111111111111!\n"); 425 | auto start_vertex = getNearestVertex(robot_pose.x, robot_pose.y); 426 | printf("2222222222222222!\n"); 427 | auto target_vertex = getNearestVertex(req_target.target_point.x, req_target.target_point.y); 428 | printf("3333333333333333333!\n"); 429 | std::cout << start_vertex <<", " << target_vertex << std::endl; 430 | Vertices shortest_path = findBestPath(start_vertex, target_vertex); 431 | std::cout << shortest_path.size() << std::endl; 432 | nav_msgs::Path target_path; 433 | target_path.header.frame_id = "map"; 434 | target_path.header.stamp = ros::Time::now(); 435 | for(auto vertex : shortest_path){ 436 | // Start -> End 437 | geometry_msgs::PoseStamped v_pos; 438 | v_pos.pose.position = vertex->location; 439 | // target_path.poses.insert(target_path.poses.begin(), v_pos); 440 | target_path.poses.push_back(v_pos); 441 | } 442 | res_path.global_path = target_path; 443 | pub_path.publish(target_path); 444 | return true; 445 | } 446 | 447 | bool graph_planner::getNearest(sea_planner::get_path::Request& req_target, 448 | sea_planner::get_path::Response& res_path){ 449 | 450 | return true; 451 | } 452 | -------------------------------------------------------------------------------- /planner/src/rtrrtp.cpp: -------------------------------------------------------------------------------- 1 | #include "sea_planner/rtrrtp.h" 2 | using namespace termcolor; 3 | 4 | void rtrrtp::init(){ 5 | rtRRT.reset(new rtrrt); 6 | target_node.reset(); 7 | global_frame = globalFrame; 8 | base_frame = baseFrame; 9 | useFinalGoal = false; 10 | useSubGoal = false; 11 | HasNewVertices = false; 12 | cur_state = PlannerState::WAITING; 13 | // Publisher init 14 | pub_rtrrt_path = nh.advertise("/rtrrt_path",10); 15 | pub_selected_branch = nh.advertise("selected_branch",10); 16 | pub_ref_line = nh.advertise("reference_line",10); 17 | pub_fake_path = nh.advertise("expected_path", 10); 18 | pub_waypoint = nh.advertise("way_point", 10); 19 | pub_waypoint_marker = nh.advertise("way_point_marker", 10); 20 | pub_goal_marker = nh.advertise("Goal", 10); 21 | pub_target_marker = nh.advertise("target", 10); 22 | pub_global_frontier = nh.advertise("global_frontier", 10); 23 | pub_graph_vertices = nh.advertise("new_vertices", 10); 24 | // Subscriber init 25 | sub_goal = nh.subscribe("/move_base_simple/goal",1, &rtrrtp::goalHandler,this); 26 | sub_robotpose = nh.subscribe("/base_link_pose",1, &rtrrtp::updateRobotPoseCB,this); 27 | // Service init 28 | stopServer = nh.advertiseService("sea_planner/stop_planning", &rtrrtp::stopPlanner, this); 29 | resetServer = nh.advertiseService("sea_planner/reset_planner", &rtrrtp::resetPlanner, this); 30 | getStateServ = nh.advertiseService("sea_planner/get_state", &rtrrtp::getStateHandler, this); 31 | reselectionTri = nh.serviceClient("global_graph/get_global_path"); 32 | 33 | ros::Rate Rater(15); 34 | ros::Rate visRater(2); 35 | // poseUpdater = nh.createTimer(Rater, &rtrrtp::updateRobotPoseCB, this); 36 | // visualizer = nh.createTimer(visRater, &rtrrtp::visualizationCB, this); 37 | last_time = ros::Time::now(); 38 | } 39 | 40 | rtrrtp::rtrrtp(){ 41 | init(); 42 | std::cout << bold << green 43 | << "SEAP initialization accomplished" 44 | << reset << std::endl; 45 | std::thread executeThread(&rtrrtp::ExecuteThread, this); 46 | executeThread.detach(); // Detach the sub-thread for parallelism 47 | } 48 | 49 | rtrrtp::~rtrrtp(){ 50 | rtRRT.reset(new rtrrt); 51 | closeList.clear(); 52 | global_frame.clear(); 53 | selectedBranch_nodes.clear(); 54 | } 55 | 56 | void rtrrtp::resetResource(){ 57 | std::cout << "Delete rtRRT" << std::endl; 58 | rtRRT.reset(new rtrrt); 59 | target_node.reset(); 60 | global_frame = globalFrame; 61 | base_frame = baseFrame; 62 | useFinalGoal = false; 63 | useSubGoal = false; 64 | cur_state = PlannerState::WAITING; 65 | } 66 | 67 | bool rtrrtp::resetPlanner(std_srvs::Empty::Request& req, 68 | std_srvs::Empty::Response& res){ 69 | if(cur_state == PlannerState::ACTIVE){ 70 | ROS_WARN("Planner is active! Please stop planner firstly."); 71 | return false; 72 | } 73 | std::cout << cyan 74 | << "Reseting planner" 75 | << reset << std::endl; 76 | mtx.lock(); 77 | rtRRT->sub_grid_map.shutdown(); // Close grid map spin Channel 78 | closeList.clear(); 79 | globalFrontier.clear(); 80 | resetResource(); 81 | mtx.unlock(); 82 | return true; 83 | } 84 | 85 | bool rtrrtp::stopPlanner(std_srvs::Empty::Request& req, 86 | std_srvs::Empty::Response& res){ 87 | std::cout << yellow 88 | << "Stop path planning" 89 | << reset << std::endl; 90 | mtx.lock(); 91 | target_node = NULL; 92 | cur_state = PlannerState::WAITING; 93 | newGoalReceived = false; 94 | rtRRT->newGoalReceived = false; 95 | mtx.unlock(); 96 | stayCurrentPose(); 97 | return true; 98 | } 99 | 100 | bool rtrrtp::getStateHandler(sea_planner::get_state::Request& req, 101 | sea_planner::get_state::Response& state_res){ 102 | state_res.stamp = ros::Time::now(); 103 | if(cur_state == PlannerState::ACTIVE){ 104 | state_res.state = "ACTIVE"; 105 | } 106 | if(cur_state == PlannerState::WAITING){ 107 | state_res.state = "WAITING"; 108 | } 109 | state_res.estimated_distance = distance(robot_pose.x, rtRRT->goal.position.x, robot_pose.y, rtRRT->goal.position.y); 110 | return true; 111 | } 112 | 113 | void rtrrtp::ExecuteThread(){ 114 | while(ros::ok()){ 115 | if(cur_state != PlannerState::WAITING){ 116 | float timeStart = ros::Time::now().toSec(); 117 | executeCycle(); 118 | float timeEnd = ros::Time::now().toSec(); 119 | if(cur_state == PlannerState::WAITING){ 120 | ROS_INFO("Navigation Duration: %f", timeEnd-timeStart); 121 | std::cout << bold << green 122 | << "Arrived Goal" 123 | << reset << std::endl; 124 | } 125 | mtx.lock(); 126 | newGoalReceived = false; 127 | rtRRT->newGoalReceived = false; 128 | cur_state = PlannerState::WAITING; // Reset flag 129 | mtx.unlock(); 130 | } 131 | } 132 | } 133 | 134 | void rtrrtp::stayCurrentPose(){ 135 | way_point.header.frame_id = global_frame; 136 | way_point.header.stamp = ros::Time::now(); 137 | way_point.point.x = robot_pose.x; 138 | way_point.point.y = robot_pose.y; 139 | way_point.point.z = robot_pose.z; 140 | pub_waypoint.publish(way_point); 141 | } 142 | 143 | void rtrrtp::executeCycle(){ 144 | if(updatedPose){ 145 | // Navigate to final goal 146 | // bool record_gen_time = false; 147 | // float record_generation_start = ros::Time::now().toSec(); 148 | int random_counter = 0; 149 | nav_msgs::Path target_path; 150 | float disToGoal = distance(robot_pose.x, rtRRT->goal.position.x, robot_pose.y, rtRRT->goal.position.y); 151 | while(cur_state != PlannerState::WAITING && 152 | disToGoal > goalTolerance && ros::ok()){ 153 | // For Initializing Tree with enough nodes and edge 154 | while(rtRRT->node_set.size()<=minTreeNodeNum && ros::ok()){ 155 | if(rtRRT->node_set.empty()){ 156 | HasNewVertices = true; 157 | } 158 | // ros::Duration(0.01).sleep(); // Control rrt grow speed 159 | rtRRT->executeTreeUpdate(); 160 | publishNewVertices(); 161 | } 162 | printf("ExecuteCycle iteration start\n"); 163 | // Select navigation type: goal or subgoal 164 | float timeKeeper = ros::Time::now().toSec(); 165 | selectNavType(); 166 | printf("Selected target type\n"); 167 | if(!globalFrontier.empty()){ 168 | validateFrontier(); 169 | } 170 | printf("Select subgoal\n"); 171 | if(useSubGoal && !useFinalGoal && rtRRT->current_target_node==NULL){ 172 | // while((ros::Time::now().toSec() - timeKeeper) < 0.1){ 173 | // // For exploring env, prolong the time for tree growing 174 | // } 175 | rtRRT->executeTreeUpdate(); 176 | 177 | //! Random node as target exp2 178 | // random_counter--; 179 | // if(random_counter < -5){ 180 | // while(rtRRT->current_target_node==NULL){ 181 | // globalFrontier.clear(); 182 | // rtRRT->getFrontierNode(); 183 | // if(rtRRT->frontierNodes.size()!=0){ 184 | // int index = round((((double)rand()) / ((double)RAND_MAX))*(rtRRT->frontierNodes.size()-1)); 185 | // for(auto it=rtRRT->frontierNodes.begin(); it!=rtRRT->frontierNodes.end(); it++){ 186 | // index--; 187 | // if(index<=0){ 188 | // rtRRT->current_target_node = (*it).first; 189 | // break; 190 | // } 191 | // } 192 | // } 193 | // } 194 | // random_counter = 1; 195 | // } 196 | // else{ 197 | //! Ours frontier method 198 | // std::cout << "3333333333333333333" << std::endl; 199 | rtRRT->getFrontierNode(); 200 | // std::cout << "4444444444444444444" << std::endl; 201 | localFrontier.clear(); 202 | localFrontier = rtRRT->frontierNodes; 203 | unordered_map* CANDIDATA_SET = NULL; 204 | // Try to select subgoal from local frontier 205 | if(!localFrontier.empty()){ 206 | HasNewVertices = true; 207 | publishNewVertices(); 208 | 209 | for(auto iter1=rtRRT->frontierNodes.begin(); iter1!=rtRRT->frontierNodes.end(); iter1++){ 210 | // No found, add to global frontier 211 | if(globalFrontier.find((*iter1).first) == globalFrontier.end()){ 212 | globalFrontier.insert(*iter1); 213 | } 214 | // Found, add to global frontier after delete operation 215 | else{ 216 | globalFrontier.erase((*iter1).first); 217 | globalFrontier.insert(*iter1); 218 | } 219 | } 220 | //! No longer delete all global frontier 221 | // globalFrontier.clear(); 222 | // globalFrontier = localFrontier; 223 | // std::cout << "55555555555555555" << std::endl; 224 | 225 | // Find best frontier as target node 226 | float totalDis = 0; 227 | float totalGradient = 0; 228 | for(auto iter2=localFrontier.begin(); iter2!=localFrontier.end(); iter2++){ 229 | // Except nodes in closeList 230 | auto res = find(closeList.begin(), closeList.end(), (*iter2).first); 231 | if(res != closeList.end()){ 232 | continue; 233 | } 234 | totalDis += (*iter2).first->disToRoot; 235 | totalGradient += (*iter2).first->gradientToRoot; 236 | } 237 | float best_cost = inf; 238 | rtrrt_ns::Node::Ptr best_node; 239 | for(auto iter3=localFrontier.begin(); iter3!=localFrontier.end(); iter3++){ 240 | // Except nodes in closeList 241 | //TODO evaluation Func should be desiged independently 242 | float distance_heuristic = util::distance((*iter3).first->state.x, final_goal.pose.position.x, (*iter3).first->state.y, final_goal.pose.position.y); 243 | float costVal = costFunc((*iter3).first->disToRoot, distance_heuristic, (*iter3).first->gradientToRoot, totalDis, totalGradient); 244 | //! Nearest node for Goal (Greedy) 245 | // float costVal = util::distance((*iter3).first->state.x, final_goal.pose.position.x, (*iter3).first->state.y, final_goal.pose.position.y); 246 | //! Max info method 247 | // float costVal = -1 * 0.9 * util::getInfoGain((*iter3).first->state.x, (*iter3).first->state.y, rtRRT->full_grid_map, "elevation") + 0.1* distance_heuristic; 248 | //! Nearest frontier for robot 249 | // float costVal = util::distance((*iter3).first->state.x, robot_pose.x, (*iter3).first->state.y, robot_pose.y); 250 | if(costVal < best_cost){ 251 | best_cost = costVal; 252 | best_node = (*iter3).first; 253 | } 254 | } 255 | rtRRT->current_target_node = best_node; 256 | } 257 | // std::cout << "66666666666666666" << std::endl; } 258 | // Try to select subgoal from global frontier 259 | else if(!globalFrontier.empty()){ 260 | ROS_WARN("No local subgoals! Relocalzing in global!"); 261 | // Relocate the other global frontier 262 | float total_dis; 263 | float total_info; 264 | unordered_map> GFs; 265 | // Calculate total information 266 | for(auto gf=globalFrontier.begin(); gf!=globalFrontier.end(); gf++){ 267 | float disToGoal = distance(gf->first->state.x, rtRRT->goal.position.x, gf->first->state.y, rtRRT->goal.position.y); 268 | GFs.insert(make_pair(gf->first, make_pair(disToGoal, gf->second))); // 269 | total_dis += disToGoal; 270 | total_info += gf->second; 271 | } 272 | rtrrt_ns::Node::Ptr best_frontier = NULL; 273 | double best_cost = inf; 274 | double cost_per_gf; 275 | for(auto GF=GFs.begin(); GF!=GFs.end(); GF++){ 276 | cost_per_gf = 5.0 * (GF->second.first);// + 1.0 * gf->second.second/total_info; 277 | if(cost_per_gf < best_cost){ 278 | best_cost = cost_per_gf; 279 | best_frontier = GF->first; 280 | } 281 | } 282 | 283 | // std::cout << "get_path_srv.request.target_point: " << best_frontier << std::endl; 284 | sea_planner::get_path get_path_srv; 285 | get_path_srv.request.target_point = best_frontier->state; 286 | // std::cout << "get_path_srv.request.target_point: " << get_path_srv.request.target_point.x << ", " << get_path_srv.request.target_point.y << std::endl; 287 | // if(!reselectionTri.waitForExistence(ros::Duration(3.0))){ 288 | // printf("Service no exist! Waiting! \n"); 289 | // } 290 | // else{ 291 | if(reselectionTri.call(get_path_srv)){ 292 | printf("Service call successful! Path size:%d \n",get_path_srv.response.global_path.poses.size()); 293 | target_path.poses.clear(); 294 | target_path = get_path_srv.response.global_path; 295 | target_path.header.frame_id = globalFrame; 296 | rtRRT->current_target_node = best_frontier; 297 | cur_state = PlannerState::RESELECTION; 298 | } 299 | // } 300 | } 301 | // No frontier. Stay 302 | else{ 303 | ROS_WARN("No any subgoals pointing to target!\n"); 304 | // Reset all resource 305 | target_node = NULL; 306 | rtRRT->current_target = NULL; 307 | rtRRT->current_target_node = NULL; 308 | return; 309 | } 310 | 311 | } 312 | // printf("Select Final goal\n"); 313 | // If have node close enough to final_goal, it will become target 314 | if(!useSubGoal && useFinalGoal){ 315 | // Update frontiers 316 | rtRRT->getFrontierNode(); 317 | localFrontier.clear(); 318 | localFrontier = rtRRT->frontierNodes; 319 | HasNewVertices = true; 320 | publishNewVertices(); 321 | for(auto iter1=rtRRT->frontierNodes.begin(); iter1!=rtRRT->frontierNodes.end(); iter1++){ 322 | // No found, add to global frontier 323 | if(globalFrontier.find((*iter1).first) == globalFrontier.end()){ 324 | globalFrontier.insert(*iter1); 325 | } 326 | // Found, add to global frontier after delete operation 327 | else{ 328 | globalFrontier.erase((*iter1).first); 329 | globalFrontier.insert(*iter1); 330 | } 331 | } 332 | //! No longer delete all global frontier 333 | // globalFrontier.clear(); 334 | // globalFrontier = localFrontier; 335 | 336 | rtrrt_ns::Node::Ptr closeNode = rtRRT->findTarget(); 337 | if(closeNode != NULL){ 338 | rtRRT->current_target_node = closeNode; 339 | } 340 | } 341 | rtRRT->executeTreeUpdate(); 342 | // Driving to target_node 343 | float DisToTarget = inf; 344 | if(cur_state!=PlannerState::WAITING){ 345 | target_node = rtRRT->current_target_node; 346 | DisToTarget = util::distance(robot_pose.x, target_node->state.x, robot_pose.y, target_node->state.y); 347 | } 348 | // printf("Reselection step\n"); 349 | bool state = ros::ok(); 350 | while(cur_state == PlannerState::RESELECTION && DisToTarget > goalTolerance && state){ 351 | // Release all tree resource 352 | if(target_node!=NULL && cur_state!=PlannerState::WAITING){ 353 | DisToTarget = util::distance(robot_pose.x, target_node->state.x, robot_pose.y, target_node->state.y); 354 | } 355 | // printf("111111111111111\n"); 356 | mtx.lock(); 357 | selectedBranch_nodes.clear(); 358 | for(auto pos: target_path.poses){ 359 | rtrrt_ns::Node::Ptr path_node = make_shared(); 360 | path_node->state = pos.pose.position; 361 | // According to find parent one by one until retrace root node(positive sequence) 362 | selectedBranch_nodes.push_back(path_node); 363 | } 364 | mtx.unlock(); 365 | // printf("222222222222222222\n"); 366 | // rtrrt_path direction = selected branch 367 | if(!selectedBranch_nodes.empty()){ 368 | publishPath(); 369 | publishWaypoint(rtrrt_path); 370 | } 371 | // printf("3333333333333333333\n"); 372 | 373 | float disToWaypoint = util::distance(robot_pose.x, rtrrt_path.poses[1].pose.position.x, 374 | robot_pose.y, rtrrt_path.poses[1].pose.position.y); 375 | 376 | if(disToWaypoint < waypointTolerance){ 377 | if(target_path.poses.size()>2) target_path.poses.erase(target_path.poses.begin()); 378 | } 379 | // printf("4444444444444444444\n"); 380 | 381 | state = ros::ok(); 382 | } 383 | // printf("Local planning step\n"); 384 | while(cur_state == PlannerState::ACTIVE && DisToTarget > goalTolerance && state && target_node!=NULL && target_node->nodeState!=NodeState::OBSTACLE){ 385 | // std::cout << "66666667777777777" << std::endl; 386 | rtRRT->executeTreeUpdate(); 387 | // std::cout << "7777777777777777777" << std::endl; 388 | 389 | if(target_node != NULL){ 390 | // Found target node, output path 391 | // if(!record_gen_time){ 392 | // float record_generation_end = ros::Time::now().toSec(); 393 | // std::cout << "The first path generation time: " << record_generation_end-record_generation_start << std::endl; 394 | // record_gen_time = true; 395 | // } 396 | searchPath(); 397 | } 398 | // std::cout << "888888888888888888888" << std::endl; 399 | 400 | if(target_node!=NULL && cur_state!=PlannerState::WAITING){ 401 | DisToTarget = util::distance(robot_pose.x, target_node->state.x, robot_pose.y, target_node->state.y); 402 | rtRRT->executeTreeUpdate(); 403 | } 404 | // std::cout << "9999999999999999999999" << std::endl; 405 | // publishNewVertices(); 406 | // std::cout << "10000000000000000000000" << std::endl; 407 | 408 | state = ros::ok(); 409 | } 410 | // printf("Iteration end! Reset resource\n"); 411 | if(useSubGoal && !useFinalGoal && rtRRT->current_target_node!=NULL){ 412 | closeList.push_back(rtRRT->current_target_node); 413 | } 414 | if(useSubGoal && !useFinalGoal && cur_state == PlannerState::RESELECTION && rtRRT->current_target_node!=NULL){ 415 | // Reset tree resource 416 | rtRRT->clearAll(); 417 | cur_state = PlannerState::ACTIVE; 418 | } 419 | useFinalGoal = false; 420 | useSubGoal = false; 421 | target_node = NULL; 422 | rtRRT->current_target = NULL; 423 | rtRRT->current_target_node = NULL; 424 | 425 | // ros::Duration(0.02).sleep(); // Control rrt grow speed 426 | disToGoal = distance(robot_pose.x, rtRRT->goal.position.x, robot_pose.y, rtRRT->goal.position.y); 427 | } 428 | if(cur_state == PlannerState::ACTIVE){ 429 | std::cout << "Node Num: " << rtRRT->node_set.size() << ", Edge Num: " << rtRRT->edge_set.size() << std::endl; 430 | std::cout << "Executed Path node num: " << Path_node.size() << std::endl; 431 | } 432 | // Reset all resource 433 | target_node = NULL; 434 | rtRRT->current_target = NULL; 435 | rtRRT->current_target_node = NULL; 436 | cur_state = PlannerState::WAITING; 437 | } 438 | } 439 | 440 | void rtrrtp::searchPath(){ 441 | // Empty selectedBranch_nodes set for updating path 442 | selectedBranch_nodes.clear(); 443 | if(target_node != NULL){ 444 | rtrrt_ns::Node::Ptr current_iterator = target_node; 445 | while(current_iterator != NULL){ 446 | // According to find parent one by one until retrace root node(positive sequence) 447 | if(selectedBranch_nodes.empty()){ 448 | selectedBranch_nodes.push_back(current_iterator); 449 | } 450 | else{ 451 | selectedBranch_nodes.insert(selectedBranch_nodes.begin(), current_iterator); 452 | } 453 | current_iterator = current_iterator->parent; 454 | } 455 | if(!selectedBranch_nodes.empty()){ 456 | publishPath(); 457 | publishWaypoint(rtrrt_path); 458 | } 459 | } 460 | // Change rtrrt root 461 | if(!selectedBranch_nodes.empty()){ 462 | if(selectedBranch_nodes.size()>1){ 463 | float dis = util::distance(robot_pose.x, selectedBranch_nodes[1]->state.x, robot_pose.y, selectedBranch_nodes[1]->state.y); 464 | if(dis < changRootTolerance && rtRRT->root != selectedBranch_nodes[1]){ 465 | rtRRT->changeRoot(selectedBranch_nodes[1]); 466 | rtRRT->pruneTree(selectedBranch_nodes[1]); 467 | HasNewVertices = true; 468 | Path_node.push_back(rtRRT->root); 469 | } 470 | } 471 | else{ 472 | float dis = util::distance(robot_pose.x, selectedBranch_nodes[0]->state.x, robot_pose.y, selectedBranch_nodes[0]->state.y); 473 | if(dis < changRootTolerance && rtRRT->root != selectedBranch_nodes[0]){ 474 | rtRRT->changeRoot(selectedBranch_nodes[0]); 475 | rtRRT->pruneTree(selectedBranch_nodes[0]); 476 | HasNewVertices = true; 477 | Path_node.push_back(rtRRT->root); 478 | } 479 | } 480 | } 481 | } 482 | 483 | void rtrrtp::publishPath(){ 484 | rtrrt_path.header.frame_id = global_frame; 485 | rtrrt_path.header.stamp = ros::Time::now(); 486 | rtrrt_path.poses.clear(); 487 | geometry_msgs::PoseStamped single_pose; 488 | if(selectedBranch_nodes.size()>1){ 489 | // Add robot pose to rtrrt_path 490 | single_pose.header.frame_id = global_frame; 491 | single_pose.header.stamp = ros::Time::now(); 492 | single_pose.pose.position.x = robot_pose.x; 493 | single_pose.pose.position.y = robot_pose.y; 494 | single_pose.pose.position.z = robot_pose.z; 495 | tf::Quaternion tf_q = tf::createQuaternionFromYaw(robot_pose.yaw).normalized(); 496 | single_pose.pose.orientation.x = tf_q.getX(); 497 | single_pose.pose.orientation.y = tf_q.getY(); 498 | single_pose.pose.orientation.z = tf_q.getZ(); 499 | single_pose.pose.orientation.w = tf_q.getW(); 500 | rtrrt_path.poses.push_back(single_pose); 501 | // Add rtrrt node except root node 502 | for(auto iter=selectedBranch_nodes.begin()+1; iter!=selectedBranch_nodes.end(); iter++){ 503 | single_pose.header.frame_id = global_frame; 504 | single_pose.header.stamp = ros::Time::now(); 505 | // Get position 506 | single_pose.pose.position.x = (*iter)->state.x; 507 | single_pose.pose.position.y = (*iter)->state.y; 508 | single_pose.pose.position.z = robot_pose.z; 509 | // Get quaternion 510 | if(iter == selectedBranch_nodes.end()-1){ 511 | float deltY = (*iter)->state.y - (*(iter-1))->state.y; 512 | float deltX = (*iter)->state.x - (*(iter-1))->state.x; 513 | float yaw_angle = atan2(deltY, deltX); 514 | tf::Quaternion tf_q = tf::createQuaternionFromYaw(yaw_angle).normalized(); 515 | single_pose.pose.orientation.x = tf_q.getX(); 516 | single_pose.pose.orientation.y = tf_q.getY(); 517 | single_pose.pose.orientation.z = tf_q.getZ(); 518 | single_pose.pose.orientation.w = tf_q.getW(); 519 | rtrrt_path.poses.push_back(single_pose); 520 | 521 | // Add final_goal pose to path 522 | // single_pose.header.frame_id = global_frame; 523 | // single_pose.header.stamp = ros::Time::now(); 524 | // // Get position 525 | // single_pose.pose.position.x = rtRRT->current_target->x; 526 | // single_pose.pose.position.y = rtRRT->current_target->y; 527 | // single_pose.pose.position.z = robot_pose.z; 528 | // rtrrt_path.poses.push_back(single_pose); 529 | } 530 | else{ 531 | float deltY = (*(iter+1))->state.y - (*iter)->state.y; 532 | float deltX = (*(iter+1))->state.x - (*iter)->state.x; 533 | float yaw_angle = atan2(deltY, deltX); 534 | tf::Quaternion tf_q = tf::createQuaternionFromYaw(yaw_angle).normalized(); 535 | single_pose.pose.orientation.x = tf_q.getX(); 536 | single_pose.pose.orientation.y = tf_q.getY(); 537 | single_pose.pose.orientation.z = tf_q.getZ(); 538 | single_pose.pose.orientation.w = tf_q.getW(); 539 | rtrrt_path.poses.push_back(single_pose); 540 | } 541 | } 542 | } 543 | else{ 544 | // Add robot pose to rtrrt_path 545 | single_pose.header.frame_id = global_frame; 546 | single_pose.header.stamp = ros::Time::now(); 547 | single_pose.pose.position.x = robot_pose.x; 548 | single_pose.pose.position.y = robot_pose.y; 549 | single_pose.pose.position.z = robot_pose.z; 550 | tf::Quaternion tf_q = tf::createQuaternionFromYaw(robot_pose.yaw).normalized(); 551 | single_pose.pose.orientation.x = tf_q.getX(); 552 | single_pose.pose.orientation.y = tf_q.getY(); 553 | single_pose.pose.orientation.z = tf_q.getZ(); 554 | single_pose.pose.orientation.w = tf_q.getW(); 555 | rtrrt_path.poses.push_back(single_pose); 556 | // Add final_goal pose to path 557 | single_pose.header.frame_id = global_frame; 558 | single_pose.header.stamp = ros::Time::now(); 559 | // Get position 560 | single_pose.pose.position.x = target_node->state.x; 561 | single_pose.pose.position.y = target_node->state.y; 562 | single_pose.pose.position.z = target_node->state.z; 563 | rtrrt_path.poses.push_back(single_pose); 564 | } 565 | pub_rtrrt_path.publish(rtrrt_path); 566 | } 567 | 568 | void rtrrtp::publishWaypoint(const nav_msgs::Path &target_path){ 569 | float disTopoint = util::distance(robot_pose.x, target_path.poses[1].pose.position.x, 570 | robot_pose.y, target_path.poses[1].pose.position.y); 571 | if(disTopoint > waypointTolerance){ 572 | if(target_path.poses.size()>2){ 573 | way_point.header.frame_id = global_frame; 574 | way_point.header.stamp = ros::Time::now(); 575 | way_point.point.x = target_path.poses[1].pose.position.x; 576 | way_point.point.y = target_path.poses[1].pose.position.y; 577 | way_point.point.z = target_path.poses[1].pose.position.z; 578 | pub_waypoint.publish(way_point); 579 | } 580 | else{ 581 | way_point.header.frame_id = global_frame; 582 | way_point.header.stamp = ros::Time::now(); 583 | way_point.point.x = target_path.poses.back().pose.position.x; 584 | way_point.point.y = target_path.poses.back().pose.position.y; 585 | way_point.point.z = target_path.poses.back().pose.position.z; 586 | pub_waypoint.publish(way_point); 587 | } 588 | } 589 | } 590 | 591 | void rtrrtp::publishNewVertices(){ 592 | if(HasNewVertices && rtRRT->root != NULL){ 593 | // Add root node 594 | sea_planner::Graph new_vs; 595 | sea_planner::Vertex root_vertex; 596 | root_vertex.location = rtRRT->root->state; 597 | root_vertex.information_gain = 0.0; 598 | new_vs.vertices.push_back(root_vertex); 599 | if(!localFrontier.empty()){ 600 | // Add local frontier nodes 601 | auto iter = localFrontier.begin(); 602 | while(iter != localFrontier.end()){ 603 | Node::Ptr pt = iter->first; 604 | float pt_info = iter->second; 605 | auto res = find(rtRRT->node_set.begin(), rtRRT->node_set.end(), pt); 606 | if(res == rtRRT->node_set.end()){ 607 | // Frontier is outside the local bound, has removed 608 | iter++; 609 | continue; 610 | } 611 | while(pt != rtRRT->root && pt != NULL){ 612 | sea_planner::Vertex v; 613 | v.location = pt->state; 614 | v.information_gain = pt_info; 615 | new_vs.vertices.insert(new_vs.vertices.begin(), v); 616 | pt = pt->parent; 617 | // only frontier has information gian 618 | pt_info = 0.0; 619 | } 620 | iter++; 621 | } 622 | } 623 | new_vs.header.frame_id = global_frame; 624 | new_vs.header.stamp = ros::Time::now(); 625 | pub_graph_vertices.publish(new_vs); 626 | HasNewVertices = false; 627 | } 628 | } 629 | 630 | void rtrrtp::selectNavType(){ 631 | // Find near nodes of final goal 632 | float totalDistance = 0; 633 | float totalGradient = 0; 634 | float min_cost = inf; 635 | rtrrt_ns::Node::Ptr best_node = NULL; 636 | Nodes near_nodes = rtRRT->getNeiborNodes(final_goal.pose.position.x, final_goal.pose.position.y, 0.0, findNearNodeThre); 637 | if(near_nodes.empty()) 638 | { 639 | useSubGoal = true; 640 | useFinalGoal = false; 641 | return; 642 | } 643 | else{ 644 | auto iter = near_nodes.begin(); 645 | while(iter != near_nodes.end()){ 646 | rtrrt_ns::Node::Ptr node = *iter; 647 | // Except OBSTACLE nodes 648 | if(node->nodeState == NodeState::OBSTACLE){ 649 | iter++; 650 | continue; 651 | } 652 | totalDistance += node->disToRoot; 653 | totalGradient += node->gradientToRoot; 654 | iter++; 655 | } 656 | iter = near_nodes.begin(); 657 | // Iterate all neibor nodes 658 | while(iter != near_nodes.end()){ 659 | useFinalGoal = true; 660 | useSubGoal = false; 661 | 662 | rtrrt_ns::Node::Ptr near = *iter; 663 | // Except OBSTACLE nodes 664 | if(near->nodeState == NodeState::OBSTACLE){ 665 | iter++; 666 | continue; 667 | } 668 | float distance_heuristic = util::distance(near->state.x, final_goal.pose.position.x, near->state.y, final_goal.pose.position.y); 669 | float costVal = costFunc(near->disToRoot, distance_heuristic, near->gradientToRoot, totalDistance, totalGradient); 670 | // std::cout << "costVal: " << costVal << std::endl; 671 | if(costVal < min_cost){ 672 | min_cost = costVal; 673 | best_node = near; 674 | } 675 | iter++; 676 | } 677 | // kd_res_free(near_nodes); 678 | // Drive to final goal surround 679 | rtRRT->current_target_node = best_node; 680 | } 681 | } 682 | 683 | void rtrrtp::validateFrontier(){ 684 | // Global frontier is on the graph 685 | // Iterate all global frontier 686 | std::unordered_map globalFrontier_copy; 687 | mtx.lock(); 688 | for(const auto& gp : globalFrontier){ 689 | bool collisionFLAG=false, leafFLAG=false, infoFLAG=false; 690 | // Check collision 691 | rtrrt_ns::Node::Ptr nearest_obstacle = rtRRT->getObstacleNode(gp.first->state.x, gp.first->state.y); 692 | float dis = util::distance(gp.first->state.x, nearest_obstacle->state.x, gp.first->state.y, nearest_obstacle->state.y); 693 | if(dis <= inflationRadius){ 694 | collisionFLAG = false; 695 | } 696 | else collisionFLAG=true; 697 | // Only update frontiers within the local map 698 | Position frontier_pos{gp.first->state.x, gp.first->state.y}; 699 | if(!rtRRT->full_grid_map.isInside(frontier_pos)){ 700 | globalFrontier_copy.insert(gp); 701 | continue; 702 | } 703 | // If frontier node is not leaf, remove it 704 | if(gp.first->nodeState != NodeState::LEAF){ 705 | leafFLAG = false; 706 | } 707 | else leafFLAG=true; 708 | // Check frontier information gain 709 | Nodes near_nodes = rtRRT->getNeiborNodes(gp.first->state.x, gp.first->state.y, 0.0, exploredAreaRadius); 710 | int neibor_nodes_size = near_nodes.size(); 711 | 712 | // For avoiding explored area 713 | float information_gain; 714 | if(neibor_nodes_size < exploredAreaThre){ 715 | Position leaf_posi(gp.first->state.x, gp.first->state.y); 716 | Index leaf_index; 717 | Length leng(frontierRadius, frontierRadius); 718 | bool isSuccessful; 719 | auto sub_map = rtRRT->full_grid_map.getSubmap(leaf_posi, leng, leaf_index ,isSuccessful); 720 | Size submap_size = sub_map.getSize(); 721 | // No enough information gain will be erase 722 | if(submap_size[0] == submap_size[1]){ 723 | infoFLAG = false; 724 | } 725 | else{ 726 | information_gain = util::getInfoGain(gp.first->state.x, gp.first->state.y, rtRRT->full_grid_map, "elevation"); 727 | if(information_gain < informationThre){ 728 | infoFLAG = false; 729 | } 730 | else infoFLAG = true; 731 | } 732 | } 733 | else{ 734 | infoFLAG=false; 735 | } 736 | if(collisionFLAG && leafFLAG && infoFLAG){ 737 | globalFrontier_copy.insert(make_pair(gp.first, information_gain)); 738 | } 739 | } 740 | globalFrontier = std::move(globalFrontier_copy); 741 | mtx.unlock(); 742 | 743 | // auto node=globalFrontier.begin(); 744 | // mtx.lock(); 745 | // while(node!=globalFrontier.end() && ros::ok()){ 746 | // // Check collision 747 | // rtrrt_ns::Node::Ptr nearest_obstacle = rtRRT->getObstacleNode((*node).first->state.x, (*node).first->state.y); 748 | // float dis = util::distance((*node).first->state.x, nearest_obstacle->state.x, (*node).first->state.y, nearest_obstacle->state.y); 749 | // if(dis <= inflationRadius){ 750 | // node = globalFrontier.erase(node); 751 | // continue; 752 | // } 753 | // // Only update frontiers within the local map 754 | // Position frontier_pos{(*node).first->state.x, (*node).first->state.y}; 755 | // if(!rtRRT->full_grid_map.isInside(frontier_pos)){ 756 | // node++; 757 | // continue; 758 | // } 759 | // // If frontier node is not leaf, remove it 760 | // if((*node).first->nodeState != NodeState::LEAF){ 761 | // node = globalFrontier.erase(node); // For erase method will return next iterator after resleased 762 | // continue; 763 | // } 764 | 765 | // // Check frontier information gain 766 | // Nodes near_nodes = rtRRT->getNeiborNodes((*node).first->state.x, (*node).first->state.y, 0.0, exploredAreaRadius); 767 | // int neibor_nodes_size = near_nodes.size(); 768 | 769 | // // For avoiding explored area 770 | // if(neibor_nodes_size < exploredAreaThre){ 771 | // Position leaf_posi((*node).first->state.x, (*node).first->state.y); 772 | // Index leaf_index; 773 | // Length leng(frontierRadius, frontierRadius); 774 | // bool isSuccessful; 775 | // auto sub_map = rtRRT->full_grid_map.getSubmap(leaf_posi, leng, leaf_index ,isSuccessful); 776 | // Size submap_size = sub_map.getSize(); 777 | // // No enough information gain will be erase 778 | // if(submap_size[0] == submap_size[1]){ 779 | // node = globalFrontier.erase(node); 780 | // continue; 781 | // } 782 | // else{ 783 | // float information_gain = util::getInfoGain((*node).first->state.x, (*node).first->state.y, rtRRT->full_grid_map, "elevation"); 784 | // node->second = information_gain; 785 | // if(information_gain < informationThre){ 786 | // node = globalFrontier.erase(node); 787 | // continue; 788 | // } 789 | // } 790 | // } 791 | // else{ 792 | // node = globalFrontier.erase(node); 793 | // continue; 794 | // } 795 | // node++; 796 | // } 797 | // mtx.unlock(); 798 | } 799 | 800 | void rtrrtp::visualizationCB(){ 801 | if(pub_selected_branch.getNumSubscribers()==0){ 802 | return; // if all topics of visualizing tree item have no subscriber, then don't publish 803 | } 804 | visualization_msgs::MarkerArray selectedBranch_marker; 805 | visualization_msgs::Marker selectedBranch_edge_marker; 806 | visualization_msgs::Marker selectedBranch_node_marker; 807 | visualization_msgs::Marker ref_line_marker; 808 | visualization_msgs::Marker goal_marker; 809 | visualization_msgs::Marker waypoint_marker; 810 | visualization_msgs::Marker target_node_marker; 811 | visualization_msgs::Marker global_frontier_marker; 812 | 813 | // Edge Marker 814 | selectedBranch_edge_marker.header.frame_id = global_frame; 815 | selectedBranch_edge_marker.header.stamp = ros::Time::now(); 816 | selectedBranch_edge_marker.type = selectedBranch_edge_marker.LINE_LIST; 817 | selectedBranch_edge_marker.action = selectedBranch_edge_marker.ADD; 818 | selectedBranch_edge_marker.lifetime = ros::Duration(); 819 | selectedBranch_edge_marker.color.a = 1.0; 820 | selectedBranch_edge_marker.color.r = 0.0 / 255.0; 821 | selectedBranch_edge_marker.color.g = 255.0 / 255.0; 822 | selectedBranch_edge_marker.color.b = 255.0 / 255.0; 823 | selectedBranch_edge_marker.scale.x = 0.1; 824 | selectedBranch_edge_marker.scale.y = 0.1; 825 | selectedBranch_edge_marker.scale.z = 0.1; 826 | selectedBranch_edge_marker.id = 0; 827 | selectedBranch_edge_marker.pose.orientation.w = 1; 828 | selectedBranch_edge_marker.points.clear(); 829 | // Node Marker 830 | selectedBranch_node_marker.header.frame_id = global_frame; 831 | selectedBranch_node_marker.header.stamp = ros::Time::now(); 832 | selectedBranch_node_marker.type = selectedBranch_node_marker.SPHERE_LIST; 833 | selectedBranch_node_marker.action = selectedBranch_node_marker.ADD; 834 | selectedBranch_node_marker.lifetime = ros::Duration(); 835 | selectedBranch_node_marker.color.a = 1.0; 836 | selectedBranch_node_marker.color.r = 255.0 / 255.0; 837 | selectedBranch_node_marker.color.g = 255.0 / 255.0; 838 | selectedBranch_node_marker.color.b = 0.0 / 255.0; 839 | selectedBranch_node_marker.scale.x = 0.2; 840 | selectedBranch_node_marker.scale.y = 0.2; 841 | selectedBranch_node_marker.scale.z = 0.2; 842 | selectedBranch_node_marker.id = 1; 843 | selectedBranch_node_marker.pose.orientation.w = 1; 844 | selectedBranch_node_marker.points.clear(); 845 | // Reference Line 846 | ref_line_marker.header.frame_id = global_frame; 847 | ref_line_marker.header.stamp = ros::Time::now(); 848 | ref_line_marker.type = ref_line_marker.LINE_LIST; 849 | ref_line_marker.action = ref_line_marker.ADD; 850 | ref_line_marker.lifetime = ros::Duration(); 851 | ref_line_marker.color.a = 1.0; 852 | ref_line_marker.color.r = 255.0 / 255.0; 853 | ref_line_marker.color.g = 0.0 / 255.0; 854 | ref_line_marker.color.b = 0.0 / 255.0; 855 | ref_line_marker.scale.x = 0.1; 856 | ref_line_marker.scale.y = 0.1; 857 | ref_line_marker.scale.z = 0.1; 858 | ref_line_marker.id = 0; 859 | ref_line_marker.pose.orientation.w = 1; 860 | ref_line_marker.points.clear(); 861 | 862 | // Goal Marker 863 | goal_marker.header.frame_id = global_frame; 864 | goal_marker.header.stamp = ros::Time::now(); 865 | goal_marker.type = goal_marker.SPHERE; 866 | goal_marker.action = goal_marker.ADD; 867 | goal_marker.lifetime = ros::Duration(); 868 | goal_marker.color.a = 1.0; 869 | goal_marker.color.r = 255.0 / 255.0; 870 | goal_marker.color.g = 0.0 / 255.0; 871 | goal_marker.color.b = 237.0 / 255.0; 872 | goal_marker.scale.x = 0.5; 873 | goal_marker.scale.y = 0.5; 874 | goal_marker.scale.z = 0.5; 875 | goal_marker.pose.orientation.w = 1; 876 | // Waypoint Marker 877 | waypoint_marker.header.frame_id = global_frame; 878 | waypoint_marker.header.stamp = ros::Time::now(); 879 | waypoint_marker.type = waypoint_marker.SPHERE; 880 | waypoint_marker.action = waypoint_marker.ADD; 881 | waypoint_marker.lifetime = ros::Duration(); 882 | waypoint_marker.color.a = 1.0; 883 | waypoint_marker.color.r = 43.0 / 255.0; 884 | waypoint_marker.color.g = 57.0 / 255.0; 885 | waypoint_marker.color.b = 255.0 / 255.0; 886 | waypoint_marker.scale.x = 0.3; 887 | waypoint_marker.scale.y = 0.3; 888 | waypoint_marker.scale.z = 0.3; 889 | waypoint_marker.pose.orientation.w = 1; 890 | 891 | // Target Marker 892 | if(target_node != NULL){ 893 | target_node_marker.header.frame_id = global_frame; 894 | target_node_marker.header.stamp = ros::Time::now(); 895 | target_node_marker.type = target_node_marker.SPHERE; 896 | target_node_marker.action = target_node_marker.ADD; 897 | target_node_marker.lifetime = ros::Duration(); 898 | target_node_marker.color.a = 1.0; 899 | target_node_marker.color.r = 0.0 / 255.0; 900 | target_node_marker.color.g = 255.0 / 255.0; 901 | target_node_marker.color.b = 0.0 / 255.0; 902 | target_node_marker.scale.x = 0.5; 903 | target_node_marker.scale.y = 0.5; 904 | target_node_marker.scale.z = 0.5; 905 | target_node_marker.pose.orientation.x = 0; 906 | target_node_marker.pose.orientation.y = 0; 907 | target_node_marker.pose.orientation.z = 0; 908 | target_node_marker.pose.orientation.w = 1; 909 | target_node_marker.pose.position.x = target_node->state.x; 910 | target_node_marker.pose.position.y = target_node->state.y; 911 | target_node_marker.pose.position.z = target_node->state.z; 912 | pub_target_marker.publish(target_node_marker); 913 | } 914 | 915 | // Global frontier Marker 916 | if(!globalFrontier.empty()){ 917 | global_frontier_marker.header.frame_id = global_frame; 918 | global_frontier_marker.header.stamp = ros::Time::now(); 919 | global_frontier_marker.type = global_frontier_marker.SPHERE_LIST; 920 | global_frontier_marker.action = global_frontier_marker.ADD; 921 | global_frontier_marker.lifetime = ros::Duration(); 922 | global_frontier_marker.color.a = 1.0; 923 | global_frontier_marker.color.r = 129.0 / 255.0; 924 | global_frontier_marker.color.g = 208.0 / 255.0; 925 | global_frontier_marker.color.b = 255.0 / 255.0; 926 | global_frontier_marker.scale.x = 0.2; 927 | global_frontier_marker.scale.y = 0.2; 928 | global_frontier_marker.scale.z = 0.2; 929 | global_frontier_marker.pose.orientation.w = 1; 930 | global_frontier_marker.points.clear(); 931 | for(auto i=globalFrontier.begin(); i!=globalFrontier.end(); i++){ 932 | geometry_msgs::Point single_point; 933 | single_point.x = i->first->state.x; 934 | single_point.y = i->first->state.y; 935 | single_point.z = i->first->state.z; 936 | global_frontier_marker.points.push_back(single_point); 937 | } 938 | pub_global_frontier.publish(global_frontier_marker); 939 | } 940 | 941 | // Add data information 942 | if(!selectedBranch_nodes.empty()){ 943 | mtx.lock(); 944 | for(auto iter=selectedBranch_nodes.begin(); iter!=selectedBranch_nodes.end()-1; iter++){ 945 | geometry_msgs::Point single_point; 946 | single_point.x = (*iter)->state.x; 947 | single_point.y = (*iter)->state.y; 948 | single_point.z = (*iter)->state.z; 949 | selectedBranch_node_marker.points.push_back(single_point); 950 | selectedBranch_edge_marker.points.push_back(single_point); 951 | single_point.x = (*(iter+1))->state.x; 952 | single_point.y = (*(iter+1))->state.y; 953 | single_point.z = (*(iter+1))->state.z; 954 | selectedBranch_edge_marker.points.push_back(single_point); 955 | } 956 | mtx.unlock(); 957 | selectedBranch_marker.markers.push_back(selectedBranch_node_marker); 958 | selectedBranch_marker.markers.push_back(selectedBranch_edge_marker); 959 | pub_selected_branch.publish(selectedBranch_marker); 960 | } 961 | 962 | if(!selectedBranch_nodes.empty() && target_node!=NULL){ 963 | float segment_length = 0.3; 964 | float disToGoal = util::distance(target_node->state.x, final_goal.pose.position.x, target_node->state.y, final_goal.pose.position.y); 965 | geometry_msgs::Point line_point; 966 | // It will connect directly if distance is short 967 | if(disToGoal <= segment_length*2){ 968 | line_point.x = target_node->state.x; 969 | line_point.y = target_node->state.y; 970 | line_point.z = target_node->state.z; 971 | ref_line_marker.points.push_back(line_point); 972 | line_point = final_goal.pose.position; 973 | ref_line_marker.points.push_back(line_point); 974 | pub_ref_line.publish(ref_line_marker); 975 | } 976 | else{ // Connect using virtual line form 977 | float Theta = atan2(final_goal.pose.position.y-target_node->state.y, final_goal.pose.position.x-target_node->state.x); 978 | float segment_deltX = segment_length*cos(Theta); 979 | float segment_deltY = segment_length*sin(Theta); 980 | float PointX = target_node->state.x; 981 | float PointY = target_node->state.y; 982 | float PointZ = target_node->state.z; 983 | float last_disToEnd = inf; 984 | float curr_disToEnd = util::distance(PointX, final_goal.pose.position.x, PointY, final_goal.pose.position.y); 985 | while(curr_disToEnd <= last_disToEnd){ 986 | last_disToEnd = curr_disToEnd; 987 | line_point.x = PointX; 988 | line_point.y = PointY; 989 | line_point.z = PointZ; 990 | ref_line_marker.points.push_back(line_point); 991 | PointX += segment_deltX; 992 | PointY += segment_deltY; 993 | curr_disToEnd = util::distance(PointX, final_goal.pose.position.x, PointY, final_goal.pose.position.y); 994 | } 995 | if(ref_line_marker.points.size()%2 != 0){ 996 | ref_line_marker.points.push_back(*(ref_line_marker.points.end()-1)); 997 | } 998 | pub_ref_line.publish(ref_line_marker); 999 | } 1000 | 1001 | } 1002 | 1003 | goal_marker.pose = final_goal.pose; 1004 | pub_goal_marker.publish(goal_marker); 1005 | waypoint_marker.pose.position = way_point.point; 1006 | pub_waypoint_marker.publish(waypoint_marker); 1007 | } 1008 | 1009 | void rtrrtp::updateRobotPoseCB(const geometry_msgs::PoseWithCovarianceStampedConstPtr &new_pose){ 1010 | tf::StampedTransform transform; 1011 | tf::poseMsgToTF(new_pose->pose.pose, transform); 1012 | double roll, pitch, yaw; 1013 | tf::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw); 1014 | robot_pose.x = transform.getOrigin().x(); 1015 | robot_pose.y = transform.getOrigin().y(); 1016 | robot_pose.z = transform.getOrigin().z()-0.11; 1017 | robot_pose.roll = roll; 1018 | robot_pose.pitch = pitch; 1019 | robot_pose.yaw = yaw; 1020 | updatedPose = true; 1021 | 1022 | // visuliazation 1023 | auto cur_time = ros::Time::now(); 1024 | if((cur_time-last_time).toSec()>0.5){ 1025 | visualizationCB(); 1026 | } 1027 | } 1028 | 1029 | void rtrrtp::goalHandler(const geometry_msgs::PoseStampedConstPtr &goal_point){ 1030 | if(!updatedPose) 1031 | { 1032 | ROS_WARN("Cannot get the robot pose."); 1033 | return; 1034 | } 1035 | final_goal.pose.position.x = goal_point->pose.position.x; 1036 | final_goal.pose.position.y = goal_point->pose.position.y; 1037 | final_goal.pose.position.z = goal_point->pose.position.z+0.5; 1038 | final_goal.pose.orientation = goal_point->pose.orientation; 1039 | // rtRRT->updateGoal(final_goal); 1040 | rtRRT->goal.position.x = goal_point->pose.position.x; 1041 | rtRRT->goal.position.y = goal_point->pose.position.y; 1042 | rtRRT->goal.position.z = goal_point->pose.position.z; 1043 | rtRRT->goal.orientation = goal_point->pose.orientation; 1044 | 1045 | printf("\033[2J\033[1;1H"); // Clear all print msg 1046 | std::cout << bold << magenta 1047 | << "Received Goal: " << rtRRT->goal.position.x << ", " << rtRRT->goal.position.y 1048 | << reset << std::endl; 1049 | if(cur_state == PlannerState::ACTIVE){ 1050 | mtx.lock(); 1051 | target_node = NULL; 1052 | useSubGoal = false; 1053 | useFinalGoal = false; 1054 | cur_state = PlannerState::WAITING; // Set flag to stop 1055 | mtx.unlock(); 1056 | stayCurrentPose(); // Stop locomotion 1057 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Wait for ending executeCycle 1058 | } 1059 | 1060 | rtRRT->newGoalReceived = true; 1061 | newGoalReceived = true; 1062 | 1063 | std::cout << green 1064 | << "Start path planning" 1065 | << reset << std::endl; 1066 | cur_state = PlannerState::ACTIVE; // Active planner 1067 | } 1068 | 1069 | 1070 | -------------------------------------------------------------------------------- /planner/src/rtrrt.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "sea_planner/rtrrt.h" 6 | 7 | using namespace rtrrt_ns; 8 | using namespace termcolor; 9 | 10 | void rtrrt::init(){ 11 | root.reset(); 12 | current_target = NULL; 13 | current_target_node.reset(); 14 | node_set.clear(); 15 | edge_set.clear(); 16 | leaf_nodes.clear(); 17 | tree.clear(); 18 | global_frame = "map"; 19 | base_frame = "base_link"; 20 | receivedGridMap = false; 21 | updatedPose = false; 22 | newGoalReceived = false; 23 | robot_pose = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 24 | // Tree initialization 25 | // Kdtree Init 26 | kdtree_pts.reset(new KDTreePt::PointCloud()); 27 | kdtree_.reset(new nanoKDTree(3, 10, kdtree_pts)); 28 | obstacle_pts.reset(new KDTreePt::PointCloud()); 29 | obstacleKdtree_.reset(new nanoKDTree(3, 10, obstacle_pts)); 30 | pcl_kdtree.reset(new pcl::KdTreeFLANN()); 31 | 32 | // pcl cloud init 33 | graphVertices.reset(new pcl::PointCloud()); 34 | treeNodes.reset(new pcl::PointCloud()); 35 | 36 | // Publisher init 37 | pub_rtrrt_edge = nh.advertise("/rtrrt_edges", 10); 38 | pub_rtrrt_node = nh.advertise("/rtrrt_nodes", 10); 39 | pub_removed_edge = nh.advertise("/rtrrt_last_removed_edges", 10); 40 | pub_removed_node = nh.advertise("/rtrrt_last_removed_nodes", 10); 41 | pub_root_marker = nh.advertise("/rtrrt_root", 10); 42 | pub_leaf_node = nh.advertise("/leaf_nodes", 10); 43 | pub_frontier_node = nh.advertise("/frontier_nodes", 10); 44 | pub_path = nh.advertise("/rtrrt_path",10); 45 | pub_goal_marker = nh.advertise("/goal_marker",10); 46 | pub_subgoal_marker = nh.advertise("/subgoal_marker",10); 47 | pub_range_map = nh.advertise("range_grid_map",10); 48 | pub_sample_node = nh.advertise("sample_node", 10); 49 | pub_obstacle_node = nh.advertise("obstacle_node", 10); 50 | treeNodesPub = nh.advertise("tree_nodes", 10); 51 | // Subscriber init 52 | sub_grid_map = nh.subscribe("/elevation_mapping/elevation_map", 1, &rtrrt::gridMapHandler, this); 53 | sub_graph_vertice = nh.subscribe("graph_vertices", 1, &rtrrt::graphVerticesHandler, this); 54 | sub_robot_pose = nh.subscribe("base_link_pose", 1, &rtrrt::updateRobotPose, this); 55 | // TimerVis = nh.createTimer(ros::Duration(0.2), &rtrrt::visualizationCb, this); 56 | last_time = ros::Time::now(); 57 | } 58 | 59 | rtrrt::rtrrt(){ 60 | init(); 61 | ROS_INFO("RTRRT initialization accomplished"); 62 | } 63 | 64 | rtrrt::~rtrrt(){ 65 | clearAll(); 66 | } 67 | 68 | void rtrrt::clearAll(){ 69 | root = NULL; 70 | id_nodes.clear(); 71 | leaf_nodes.clear(); 72 | node_set.clear(); 73 | edge_set.clear(); 74 | tree.clear(); 75 | kdtree_.reset(new nanoKDTree(3, 10, kdtree_pts)); 76 | } 77 | 78 | rtrrt_ns::Node rtrrt::sample(){ 79 | rtrrt_ns::Node sample_node; 80 | sample_node.state.z = 0.0; 81 | bool pass_flag = false; // Avoid node is too close with others 82 | while(!pass_flag){ 83 | float rand_num = (((double)rand()) / ((double)RAND_MAX)); 84 | // std::cout << "rand_num: " << rand_num << std::endl; 85 | if (rand_num > 1 - alpha && current_target_node != NULL) 86 | { 87 | // Sample between robot and current goal. 88 | double x = (((double)rand()) / ((double)RAND_MAX))*(root->state.x - current_target_node->state.x) + root->state.x; 89 | double y = (((double)rand()) / ((double)RAND_MAX))*(root->state.y - current_target_node->state.y) + root->state.y; 90 | 91 | sample_node.state.x = x; 92 | sample_node.state.y = y; 93 | } 94 | else if (rand_num >= (1 - alpha) / beta && current_target_node != NULL) 95 | { 96 | // Using informed rrt method 97 | float major_axis = path_length; 98 | float focal_length = util::distance(robot_pose.x, current_target_node->state.x, robot_pose.y, current_target_node->state.y); 99 | Eigen::Vector2d ellipse_centre; 100 | ellipse_centre[0] = (robot_pose.x+current_target_node->state.x)/2; 101 | ellipse_centre[1] = (robot_pose.y+current_target_node->state.y)/2; 102 | 103 | float angle = std::atan2(-(current_target_node->state.y-robot_pose.y),current_target_node->state.x-robot_pose.x); //Frame is with y pointing downwards 104 | 105 | float r1 = major_axis / 2; 106 | float r2 = sqrt(SQ(major_axis) - SQ(focal_length)) / 2; 107 | 108 | float x = (((double)rand()) / ((double)RAND_MAX)); 109 | float y = (((double)rand()) / ((double)RAND_MAX)); 110 | 111 | float x2 = x * r1 * std::cos(angle) + y * r2 * std::sin(angle); 112 | float y2 = -x * r1 * std::sin(angle) + y * r2 * std::cos(angle); 113 | 114 | Eigen::Vector2d rot_sample, rot_trans_sample; 115 | rot_sample << x2, y2; 116 | rot_trans_sample = rot_sample + ellipse_centre; //! rotation first, then locomotion 117 | 118 | sample_node.state.x = rot_trans_sample[0]; 119 | sample_node.state.y = rot_trans_sample[1]; 120 | } 121 | else if (rand_num <= ((1 - alpha) / beta * Gamma)){ 122 | // Using sensor range sample method in grid map 123 | float local_x = map_x*sqrt(2)/2 *(((double)rand()) / ((double)RAND_MAX)); 124 | float local_y = local_x * TAN60 * (((double)rand()) / ((double)RAND_MAX) - 0.5)*2; 125 | // Transform Cartesian coordinates to polar coordinates 126 | float r = sqrt(SQ(local_x)+SQ(local_y)); 127 | float theta = atan2(local_y, local_x); 128 | theta += robot_pose.yaw; 129 | // Transform Polar coordinates to Cartesian coordinates 130 | local_x = r * cos(theta); 131 | local_y = r * sin(theta); 132 | // Transform frame : global -> base 133 | sample_node.state.x = local_x + robot_pose.x; 134 | sample_node.state.y = local_y + robot_pose.y; 135 | visualizeSample(sample_node); 136 | } 137 | else{ 138 | // Using random sample method in the whole map 139 | float x = map_x*(((double)rand()) / ((double)RAND_MAX) - 0.5); 140 | float y = map_y*(((double)rand()) / ((double)RAND_MAX) - 0.5); 141 | sample_node.state.x = x + robot_pose.x; 142 | sample_node.state.y = y + robot_pose.y; 143 | // visualizeSample(sample_node); 144 | } 145 | // Update flag 146 | Nodes neibor_nodes = getNeiborNodes(sample_node.state.x, sample_node.state.y, 0.0, minNodeDistance); 147 | if(neibor_nodes.empty()){ 148 | // No any too close node 149 | pass_flag = true; 150 | } 151 | } 152 | return sample_node; 153 | 154 | } 155 | 156 | void rtrrt::visualizeSample(rtrrt_ns::Node sample_node){ 157 | visualization_msgs::Marker sample_node_marker; 158 | sample_node_marker.header.frame_id = global_frame; 159 | sample_node_marker.header.stamp = ros::Time::now(); 160 | sample_node_marker.type = sample_node_marker.SPHERE; 161 | sample_node_marker.action = sample_node_marker.ADD; 162 | sample_node_marker.lifetime = ros::Duration(); 163 | sample_node_marker.color.a = 1.0; 164 | sample_node_marker.color.r = 255.0 / 255.0; 165 | sample_node_marker.color.g = 199.0 / 255.0; 166 | sample_node_marker.color.b = 65.0 / 255.0; 167 | sample_node_marker.scale.x = 0.5; 168 | sample_node_marker.scale.y = 0.5; 169 | sample_node_marker.scale.z = 0.5; 170 | sample_node_marker.pose.orientation.x = 0; 171 | sample_node_marker.pose.orientation.y = 0; 172 | sample_node_marker.pose.orientation.z = 0; 173 | sample_node_marker.pose.orientation.w = 1; 174 | sample_node_marker.points.clear(); 175 | sample_node_marker.pose.position.x = sample_node.state.x; 176 | sample_node_marker.pose.position.y = sample_node.state.y; 177 | sample_node_marker.pose.position.z = 1.0; 178 | pub_sample_node.publish(sample_node_marker); 179 | } 180 | 181 | rtrrt_ns::Node rtrrt::steer(rtrrt_ns::Node x_s){ 182 | rtrrt_ns::Node x_new; 183 | // Find the nearest node using kd-tree 184 | rtrrt_ns::Node::Ptr nearestNode = getNearestNode(x_s.state.x, x_s.state.y); 185 | if(nearestNode == NULL){ 186 | return x_new; 187 | } 188 | float dis = distance(x_s.state.x, nearestNode->state.x, x_s.state.y, nearestNode->state.y); 189 | if(dis >= epsilon){ 190 | x_new.state.x = nearestNode->state.x+(x_s.state.x-nearestNode->state.x) * epsilon/dis; 191 | x_new.state.y = nearestNode->state.y+(x_s.state.y-nearestNode->state.y) * epsilon/dis; 192 | x_new.state.z = 0.0; 193 | return x_new; 194 | } 195 | else{ 196 | return x_s; 197 | } 198 | } 199 | 200 | void rtrrt::updateRobotPose(const geometry_msgs::PoseWithCovarianceStampedConstPtr &new_pose) 201 | { 202 | tf::StampedTransform transform; 203 | tf::poseMsgToTF(new_pose->pose.pose, transform); 204 | double roll, pitch, yaw; 205 | tf::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw); 206 | robot_pose.x = transform.getOrigin().x(); 207 | robot_pose.y = transform.getOrigin().y(); 208 | robot_pose.z = transform.getOrigin().z()-0.11; 209 | robot_pose.roll = roll; 210 | robot_pose.pitch = pitch; 211 | robot_pose.yaw = yaw; 212 | updatedPose = true; 213 | 214 | // tf::StampedTransform transform; 215 | // tf::TransformListener listener; 216 | // try 217 | // { 218 | // listener.waitForTransform("map","base_link",ros::Time::now(),ros::Duration(0.5)); 219 | // listener.lookupTransform("map","base_link",ros::Time(0),transform); 220 | // double roll, pitch, yaw; 221 | // tf::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw); 222 | // robot_pose.x = transform.getOrigin().x(); 223 | // robot_pose.y = transform.getOrigin().y(); 224 | // robot_pose.z = transform.getOrigin().z()-0.11; 225 | // robot_pose.roll = roll; 226 | // robot_pose.pitch = pitch; 227 | // robot_pose.yaw = yaw; 228 | // updatedPose = true; 229 | // } 230 | // catch(const std::exception& e) 231 | // { 232 | // std::cerr << e.what() << '\n'; 233 | // } 234 | } 235 | 236 | bool rtrrt::checkNewNode(rtrrt_ns::Node x_new){ 237 | Position node_state{x_new.state.x, x_new.state.y}; 238 | if(!full_grid_map.isInside(node_state)){ 239 | return false; 240 | } 241 | float elevation_value = getElevation(x_new.state.x, x_new.state.y, full_grid_map); 242 | if(isnan(elevation_value)){ 243 | return false; 244 | } 245 | return true; 246 | } 247 | 248 | bool rtrrt::checkEdge(rtrrt_ns::Edge edge){ 249 | if(fabs(edge.gradient) > gradientDiffThre){ 250 | return false; 251 | } 252 | else return true; 253 | } 254 | 255 | bool rtrrt::addNodeEdge(rtrrt_ns::Node x_new, rtrrt_ns::Node::Ptr x_parent){ 256 | rtrrt_ns::Node::Ptr new_node = make_shared(); 257 | bool outcome = false; // Whether or not add node-edge successfully 258 | if(checkNewNode(x_new)){ 259 | float dis_new_parent = distance(x_new.state.x, x_parent->state.x, x_new.state.y, x_parent->state.y); 260 | 261 | // Check neibor nodes 262 | Nodes neibor_nodes = getNeiborNodes(x_new.state.x, x_new.state.y, 0.0, minNodeDistance); 263 | if(!neibor_nodes.empty()){ 264 | //? delete new_node; 265 | new_node = NULL; 266 | return false; 267 | } 268 | 269 | x_new.state.z = getElevation(x_new.state.x, x_new.state.y, full_grid_map)+0.1; 270 | rtrrt_ns::Edge::Ptr parent_to_new = make_shared(); 271 | parent_to_new->elevation_diff = x_new.state.z - x_parent->state.z; 272 | parent_to_new->gradient = util::gradient(dis_new_parent, parent_to_new->elevation_diff); 273 | 274 | if(checkEdge(*parent_to_new)){ 275 | // Add node and edge to Tree 276 | new_node->state = x_new.state; 277 | new_node->children.clear(); 278 | new_node->disToRoot = dis_new_parent+x_parent->disToRoot; 279 | new_node->gradientToRoot = parent_to_new->gradient + x_parent->gradientToRoot; 280 | new_node->parent = x_parent; 281 | new_node->prevParent = NULL; 282 | new_node->nodeState = NodeState::LEAF; 283 | 284 | // Add to PCL pointcloud 285 | new_node->pcl_pt.x = new_node->state.x; 286 | new_node->pcl_pt.y = new_node->state.y; 287 | new_node->pcl_pt.z = 0.0; 288 | new_node->pcl_pt.intensity = 0.0; 289 | treeNodes->points.push_back(new_node->pcl_pt); 290 | 291 | // Add to node set 292 | node_set.push_back(new_node); 293 | parent_to_new->fromNode = x_parent; 294 | parent_to_new->toNode = new_node; 295 | parent_to_new->length = dis_new_parent; 296 | // Add to edge set 297 | edge_set.push_back(parent_to_new); 298 | new_node->endEdge = edge_set.back(); 299 | // Add to tree 300 | KDTreePt::Point pt(new_node->state.x, new_node->state.y, 0.0); 301 | kdtree_pts->points.push_back(pt); 302 | kdtree_->AddPoint(kdtree_pts->points.size()-1, kdtree_pts->points.size()-1); 303 | id_nodes.insert(make_pair(kdtree_pts->points.size()-1, new_node)); 304 | new_node->node_id = kdtree_pts->points.size()-1; 305 | NodePair new_parent_pair{x_parent, new_node}; 306 | tree.insert(make_pair(parent_to_new, new_parent_pair)); 307 | leaf_nodes.push_back(new_node); 308 | 309 | x_parent->startEdges.push_back(parent_to_new); 310 | x_parent->children.push_back(new_node); 311 | if(x_parent->nodeState == NodeState::LEAF){ 312 | x_parent->nodeState = NodeState::NORMAL; 313 | leaf_nodes.remove(x_parent); 314 | } 315 | // Get node penalty factor 316 | 317 | outcome = true; 318 | } 319 | else { 320 | //? delete parent_to_new; 321 | //? delete new_node; 322 | updateOrientationArray(x_new, x_parent); 323 | if(util::getOrientation(x_parent->obstacle_orientation) > obstacleSectorNumThre){ 324 | auto find_result = find(obstacle_node_set.begin(), obstacle_node_set.end(), x_parent); 325 | if(find_result == obstacle_node_set.end() && x_parent->nodeState == NodeState::LEAF){ // No found 326 | // Delete all nodes which are relevant with x_parent 327 | // Delete x_parent's parent connection 328 | if(x_parent->nodeState != NodeState::ROOT){ 329 | x_parent->parent->startEdges.remove(x_parent->endEdge); 330 | x_parent->parent->children.remove(x_parent); 331 | if(x_parent->parent->children.size() == 0 && 332 | x_parent->parent->nodeState!=NodeState::ROOT && 333 | x_parent->parent->nodeState!=NodeState::LEAF){ 334 | 335 | x_parent->parent->nodeState = NodeState::LEAF; 336 | leaf_nodes.push_back(x_parent->parent); 337 | } 338 | x_parent->parent = NULL; 339 | x_parent->prevParent = NULL; 340 | // Cost value is close to Inf for avoid obstacle 341 | x_parent->disToRoot = inf; 342 | x_parent->gradientToRoot = inf; 343 | x_parent->penaltyFactor = maxPenalty; 344 | } 345 | node_set.remove(x_parent); 346 | edge_set.remove(x_parent->endEdge); 347 | tree.erase(x_parent->endEdge); 348 | x_parent->endEdge = NULL; 349 | leaf_nodes.remove(x_parent); 350 | x_parent->nodeState = NodeState::OBSTACLE; 351 | // Remove from Kdtree 352 | kdtree_->RemovePoint(x_parent->node_id); 353 | id_nodes.erase(x_parent->node_id); 354 | 355 | obstacle_node_set.push_back(x_parent); 356 | // Add to obstacle tree 357 | KDTreePt::Point pt(x_parent->state.x, x_parent->state.y, 0.0); 358 | obstacle_pts->points.push_back(pt); 359 | obstacleKdtree_->AddPoint(obstacle_pts->points.size()-1, obstacle_pts->points.size()-1); 360 | obs_id_nodes.insert(make_pair(obstacle_pts->points.size()-1, x_parent)); 361 | x_parent->node_id = obstacle_pts->points.size()-1; 362 | 363 | // Update neibor nodes property 364 | Nodes near_nodes = getNeiborNodes(x_parent->state.x, x_parent->state.y, 0.0, inflationRadius); 365 | if(!near_nodes.empty()){ 366 | auto iter = near_nodes.begin(); 367 | while(iter!=near_nodes.end()){ 368 | rtrrt_ns::Node::Ptr node = *iter; 369 | // Except OBSTACLE nodes 370 | if(node->nodeState == NodeState::OBSTACLE || node->nodeState == NodeState::ROOT){ 371 | iter++; 372 | continue; 373 | } 374 | float disToObs = util::distance(node->state.x, x_parent->state.x, node->state.y, x_parent->state.y); 375 | // Update penalty factor 376 | float new_penalty = util::getPenalty(disToObs); 377 | if(new_penalty < node->penaltyFactor){ 378 | iter++; 379 | continue; 380 | } 381 | node->penaltyFactor = util::getPenalty(disToObs); 382 | queue q_update; 383 | q_update.push(node); 384 | while(!q_update.empty()){ 385 | rtrrt_ns::Node::Ptr n = q_update.front(); 386 | q_update.pop(); 387 | n->disToRoot = n->parent->disToRoot + n->endEdge->length*(n->penaltyFactor); 388 | n->gradientToRoot = n->parent->gradientToRoot + n->endEdge->gradient*(n->penaltyFactor); 389 | if(n->nodeState != NodeState::LEAF){ 390 | for(auto it=n->children.begin(); it!=n->children.end();it++){ 391 | q_update.push((*it)); 392 | } 393 | } 394 | } 395 | iter++; 396 | } 397 | } 398 | } 399 | } 400 | } 401 | } 402 | else new_node = NULL; 403 | return outcome; 404 | } 405 | 406 | void rtrrt::reselectParent(rtrrt_ns::Node::Ptr x_new){ 407 | 408 | float dis_min = x_new->disToRoot; 409 | rtrrt_ns::Node::Ptr best_neibor = NULL; 410 | rtrrt_ns::Edge::Ptr best_neibo_edge = make_shared(); 411 | // Find near nodes using kd-tree within a range 412 | Nodes neibor_nodes = getNeiborNodes(x_new->state.x, x_new->state.y, 0.0, neiborRangeThre); 413 | if(neibor_nodes.size()<=1){ 414 | // If none or only one node, no reselection 415 | best_neibo_edge = NULL; 416 | return; 417 | } 418 | else{ 419 | // Iterate all neibor nodes to find shortest branch 420 | auto iter = neibor_nodes.begin(); 421 | while(iter != neibor_nodes.end()){ 422 | rtrrt_ns::Node::Ptr neibor = *iter; 423 | // Except OBSTACLE nodes 424 | if(neibor->nodeState == NodeState::OBSTACLE){ 425 | iter++; 426 | continue; 427 | } 428 | rtrrt_ns::Edge neibor_new_edge; 429 | neibor_new_edge.fromNode = neibor; 430 | neibor_new_edge.toNode = x_new; 431 | neibor_new_edge.elevation_diff = x_new->state.z - neibor->state.z; 432 | neibor_new_edge.length = distance(x_new->state.x, neibor->state.x, x_new->state.y, neibor->state.y); 433 | neibor_new_edge.gradient = util::gradient(neibor_new_edge.length, neibor_new_edge.elevation_diff); 434 | if(checkEdge(neibor_new_edge)){ 435 | float dis = neibor->disToRoot + neibor_new_edge.length*x_new->penaltyFactor; 436 | if(dis < dis_min){ 437 | // Save best node info 438 | dis_min = dis; 439 | best_neibor = neibor; 440 | best_neibo_edge->fromNode = neibor_new_edge.fromNode; 441 | best_neibo_edge->toNode = neibor_new_edge.toNode; 442 | best_neibo_edge->elevation_diff = neibor_new_edge.elevation_diff; 443 | best_neibo_edge->gradient = neibor_new_edge.gradient; 444 | best_neibo_edge->length = neibor_new_edge.length; 445 | } 446 | } 447 | iter++; 448 | } 449 | // kd_res_free(neibor_nodes); 450 | if(x_new->parent != best_neibor && best_neibor != NULL){ 451 | // Change current parent node, select neibor node as parent 452 | // For old parent, remove items 453 | x_new->prevParent = x_new->parent; 454 | x_new->parent->startEdges.remove(x_new->endEdge); 455 | x_new->parent->children.remove(x_new); 456 | if(x_new->parent->nodeState!=NodeState::LEAF && 457 | x_new->parent->nodeState!=NodeState::ROOT && 458 | x_new->parent->children.size() == 0){ 459 | x_new->parent->nodeState = NodeState::LEAF; 460 | leaf_nodes.push_back(x_new->parent); 461 | } 462 | // Delete specific edge 463 | edge_set.remove(x_new->endEdge); 464 | tree.erase(x_new->endEdge); 465 | // For new parent, add new item 466 | x_new->parent = best_neibor; 467 | x_new->parent->children.push_back(x_new); 468 | if(x_new->parent->nodeState != NodeState::ROOT){ 469 | if(x_new->parent->nodeState == LEAF){ 470 | leaf_nodes.remove(x_new->parent); 471 | } 472 | x_new->parent->nodeState = NodeState::NORMAL; 473 | } 474 | edge_set.push_back(best_neibo_edge); 475 | x_new->endEdge = best_neibo_edge; 476 | x_new->disToRoot = x_new->parent->disToRoot + best_neibo_edge->length*x_new->penaltyFactor; 477 | x_new->gradientToRoot = x_new->parent->gradientToRoot+best_neibo_edge->gradient*x_new->penaltyFactor; 478 | NodePair new_parent_pair{best_neibor, x_new}; 479 | tree.insert(make_pair(best_neibo_edge, new_parent_pair)); 480 | } 481 | else best_neibo_edge = NULL; 482 | } 483 | } 484 | 485 | void rtrrt::rewireRandomNode(){ 486 | float timeKeeper = ros::Time::now().toSec(); 487 | while(q_rewire_from_rand.size()!=0 && (ros::Time::now().toSec() - timeKeeper) < allowRewiringFromRand){ 488 | // Take the front of RTRRTstarrewireRand to rewire 489 | Node::Ptr x_rand = q_rewire_from_rand.front(); 490 | q_rewire_from_rand.pop(); 491 | // Find near nodes using kd-tree within a range 492 | Nodes neibor_nodes = getNeiborNodes(x_rand->state.x, x_rand->state.y, 0.0, neiborRangeThre); 493 | if(neibor_nodes.size()<=1) 494 | { 495 | continue; 496 | } 497 | else{ 498 | // Iterate all neibor nodes 499 | auto iter = neibor_nodes.begin(); 500 | while(iter != neibor_nodes.end()){ 501 | rtrrt_ns::Node::Ptr neibor = *iter; 502 | // Except OBSTACLE nodes 503 | if(neibor->nodeState == NodeState::OBSTACLE){ 504 | iter++; 505 | continue; 506 | } 507 | rtrrt_ns::Edge::Ptr rand_neibor_edge = make_shared(); 508 | rand_neibor_edge->fromNode = x_rand; 509 | rand_neibor_edge->toNode = neibor; 510 | rand_neibor_edge->elevation_diff = neibor->state.z - x_rand->state.z; 511 | rand_neibor_edge->length = distance(neibor->state.x, x_rand->state.x, neibor->state.y, x_rand->state.y); 512 | rand_neibor_edge->gradient = util::gradient(rand_neibor_edge->length, rand_neibor_edge->elevation_diff); 513 | if(checkEdge(*rand_neibor_edge)){ 514 | float dis = x_rand->disToRoot + rand_neibor_edge->length*neibor->penaltyFactor; 515 | if(dis < neibor->disToRoot && neibor->parent != x_rand){ 516 | // X_rand become neibor parent 517 | // For old parent, remove item 518 | neibor->parent->children.remove(neibor); 519 | if(neibor->parent->children.size() ==0 && neibor->parent->nodeState!= NodeState::LEAF){ 520 | neibor->parent->nodeState= NodeState::LEAF; 521 | leaf_nodes.push_back(neibor->parent); 522 | } 523 | neibor->parent->startEdges.remove(neibor->endEdge); 524 | // Delete specific edge 525 | edge_set.remove(neibor->endEdge); 526 | tree.erase(neibor->endEdge); 527 | // For new parent(x_rand), update item 528 | edge_set.push_back(rand_neibor_edge); 529 | neibor->prevParent = neibor->parent; 530 | neibor->parent = x_rand; 531 | neibor->disToRoot = dis; 532 | neibor->gradientToRoot = x_rand->gradientToRoot+rand_neibor_edge->gradient*neibor->penaltyFactor; 533 | neibor->endEdge = rand_neibor_edge; 534 | x_rand->children.push_back(neibor); 535 | x_rand->startEdges.push_back(rand_neibor_edge); 536 | if(x_rand->nodeState == NodeState::LEAF){ 537 | x_rand->nodeState = NodeState::NORMAL; 538 | leaf_nodes.remove(x_rand); 539 | } 540 | NodePair rand_neibor_pair{x_rand, neibor}; 541 | tree.insert(make_pair(rand_neibor_edge, rand_neibor_pair)); 542 | } 543 | else rand_neibor_edge = NULL; 544 | } 545 | else rand_neibor_edge = NULL; 546 | iter++; 547 | } 548 | } 549 | } 550 | } 551 | 552 | void rtrrt::rewireFromRoot(){ 553 | unordered_set close_node_set; 554 | // Empty queue will start at root node 555 | if (q_rewire_from_root.empty()){ 556 | q_rewire_from_root.push(root); 557 | } 558 | float timeKeeper = ros::Time::now().toSec(); 559 | while(!q_rewire_from_root.empty() && (ros::Time::now().toSec() - timeKeeper) < allowRewiringFromRoot){ 560 | // Take the front of RTRRTstarrewireRand to rewire 561 | Node::Ptr x_r = q_rewire_from_root.front(); 562 | q_rewire_from_root.pop(); 563 | // Find near nodes using kd-tree within a range 564 | Nodes neibor_nodes = getNeiborNodes(x_r->state.x, x_r->state.y, 0.0, neiborRangeThre); 565 | if(neibor_nodes.size()<=1) 566 | { 567 | continue; 568 | } 569 | else{ 570 | // Iterate all neibor nodes 571 | auto iter = neibor_nodes.begin(); 572 | while(iter != neibor_nodes.end()){ 573 | rtrrt_ns::Node::Ptr neibor = *iter; 574 | // Avoid residual edge-node 575 | if(neibor->parent == NULL && neibor->nodeState!=NodeState::ROOT){ 576 | queue q_remove; 577 | q_remove.push(neibor); 578 | while(!q_remove.empty()){ 579 | // Get top element 580 | rtrrt_ns::Node::Ptr node = q_remove.front(); 581 | q_remove.pop(); 582 | // Delete all parent items 583 | if(node->parent != NULL){ 584 | node->parent->children.remove(node); 585 | node->parent->startEdges.remove(node->endEdge); 586 | } 587 | // Add children to queue 588 | for(auto it_n=node->children.begin(); it_n!=node->children.end(); it_n++){ 589 | q_remove.push(*it_n); 590 | } 591 | 592 | // Remove from node set and edge set 593 | node_set.remove(node); 594 | edge_set.remove(node->endEdge); 595 | tree.erase(node->endEdge); 596 | 597 | // Remove from leaf set 598 | if(node->nodeState == NodeState::LEAF){ 599 | leaf_nodes.remove(node); 600 | } 601 | // Remove from kdtree 602 | kdtree_->RemovePoint(node->node_id); 603 | id_nodes.erase(node->node_id); 604 | } 605 | 606 | iter++; 607 | continue; 608 | } 609 | // Except OBSTACLE nodes 610 | if(neibor->nodeState == NodeState::OBSTACLE){ 611 | iter++; 612 | continue; 613 | } 614 | rtrrt_ns::Edge::Ptr xr_neibor_edge = make_shared(); 615 | xr_neibor_edge->fromNode = x_r; 616 | xr_neibor_edge->toNode = neibor; 617 | xr_neibor_edge->elevation_diff = neibor->state.z - x_r->state.z; 618 | xr_neibor_edge->length = distance(neibor->state.x, x_r->state.x, neibor->state.y, x_r->state.y); 619 | xr_neibor_edge->gradient = util::gradient(xr_neibor_edge->length, xr_neibor_edge->elevation_diff); 620 | if(checkEdge(*xr_neibor_edge)){ 621 | float dis = x_r->disToRoot + xr_neibor_edge->length*neibor->penaltyFactor; 622 | if(dis < neibor->disToRoot && neibor->parent != x_r){ 623 | // X_r become neibor new parent 624 | // For old parent, remove item 625 | neibor->parent->children.remove(neibor); 626 | if(neibor->parent->children.size() == 0 && neibor->parent->nodeState!= NodeState::LEAF){ 627 | neibor->parent->nodeState= NodeState::LEAF; 628 | leaf_nodes.push_back(neibor->parent); 629 | } 630 | neibor->parent->startEdges.remove(neibor->endEdge); 631 | // Delete specific edge 632 | edge_set.remove(neibor->endEdge); 633 | tree.erase(neibor->endEdge); 634 | // For new parent(x_r), update item 635 | edge_set.push_back(xr_neibor_edge); 636 | neibor->prevParent = neibor->parent; 637 | neibor->parent = x_r; 638 | neibor->disToRoot = dis; 639 | neibor->gradientToRoot = x_r->gradientToRoot+xr_neibor_edge->gradient*neibor->penaltyFactor; 640 | neibor->endEdge = xr_neibor_edge; 641 | x_r->children.push_back(neibor); 642 | x_r->startEdges.push_back(xr_neibor_edge); 643 | if(x_r->nodeState == NodeState::LEAF){ 644 | x_r->nodeState = NodeState::NORMAL; 645 | leaf_nodes.remove(x_r); 646 | } 647 | NodePair rand_neibor_pair{x_r, neibor}; 648 | tree.insert(make_pair(xr_neibor_edge, rand_neibor_pair)); 649 | } 650 | else xr_neibor_edge = NULL; 651 | } 652 | else xr_neibor_edge = NULL; 653 | if(close_node_set.find(neibor) == close_node_set.end()){ // Check in close-set, if found, no longer add to queue 654 | q_rewire_from_root.push(neibor); 655 | close_node_set.insert(neibor); 656 | } 657 | iter++; 658 | } 659 | } 660 | } 661 | } 662 | 663 | void rtrrt::changeRoot(rtrrt_ns::Node::Ptr new_root){ 664 | if(new_root == root){ 665 | // Avoid change root to repeat node 666 | return; 667 | } 668 | ROS_INFO("New root: %f, %f, %f", new_root->state.x, new_root->state.y, new_root->state.z); 669 | if(new_root->parent == root){ 670 | root->startEdges.remove(new_root->endEdge); 671 | edge_set.remove(new_root->endEdge); 672 | tree.erase(new_root->endEdge); 673 | } 674 | rtrrt_ns::Edge::Ptr new_root_edge = make_shared(); 675 | new_root_edge->fromNode = new_root; 676 | new_root_edge->toNode = root; 677 | new_root_edge->elevation_diff = root->state.z - new_root->state.z; 678 | new_root_edge->length = distance(root->state.x, new_root->state.x, root->state.y, new_root->state.y); 679 | new_root_edge->gradient = util::gradient(new_root_edge->length, new_root_edge->elevation_diff); 680 | //! New root must connect with old root, therfore no need to check edge, ortherwise algorithm will fail 681 | // Update old root item 682 | root->children.remove(new_root); 683 | // Update old root children's disToRoot and gradientToRoot 684 | queue q_old_root_children; 685 | q_old_root_children.push(root); 686 | while(!q_old_root_children.empty()){ 687 | rtrrt_ns::Node::Ptr old_child = q_old_root_children.front(); 688 | q_old_root_children.pop(); 689 | if(old_child->children.empty()) continue; 690 | for(auto it = old_child->children.begin(); it!=old_child->children.end(); it++){ 691 | q_old_root_children.push(*it); 692 | (*it)->disToRoot += new_root_edge->length*root->penaltyFactor; 693 | (*it)->gradientToRoot += new_root_edge->gradient*root->penaltyFactor; 694 | } 695 | } 696 | root->disToRoot = new_root_edge->length*root->penaltyFactor; 697 | root->endEdge = new_root_edge; 698 | root->gradientToRoot = new_root_edge->gradient*root->penaltyFactor; 699 | root->parent = new_root; 700 | root->prevParent = NULL; 701 | 702 | if(root->children.size()!=0) root->nodeState = NodeState::NORMAL; 703 | else{ 704 | root->nodeState = NodeState::LEAF; 705 | leaf_nodes.push_back(root); 706 | } 707 | // Update new root item 708 | // Update new root children's disToRoot and gradientToRoot 709 | queue q_new_root_children; 710 | q_new_root_children.push(new_root); 711 | while(!q_new_root_children.empty()){ 712 | rtrrt_ns::Node::Ptr child = q_new_root_children.front(); 713 | q_new_root_children.pop(); 714 | if(child->children.empty()) continue; 715 | for(auto it = child->children.begin(); it!=child->children.end(); it++){ 716 | q_new_root_children.push(*it); 717 | (*it)->disToRoot -= new_root_edge->length*new_root->penaltyFactor; 718 | (*it)->gradientToRoot -= new_root_edge->gradient*new_root->penaltyFactor; 719 | } 720 | } 721 | new_root->children.push_back(root); 722 | new_root->disToRoot = 0; 723 | new_root->gradientToRoot = 0; 724 | new_root->endEdge = NULL; 725 | if(new_root->nodeState == NodeState::LEAF) leaf_nodes.remove(new_root); 726 | new_root->nodeState = NodeState::ROOT; 727 | new_root->parent = NULL; 728 | new_root->prevParent = NULL; 729 | new_root->startEdges.push_back(new_root_edge); 730 | edge_set.push_back(new_root_edge); 731 | NodePair new_root_pair{new_root, root}; 732 | tree.insert(make_pair(new_root_edge, new_root_pair)); 733 | root = new_root; 734 | 735 | rewireFromRoot(); 736 | } 737 | 738 | void rtrrt::pruneTree(rtrrt_ns::Node::Ptr new_center){ 739 | pcl::KdTreeFLANN remove_kdtree; 740 | // remove_kdtree.setInputCloud(treeNodes); 741 | 742 | // Clear cache 743 | last_removed_nodes.clear(); 744 | last_removed_edges.clear(); 745 | treeNodes->points.clear(); 746 | // Get center point 747 | // std::cout << "11111111111111111" << std::endl; 748 | float center_x = new_center->state.x; 749 | float center_y = new_center->state.y; 750 | 751 | // Iterate all node, judge them is whether inside local map 752 | queue q_query; 753 | q_query.push(root); 754 | mtx.lock(); 755 | while(!q_query.empty()){ 756 | // Get top element 757 | rtrrt_ns::Node::Ptr queried_node = q_query.front(); 758 | q_query.pop(); 759 | // If has children, add to queue 760 | if(!queried_node->children.empty()){ 761 | for(auto it=queried_node->children.begin(); it!=queried_node->children.end(); it++){ 762 | q_query.push(*it); 763 | } 764 | } 765 | // Judge inside 766 | float delta_x_abs = fabs(queried_node->state.x - center_x); 767 | float delta_y_abs = fabs(queried_node->state.y - center_y); 768 | // Position queried_pose(queried_node->state.x, queried_node->state.x); 769 | Position queried_pose{queried_node->state.x, queried_node->state.y}; 770 | // if(delta_x_abs < map_x/2 && delta_y_abs < map_y/2){ 771 | if(full_grid_map.isInside(queried_pose)){ 772 | // Inside local map, save it 773 | treeNodes->points.push_back(queried_node->pcl_pt); 774 | continue; 775 | } 776 | // std::cout << "222222222223333333333" << std::endl; 777 | 778 | // Outside, remove relevant node and edges 779 | rtrrt_ns::Node::Ptr removed_node = queried_node; 780 | 781 | // Delete kdtree 782 | kdtree_->RemovePoint(removed_node->node_id); 783 | id_nodes.erase(removed_node->node_id); 784 | // Delete x_parent item 785 | removed_node->parent->startEdges.remove(removed_node->endEdge); 786 | // std::cout << "33333333333333333333333" << std::endl; 787 | removed_node->parent->children.remove(removed_node); 788 | if(removed_node->parent->children.empty()&& 789 | removed_node->parent->nodeState != NodeState::ROOT){ 790 | removed_node->parent->nodeState = NodeState::LEAF; 791 | leaf_nodes.push_back(removed_node->parent); 792 | } 793 | 794 | // Delete node and edges from the node set 795 | node_set.remove(removed_node); 796 | edge_set.remove(removed_node->endEdge); 797 | tree.erase(removed_node->endEdge); 798 | 799 | // Add removed set for visualization 800 | last_removed_nodes.push_back(removed_node); 801 | last_removed_edges.push_back(removed_node->endEdge); 802 | // Remove from leaf set 803 | if(removed_node->nodeState == NodeState::LEAF){ 804 | leaf_nodes.remove(removed_node); 805 | } 806 | else{ 807 | queue q_remove; 808 | for(auto it_r=removed_node->children.begin(); it_r!=removed_node->children.end(); it_r++){ 809 | q_remove.push(*it_r); 810 | } 811 | while(!q_remove.empty()){ 812 | // Get top element 813 | rtrrt_ns::Node::Ptr node = q_remove.front(); 814 | q_remove.pop(); 815 | // Delete all parent items 816 | node->parent->children.remove(node); 817 | node->parent->startEdges.remove(node->endEdge); 818 | // Add children to queue 819 | for(auto it_n=node->children.begin(); it_n!=node->children.end(); it_n++){ 820 | q_remove.push(*it_n); 821 | } 822 | 823 | // Remove from node set and edge set 824 | node_set.remove(node); 825 | edge_set.remove(node->endEdge); 826 | tree.erase(node->endEdge); 827 | 828 | // Remove from leaf set 829 | if(node->nodeState == NodeState::LEAF){ 830 | leaf_nodes.remove(node); 831 | } 832 | // Remove from kdtree 833 | kdtree_->RemovePoint(node->node_id); 834 | id_nodes.erase(node->node_id); 835 | last_removed_nodes.push_back(node); 836 | last_removed_edges.push_back(node->endEdge); 837 | } 838 | } 839 | } 840 | mtx.unlock(); 841 | publishCloud(treeNodesPub, treeNodes, ros::Time::now(), global_frame); 842 | 843 | } 844 | 845 | rtrrt_ns::Node::Ptr rtrrt::findTarget(){ 846 | if(newGoalReceived){ 847 | float totalDistance = 0; 848 | float totalGradient = 0; 849 | float best_cost = inf; 850 | rtrrt_ns::Node::Ptr best_node = NULL; 851 | // Find near node to decide use final_goal or subgoal 852 | Nodes candidate_targets = getNeiborNodes(goal.position.x, goal.position.y, 0.0, targetTolerance); 853 | if(candidate_targets.empty()) 854 | { 855 | return NULL; 856 | } 857 | else{ // Candidate nodes found, use goal.and select best one(smallest costVal) as target 858 | // Firstly, calculate total distance and total gradient 859 | auto iter = candidate_targets.begin(); 860 | while(iter != candidate_targets.end()){ 861 | rtrrt_ns::Node::Ptr node = *iter; 862 | // Except OBSTACLE nodes 863 | if(node->nodeState == NodeState::OBSTACLE){ 864 | iter++; 865 | continue; 866 | } 867 | totalDistance += node->disToRoot; 868 | totalGradient += node->gradientToRoot; 869 | // kd_res_next(candidate_targets); 870 | iter++; 871 | } 872 | iter = candidate_targets.begin(); 873 | while(iter != candidate_targets.end()){ 874 | rtrrt_ns::Node::Ptr node = *iter; 875 | // Except OBSTACLE nodes 876 | if(node->nodeState == NodeState::OBSTACLE){ 877 | iter++; 878 | continue; 879 | } 880 | float costVal = costFunc(node->disToRoot, 0.0, node->gradientToRoot, totalDistance, totalGradient); 881 | if(costVal < best_cost){ 882 | best_cost = costVal; 883 | best_node = node; 884 | } 885 | // kd_res_next(candidate_targets); 886 | iter++; 887 | } 888 | // kd_res_free(candidate_targets); 889 | current_target = &(goal.position); 890 | current_target_node = best_node; 891 | return best_node; 892 | } 893 | } 894 | } 895 | 896 | 897 | void rtrrt::executeTreeUpdate(){ 898 | // If tree is empty, then add root node 899 | // std::cout << "11111111111111111" << std::endl; 900 | if(tree.empty() && node_set.size() == 0 && updatedPose){ 901 | rtrrt_ns::Node::Ptr root_node = make_shared(); 902 | root_node->state.x = robot_pose.x; 903 | root_node->state.y = robot_pose.y; 904 | root_node->state.z = robot_pose.z; 905 | root_node->parent = NULL; 906 | root_node->prevParent = NULL; 907 | root_node->disToRoot = 0.0; 908 | root_node->gradientToRoot = 0.0; 909 | root_node->nodeState = NodeState::ROOT; 910 | root_node->endEdge = NULL; 911 | node_set.push_back(root_node); 912 | root = node_set.back(); 913 | // Add to PCL points 914 | root_node->pcl_pt.x = robot_pose.x; 915 | root_node->pcl_pt.y = robot_pose.y; 916 | root_node->pcl_pt.z = 0.0; 917 | root_node->pcl_pt.intensity = 0.0; 918 | treeNodes->points.push_back(root_node->pcl_pt); 919 | // kd_insert3(kdTree_, root_node->state.x, root_node->state.y, 0.0, root_node); 920 | KDTreePt::Point pt(root_node->state.x, root_node->state.y, 0.0); 921 | kdtree_pts->points.push_back(pt); 922 | kdtree_->AddPoint(kdtree_pts->points.size()-1, kdtree_pts->points.size()-1); 923 | id_nodes.insert(make_pair(kdtree_pts->points.size()-1, root_node)); 924 | root_node->node_id = kdtree_pts->points.size()-1; 925 | } 926 | // Assume root is existed 927 | // Sampling 928 | // std::cout << "222222222222222222" << std::endl; 929 | sampleNode = sample(); 930 | 931 | // Find new node 932 | // std::cout << "33333333333333333" << std::endl; 933 | newNode = steer(sampleNode); 934 | 935 | // Find nearest node in the tree 936 | // std::cout << "44444444444444444" << std::endl; 937 | 938 | rtrrt_ns::Node::Ptr nearestNode = getNearestNode(newNode.state.x, newNode.state.y); 939 | if(nearestNode == NULL){ 940 | return; 941 | } 942 | // std::cout << "55555555555555555" << std::endl; 943 | 944 | if(nearestNode->nodeState!=NodeState::OBSTACLE && addNodeEdge(newNode, nearestNode)){ 945 | // std::cout << "Add node edge successfully!" << std::endl; 946 | reselectParent(node_set.back()); // New added node need to reselect parent 947 | q_rewire_from_rand.push(node_set.back()); // New added node need to reselect parent 948 | } 949 | // std::cout << "666666666666666666" << std::endl; 950 | 951 | rewireRandomNode(); 952 | // std::cout << "777777777777777777" << std::endl; 953 | 954 | skipCountRewiring--; 955 | if (skipCountRewiring < 0) { 956 | // std::cout << "88888888888888888888" << std::endl; 957 | rewireFromRoot(); 958 | // std::cout << "99999999999999999999" << std::endl; 959 | skipCountRewiring = 3; 960 | } 961 | 962 | // visualization 963 | auto cur_time = ros::Time::now(); 964 | if((cur_time-last_time).toSec() > 1.0){ 965 | visualizationCb(); 966 | } 967 | } 968 | 969 | void rtrrt::updateGoal(geometry_msgs::PoseStamped goal_point){ 970 | 971 | if(!updatedPose) 972 | { 973 | ROS_WARN("Cannot get the robot pose."); 974 | return; 975 | } 976 | std::cout << "goal_point: " << goal_point.pose.position.x << ", " << goal_point.pose.position.y << std::endl; 977 | goal.position.x = goal_point.pose.position.x; 978 | goal.position.y = goal_point.pose.position.y; 979 | goal.position.z = goal_point.pose.position.z; 980 | goal.orientation = goal_point.pose.orientation; 981 | 982 | newGoalReceived = true; 983 | } 984 | 985 | void rtrrt::getFrontierNode(){ 986 | frontierNodes.clear(); 987 | auto pt = leaf_nodes.begin(); 988 | while(pt != leaf_nodes.end()){ 989 | // Only leaf nodes inside current grid map can be considered 990 | Position node_posi{(*pt)->state.x, (*pt)->state.y}; 991 | if(full_grid_map.isInside(node_posi)){ 992 | // The nodes close to robot are abandoned 993 | float dis = util::distance((*pt)->state.x, robot_pose.x, (*pt)->state.y, robot_pose.y); 994 | if(dis > frontierDisThre){ 995 | Nodes neibor_nodes = getNeiborNodes((*pt)->state.x, (*pt)->state.y, 0.0, exploredAreaRadius); 996 | int neibor_nodes_size = neibor_nodes.size(); 997 | // For avoiding explored area 998 | if(neibor_nodes_size < exploredAreaThre){ 999 | // Avoid collision with obstacle nodes 1000 | float query_pt[3]={(*pt)->state.x, (*pt)->state.y, 0.0}; 1001 | vector indices; 1002 | vector dists_sq; 1003 | obstacleKdtree_->RadiusSearch(query_pt, inflationRadius*1.5, indices, dists_sq); 1004 | 1005 | if(indices.empty()){ 1006 | Position leaf_posi((*pt)->state.x, (*pt)->state.y); 1007 | Index leaf_index; 1008 | Length leng(frontierRadius, frontierRadius); 1009 | bool isSuccessful; 1010 | auto sub_map = full_grid_map.getSubmap(leaf_posi, leng, leaf_index ,isSuccessful); 1011 | Size submap_size = sub_map.getSize(); 1012 | if(submap_size[0] != submap_size[1]){ 1013 | // Check exist global vertices 1014 | if(!graphVertices->empty()){ 1015 | pcl_kdtree->setInputCloud(graphVertices); 1016 | pcl_kdtree->setSortedResults(false); 1017 | std::vector indices; 1018 | std::vector dist_list; 1019 | pcl_kdtree->radiusSearch((*pt)->pcl_pt, neiborRangeThre*2, indices, dist_list); 1020 | if(!indices.empty()){ 1021 | // Explored area 1022 | pt++; 1023 | continue; 1024 | } 1025 | } 1026 | // Add to frontier set 1027 | float information_gain = util::getInfoGain((*pt)->state.x, (*pt)->state.y, full_grid_map, "elevation"); 1028 | frontierNodes.insert(make_pair(*pt, information_gain)); 1029 | (*pt)->pcl_pt.intensity = information_gain; 1030 | } 1031 | else{ 1032 | // Check exist global vertices 1033 | if(!graphVertices->empty()){ 1034 | pcl_kdtree->setInputCloud(graphVertices); 1035 | pcl_kdtree->setSortedResults(false); 1036 | std::vector indices; 1037 | std::vector dist_list; 1038 | pcl_kdtree->radiusSearch((*pt)->pcl_pt, neiborRangeThre, indices, dist_list); 1039 | if(!indices.empty()){ 1040 | // Explored area 1041 | pt++; 1042 | continue; 1043 | } 1044 | } 1045 | // Add to frontier set 1046 | float information_gain = util::getInfoGain((*pt)->state.x, (*pt)->state.y, full_grid_map, "elevation"); 1047 | if(information_gain > informationThre){ 1048 | frontierNodes.insert(make_pair(*pt, information_gain)); 1049 | (*pt)->pcl_pt.intensity = information_gain; 1050 | } 1051 | } 1052 | } 1053 | } 1054 | } 1055 | } 1056 | pt++; 1057 | } 1058 | } 1059 | 1060 | Nodes rtrrt::getNeiborNodes(float point_x, float point_y, float point_z, float range){ 1061 | Nodes result_set; 1062 | float query_pt[3]={point_x, point_y, point_z}; 1063 | vector indices; 1064 | vector dists_sq; 1065 | kdtree_->RadiusSearch(query_pt, range, indices, dists_sq); 1066 | if(indices.empty()){ 1067 | return result_set; 1068 | } 1069 | for(int i=0; inodeState != NodeState::OBSTACLE){ 1071 | result_set.push_back(id_nodes.at(indices[i])); 1072 | } 1073 | } 1074 | return result_set; 1075 | } 1076 | 1077 | rtrrt_ns::Node::Ptr rtrrt::getNearestNode(float point_x, float point_y){ 1078 | float query_pt[3]={point_x, point_y, 0.0}; 1079 | vector indices; 1080 | vector dists_sq; 1081 | kdtree_->NearestSearch(query_pt, 1, indices, dists_sq); 1082 | if(indices.empty()){ 1083 | return NULL; 1084 | } 1085 | 1086 | rtrrt_ns::Node::Ptr nearestNode = id_nodes.at(indices[0]); 1087 | return nearestNode; 1088 | } 1089 | 1090 | rtrrt_ns::Node::Ptr rtrrt::getObstacleNode(float point_x, float point_y){ 1091 | float query_pt[3]={point_x, point_y, 0.0}; 1092 | vector indices; 1093 | vector dists_sq; 1094 | obstacleKdtree_->NearestSearch(query_pt, 1, indices, dists_sq); 1095 | if(indices.empty()){ 1096 | return NULL; 1097 | } 1098 | 1099 | rtrrt_ns::Node::Ptr nearestNode = obs_id_nodes.at(indices[0]); 1100 | return nearestNode; 1101 | } 1102 | 1103 | 1104 | void rtrrt::updateOrientationArray(rtrrt_ns::Node new_node, rtrrt_ns::Node::Ptr rrt_node){ 1105 | // Get ralative x-y angle between two input point 1106 | float deltX = new_node.state.x - rrt_node->state.x; 1107 | float deltY = new_node.state.y - rrt_node->state.y; 1108 | float deltAngle = atan2(deltY, deltX) * 180/M_PI; // Degree form 1109 | // 8 sector for rrt_node, begin with positive area 1110 | if (deltAngle>0 && deltAngle<=45){ 1111 | rrt_node->obstacle_orientation[0] = 1; 1112 | } 1113 | if (deltAngle>45 && deltAngle<=90){ 1114 | rrt_node->obstacle_orientation[1] = 1; 1115 | } 1116 | if (deltAngle>90 && deltAngle<=135){ 1117 | rrt_node->obstacle_orientation[2] = 1; 1118 | } 1119 | if (deltAngle>135 && deltAngle<=180){ 1120 | rrt_node->obstacle_orientation[3] = 1; 1121 | } 1122 | if (deltAngle>-180 && deltAngle<=-135){ 1123 | rrt_node->obstacle_orientation[4] = 1; 1124 | } 1125 | if (deltAngle>-135 && deltAngle<=-90){ 1126 | rrt_node->obstacle_orientation[5] = 1; 1127 | } 1128 | if (deltAngle>-90 && deltAngle<=-45){ 1129 | rrt_node->obstacle_orientation[6] = 1; 1130 | } 1131 | if (deltAngle>-45 && deltAngle<=0){ 1132 | rrt_node->obstacle_orientation[7] = 1; 1133 | } 1134 | } 1135 | 1136 | void rtrrt::publishRemovedItem(){ 1137 | // Publish last removed nodes and edges 1138 | if(pub_removed_edge.getNumSubscribers()!=0 || pub_removed_node.getNumSubscribers()!=0){ 1139 | 1140 | visualization_msgs::Marker removed_edge_marker; // last removed edge 1141 | visualization_msgs::Marker removed_node_marker; // last removed node 1142 | removed_edge_marker.header.frame_id = global_frame; 1143 | removed_edge_marker.header.stamp = ros::Time::now(); 1144 | removed_edge_marker.type = removed_edge_marker.LINE_LIST; 1145 | removed_edge_marker.action = removed_edge_marker.ADD; 1146 | removed_edge_marker.lifetime = ros::Duration(); 1147 | removed_edge_marker.color.a = 1.0; 1148 | removed_edge_marker.color.r = 255.0/255.0; 1149 | removed_edge_marker.color.g = 51.0/255.0; 1150 | removed_edge_marker.color.b = 202.0/255.0; 1151 | removed_edge_marker.scale.x = 0.02; 1152 | removed_edge_marker.scale.y = 0.02; 1153 | removed_edge_marker.scale.z = 0.02; 1154 | removed_edge_marker.pose.orientation.w = 1; 1155 | removed_edge_marker.points.clear(); 1156 | 1157 | removed_node_marker.header.frame_id = global_frame; 1158 | removed_node_marker.header.stamp = ros::Time::now(); 1159 | removed_node_marker.type = removed_node_marker.CUBE_LIST; 1160 | removed_node_marker.action = removed_node_marker.ADD; 1161 | removed_node_marker.lifetime = ros::Duration(); 1162 | removed_node_marker.color.a = 1.0; 1163 | removed_node_marker.color.r = 169.0/255.0; 1164 | removed_node_marker.color.g = 51.0/255.0; 1165 | removed_node_marker.color.b = 255.0/255.0; 1166 | removed_node_marker.scale.x = 0.06; 1167 | removed_node_marker.scale.y = 0.06; 1168 | removed_node_marker.scale.z = 0.06; 1169 | removed_node_marker.pose.orientation.w = 1; 1170 | removed_node_marker.points.clear(); 1171 | 1172 | // Add data 1173 | geometry_msgs::Point point_3d; 1174 | for(auto iter2=last_removed_edges.begin(); iter2!=last_removed_edges.end(); iter2++){ 1175 | point_3d.x = (*iter2)->fromNode->state.x; 1176 | point_3d.y = (*iter2)->fromNode->state.y; 1177 | point_3d.z = (*iter2)->fromNode->state.z; 1178 | removed_edge_marker.points.push_back(point_3d); 1179 | point_3d.x = (*iter2)->toNode->state.x; 1180 | point_3d.y = (*iter2)->toNode->state.y; 1181 | point_3d.z = (*iter2)->toNode->state.z; 1182 | removed_edge_marker.points.push_back(point_3d); 1183 | } 1184 | 1185 | geometry_msgs::Point removed_n; 1186 | auto iter = last_removed_nodes.begin(); 1187 | while(iter != last_removed_nodes.end()){ 1188 | removed_n.x = (*iter)->state.x; 1189 | removed_n.y = (*iter)->state.y; 1190 | removed_n.z = (*iter)->state.z; 1191 | removed_node_marker.points.push_back(removed_n); 1192 | iter++; 1193 | } 1194 | pub_removed_edge.publish(removed_edge_marker); 1195 | pub_removed_node.publish(removed_node_marker); 1196 | } 1197 | } 1198 | 1199 | void rtrrt::gridMapHandler(const grid_map_msgs::GridMapConstPtr &raw_map){ 1200 | GridMapRosConverter::fromMessage(*raw_map, full_grid_map); 1201 | GridMapRosConverter::fromMessage(*raw_map, sensor_range_map); 1202 | map_x = raw_map->info.length_x; 1203 | map_y = raw_map->info.length_y; 1204 | map_resolution = raw_map->info.resolution; 1205 | receivedGridMap = true; 1206 | tf::TransformListener tempTFListener; 1207 | tf::StampedTransform tempTransform; 1208 | try 1209 | { 1210 | ros::Time tempTime; 1211 | tempTime = ros::Time::now(); 1212 | 1213 | tempTFListener.waitForTransform(base_frame,global_frame,ros::Time::now(),ros::Duration(0.5)); 1214 | tempTFListener.lookupTransform(base_frame,global_frame,ros::Time(0),tempTransform); 1215 | geometry_msgs::PointStamped point_in, point_out; 1216 | point_out.header.frame_id = base_frame; 1217 | point_in.header.frame_id = global_frame; 1218 | for(GridMapIterator it(sensor_range_map); !it.isPastEnd(); ++it) 1219 | { 1220 | Position post; 1221 | sensor_range_map.getPosition(*it, post); 1222 | point_in.point.x = post.x(); 1223 | point_in.point.y = post.y(); 1224 | tempTFListener.transformPoint(base_frame,point_in, point_out); 1225 | float h = fabs(point_out.point.x/(point_out.point.y+0.01)); 1226 | if(!(point_out.point.x>0 && h > 1.0/TAN60)){ 1227 | try{ 1228 | sensor_range_map.at("elevation", *it) = NAN; 1229 | } 1230 | catch(const std::out_of_range& exception){ 1231 | std::cout << "sensor_range_map no found" << std::endl; 1232 | } 1233 | 1234 | } 1235 | } 1236 | grid_map_msgs::GridMap rangeGridMapMsg; 1237 | GridMapRosConverter::toMessage(sensor_range_map, rangeGridMapMsg); 1238 | pub_range_map.publish(rangeGridMapMsg); 1239 | } 1240 | catch(const std::exception& e) 1241 | { 1242 | std::cerr << e.what() << '\n'; 1243 | } 1244 | } 1245 | 1246 | void rtrrt::graphVerticesHandler(const sensor_msgs::PointCloud2ConstPtr &vertices_msgs){ 1247 | graphVertices->clear(); 1248 | pcl::fromROSMsg(*vertices_msgs, *graphVertices); 1249 | } 1250 | 1251 | void rtrrt::visualizationCb(){ 1252 | if(!tree.empty() && !node_set.empty() && !edge_set.empty()){ 1253 | // if all topics of visualizing tree item have no subscriber, then don't publish 1254 | if(pub_rtrrt_edge.getNumSubscribers()!=0 || pub_rtrrt_node.getNumSubscribers()!=0 1255 | || pub_leaf_node.getNumSubscribers()!=0 || pub_root_marker.getNumSubscribers()!=0){ 1256 | 1257 | visualization_msgs::Marker edge_marker; // edge represent by linelist 1258 | visualization_msgs::Marker node_marker; // node represent by cube 1259 | visualization_msgs::Marker root_marker; // root_node represent by cube 1260 | visualization_msgs::Marker leaf_marker; // leaf represent by cube 1261 | visualization_msgs::Marker obstacle_node_marker; // Transparent sphere list 1262 | 1263 | // Marker init 1264 | // Edge 1265 | edge_marker.header.frame_id = global_frame; 1266 | edge_marker.header.stamp = ros::Time::now(); 1267 | edge_marker.type = edge_marker.LINE_LIST; 1268 | edge_marker.action = edge_marker.ADD; 1269 | edge_marker.lifetime = ros::Duration(); 1270 | edge_marker.color.a = 1.0; 1271 | edge_marker.color.r = 1.0; 1272 | edge_marker.color.g = 1.0; 1273 | // edge_marker.color.g = 0.6; 1274 | edge_marker.color.b = 0.0; 1275 | edge_marker.scale.x = 0.02; 1276 | edge_marker.scale.y = 0.02; 1277 | edge_marker.scale.z = 0.02; 1278 | edge_marker.pose.orientation.w = 1; 1279 | edge_marker.points.clear(); 1280 | // Node 1281 | node_marker.header.frame_id = global_frame; 1282 | node_marker.header.stamp = ros::Time::now(); 1283 | node_marker.type = node_marker.CUBE_LIST; 1284 | node_marker.action = node_marker.ADD; 1285 | node_marker.lifetime = ros::Duration(); 1286 | node_marker.color.a = 1.0; 1287 | node_marker.color.r = 0.0; 1288 | node_marker.color.g = 0.0; 1289 | node_marker.color.b = 1.0; 1290 | node_marker.scale.x = 0.06; 1291 | node_marker.scale.y = 0.06; 1292 | node_marker.scale.z = 0.06; 1293 | node_marker.pose.orientation.w = 1; 1294 | node_marker.points.clear(); 1295 | // Leaf 1296 | leaf_marker.header.frame_id = global_frame; 1297 | leaf_marker.header.stamp = ros::Time::now(); 1298 | leaf_marker.type = leaf_marker.CUBE_LIST; 1299 | leaf_marker.action = leaf_marker.ADD; 1300 | leaf_marker.lifetime = ros::Duration(); 1301 | leaf_marker.color.a = 1.0; 1302 | leaf_marker.color.r = 1.0; 1303 | leaf_marker.color.g = 0.0; 1304 | leaf_marker.color.b = 0.0; 1305 | leaf_marker.scale.x = 0.08; 1306 | leaf_marker.scale.y = 0.08; 1307 | leaf_marker.scale.z = 0.08; 1308 | leaf_marker.pose.orientation.w = 1; 1309 | leaf_marker.points.clear(); 1310 | // Root node 1311 | root_marker.header.frame_id = global_frame; 1312 | root_marker.header.stamp = ros::Time::now(); 1313 | root_marker.type = root_marker.SPHERE; 1314 | root_marker.action = root_marker.ADD; 1315 | root_marker.lifetime = ros::Duration(); 1316 | root_marker.color.a = 1.0; 1317 | root_marker.color.r = 1.0; 1318 | root_marker.color.g = 0.6; 1319 | root_marker.color.b = 0.0; 1320 | root_marker.scale.x = 0.5; 1321 | root_marker.scale.y = 0.5; 1322 | root_marker.scale.z = 0.5; 1323 | root_marker.pose.orientation.w = 1; 1324 | // Obstacle node 1325 | obstacle_node_marker.header.frame_id = global_frame; 1326 | obstacle_node_marker.header.stamp = ros::Time::now(); 1327 | obstacle_node_marker.type = obstacle_node_marker.SPHERE_LIST; 1328 | obstacle_node_marker.action = obstacle_node_marker.ADD; 1329 | obstacle_node_marker.lifetime = ros::Duration(); 1330 | obstacle_node_marker.color.a = 0.8; 1331 | obstacle_node_marker.color.r = 1.0; 1332 | obstacle_node_marker.color.g = 0.0; 1333 | obstacle_node_marker.color.b = 0.0; 1334 | obstacle_node_marker.scale.x = inflationRadius; 1335 | obstacle_node_marker.scale.y = inflationRadius; 1336 | obstacle_node_marker.scale.z = 0.1; 1337 | obstacle_node_marker.pose.orientation.w = 1; 1338 | obstacle_node_marker.points.clear(); 1339 | // Add data 1340 | geometry_msgs::Point point_3d; 1341 | for(auto iter2=edge_set.begin(); iter2!=edge_set.end(); iter2++){ 1342 | point_3d.x = (*iter2)->fromNode->state.x; 1343 | point_3d.y = (*iter2)->fromNode->state.y; 1344 | point_3d.z = (*iter2)->fromNode->state.z; 1345 | edge_marker.points.push_back(point_3d); 1346 | point_3d.x = (*iter2)->toNode->state.x; 1347 | point_3d.y = (*iter2)->toNode->state.y; 1348 | point_3d.z = (*iter2)->toNode->state.z; 1349 | edge_marker.points.push_back(point_3d); 1350 | } 1351 | 1352 | for(auto iter1=node_set.begin(); iter1!=node_set.end(); iter1++){ 1353 | point_3d.x = (*iter1)->state.x; 1354 | point_3d.y = (*iter1)->state.y; 1355 | point_3d.z = (*iter1)->state.z; 1356 | if((*iter1)->nodeState == NodeState::LEAF){ 1357 | leaf_marker.points.push_back(point_3d); 1358 | node_marker.points.push_back(point_3d); 1359 | continue; 1360 | } 1361 | // if((*iter1)->nodeState == NodeState::OBSTACLE){ 1362 | // obstacle_node_marker.points.push_back(point_3d); 1363 | // continue; 1364 | // } 1365 | if((*iter1)->nodeState == NodeState::ROOT){ 1366 | root_marker.pose.position = point_3d; 1367 | continue; 1368 | } 1369 | if((*iter1)->nodeState == NodeState::NORMAL){ 1370 | node_marker.points.push_back(point_3d); 1371 | continue; 1372 | } 1373 | } 1374 | 1375 | for(auto iter2=obstacle_node_set.begin(); iter2!=obstacle_node_set.end(); iter2++){ 1376 | point_3d.x = (*iter2)->state.x; 1377 | point_3d.y = (*iter2)->state.y; 1378 | point_3d.z = (*iter2)->state.z; 1379 | obstacle_node_marker.points.push_back(point_3d); 1380 | } 1381 | 1382 | pub_rtrrt_edge.publish(edge_marker); 1383 | pub_rtrrt_node.publish(node_marker); 1384 | pub_leaf_node.publish(leaf_marker); 1385 | pub_root_marker.publish(root_marker); 1386 | pub_obstacle_node.publish(obstacle_node_marker); 1387 | } 1388 | } 1389 | 1390 | if(pub_frontier_node.getNumSubscribers()!=0 && !frontierNodes.empty()){ 1391 | visualization_msgs::Marker frontier_marker; // frontier represent by sphere 1392 | frontier_marker.header.frame_id = global_frame; 1393 | frontier_marker.header.stamp = ros::Time::now(); 1394 | frontier_marker.type = frontier_marker.SPHERE_LIST; 1395 | frontier_marker.action = frontier_marker.ADD; 1396 | frontier_marker.lifetime = ros::Duration(); 1397 | frontier_marker.color.a = 1.0; 1398 | frontier_marker.color.r = 0.0; 1399 | frontier_marker.color.g = 1.0; 1400 | frontier_marker.color.b = 0.0; 1401 | frontier_marker.scale.x = 0.3; 1402 | frontier_marker.scale.y = 0.3; 1403 | frontier_marker.scale.z = 0.3; 1404 | frontier_marker.pose.orientation.w = 1; 1405 | frontier_marker.points.clear(); 1406 | 1407 | geometry_msgs::Point frontier_p; 1408 | auto iter = frontierNodes.begin(); 1409 | while(iter != frontierNodes.end()){ 1410 | frontier_p.x = iter->first->state.x; 1411 | frontier_p.y = iter->first->state.y; 1412 | frontier_p.z = iter->first->state.z; 1413 | frontier_marker.points.push_back(frontier_p); 1414 | iter++; 1415 | } 1416 | pub_frontier_node.publish(frontier_marker); 1417 | } 1418 | 1419 | publishRemovedItem(); 1420 | 1421 | } 1422 | 1423 | 1424 | 1425 | 1426 | 1427 | 1428 | 1429 | 1430 | 1431 | 1432 | 1433 | 1434 | 1435 | 1436 | 1437 | 1438 | 1439 | 1440 | --------------------------------------------------------------------------------