├── .gitignore ├── README.md ├── img ├── control_panel.png ├── indoor.gif ├── matterport.gif ├── method.png └── ps3_controller.jpg └── src ├── CMakeLists.txt ├── boundary_handler ├── CMakeLists.txt ├── config │ └── default.yaml ├── include │ └── boundary_handler │ │ ├── graph_extractor.h │ │ ├── intersection.h │ │ └── point_struct.h ├── launch │ └── boundary_handler.launch ├── package.xml ├── rviz │ └── default.rviz └── src │ └── graph_extractor.cpp ├── far_planner ├── CMakeLists.txt ├── config │ ├── default.yaml │ └── matterport.yaml ├── data │ ├── boundary.ply │ ├── boundary_graph.vgh │ ├── indoor.vgh │ └── trajectory.txt ├── include │ └── far_planner │ │ ├── contour_detector.h │ │ ├── contour_graph.h │ │ ├── dynamic_graph.h │ │ ├── far_planner.h │ │ ├── graph_msger.h │ │ ├── graph_planner.h │ │ ├── grid.h │ │ ├── intersection.h │ │ ├── map_handler.h │ │ ├── node_struct.h │ │ ├── planner_visualizer.h │ │ ├── point_struct.h │ │ ├── scan_handler.h │ │ ├── terrain_planner.h │ │ ├── time_measure.h │ │ └── utility.h ├── launch │ └── far_planner.launch ├── package.xml ├── rviz │ ├── default.rviz │ └── matterport.rviz └── src │ ├── contour_detector.cpp │ ├── contour_graph.cpp │ ├── dynamic_graph.cpp │ ├── far_planner.cpp │ ├── graph_msger.cpp │ ├── graph_planner.cpp │ ├── map_handler.cpp │ ├── planner_visualizer.cpp │ ├── scan_handler.cpp │ ├── terrain_planner.cpp │ └── utility.cpp ├── goalpoint_rviz_plugin ├── CMakeLists.txt ├── include │ └── goalpoint_tool.h ├── package.xml ├── plugin_description.xml └── src │ └── goalpoint_tool.cpp ├── graph_decoder ├── CMakeLists.txt ├── config │ └── default.yaml ├── decoder.rviz ├── include │ └── graph_decoder │ │ ├── decoder_node.h │ │ └── point_struct.h ├── launch │ └── decoder.launch ├── package.xml ├── rviz │ └── decoder.rviz └── src │ └── decoder_node.cpp ├── teleop_rviz_plugin ├── CMakeLists.txt ├── package.xml ├── plugin_description.xml └── src │ ├── drive_widget.cpp │ ├── drive_widget.h │ ├── teleop_panel.cpp │ └── teleop_panel.h └── visibility_graph_msg ├── CMakeLists.txt ├── msg ├── Graph.msg └── Node.msg └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | 3 | devel 4 | 5 | install 6 | 7 | logs 8 | 9 | log 10 | 11 | .catkin_workspace 12 | 13 | .catkin_tools 14 | 15 | .vscode 16 | 17 | *~ 18 | 19 | .DS_Store 20 | 21 | src/dynamic_router_planner/contour_test/*.out 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | FAR Planner uses a dynamically updated visibility graph for fast replanning. The planner models the environment with polygons and builds a global visibility graph along with the navigation. The planner is capable of handling both known and unknown environments. In a known environment, paths are planned based on a prior map. In an unknown environment, multiple paths are attempted to guide the vehicle to goal based on the environment observed during the navigation. When dynamic obstacles are present, FAR Planner disconnects visibility edges blocked by the dynamic obstacles and reconnects them after regaining visibility. The software implementation uses two CPU threads - one for dynamically updating the visibility graph using ~20% of the thread and the other for path search that can find a path within 3ms, as evaluated on an i7 computer. 2 | 3 | FAR Planner was used by the [CMU-OSU Team](https://www.subt-explorer.com) in attending [DARPA Subterranean Challenge](https://www.subtchallenge.com). In the final competition which took place in Louisville Mega Cavern, KY, the team's robots conducted the most complete traversing and mapping across the site (26 out of 28 sectors) among all teams, winning a "Most Sectors Explored Award". 4 | 5 | [A video showing functionalities of FAR Planner is available.](https://youtu.be/5shnlyZ4L9M) 6 | 7 |

8 | Method 9 |

10 | 11 | ## Usage 12 | 13 | The repository has been tested in Ubuntu 18.04 with ROS Melodic and Ubuntu 20.04 with ROS Noetic. Follow instructions in [Autonomous Exploration Development Environment](http://cmu-exploration.com) to setup the development environment. Make sure to checkout the branch that matches the computer setup, compile, and download the simulation environments. 14 | 15 | To setup FAR Planner, clone the repository. 16 | ``` 17 | git clone https://github.com/MichaelFYang/far_planner 18 | ``` 19 | In a terminal, go to the folder, checkout the 'melodic-noetic' branch, and compile. 20 | ``` 21 | cd far_planner 22 | git checkout melodic-noetic 23 | catkin_make 24 | ``` 25 | To run the code, go to the development environment folder in a terminal, source the ROS workspace, and launch. 26 | ``` 27 | source devel/setup.sh 28 | roslaunch vehicle_simulator system_indoor.launch 29 | ``` 30 | In another terminal, go to the FAR Planner folder, source the ROS workspace, and launch. 31 | ``` 32 | source devel/setup.sh 33 | roslaunch far_planner far_planner.launch 34 | ``` 35 | Now, users can send a goal by pressing the 'Goalpoint' button in RVIZ and then clicking a point to set the goal. The vehicle will navigate to the goal and build a visibility graph (in cyan) along the way. Areas covered by the visibility graph become free space. When navigating in free space, the planner uses the built visibility graph, and when navigating in unknown space, the planner attempts to discover a way to the goal. By pressing the 'Reset Visibility Graph' button, the planner will reinitialize the visibility graph. By unchecking the 'Planning Attemptable' checkbox, the planner will first try to find a path through the free space. The path will show in green. If such a path does not exist, the planner will consider unknown space together. The path will show in blue. By unchecking the 'Update Visibility Graph' checkbox, the planner will stop updating the visibility graph. To read/save the visibility graph from/to a file, press the 'Read'/'Save' button. An example visibility graph file for indoor environment is available at 'src/far_planner/data/indoor.vgh'. 36 | 37 |

38 | Indoor 39 |

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

44 | ControlPanel 45 |     46 | PS3 Controller 47 |

48 | 49 | To launch with a different environment, use the command lines below and replace '\' with one of the environment names in the development environment, i.e. 'campus', 'indoor', 'garage', 'tunnel', and 'forest'. Note that when running in campus environment, set *checkTerrainConn* to true in system_campus.launch in the 'src/vehicle_simulator/launch' folder of the development environment. 50 | ``` 51 | roslaunch vehicle_simulator system_.launch 52 | roslaunch far_planner far_planner.launch 53 | ``` 54 | To run FAR Planner in a [Matterport3D](https://niessner.github.io/Matterport) environment, follow instructions on the development environment page to setup the Matterport3D environment. Then, use the command lines below to launch the system and FAR Planner. 55 | ``` 56 | roslaunch vehicle_simulator system_matterport.launch 57 | roslaunch far_planner far_planner.launch config:=matterport 58 | ``` 59 | 60 |

61 | Matterport 62 |

63 | 64 | Users have the option to define custom navigation boundaries. To do this, users need to supply a boundary file and a trajectory file. Examples are provided as 'boundary.ply' and 'trajectory.txt' in the 'src/far_planner/data' folder. The files can be viewed by text editors. Specifically, the boundary file contains user-defined polygons where each polygon has an index. The trajectory file contains poses where each pose is a row with x (m), y (m), z (m), roll (rad), pitch (rad), yaw (rad), and time duration from start of the run (second). The trajectory file is in the same format as those saved by the development environment and needs to contain at least one pose to determine the traversable side of the navigation boundaries. Users can use the command line below to generate a visibility graph file in the same folder (provided as 'boundary_graph.vgh'), which can be loaded to FAR Planner using the 'Read' button. 65 | ``` 66 | roslaunch boundary_handler boundary_handler.launch 67 | ``` 68 | 69 | ## Configuration 70 | 71 | FAR Planner settings are kept in default.yaml in the 'src/far_planner/config' folder. For Matterport3D environments, the settings are in matterport.yaml in the same folder. 72 | - *is_static_env* (default: true) - set to false if the environment contains dynamic obstacles. 73 | - *is_attempt_autoswitch* (default: true) - set to false to turn off auto switch from non-attemptable navigation (for known environment) to attemptable navigation (for unknown environment). 74 | - *is_viewpoint_extend* (default: true) - set to false to stop extending visibility graph vertices away from objects to gain better viewpoints. 75 | - *is_pub_boundary* (default: false) - set to true to send custom navigation boundaries to the local planner in the development environment. 76 | 77 | ## Reference 78 | 79 | - F. Yang, C. Cao, H. Zhu, J. Oh, and J. Zhang. FAR Planner: Fast, Attemptable Route Planner using Dynamic Visibility Update. IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS). Kyoto, Japan, Oct. 2022. **Best Student Paper Award.** 80 | 81 | ## Author 82 | 83 | [Fan Yang](https://github.com/MichaelFYang) (michael.yfan24@gmail.com) 84 | -------------------------------------------------------------------------------- /img/control_panel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MichaelFYang/far_planner/2799b6964c141cacd1c32a14b19bc7abffbe0e52/img/control_panel.png -------------------------------------------------------------------------------- /img/indoor.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MichaelFYang/far_planner/2799b6964c141cacd1c32a14b19bc7abffbe0e52/img/indoor.gif -------------------------------------------------------------------------------- /img/matterport.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MichaelFYang/far_planner/2799b6964c141cacd1c32a14b19bc7abffbe0e52/img/matterport.gif -------------------------------------------------------------------------------- /img/method.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MichaelFYang/far_planner/2799b6964c141cacd1c32a14b19bc7abffbe0e52/img/method.png -------------------------------------------------------------------------------- /img/ps3_controller.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MichaelFYang/far_planner/2799b6964c141cacd1c32a14b19bc7abffbe0e52/img/ps3_controller.jpg -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /src/boundary_handler/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(boundary_handler) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | set(CMAKE_BUILD_TYPE Release) 7 | set(BUILD_STATIC_LIBS ON) 8 | set(BUILD_SHARED_LIBS OFF) 9 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 10 | 11 | ## Find catkin macros and libraries 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | std_msgs 15 | sensor_msgs 16 | visibility_graph_msg 17 | pcl_ros 18 | ) 19 | 20 | find_package(PCL REQUIRED) 21 | find_package(OpenCV REQUIRED) 22 | 23 | ################################### 24 | ## catkin specific configuration ## 25 | ################################### 26 | 27 | catkin_package( 28 | CATKIN_DEPENDS 29 | roscpp 30 | std_msgs 31 | sensor_msgs 32 | pcl_ros 33 | ) 34 | 35 | ########### 36 | ## Build ## 37 | ########### 38 | 39 | ## Specify additional locations of header files 40 | ## Your package locations should be listed before other locations 41 | include_directories( 42 | ${catkin_INCLUDE_DIRS} 43 | ${PCL_INCLUDE_DIRS} 44 | ${OpenCV_INCLUDE_DIRS} 45 | "${PROJECT_SOURCE_DIR}/include" 46 | /usr/local/include # Location when using 'make system_install' 47 | /usr/include # More usual location (e.g. when installing using a package) 48 | ) 49 | 50 | ## Specify additional locations for library files 51 | link_directories( 52 | /usr/local/lib # Location when using 'make system_install' 53 | /usr/lib # More usual location (e.g. when installing using a package) 54 | ) 55 | 56 | ## Depedent cpp files 57 | set(SOURCES ${SOURCES} 58 | ) 59 | 60 | ## Declare executables 61 | add_executable(${PROJECT_NAME} src/graph_extractor.cpp ${SOURCES}) 62 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 63 | 64 | ## Specify libraries to link a library or executable target against 65 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) 66 | 67 | install(TARGETS ${PROJECT_NAME} 68 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 69 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 70 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 71 | ) 72 | 73 | install(DIRECTORY launch/ 74 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 75 | ) 76 | -------------------------------------------------------------------------------- /src/boundary_handler/config/default.yaml: -------------------------------------------------------------------------------- 1 | # Boundary Graph Extractor Params 2 | world_frame : map 3 | visual_scale_ratio : 0.5 4 | height_tolz : 2.0 -------------------------------------------------------------------------------- /src/boundary_handler/include/boundary_handler/graph_extractor.h: -------------------------------------------------------------------------------- 1 | #ifndef GRAPH_EXTRACTOR_H 2 | #define GRAPH_EXTRACTOR_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include "boundary_handler/point_struct.h" 19 | 20 | typedef visualization_msgs::Marker Marker; 21 | typedef visualization_msgs::MarkerArray MarkerArray; 22 | 23 | typedef std::pair PointPair; 24 | typedef std::vector PointStack; 25 | 26 | enum VizColor { 27 | RED = 0, 28 | ORANGE = 1, 29 | BLACK = 2, 30 | YELLOW = 3, 31 | BLUE = 4, 32 | GREEN = 5, 33 | EMERALD = 6, 34 | WHITE = 7, 35 | MAGNA = 8, 36 | PURPLE = 9 37 | }; 38 | 39 | enum NodeFreeDirect { 40 | UNKNOW = 0, 41 | CONVEX = 1, 42 | CONCAVE = 2, 43 | PILLAR = 3 44 | }; 45 | 46 | struct Polygon 47 | { 48 | Polygon() = default; 49 | std::size_t N; 50 | std::vector vertices; 51 | bool is_robot_inside; 52 | }; 53 | 54 | typedef std::shared_ptr PolygonPtr; 55 | typedef std::vector PolygonStack; 56 | 57 | struct NavNode { 58 | NavNode() = default; 59 | std::size_t id; 60 | Point3D position; 61 | NodeFreeDirect free_direct; 62 | PointPair surf_dirs; 63 | PolygonPtr poly_ptr; 64 | 65 | bool is_covered; 66 | bool is_frontier; 67 | bool is_navpoint; 68 | bool is_boundary; 69 | std::vector connect_idxs; 70 | std::vector> connect_nodes; 71 | 72 | std::vector poly_idxs; 73 | std::vector> poly_connects; 74 | 75 | std::vector contour_idxs; 76 | std::vector> contour_connects; 77 | 78 | std::vector traj_idxs; 79 | std::vector> traj_connects; 80 | }; 81 | 82 | typedef std::shared_ptr NavNodePtr; 83 | typedef std::vector NodePtrStack; 84 | 85 | struct GraphExtractorParams { 86 | GraphExtractorParams() = default; 87 | std::string frame_id; 88 | std::string vgraph_path; 89 | std::string bd_file_path; 90 | std::string traj_file_path; 91 | float viz_scale_ratio; 92 | float height_tolz; 93 | }; 94 | 95 | class GraphExtractor { 96 | public: 97 | GraphExtractor() = default; 98 | ~GraphExtractor() = default; 99 | 100 | void Init(); 101 | void Run(); 102 | void Loop(); 103 | 104 | private: 105 | ros::NodeHandle nh; 106 | ros::Publisher graph_viz_pub_, viz_node_pub_; 107 | 108 | std::size_t id_ = 0; 109 | 110 | GraphExtractorParams ge_params_; 111 | PolygonStack extracted_polys_; 112 | NodePtrStack extracted_graph_; 113 | MarkerArray graph_marker_array_; 114 | std::size_t robot_id_; 115 | 116 | Point3D free_p_; 117 | PointStack traj_; 118 | 119 | PointCloudPtr boundary_ptr_; 120 | 121 | void LoadParmas(); 122 | 123 | void ConstructVGraph(const PolygonStack& polysIn, NodePtrStack& graphOut); 124 | 125 | void SetMarker(const VizColor& color, 126 | const std::string& ns, 127 | const float scale, 128 | const float alpha, 129 | Marker& scan_marker); 130 | 131 | void SetColor(const VizColor& color, const float alpha, Marker& scan_marker); 132 | 133 | void ReadBoundaryFile(const PointCloudPtr& bd_cloud, PolygonStack& polysOut); 134 | 135 | void ReadTrajFile(Point3D& free_p); 136 | 137 | void FilterPolyBoundary(const PointCloudPtr& bd_cloud, PolygonStack& polysOut); 138 | 139 | bool IsValidConnect(const NavNodePtr& node_ptr1, const NavNodePtr& node_ptr2); 140 | 141 | void AnalysisConvexity(const NavNodePtr& node_ptr, const PolygonPtr& poly_ptr); 142 | 143 | bool IsOutReducedDirs(const Point3D& diff_p, const PointPair& surf_dirs); 144 | 145 | bool IsNavNodesConnectFreePolygon(const NavNodePtr& node_ptr1, const NavNodePtr& node_ptr2); 146 | 147 | bool IsEdgeCollideSegment(const PointPair& line, const PointPair& edge); 148 | 149 | PointPair ReprojectEdge(const NavNodePtr& node_ptr1, const NavNodePtr& node_ptr2, const float& dist); 150 | 151 | /** 152 | * @brief Save the current vgraph to a vgh file 153 | * 154 | * @param graphIn current stored v-graph 155 | */ 156 | void SaveVGraph(const NodePtrStack& graphIn); 157 | 158 | void VisualizeGraph(const NodePtrStack& graphIn); 159 | 160 | void VisualizeFreePoint(const Point3D& free_p); 161 | 162 | void CreateNavNode(const Point3D& p, NavNodePtr& node_ptr); 163 | 164 | bool IsInDirectConstraint(const NavNodePtr& node_ptr1, const NavNodePtr& node_ptr2); 165 | 166 | 167 | /** 168 | * @brief encode graph to ros graph message 169 | * 170 | * @param graphIn current stored v-graph 171 | * @param graphOut [out] the encoded v-graph message 172 | */ 173 | void EncodeGraph(const NodePtrStack& graphIn, visibility_graph_msg::Graph& graphOut); 174 | 175 | template 176 | geometry_msgs::Point inline ToGeoMsgP(const Point& p) { 177 | geometry_msgs::Point geo_p; 178 | geo_p.x = p.x; 179 | geo_p.y = p.y; 180 | geo_p.z = p.z; 181 | return geo_p; 182 | } 183 | 184 | template 185 | inline bool IsTypeInStack(const T& elem, const std::vector& T_stack) { 186 | if (std::find(T_stack.begin(), T_stack.end(), elem) != T_stack.end()) { 187 | return true; 188 | } 189 | return false; 190 | } 191 | 192 | inline void ConnectBoundary(const NavNodePtr& node_ptr1, const NavNodePtr& node_ptr2) { 193 | if (node_ptr1 == node_ptr2) return; 194 | if (this->IsTypeInStack(node_ptr2->id, node_ptr1->contour_idxs)) return; 195 | node_ptr1->contour_idxs.push_back(node_ptr2->id); 196 | node_ptr2->contour_idxs.push_back(node_ptr1->id); 197 | node_ptr1->contour_connects.push_back(node_ptr2); 198 | node_ptr2->contour_connects.push_back(node_ptr1); 199 | } 200 | 201 | inline void ConnectVEdge(const NavNodePtr& node_ptr1, const NavNodePtr& node_ptr2) { 202 | if (node_ptr1 == node_ptr2) return; 203 | // poly connects 204 | if (!this->IsTypeInStack(node_ptr2->id, node_ptr1->poly_idxs)) { 205 | node_ptr1->poly_idxs.push_back(node_ptr2->id); 206 | node_ptr2->poly_idxs.push_back(node_ptr1->id); 207 | node_ptr1->poly_connects.push_back(node_ptr2); 208 | node_ptr2->poly_connects.push_back(node_ptr1); 209 | } 210 | // graph connects 211 | if (!this->IsTypeInStack(node_ptr2->id, node_ptr1->connect_idxs)) { 212 | node_ptr1->connect_idxs.push_back(node_ptr2->id); 213 | node_ptr2->connect_idxs.push_back(node_ptr1->id); 214 | node_ptr1->connect_nodes.push_back(node_ptr2); 215 | node_ptr2->connect_nodes.push_back(node_ptr1); 216 | } 217 | } 218 | 219 | inline void ResetGraph(NodePtrStack& graphOut) { 220 | graphOut.clear(); 221 | } 222 | 223 | inline int Mod(const int& a, const int& b) { 224 | return (b + (a % b)) % b; 225 | } 226 | 227 | inline void CreatePolygon(const PointStack& poly_points, PolygonPtr& poly_ptr, const Point3D& free_p) { 228 | poly_ptr = std::make_shared(); 229 | poly_ptr->N = poly_points.size(); 230 | poly_ptr->vertices = poly_points; 231 | poly_ptr->is_robot_inside = this->PointInsideAPoly(poly_points, free_p); 232 | } 233 | 234 | inline void ConvertGraphToMsg(const NodePtrStack& graph, visibility_graph_msg::Graph& graph_msg) { 235 | graph_msg.header.frame_id = ge_params_.frame_id; 236 | graph_msg.header.stamp = ros::Time::now(); 237 | graph_msg.robot_id = robot_id_; 238 | EncodeGraph(graph, graph_msg); 239 | } 240 | 241 | inline bool AddNodePtrToGraph(const NavNodePtr& node_ptr, NodePtrStack& graphOut) { 242 | if (node_ptr != NULL) { 243 | graphOut.push_back(node_ptr); 244 | return true; 245 | } 246 | return false; 247 | } 248 | 249 | inline bool IsConvexConnect(const NavNodePtr& node_ptr1, const NavNodePtr& node_ptr2) { 250 | if (node_ptr1->free_direct != NodeFreeDirect::CONCAVE && 251 | node_ptr2->free_direct != NodeFreeDirect::CONCAVE) 252 | { 253 | return true; 254 | } 255 | return false; 256 | } 257 | 258 | inline Point3D SurfTopoDirect(const PointPair& dirs, bool& _is_wall) { 259 | const Point3D topo_dir = dirs.first + dirs.second; 260 | _is_wall = false; 261 | if (topo_dir.norm_flat() > 1e-7) { 262 | return topo_dir.normalize_flat(); 263 | } else { 264 | _is_wall = true; 265 | return Point3D(0,0,0); 266 | } 267 | } 268 | 269 | template 270 | inline bool PointInsideAPoly(const std::vector& poly, const Point& p) { 271 | // By Randolph Franklin, https://www.eecs.umich.edu/courses/eecs380/HANDOUTS/PROJ2/InsidePoly.html 272 | int i, j, c = 0; 273 | int npol = poly.size(); 274 | if (npol < 3) { 275 | ROS_WARN("The vertices number of a polygon is less than 3."); 276 | return false; 277 | } 278 | for (i = 0, j = npol-1; i < npol; j = i++) { 279 | const Point vetex1 = poly[i]; 280 | const Point vetex2 = poly[j]; 281 | if ((((vetex1.y <= p.y) && (p.y < vetex2.y)) || 282 | ((vetex2.y <= p.y) && (p.y < vetex1.y))) && 283 | (p.x < (vetex2.x - vetex1.x) * (p.y - vetex1.y) / (vetex2.y - vetex1.y) + vetex1.x)) 284 | c = !c; 285 | } 286 | return c; 287 | } 288 | 289 | template 290 | inline bool IsConvexPoint(const std::vector& poly, const Point& ev_p, const Point& free_p) { 291 | const bool in_or_out = this->PointInsideAPoly(poly, free_p); 292 | if (!this->PointInsideAPoly(poly, ev_p) == in_or_out) { 293 | return true; 294 | } 295 | return false; 296 | } 297 | 298 | }; 299 | 300 | #endif -------------------------------------------------------------------------------- /src/boundary_handler/include/boundary_handler/intersection.h: -------------------------------------------------------------------------------- 1 | #ifndef INTERSECTION_H 2 | #define INTERSECTION_H 3 | 4 | // A C++ program to check if two given line segments intersect 5 | // See https://www.geeksforgeeks.org/orientation-3-ordered-points/ 6 | 7 | #include 8 | #include 9 | using namespace std; 10 | using namespace cv; 11 | 12 | // Given three colinear points p, q, r, the function checks if 13 | // point q lies on line segment 'pr' 14 | 15 | namespace POLYOPS { 16 | 17 | static bool onSegment(Point2f p, Point2f q, Point2f r) 18 | { 19 | if (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) && 20 | q.y <= max(p.y, r.y) && q.y >= min(p.y, r.y)) 21 | return true; 22 | 23 | return false; 24 | } 25 | 26 | // To find orientation of ordered triplet (p, q, r). 27 | // The function returns following values 28 | // 0 --> p, q and r are colinear 29 | // 1 --> Clockwise 30 | // 2 --> Counterclockwise 31 | static int orientation(Point2f p, Point2f q, Point2f r) 32 | { 33 | // for details of below formula. 34 | double val = (q.y - p.y) * (r.x - q.x) - 35 | (q.x - p.x) * (r.y - q.y); 36 | 37 | if (abs(val) < 1e-7) return 0; // colinear 38 | 39 | return (val > 0)? 1: 2; // clock or counterclock wise 40 | } 41 | 42 | // The main function that returns true if line segment 'p1q1' 43 | // and 'p2q2' intersect. 44 | static bool doIntersect(Point2f p1, Point2f q1, Point2f p2, Point2f q2) 45 | { 46 | // Find the four orientations needed for general and 47 | // special cases 48 | int o1 = orientation(p1, q1, p2); 49 | int o2 = orientation(p1, q1, q2); 50 | int o3 = orientation(p2, q2, p1); 51 | int o4 = orientation(p2, q2, q1); 52 | 53 | // General case 54 | if (o1 != o2 && o3 != o4) 55 | return true; 56 | 57 | // Special Cases 58 | // p1, q1 and p2 are colinear and p2 lies on segment p1q1 59 | if (o1 == 0 && onSegment(p1, p2, q1)) return true; 60 | 61 | // p1, q1 and q2 are colinear and q2 lies on segment p1q1 62 | if (o2 == 0 && onSegment(p1, q2, q1)) return true; 63 | 64 | // p2, q2 and p1 are colinear and p1 lies on segment p2q2 65 | if (o3 == 0 && onSegment(p2, p1, q2)) return true; 66 | 67 | // p2, q2 and q1 are colinear and q1 lies on segment p2q2 68 | if (o4 == 0 && onSegment(p2, q1, q2)) return true; 69 | 70 | return false; // Doesn't fall in any of the above cases 71 | } 72 | } 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /src/boundary_handler/include/boundary_handler/point_struct.h: -------------------------------------------------------------------------------- 1 | #ifndef POINT_STRUCT_H 2 | #define POINT_STRUCT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "Eigen/Core" 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // ROS message support 18 | #include 19 | 20 | 21 | typedef pcl::PointXYZI PCLPoint; 22 | typedef pcl::PointCloud PointCloud; 23 | typedef pcl::PointCloud::Ptr PointCloudPtr; 24 | 25 | struct Point3D { 26 | float x, y, z; 27 | float intensity; 28 | Point3D() = default; 29 | Point3D(float _x, float _y, float _z):\ 30 | x(_x), y(_y), z(_z), intensity(0) {} 31 | Point3D(float _x, float _y, float _z, float _i):\ 32 | x(_x), y(_y), z(_z), intensity(_i) {} 33 | Point3D(Eigen::Vector3f vec):\ 34 | x(vec(0)), y(vec(1)), z(vec(2)), intensity(0) {} 35 | Point3D(Eigen::Vector3d vec):\ 36 | x(vec(0)), y(vec(1)), z(vec(2)), intensity(0) {} 37 | Point3D(PCLPoint pt):\ 38 | x(pt.x), y(pt.y), z(pt.z), intensity(pt.intensity) {} 39 | 40 | bool operator ==(const Point3D& pt) const 41 | { 42 | return x == pt.x && y == pt.y && z == pt.z; 43 | } 44 | 45 | bool operator !=(const Point3D& pt) const 46 | { 47 | return x != pt.x || y != pt.y || z != pt.z; 48 | } 49 | 50 | float operator *(const Point3D& pt) const 51 | { 52 | return x * pt.x + y * pt.y + z * pt.z; 53 | } 54 | 55 | Point3D operator *(const float factor) const 56 | { 57 | return Point3D(x*factor, y*factor, z*factor); 58 | } 59 | 60 | Point3D operator /(const float factor) const 61 | { 62 | return Point3D(x/factor, y/factor, z/factor); 63 | } 64 | 65 | Point3D operator +(const Point3D& pt) const 66 | { 67 | return Point3D(x+pt.x, y+pt.y, z+pt.z); 68 | } 69 | template 70 | Point3D operator -(const Point& pt) const 71 | { 72 | return Point3D(x-pt.x, y-pt.y, z-pt.z); 73 | } 74 | Point3D operator -() const 75 | { 76 | return Point3D(-x, -y, -z); 77 | } 78 | float norm() const 79 | { 80 | return std::hypotf(x, std::hypotf(y,z)); 81 | }; 82 | 83 | float norm_flat() const 84 | { 85 | return std::hypotf(x, y); 86 | }; 87 | 88 | Point3D normalize() const 89 | { 90 | const float n = std::hypotf(x, std::hypotf(y,z)); 91 | if (n > 1e-7f) { 92 | return Point3D(x/n, y/n, z/n); 93 | } else { 94 | // DEBUG 95 | // ROS_WARN("Point3D: vector normalization fails, vector norm is too small."); 96 | return Point3D(0,0,0); 97 | } 98 | }; 99 | 100 | Point3D normalize_flat() const 101 | { 102 | const float n = std::hypotf(x, y); 103 | if (n > 1e-7f) { 104 | return Point3D(x/n, y/n, 0.0f); 105 | } else { 106 | // DEBUG 107 | // ROS_WARN("Point3D: flat vector normalization fails, vector norm is too small."); 108 | return Point3D(0,0,0); 109 | } 110 | }; 111 | 112 | float norm_dot(Point3D p) const 113 | { 114 | const float n1 = std::hypotf(x, std::hypotf(y,z)); 115 | const float n2 = std::hypotf(p.x, std::hypotf(p.y,p.z)); 116 | if (n1 < 1e-7f || n2 < 1e-7f) { 117 | // DEBUG 118 | // ROS_WARN("Point3D: vector norm dot fails, vector norm is too small."); 119 | return 0.f; 120 | } 121 | const float dot_value = (x * p.x + y * p.y + z * p.z) / (n1 * n2); 122 | return std::min(std::max(-1.0f, dot_value), 1.0f); 123 | }; 124 | 125 | float norm_flat_dot(Point3D p) const 126 | { 127 | const float n1 = std::hypotf(x, y); 128 | const float n2 = std::hypotf(p.x, p.y); 129 | if (n1 < 1e-7f || n2 < 1e-7f) { 130 | // DEBUG 131 | // ROS_WARN("Point3D: flat vector norm dot fails, vector norm is too small."); 132 | return 0.f; 133 | } 134 | const float dot_value = (x * p.x + y * p.y) / (n1 * n2); 135 | return std::min(std::max(-1.0f, dot_value), 1.0f); 136 | }; 137 | 138 | std::string ToString() const 139 | { 140 | return "x = " + std::to_string(x).substr(0,6) + "; " + 141 | "y = " + std::to_string(y).substr(0,6) + "; " + 142 | "z = " + std::to_string(z).substr(0,6) + " "; 143 | }; 144 | 145 | friend std::ostream& operator<<(std::ostream& os, const Point3D& p) 146 | { 147 | os << "["<< p.ToString() << "] "; 148 | return os; 149 | } 150 | }; 151 | 152 | struct point_hash 153 | { 154 | std::size_t operator() (const Point3D& p) const 155 | { 156 | std::size_t seed = 0; 157 | boost::hash_combine(seed, p.x); 158 | boost::hash_combine(seed, p.y); 159 | boost::hash_combine(seed, p.z); 160 | return seed; 161 | } 162 | }; 163 | 164 | struct point_comp 165 | { 166 | bool operator()(const Point3D& p1, const Point3D& p2) const 167 | { 168 | return p1.x == p2.x && p1.y == p2.y && p1.z == p2.z; 169 | } 170 | }; 171 | 172 | struct intensity_comp 173 | { 174 | bool operator()(const Point3D& p1, const Point3D& p2) const 175 | { 176 | return p1.intensity < p2.intensity; 177 | } 178 | }; 179 | 180 | #endif -------------------------------------------------------------------------------- /src/boundary_handler/launch/boundary_handler.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/boundary_handler/package.xml: -------------------------------------------------------------------------------- 1 | 2 | boundary_handler 3 | 0.0.1 4 | Boundary Graph Extractor 5 | Fan Yang 6 | BSD 7 | 8 | catkin 9 | 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | visibility_graph_msg 14 | pcl_ros 15 | 16 | roscpp 17 | std_msgs 18 | sensor_msgs 19 | visibility_graph_msg 20 | pcl_ros 21 | 22 | -------------------------------------------------------------------------------- /src/boundary_handler/rviz/default.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /BoundaryGraph1/Namespaces1 8 | Splitter Ratio: 0.8294117450714111 9 | Tree Height: 1234 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: ~ 14 | Name: Tool Properties 15 | Splitter Ratio: 0.5886790156364441 16 | - Class: rviz/Views 17 | Expanded: 18 | - /Current View1 19 | Name: Views 20 | Splitter Ratio: 0.5 21 | - Class: rviz/Time 22 | Experimental: false 23 | Name: Time 24 | SyncMode: 0 25 | SyncSource: OverallMap 26 | Preferences: 27 | PromptSaveOnExit: true 28 | Toolbars: 29 | toolButtonStyle: 2 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.10000000149011612 34 | Autocompute Intensity Bounds: true 35 | Autocompute Value Bounds: 36 | Max Value: 10 37 | Min Value: -10 38 | Value: true 39 | Axis: Z 40 | Channel Name: intensity 41 | Class: rviz/PointCloud2 42 | Color: 255; 255; 255 43 | Color Transformer: Intensity 44 | Decay Time: 0 45 | Enabled: true 46 | Invert Rainbow: false 47 | Max Color: 255; 255; 255 48 | Min Color: 0; 0; 0 49 | Name: OverallMap 50 | Position Transformer: XYZ 51 | Queue Size: 10 52 | Selectable: true 53 | Size (Pixels): 2 54 | Size (m): 0.009999999776482582 55 | Style: Points 56 | Topic: /overall_map 57 | Unreliable: false 58 | Use Fixed Frame: true 59 | Use rainbow: true 60 | Value: true 61 | - Class: rviz/MarkerArray 62 | Enabled: true 63 | Marker Topic: /graph_decoder_viz 64 | Name: BoundaryGraph 65 | Namespaces: 66 | angle_direct: false 67 | boundary_edge: true 68 | freespace_vertex: false 69 | freespace_vgraph: true 70 | global_vertex: false 71 | global_vgraph: false 72 | polygon_edge: true 73 | vertex_angle: true 74 | visibility_edge: false 75 | Queue Size: 100 76 | Value: true 77 | - Class: rviz/Marker 78 | Enabled: true 79 | Marker Topic: /free_p_viz 80 | Name: StartPoint 81 | Namespaces: 82 | free_position: true 83 | Queue Size: 100 84 | Value: true 85 | Enabled: true 86 | Global Options: 87 | Background Color: 0; 0; 0 88 | Default Light: true 89 | Fixed Frame: map 90 | Frame Rate: 15 91 | Name: root 92 | Tools: 93 | - Class: rviz/MoveCamera 94 | Value: true 95 | Views: 96 | Current: 97 | Class: rviz/Orbit 98 | Distance: 113.25836181640625 99 | Enable Stereo Rendering: 100 | Stereo Eye Separation: 0.05999999865889549 101 | Stereo Focal Distance: 1 102 | Swap Stereo Eyes: false 103 | Value: false 104 | Focal Point: 105 | X: 34.276573181152344 106 | Y: 24.689598083496094 107 | Z: -8.78692626953125 108 | Focal Shape Fixed Size: true 109 | Focal Shape Size: 0.05000000074505806 110 | Invert Z Axis: false 111 | Name: Current View 112 | Near Clip Distance: 0.009999999776482582 113 | Pitch: 1.5697963237762451 114 | Target Frame: 115 | Value: Orbit (rviz) 116 | Yaw: 4.713589668273926 117 | Saved: ~ 118 | Window Geometry: 119 | Displays: 120 | collapsed: true 121 | Height: 818 122 | Hide Left Dock: true 123 | Hide Right Dock: true 124 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000050ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000050f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006970000003efc0100000002fb0000000800540069006d0065000000000000000697000004f300fffffffb0000000800540069006d006501000000000000045000000000000000000000035c000002d800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 125 | Selection: 126 | collapsed: false 127 | Time: 128 | collapsed: false 129 | Tool Properties: 130 | collapsed: false 131 | Views: 132 | collapsed: true 133 | Width: 860 134 | X: 1306 135 | Y: 305 136 | -------------------------------------------------------------------------------- /src/far_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(far_planner) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | set(CMAKE_BUILD_TYPE Release) 7 | set(BUILD_STATIC_LIBS ON) 8 | set(BUILD_SHARED_LIBS OFF) 9 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 10 | 11 | ## Find catkin macros and libraries 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | std_msgs 15 | sensor_msgs 16 | visibility_graph_msg 17 | pcl_ros 18 | ) 19 | 20 | find_package(PCL REQUIRED) 21 | find_package(Eigen3 REQUIRED) 22 | find_package(OpenCV REQUIRED) 23 | 24 | ################################### 25 | ## catkin specific configuration ## 26 | ################################### 27 | 28 | catkin_package( 29 | CATKIN_DEPENDS 30 | roscpp 31 | std_msgs 32 | sensor_msgs 33 | pcl_ros 34 | ) 35 | 36 | ########### 37 | ## Build ## 38 | ########### 39 | 40 | ## Specify additional locations of header files 41 | ## Your package locations should be listed before other locations 42 | include_directories( 43 | ${catkin_INCLUDE_DIRS} 44 | ${PCL_INCLUDE_DIRS} 45 | ${EIGEN3_INCLUDE_DIR} 46 | ${OpenCV_INCLUDE_DIRS} 47 | "${PROJECT_SOURCE_DIR}/include" 48 | /usr/local/include # Location when using 'make system_install' 49 | /usr/include # More usual location (e.g. when installing using a package) 50 | ) 51 | 52 | ## Specify additional locations for library files 53 | link_directories( 54 | /usr/local/lib # Location when using 'make system_install' 55 | /usr/lib # More usual location (e.g. when installing using a package) 56 | ) 57 | 58 | ## Depedent cpp files 59 | set(SOURCES ${SOURCES} 60 | src/dynamic_graph.cpp 61 | src/planner_visualizer.cpp 62 | src/utility.cpp 63 | src/graph_planner.cpp 64 | src/contour_detector.cpp 65 | src/contour_graph.cpp 66 | src/map_handler.cpp 67 | src/scan_handler.cpp 68 | src/graph_msger.cpp 69 | src/terrain_planner.cpp 70 | ) 71 | 72 | ## Declare executables 73 | add_executable(${PROJECT_NAME} src/far_planner.cpp ${SOURCES}) 74 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 75 | 76 | ## Specify libraries to link a library or executable target against 77 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) 78 | 79 | install(TARGETS ${PROJECT_NAME} 80 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 81 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 82 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 83 | ) 84 | 85 | install(DIRECTORY launch/ 86 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 87 | ) 88 | -------------------------------------------------------------------------------- /src/far_planner/config/default.yaml: -------------------------------------------------------------------------------- 1 | # Dynamic Planner Default Params 2 | main_run_freq : 2.5 3 | voxel_dim : 0.15 # Unit: meter 4 | robot_dim : 0.8 # Unit: meter 5 | vehicle_height : 0.75 # Unit: meter 6 | sensor_range : 30.0 # Unit: meter 7 | terrain_range : 15.0 # Unit: meter 8 | local_planner_range : 5.0 # Unit: meter 9 | visualize_ratio : 0.75 10 | is_viewpoint_extend : true 11 | is_multi_layer : false 12 | is_opencv_visual : false # Obstalce Image Display 13 | is_static_env : true 14 | is_pub_boundary : false 15 | is_debug_output : false 16 | is_attempt_autoswitch : true # Auto switch to attemptable navigation 17 | world_frame : map 18 | 19 | # Graph Messager 20 | GraphMsger/robot_id : 1 # graph from robot id "0" is extracted from files 21 | 22 | # Map Handler Params 23 | MapHandler/floor_height : 2.0 # Unit: meter 24 | MapHandler/cell_length : 5.0 # Unit: meter 25 | MapHandler/map_grid_max_length : 1000.0 # Unit: meter 26 | MapHandler/map_grad_max_height : 100.0 # Unit: meter 27 | 28 | # Dynamic Planner Utility Params 29 | Util/angle_noise : 15.0 # Unit: degree 30 | Util/accept_max_align_angle : 4.0 # Unit: degree 31 | Util/obs_inflate_size : 1 32 | Util/new_intensity_thred : 2.0 33 | Util/terrain_free_Z : 0.2 34 | Util/dyosb_update_thred : 4 35 | Util/new_point_counter : 5 36 | Util/dynamic_obs_dacay_time : 10.0 # Unit: second 37 | Util/new_points_decay_time : 1.0 # Unit: second 38 | 39 | # Dynamic Graph Params 40 | Graph/connect_votes_size : 10 41 | Graph/clear_dumper_thred : 4 42 | Graph/node_finalize_thred : 6 43 | Graph/filter_pool_size : 12 44 | 45 | # Corner Detector Params 46 | CDetector/resize_ratio : 3.0 47 | CDetector/filter_count_value : 3 48 | CDetector/is_save_img : false 49 | CDetector/img_folder_path : /path 50 | 51 | # Graph Planner Params 52 | GPlanner/converge_distance : 0.5 # Unit: meter 53 | GPlanner/goal_adjust_radius : 2.0 # Unit: meter 54 | GPlanner/free_counter_thred : 7 55 | GPlanner/reach_goal_vote_size : 3 56 | GPlanner/path_momentum_thred : 3 -------------------------------------------------------------------------------- /src/far_planner/config/matterport.yaml: -------------------------------------------------------------------------------- 1 | # Dynamic Planner Default Params 2 | main_run_freq : 2.5 3 | voxel_dim : 0.1 # Unit: meter 4 | robot_dim : 0.5 # Unit: meter 5 | vehicle_height : 0.5 # Unit: meter 6 | sensor_range : 15.0 # Unit: meter 7 | terrain_range : 7.5 # Unit: meter 8 | local_planner_range : 2.5 # Unit: meter 9 | visualize_ratio : 0.4 10 | is_viewpoint_extend : true 11 | is_multi_layer : false 12 | is_opencv_visual : false # Obstalce Image Display 13 | is_static_env : true 14 | is_pub_boundary : false 15 | is_debug_output : false 16 | is_attempt_autoswitch : true # Auto switch to attemptable navigation 17 | world_frame : map 18 | 19 | # Graph Messager 20 | GraphMsger/robot_id : 1 # graph from robot id "0" is extracted from files 21 | 22 | # Map Handler Params 23 | MapHandler/floor_height : 2.0 # Unit: meter 24 | MapHandler/cell_length : 2.5 # Unit: meter 25 | MapHandler/map_grid_max_length : 200.0 # Unit: meter 26 | MapHandler/map_grad_max_height : 10.0 # Unit: meter 27 | 28 | # Dynamic Planner Utility Params 29 | Util/angle_noise : 15.0 # Unit: degree 30 | Util/accept_max_align_angle : 4.0 # Unit: degree 31 | Util/obs_inflate_size : 1 32 | Util/new_intensity_thred : 2.0 33 | Util/terrain_free_Z : 0.15 34 | Util/dyosb_update_thred : 4 35 | Util/new_point_counter : 5 36 | Util/dynamic_obs_dacay_time : 10.0 # Unit: second 37 | Util/new_points_decay_time : 1.0 # Unit: second 38 | 39 | # Dynamic Graph Params 40 | Graph/connect_votes_size : 10 41 | Graph/clear_dumper_thred : 4 42 | Graph/node_finalize_thred : 6 43 | Graph/filter_pool_size : 12 44 | 45 | # Corner Detector Params 46 | CDetector/resize_ratio : 3.0 47 | CDetector/filter_count_value : 3 48 | CDetector/is_save_img : false 49 | CDetector/img_folder_path : /path 50 | 51 | # Graph Planner Params 52 | GPlanner/converge_distance : 0.25 # Unit: meter 53 | GPlanner/goal_adjust_radius : 1.0 # Unit: meter 54 | GPlanner/free_counter_thred : 7 55 | GPlanner/reach_goal_vote_size : 3 56 | GPlanner/path_momentum_thred : 3 -------------------------------------------------------------------------------- /src/far_planner/data/boundary.ply: -------------------------------------------------------------------------------- 1 | ply 2 | format ascii 1.0 3 | element vertex 25 4 | property float x 5 | property float y 6 | property float z 7 | property float poly_index 8 | end_header 9 | 74.0 60.0 0.75 0 10 | 74.0 -10.0 0.75 0 11 | -5.0 -10.0 0.75 0 12 | -5.5 60.0 0.75 0 13 | 8.0 44.0 0.75 1 14 | 12.0 44.0 0.75 1 15 | 10.0 41.0 0.75 1 16 | 23.0 36.0 0.75 2 17 | 20.0 32.0 0.75 2 18 | 23.0 28.0 0.75 2 19 | 26.0 32.0 0.75 2 20 | 42.0 54.0 0.75 3 21 | 57.0 54.0 0.75 3 22 | 57.0 49.0 0.75 3 23 | 42.0 49.0 0.75 3 24 | 24.5 22.0 0.75 4 25 | 24.5 2.0 0.75 4 26 | 33.0 2.0 0.75 4 27 | 33.0 -5.0 0.75 4 28 | 46.0 -5.0 0.75 4 29 | 46.0 22.0 0.75 4 30 | 15.0 36.0 0.75 5 31 | 15.0 12.0 0.75 5 32 | 7.5 12.0 0.75 5 33 | 7.5 36.0 0.75 5 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /src/far_planner/data/boundary_graph.vgh: -------------------------------------------------------------------------------- 1 | 0 2 74.000000 60.000000 0.750000 -1.000000 0.000000 0.000000 0.000000 -1.000000 0.000000 1 0 0 1 1 3 | 1 3 | 1 3 | 2 | 1 2 74.000000 -10.000000 0.750000 0.000000 1.000000 0.000000 -1.000000 0.000000 0.000000 1 0 0 1 0 2 | 0 2 | 0 2 | 3 | 2 2 -5.000000 -10.000000 0.750000 1.000000 0.000000 0.000000 -0.007143 0.999974 0.000000 1 0 0 1 1 3 | 1 3 | 1 3 | 4 | 3 2 -5.500000 60.000000 0.750000 0.007143 -0.999974 0.000000 1.000000 0.000000 0.000000 1 0 0 1 0 2 | 0 2 | 2 0 | 5 | 4 1 8.000000 44.000000 0.750000 0.554700 -0.832050 0.000000 1.000000 0.000000 0.000000 1 0 0 1 5 6 11 13 24 | 5 6 11 13 24 | 5 6 | 6 | 5 1 12.000000 44.000000 0.750000 -1.000000 0.000000 0.000000 -0.554700 -0.832050 0.000000 1 0 0 1 4 6 7 8 20 21 24 | 4 6 7 8 20 21 24 | 4 6 | 7 | 6 1 10.000000 41.000000 0.750000 0.554700 0.832050 0.000000 -0.554700 0.832050 0.000000 1 0 0 1 4 5 7 11 13 21 | 4 5 7 11 13 21 | 5 4 | 8 | 7 1 23.000000 36.000000 0.750000 0.600000 -0.800000 0.000000 -0.600000 -0.800000 0.000000 1 0 0 1 5 6 8 10 11 13 20 | 5 6 8 10 11 13 20 | 8 10 | 9 | 8 1 20.000000 32.000000 0.750000 0.600000 0.800000 0.000000 0.600000 -0.800000 0.000000 1 0 0 1 5 7 9 16 22 | 5 7 9 16 22 | 7 9 | 10 | 9 1 23.000000 28.000000 0.750000 -0.600000 0.800000 0.000000 0.600000 0.800000 0.000000 1 0 0 1 8 10 13 20 21 | 8 10 13 20 21 | 8 10 | 11 | 10 1 26.000000 32.000000 0.750000 -0.600000 -0.800000 0.000000 -0.600000 0.800000 0.000000 1 0 0 1 7 9 11 15 22 | 7 9 11 15 22 | 9 7 | 12 | 11 1 42.000000 54.000000 0.750000 0.000000 -1.000000 0.000000 1.000000 0.000000 0.000000 1 0 0 1 4 6 7 10 12 14 15 22 24 | 4 6 7 10 12 14 15 22 24 | 12 14 | 13 | 12 1 57.000000 54.000000 0.750000 -1.000000 0.000000 0.000000 0.000000 -1.000000 0.000000 1 0 0 1 11 13 | 11 13 | 11 13 | 14 | 13 1 57.000000 49.000000 0.750000 0.000000 1.000000 0.000000 -1.000000 0.000000 0.000000 1 0 0 1 4 6 7 9 12 14 15 19 24 | 4 6 7 9 12 14 15 19 24 | 12 14 | 15 | 14 1 42.000000 49.000000 0.750000 1.000000 0.000000 0.000000 0.000000 1.000000 0.000000 1 0 0 1 11 13 20 | 11 13 20 | 13 11 | 16 | 15 1 24.500000 22.000000 0.750000 1.000000 0.000000 0.000000 0.000000 -1.000000 0.000000 1 0 0 1 10 11 13 16 20 22 | 10 11 13 16 20 22 | 16 20 | 17 | 16 1 24.500000 2.000000 0.750000 0.000000 1.000000 0.000000 1.000000 0.000000 0.000000 1 0 0 1 8 15 17 18 21 23 | 8 15 17 18 21 23 | 15 17 | 18 | 17 2 33.000000 2.000000 0.750000 -1.000000 0.000000 0.000000 0.000000 -1.000000 0.000000 1 0 0 1 16 18 | 16 18 | 16 18 | 19 | 18 1 33.000000 -5.000000 0.750000 0.000000 1.000000 0.000000 1.000000 0.000000 0.000000 1 0 0 1 16 17 19 23 | 16 17 19 23 | 17 19 | 20 | 19 1 46.000000 -5.000000 0.750000 -1.000000 0.000000 0.000000 0.000000 1.000000 0.000000 1 0 0 1 13 18 20 | 13 18 20 | 18 20 | 21 | 20 1 46.000000 22.000000 0.750000 0.000000 -1.000000 0.000000 -1.000000 0.000000 0.000000 1 0 0 1 5 7 9 14 15 19 | 5 7 9 14 15 19 | 19 15 | 22 | 21 1 15.000000 36.000000 0.750000 -1.000000 0.000000 0.000000 0.000000 -1.000000 0.000000 1 0 0 1 5 6 9 16 22 24 | 5 6 9 16 22 24 | 22 24 | 23 | 22 1 15.000000 12.000000 0.750000 0.000000 1.000000 0.000000 -1.000000 0.000000 0.000000 1 0 0 1 8 10 11 15 21 23 | 8 10 11 15 21 23 | 21 23 | 24 | 23 1 7.500000 12.000000 0.750000 1.000000 0.000000 0.000000 0.000000 1.000000 0.000000 1 0 0 1 16 18 22 24 | 16 18 22 24 | 22 24 | 25 | 24 1 7.500000 36.000000 0.750000 0.000000 -1.000000 0.000000 1.000000 0.000000 0.000000 1 0 0 1 4 5 11 13 21 23 | 4 5 11 13 21 23 | 23 21 | 26 | -------------------------------------------------------------------------------- /src/far_planner/data/trajectory.txt: -------------------------------------------------------------------------------- 1 | 0 0 0.75 0 0 0 0 2 | -------------------------------------------------------------------------------- /src/far_planner/include/far_planner/contour_detector.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTOUR_DETECTOR_H 2 | #define CONTOUR_DETECTOR_H 3 | 4 | #include "utility.h" 5 | 6 | 7 | struct ContourDetectParams { 8 | ContourDetectParams() = default; 9 | float sensor_range; 10 | float voxel_dim; 11 | float kRatio; 12 | int kThredValue; 13 | int kBlurSize; 14 | bool is_save_img; 15 | std::string img_path; 16 | }; 17 | 18 | class ContourDetector { 19 | private: 20 | Point3D odom_pos_; 21 | cv::Point2f free_odom_resized_; 22 | ContourDetectParams cd_params_; 23 | PointCloudPtr new_corners_cloud_; 24 | cv::Mat img_mat_; 25 | std::size_t img_counter_; 26 | std::vector refined_contours_; 27 | std::vector refined_hierarchy_; 28 | NavNodePtr odom_node_ptr_; 29 | 30 | int MAT_SIZE, CMAT; 31 | int MAT_RESIZE, CMAT_RESIZE; 32 | float DIST_LIMIT; 33 | float ALIGN_ANGLE_COS; 34 | float VOXEL_DIM_INV; 35 | 36 | 37 | void UpdateImgMatWithCloud(const PointCloudPtr& pc, cv::Mat& img_mat); 38 | 39 | void ExtractContourFromImg(const cv::Mat& img, 40 | std::vector& img_contours, 41 | std::vector& realworld_contour); 42 | 43 | void ExtractRefinedContours(const cv::Mat& imgIn, 44 | std::vector& refined_contours); 45 | 46 | void ResizeAndBlurImg(const cv::Mat& img, cv::Mat& Rimg); 47 | 48 | void ConvertContoursToRealWorld(const std::vector& ori_contours, 49 | std::vector& realWorld_contours); 50 | 51 | void TopoFilterContours(std::vector& contoursInOut); 52 | 53 | void AdjecentDistanceFilter(std::vector& contoursInOut); 54 | 55 | /* inline functions */ 56 | inline void UpdateOdom(const NavNodePtr& odom_node_ptr) { 57 | odom_pos_ = odom_node_ptr->position; 58 | odom_node_ptr_ = odom_node_ptr; 59 | free_odom_resized_ = ConvertPoint3DToCVPoint(FARUtil::free_odom_p, odom_pos_, true); 60 | } 61 | 62 | inline void ConvertCVToPoint3DVector(const CVPointStack& cv_vec, 63 | PointStack& p_vec, 64 | const bool& is_resized_img) { 65 | const std::size_t vec_size = cv_vec.size(); 66 | p_vec.clear(), p_vec.resize(vec_size); 67 | for (std::size_t i=0; i& hierarchy, 88 | const std::size_t& high_idx, 89 | std::unordered_set& internal_idxs) 90 | { 91 | if (hierarchy[high_idx][2] == -1) return; 92 | SameAndLowLevelIdxs(hierarchy, hierarchy[high_idx][2], internal_idxs); 93 | } 94 | 95 | inline void SameAndLowLevelIdxs(const std::vector& hierarchy, 96 | const std::size_t& cur_idx, 97 | std::unordered_set& remove_idxs) 98 | { 99 | if (cur_idx == -1) return; 100 | int next_idx = cur_idx; 101 | while (next_idx != -1) { 102 | remove_idxs.insert(next_idx); 103 | SameAndLowLevelIdxs(hierarchy, hierarchy[next_idx][2], remove_idxs); 104 | next_idx = hierarchy[next_idx][0]; 105 | } 106 | } 107 | 108 | template 109 | inline cv::Point2f ConvertPoint3DToCVPoint(const Point& p, 110 | const Point3D& c_pos, 111 | const bool& is_resized_img=false) { 112 | cv::Point2f cv_p; 113 | int row_idx, col_idx; 114 | this->PointToImgSub(p, c_pos, row_idx, col_idx, is_resized_img); 115 | cv_p.x = col_idx; 116 | cv_p.y = row_idx; 117 | return cv_p; 118 | } 119 | 120 | 121 | template 122 | inline void PointToImgSub(const Point& posIn, const Point3D& c_posIn, 123 | int& row_idx, int& col_idx, 124 | const bool& is_resized_img=false, 125 | const bool& is_crop_idx=true) 126 | { 127 | const float ratio = is_resized_img ? cd_params_.kRatio : 1.0f; 128 | const int c_idx = is_resized_img ? CMAT_RESIZE : CMAT; 129 | row_idx = c_idx + (int)std::round((posIn.x - c_posIn.x) * VOXEL_DIM_INV * ratio); 130 | col_idx = c_idx + (int)std::round((posIn.y - c_posIn.y) * VOXEL_DIM_INV * ratio); 131 | if (is_crop_idx) { 132 | CropIdxes(row_idx, col_idx, is_resized_img); 133 | } 134 | } 135 | 136 | inline void CropIdxes(int& row_idx, int& col_idx, const bool& is_resized_img=false) { 137 | const int max_size = is_resized_img ? MAT_RESIZE : MAT_SIZE; 138 | row_idx = (int)std::max(std::min(row_idx, max_size-1),0); 139 | col_idx = (int)std::max(std::min(col_idx, max_size-1),0); 140 | } 141 | 142 | inline bool IsIdxesInImg(int& row_idx, int& col_idx, const bool& is_resized_img=false) { 143 | const int max_size = is_resized_img ? MAT_RESIZE : MAT_SIZE; 144 | if (row_idx < 0 || row_idx > max_size-1 || col_idx < 0 || col_idx > max_size-1) { 145 | return false; 146 | } 147 | return true; 148 | } 149 | 150 | inline void ResetImgMat(cv::Mat& img_mat) { 151 | img_mat.release(); 152 | img_mat = cv::Mat::zeros(MAT_SIZE, MAT_SIZE, CV_32FC1); 153 | } 154 | 155 | inline Point3D ConvertCVPointToPoint3D(const cv::Point2f& cv_p, 156 | const Point3D& c_pos, 157 | const bool& is_resized_img=false) { 158 | Point3D p; 159 | const int c_idx = is_resized_img ? CMAT_RESIZE : CMAT; 160 | const float ratio = is_resized_img ? cd_params_.kRatio : 1.0f; 161 | p.x = (cv_p.y - c_idx) * cd_params_.voxel_dim / ratio + c_pos.x; 162 | p.y = (cv_p.x - c_idx) * cd_params_.voxel_dim / ratio + c_pos.y; 163 | p.z = odom_pos_.z; 164 | return p; 165 | } 166 | 167 | inline void SaveCurrentImg(const cv::Mat& img) { 168 | if (img.empty()) return; 169 | cv::Mat img_save; 170 | img.convertTo(img_save, CV_8UC1, 255); 171 | std::string filename = std::to_string(img_counter_); 172 | std::string img_name = cd_params_.img_path + filename + ".tiff"; 173 | cv::imwrite(img_name, img_save); 174 | if (FARUtil::IsDebug) ROS_WARN_THROTTLE(1.0, "CD: image save success!"); 175 | img_counter_ ++; 176 | } 177 | 178 | inline bool IsPrevWallVertex(const cv::Point2f& first_p, 179 | const cv::Point2f& mid_p, 180 | const cv::Point2f& add_p) 181 | { 182 | cv::Point2f diff_p1 = first_p - mid_p; 183 | cv::Point2f diff_p2 = add_p - mid_p; 184 | diff_p1 /= std::hypotf(diff_p1.x, diff_p1.y); 185 | diff_p2 /= std::hypotf(diff_p2.x, diff_p2.y); 186 | if (abs(diff_p1.dot(diff_p2)) > ALIGN_ANGLE_COS) return true; 187 | return false; 188 | } 189 | 190 | inline void CopyContours(const std::vector>& raw_contours, 191 | std::vector>& contours) 192 | { 193 | const std::size_t N = raw_contours.size(); 194 | contours.clear(), contours.resize(N); 195 | for (std::size_t i=0; i>& filtered_contours, 206 | std::vector>& round_contours) 207 | { 208 | const std::size_t N = filtered_contours.size(); 209 | round_contours.clear(), round_contours.resize(N); 210 | for (std::size_t i=0; i& realworl_contour); 236 | 237 | /** 238 | * Show Corners on Pointcloud projection image 239 | * @param img_mat pointcloud projection image 240 | * @param point_vec corners vector detected from cv corner detector 241 | */ 242 | void ShowCornerImage(const cv::Mat& img_mat, 243 | const PointCloudPtr& pc); 244 | /* Get Internal Values */ 245 | const PointCloudPtr GetNewVertices() const { return new_corners_cloud_;}; 246 | const cv::Mat GetCloudImgMat() const { return img_mat_;}; 247 | }; 248 | 249 | #endif 250 | -------------------------------------------------------------------------------- /src/far_planner/include/far_planner/contour_graph.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTOUR_GRAPH_H 2 | #define CONTOUR_GRAPH_H 3 | 4 | #include "utility.h" 5 | 6 | struct ConnectPair 7 | { 8 | cv::Point2f start_p; 9 | cv::Point2f end_p; 10 | 11 | ConnectPair() = default; 12 | ConnectPair(const cv::Point2f& p1, const cv::Point2f& p2):start_p(p1), end_p(p2) {} 13 | ConnectPair(const Point3D& p1, const Point3D& p2) { 14 | this->start_p.x = p1.x; 15 | this->start_p.y = p1.y; 16 | this->end_p.x = p2.x; 17 | this->end_p.y = p2.y; 18 | } 19 | 20 | bool operator ==(const ConnectPair& pt) const 21 | { 22 | return (this->start_p == pt.start_p && this->end_p == pt.end_p) || (this->start_p == pt.end_p && this->end_p == pt.start_p); 23 | } 24 | }; 25 | 26 | struct HeightPair 27 | { 28 | float minH; 29 | float maxH; 30 | HeightPair() = default; 31 | HeightPair(const float& minV, const float& maxV):minH(minV), maxH(maxV) {} 32 | HeightPair(const Point3D& p1, const Point3D p2) { 33 | this->minH = std::min(p1.z, p2.z); 34 | this->maxH = std::max(p1.z, p2.z); 35 | } 36 | }; 37 | 38 | struct ContourGraphParams { 39 | ContourGraphParams() = default; 40 | float kPillarPerimeter; 41 | }; 42 | 43 | class ContourGraph { 44 | public: 45 | ContourGraph() = default; 46 | ~ContourGraph() = default; 47 | 48 | static CTNodeStack contour_graph_; 49 | static std::vector global_contour_; 50 | static std::vector inactive_contour_; 51 | static std::vector unmatched_contour_; 52 | static std::vector boundary_contour_; 53 | static std::vector local_boundary_; 54 | 55 | void Init(const ContourGraphParams& params); 56 | 57 | // static functions 58 | void UpdateContourGraph(const NavNodePtr& odom_node_ptr, 59 | const std::vector>& filtered_contours); 60 | 61 | /* Match current contour with global navigation nodes */ 62 | void MatchContourWithNavGraph(const NodePtrStack& global_nodes, 63 | const NodePtrStack& near_nodes, 64 | CTNodeStack& new_convex_vertices); 65 | 66 | void ExtractGlobalContours(); 67 | 68 | static NavNodePtr MatchOutrangeNodeWithCTNode(const NavNodePtr& out_node_ptr, const NodePtrStack& near_nodes); 69 | 70 | static bool IsContourLineMatch(const NavNodePtr& inNode_ptr, const NavNodePtr& outNode_ptr, CTNodePtr& matched_ctnode); 71 | 72 | static bool IsNavNodesConnectFromContour(const NavNodePtr& node_ptr1, 73 | const NavNodePtr& node_ptr2); 74 | 75 | static bool IsCTNodesConnectFromContour(const CTNodePtr& ctnode1, 76 | const CTNodePtr& ctnode2); 77 | 78 | static bool IsNavNodesConnectFreePolygon(const NavNodePtr& node_ptr1, 79 | const NavNodePtr& node_ptr2); 80 | 81 | static bool IsNavToGoalConnectFreePolygon(const NavNodePtr& node_ptr, 82 | const NavNodePtr& goal_ptr); 83 | 84 | static bool IsPoint3DConnectFreePolygon(const Point3D& p1, const Point3D& p2); 85 | 86 | static bool IsEdgeCollideBoundary(const Point3D& p1, const Point3D& p2); 87 | 88 | static bool IsPointsConnectFreePolygon(const ConnectPair& cedge, 89 | const ConnectPair& bd_cedge, 90 | const HeightPair h_pair, 91 | const bool& is_global_check); 92 | 93 | static inline void MatchCTNodeWithNavNode(const CTNodePtr& ctnode_ptr, const NavNodePtr& node_ptr) { 94 | if (ctnode_ptr == NULL || node_ptr == NULL) return; 95 | ctnode_ptr->is_global_match = true; 96 | ctnode_ptr->nav_node_id = node_ptr->id; 97 | node_ptr->ctnode = ctnode_ptr; 98 | node_ptr->is_contour_match = true; 99 | } 100 | 101 | static bool ReprojectPointOutsidePolygons(Point3D& point, const float& free_radius); 102 | 103 | static void AddContourToSets(const NavNodePtr& node_ptr1, const NavNodePtr& node_ptr2); 104 | 105 | static void DeleteContourFromSets(const NavNodePtr& node_ptr1, const NavNodePtr& node_ptr2); 106 | 107 | bool IsPointInVetexAngleRestriction(const CTNodePtr& ctnode, const Point3D end_p); 108 | 109 | void ResetCurrentContour(); 110 | 111 | private: 112 | 113 | static CTNodeStack polys_ctnodes_; 114 | static PolygonStack contour_polygons_; 115 | ContourGraphParams ctgraph_params_; 116 | float ALIGN_ANGLE_COS; 117 | NavNodePtr odom_node_ptr_ = NULL; 118 | bool is_robot_inside_poly_ = false; 119 | 120 | // global contour set 121 | static std::unordered_set global_contour_set_; 122 | static std::unordered_set boundary_contour_set_; 123 | 124 | 125 | /* static private functions */ 126 | inline void AddCTNodeToGraph(const CTNodePtr& ctnode_ptr) { 127 | if (ctnode_ptr == NULL && ctnode_ptr->free_direct == NodeFreeDirect::UNKNOW) { 128 | if (FARUtil::IsDebug) ROS_ERROR_THROTTLE(1.0, "CG: Add ctnode to contour graph fails, ctnode is invaild."); 129 | return; 130 | } 131 | ContourGraph::contour_graph_.push_back(ctnode_ptr); 132 | } 133 | 134 | inline void AddPolyToContourPolygon(const PolygonPtr& poly_ptr) { 135 | if (poly_ptr == NULL || poly_ptr->vertices.empty()) { 136 | if (FARUtil::IsDebug) ROS_ERROR_THROTTLE(1.0, "CG: Add polygon fails, polygon is invaild."); 137 | return; 138 | } 139 | ContourGraph::contour_polygons_.push_back(poly_ptr); 140 | } 141 | 142 | inline bool IsActiveEdge(const NavNodePtr& node_ptr1, const NavNodePtr& node_ptr2) { 143 | if (node_ptr1->is_active && node_ptr2->is_active) { 144 | return true; 145 | } 146 | return false; 147 | } 148 | 149 | inline void AddConnect(const CTNodePtr& ctnode_ptr1, const CTNodePtr& ctnode_ptr2) { 150 | if (ctnode_ptr1 != ctnode_ptr2 && 151 | !FARUtil::IsTypeInStack(ctnode_ptr2, ctnode_ptr1->connect_nodes) && 152 | !FARUtil::IsTypeInStack(ctnode_ptr1, ctnode_ptr2->connect_nodes)) 153 | { 154 | ctnode_ptr1->connect_nodes.push_back(ctnode_ptr2); 155 | ctnode_ptr2->connect_nodes.push_back(ctnode_ptr1); 156 | } 157 | } 158 | 159 | template 160 | static inline bool IsInMatchHeight(const NodeType1& node_ptr1, const NodeType2& node_ptr2) { 161 | if (abs(node_ptr1->position.z - node_ptr2->position.z) < FARUtil::kTolerZ) { 162 | return true; 163 | } 164 | return false; 165 | } 166 | 167 | static inline bool IsEdgeOverlapInHeight(const HeightPair& cur_hpair, HeightPair ref_hpair, const bool is_extend=true) { 168 | if (is_extend) { 169 | ref_hpair.minH -= FARUtil::kTolerZ, ref_hpair.maxH += FARUtil::kTolerZ; 170 | } 171 | if (cur_hpair.maxH < ref_hpair.minH || cur_hpair.minH > ref_hpair.maxH) { 172 | return false; 173 | } 174 | return true; 175 | } 176 | 177 | static inline bool IsEdgeInLocalRange(const NavNodePtr& node_ptr1, const NavNodePtr& node_ptr2) { 178 | if (node_ptr1->is_near_nodes || node_ptr2->is_near_nodes || FARUtil::IsNodeInLocalRange(node_ptr1) || FARUtil::IsNodeInLocalRange(node_ptr2)) { 179 | return true; 180 | } 181 | return false; 182 | } 183 | 184 | template 185 | static inline cv::Point2f NodeProjectDir(const NodeType& node) { 186 | cv::Point2f project_dir(0,0); 187 | if (node->free_direct != NodeFreeDirect::PILLAR && node->free_direct != NodeFreeDirect::UNKNOW) { 188 | const Point3D topo_dir = FARUtil::SurfTopoDirect(node->surf_dirs); 189 | if (node->free_direct == NodeFreeDirect::CONCAVE) { 190 | project_dir = cv::Point2f(topo_dir.x, topo_dir.y); 191 | } else { 192 | project_dir = cv::Point2f(-topo_dir.x, -topo_dir.y); 193 | } 194 | } 195 | return project_dir; 196 | } 197 | 198 | template 199 | static inline cv::Point2f ProjectNode(const NodeType& node, const float& dist) { 200 | const cv::Point2f node_cv = cv::Point2f(node->position.x, node->position.y); 201 | const cv::Point2f dir = NodeProjectDir(node); 202 | return node_cv + dist * dir; 203 | } 204 | 205 | static inline void RemoveMatchWithNavNode(const NavNodePtr& node_ptr) { 206 | if (!node_ptr->is_contour_match) return; 207 | node_ptr->ctnode->is_global_match = false; 208 | node_ptr->ctnode->nav_node_id = 0; 209 | node_ptr->ctnode = NULL; 210 | node_ptr->is_contour_match = false; 211 | } 212 | 213 | /** 214 | * @brief extract necessary ctnodes that are essential for contour construction 215 | */ 216 | void EnclosePolygonsCheck(); 217 | 218 | CTNodePtr FirstMatchedCTNode(const CTNodePtr& ctnode_ptr); 219 | 220 | void UpdateOdomFreePosition(const NavNodePtr& odom_ptr, Point3D& global_free_p); 221 | 222 | static bool IsCTNodesConnectWithinOrder(const CTNodePtr& ctnode1, const CTNodePtr& ctnode2, 223 | CTNodePtr& block_vertex); 224 | 225 | static ConnectPair ReprojectEdge(const NavNodePtr& node1, const NavNodePtr& node2, const float& dist, const bool& is_global_check); 226 | 227 | static ConnectPair ReprojectEdge(const CTNodePtr& node1, const NavNodePtr& node2, const float& dist); 228 | 229 | static bool IsEdgeCollidePoly(const PointStack& poly, const ConnectPair& edge); 230 | 231 | static bool IsEdgeCollideSegment(const PointPair& line, const ConnectPair& edge); 232 | 233 | static bool IsCTMatchLineFreePolygon(const CTNodePtr& matched_ctnode, const NavNodePtr& matched_navnode, const bool& is_global_check); 234 | 235 | static bool IsValidBoundary(const NavNodePtr& node_ptr1, const NavNodePtr& node_ptr2, bool& is_new); 236 | 237 | inline static bool IsNeedGlobalCheck(const Point3D& p1, const Point3D& p2) { 238 | if (!FARUtil::IsPointInLocalRange(p1) || !FARUtil::IsPointInLocalRange(p2)) { 239 | return true; 240 | } 241 | return false; 242 | } 243 | 244 | static inline bool IsOverlapRange(const HeightPair& hpair, const HeightPair& hpairRef) { 245 | if (hpair.maxH > hpairRef.minH - FARUtil::kTolerZ || hpair.minH < hpairRef.maxH + FARUtil::kTolerZ) { 246 | return true; 247 | } 248 | return false; 249 | } 250 | 251 | inline void ClearContourGraph() { 252 | ContourGraph::polys_ctnodes_.clear(); 253 | ContourGraph::contour_graph_.clear(); 254 | ContourGraph::contour_polygons_.clear(); 255 | } 256 | 257 | bool IsAPillarPolygon(const PointStack& vertex_points, float& perimeter); 258 | 259 | void CreateCTNode(const Point3D& pos, CTNodePtr& ctnode_ptr, const PolygonPtr& poly_ptr, const bool& is_pillar); 260 | 261 | void CreatePolygon(const PointStack& poly_points, PolygonPtr& poly_ptr); 262 | 263 | NavNodePtr NearestNavNodeForCTNode(const CTNodePtr& ctnode_ptr, const NodePtrStack& near_nodes); 264 | 265 | void AnalysisConvexityOfCTNode(const CTNodePtr& ctnode_ptr); 266 | 267 | /* Analysis CTNode surface angle */ 268 | void AnalysisSurfAngleAndConvexity(const CTNodeStack& contour_graph); 269 | 270 | 271 | }; 272 | 273 | 274 | 275 | #endif -------------------------------------------------------------------------------- /src/far_planner/include/far_planner/far_planner.h: -------------------------------------------------------------------------------- 1 | #ifndef FAR_PLANNER_H 2 | #define FAR_PLANNER_H 3 | 4 | #include "utility.h" 5 | #include "dynamic_graph.h" 6 | #include "contour_detector.h" 7 | #include "contour_graph.h" 8 | #include "graph_planner.h" 9 | #include "map_handler.h" 10 | #include "planner_visualizer.h" 11 | #include "scan_handler.h" 12 | #include "graph_msger.h" 13 | 14 | 15 | struct FARMasterParams { 16 | FARMasterParams() = default; 17 | float robot_dim; 18 | float vehicle_height; 19 | float voxel_dim; 20 | float sensor_range; 21 | float terrain_range; 22 | float local_planner_range; 23 | float main_run_freq; 24 | float viz_ratio; 25 | bool is_multi_layer; 26 | bool is_viewpoint_extend; 27 | bool is_visual_opencv; 28 | bool is_static_env; 29 | bool is_pub_boundary; 30 | bool is_debug_output; 31 | bool is_attempt_autoswitch; 32 | std::string world_frame; 33 | }; 34 | 35 | class FARMaster { 36 | public: 37 | FARMaster() = default; 38 | ~FARMaster() = default; 39 | 40 | void Init(); // ROS initialization 41 | void Loop(); // Main Loop Function 42 | 43 | private: 44 | ros::NodeHandle nh; 45 | ros::Subscriber reset_graph_sub_, joy_command_sub_, update_command_sub_; 46 | ros::Subscriber odom_sub_, terrain_sub_, terrain_local_sub_, scan_sub_, waypoint_sub_; 47 | ros::Subscriber read_command_sub_, save_command_sub_; // only use for terminal formatting 48 | ros::Publisher goal_pub_, boundary_pub_; 49 | ros::Publisher dynamic_obs_pub_, surround_free_debug_, surround_obs_debug_; 50 | ros::Publisher scan_grid_debug_, new_PCL_pub_, terrain_height_pub_; 51 | ros::Publisher runtime_pub_, planning_time_pub_, traverse_time_pub_, reach_goal_pub_; 52 | 53 | ros::Timer planning_event_; 54 | std_msgs::Float32 runtimer_, plan_timer_; 55 | 56 | Point3D robot_pos_, robot_heading_, nav_heading_; 57 | 58 | bool is_reset_env_, is_stop_update_; 59 | 60 | geometry_msgs::PointStamped goal_waypoint_stamped_; 61 | 62 | bool is_cloud_init_, is_scan_init_, is_odom_init_, is_planner_running_; 63 | bool is_graph_init_; 64 | 65 | PointCloudPtr new_vertices_ptr_; 66 | PointCloudPtr temp_obs_ptr_; 67 | PointCloudPtr temp_free_ptr_; 68 | PointCloudPtr temp_cloud_ptr_; 69 | PointCloudPtr scan_grid_ptr_; 70 | PointCloudPtr local_terrain_ptr_; 71 | PointCloudPtr terrain_height_ptr_; 72 | 73 | /* veiwpoint extension clouds */ 74 | PointCloudPtr viewpoint_around_ptr_; 75 | PointKdTreePtr kdtree_viewpoint_obs_cloud_; 76 | 77 | NavNodePtr odom_node_ptr_ = NULL; 78 | NavNodePtr nav_node_ptr_ = NULL; 79 | NodePtrStack new_nodes_; 80 | NodePtrStack nav_graph_; 81 | NodePtrStack near_nav_graph_; 82 | NodePtrStack clear_nodes_; 83 | 84 | CTNodeStack new_ctnodes_; 85 | std::vector realworld_contour_; 86 | 87 | tf::TransformListener* tf_listener_; 88 | 89 | /* module objects */ 90 | ContourDetector contour_detector_; 91 | DynamicGraph graph_manager_; 92 | DPVisualizer planner_viz_; 93 | GraphPlanner graph_planner_; 94 | ContourGraph contour_graph_; 95 | MapHandler map_handler_; 96 | ScanHandler scan_handler_; 97 | GraphMsger graph_msger_; 98 | 99 | /* ROS Params */ 100 | FARMasterParams master_params_; 101 | ContourDetectParams cdetect_params_; 102 | DynamicGraphParams graph_params_; 103 | GraphPlannerParams gp_params_; 104 | ContourGraphParams cg_params_; 105 | MapHandlerParams map_params_; 106 | ScanHandlerParams scan_params_; 107 | GraphMsgerParams msger_parmas_; 108 | 109 | void LoadROSParams(); 110 | 111 | void ResetEnvironmentAndGraph(); 112 | 113 | void LocalBoundaryHandler(const std::vector& local_boundary); 114 | 115 | void PlanningCallBack(const ros::TimerEvent& event); 116 | 117 | void PrcocessCloud(const sensor_msgs::PointCloud2ConstPtr& pc, 118 | const PointCloudPtr& cloudOut); 119 | 120 | Point3D ProjectNavWaypoint(const NavNodePtr& nav_node_ptr, const NavNodePtr& last_point_ptr); 121 | 122 | /* Callback Functions */ 123 | void OdomCallBack(const nav_msgs::OdometryConstPtr& msg); 124 | void TerrainCallBack(const sensor_msgs::PointCloud2ConstPtr& pc); 125 | void TerrainLocalCallBack(const sensor_msgs::PointCloud2ConstPtr& pc); 126 | 127 | Point3D ExtendViewpointOnObsCloud(const NavNodePtr& nav_node_ptr, const PointCloudPtr& obsCloudIn, float& free_dist); 128 | 129 | inline void ResetGraphCallBack(const std_msgs::EmptyConstPtr& msg) { 130 | is_reset_env_ = true; 131 | } 132 | 133 | inline void JoyCommandCallBack(const sensor_msgs::JoyConstPtr& msg) { 134 | if (msg->buttons[4] > 0.5) { 135 | is_reset_env_ = true; 136 | } 137 | } 138 | 139 | inline void UpdateCommandCallBack(const std_msgs::Bool& msg) { 140 | if (is_stop_update_ && msg.data) { 141 | if (FARUtil::IsDebug) ROS_WARN("FARMaster: Resume visibility graph update."); 142 | is_stop_update_ = !msg.data; 143 | } 144 | if (!is_stop_update_ && !msg.data) { 145 | if (FARUtil::IsDebug) ROS_WARN("FARMaster: Stop visibility graph update."); 146 | is_stop_update_ = !msg.data; 147 | } 148 | } 149 | 150 | inline void FakeTerminalInit() { 151 | std::cout<clear(); 199 | new_nodes_.clear(); 200 | nav_graph_.clear(); 201 | clear_nodes_.clear(); 202 | new_ctnodes_.clear(); 203 | near_nav_graph_.clear(); 204 | realworld_contour_.clear(); 205 | } 206 | 207 | inline void ResetInternalValues() { 208 | odom_node_ptr_ = NULL; 209 | is_planner_running_ = false; 210 | is_graph_init_ = false; 211 | ClearTempMemory(); 212 | } 213 | }; 214 | 215 | #endif -------------------------------------------------------------------------------- /src/far_planner/include/far_planner/graph_msger.h: -------------------------------------------------------------------------------- 1 | #ifndef GRAPH_MSGER_H 2 | #define GRAPH_MSGER_H 3 | 4 | #include "utility.h" 5 | #include "dynamic_graph.h" 6 | 7 | struct GraphMsgerParams { 8 | GraphMsgerParams() = default; 9 | std::string frame_id; 10 | int robot_id; 11 | int votes_size; 12 | int pool_size; 13 | float dist_margin; 14 | }; 15 | 16 | 17 | class GraphMsger { 18 | public: 19 | GraphMsger() = default; 20 | ~GraphMsger() = default; 21 | 22 | void Init(const ros::NodeHandle& nh, const GraphMsgerParams& params); 23 | 24 | void UpdateGlobalGraph(const NodePtrStack& graph); 25 | 26 | private: 27 | ros::NodeHandle nh_; 28 | GraphMsgerParams gm_params_; 29 | ros::Publisher graph_pub_; 30 | ros::Subscriber graph_sub_; 31 | 32 | NodePtrStack global_graph_; 33 | PointCloudPtr nodes_cloud_ptr_; 34 | PointKdTreePtr kdtree_graph_cloud_; 35 | 36 | void CreateDecodedNavNode(const visibility_graph_msg::Node& vnode, NavNodePtr& node_ptr); 37 | 38 | inline bool IsEncodeType(const NavNodePtr& node_ptr) { 39 | if (node_ptr->is_odom || !node_ptr->is_finalized || FARUtil::IsOutsideGoal(node_ptr)) { 40 | return false; 41 | } 42 | return true; 43 | } 44 | 45 | inline bool IsMismatchFreeNode(const NavNodePtr& nearest_ptr, const visibility_graph_msg::Node& vnode) { 46 | if (nearest_ptr == NULL) return false; 47 | const bool is_navpoint = vnode.is_navpoint == 0 ? false : true; 48 | const bool is_boundary = vnode.is_boundary == 0 ? false : true; 49 | if (nearest_ptr->is_navpoint != is_navpoint || nearest_ptr->is_boundary != is_boundary) return true; 50 | return false; 51 | } 52 | 53 | inline NavNodePtr IdToNodePtr(const std::size_t& node_id, const IdxMap& idx_map, const NodePtrStack& node_stack) { 54 | const auto it = idx_map.find(node_id); 55 | if (it != idx_map.end()) { 56 | const std::size_t idx = it->second; 57 | if (idx < node_stack.size()) { 58 | return node_stack[idx]; 59 | } 60 | } 61 | return NULL; 62 | } 63 | 64 | NavNodePtr NearestNodePtrOnGraph(const Point3D p, const float radius); 65 | 66 | void EncodeGraph(const NodePtrStack& graphIn, visibility_graph_msg::Graph& graphOut); 67 | 68 | void GraphCallBack(const visibility_graph_msg::GraphConstPtr& msg); 69 | 70 | void PublishGlobalGraph(const NodePtrStack& graphIn); 71 | 72 | bool PublishGraphService(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); 73 | 74 | void ExtractConnectIdxs(const visibility_graph_msg::Node& node, 75 | IdxStack& connect_idxs, 76 | IdxStack& poly_idxs, 77 | IdxStack& contour_idxs, 78 | IdxStack& traj_idxs); 79 | 80 | }; 81 | 82 | 83 | #endif -------------------------------------------------------------------------------- /src/far_planner/include/far_planner/graph_planner.h: -------------------------------------------------------------------------------- 1 | #ifndef GRAPH_PLANNER_H 2 | #define GRAPH_PLANNER_H 3 | 4 | #include "utility.h" 5 | #include "dynamic_graph.h" 6 | #include "contour_graph.h" 7 | 8 | enum ReachVote { 9 | BLOCK = 0, 10 | REACH = 1 11 | }; 12 | 13 | struct GraphPlannerParams { 14 | GraphPlannerParams() = default; 15 | float converge_dist; 16 | float adjust_radius; 17 | float momentum_dist; 18 | bool is_autoswitch; 19 | int free_thred; 20 | int votes_size; 21 | int momentum_thred; 22 | }; 23 | 24 | 25 | class GraphPlanner { 26 | private: 27 | ros::NodeHandle nh_; 28 | ros::Subscriber attemptable_sub_; 29 | GraphPlannerParams gp_params_; 30 | NavNodePtr odom_node_ptr_ = NULL; 31 | 32 | // goal related values 33 | NavNodePtr goal_node_ptr_ = NULL; 34 | Point3D origin_goal_pos_ = Point3D(0,0,0); 35 | bool is_use_internav_goal_ = false; 36 | bool command_is_free_nav_ = false; 37 | bool is_goal_in_freespace_ = false; 38 | bool is_terrain_associated_ = false; 39 | bool is_goal_init_; 40 | NodePtrStack current_graph_; 41 | bool is_free_nav_goal_; 42 | 43 | // momentum planning values 44 | NodePtrStack recorded_path_; 45 | Point3D next_waypoint_; 46 | int path_momentum_counter_; 47 | bool is_global_path_init_; 48 | float last_waypoint_dist_; 49 | Point3D last_planning_odom_; 50 | 51 | // local terrain map for freespace adjustment 52 | Point3D grid_center_ = Point3D(0,0,0); 53 | std::unique_ptr> free_terrain_grid_; 54 | 55 | float PriorityScore(const NavNodePtr& node_ptr); 56 | 57 | bool ReconstructPath(const NavNodePtr& goal_node_ptr, 58 | const bool& is_free_nav, 59 | NodePtrStack& global_path); 60 | 61 | bool IsNodeConnectInFree(const NavNodePtr& current_node, 62 | const NavNodePtr& neighbor_node); 63 | 64 | bool IsValidConnectToGoal(const NavNodePtr& node_ptr, 65 | const NavNodePtr& goal_node_ptr); 66 | 67 | NavNodePtr NextNavWaypointFromPath(const NodePtrStack& global_path, const NavNodePtr goal_ptr); 68 | 69 | void AttemptStatusCallBack(const std_msgs::Bool& msg); 70 | 71 | inline bool IsInvalidBoundary(const NavNodePtr& node_ptr1, const NavNodePtr& node_ptr2) { 72 | if (node_ptr1->is_boundary && node_ptr2->is_boundary) { 73 | if (node_ptr1->invalid_boundary.find(node_ptr2->id) != node_ptr1->invalid_boundary.end()) { 74 | return true; 75 | } 76 | } 77 | return false; 78 | } 79 | 80 | inline void ResetFreeTerrainGridOrigin(const Point3D& p) { 81 | Eigen::Vector3d grid_origin; 82 | grid_origin.x() = p.x - (free_terrain_grid_->GetResolution().x() * free_terrain_grid_->GetSize().x()) / 2.0f; 83 | grid_origin.y() = p.y - (free_terrain_grid_->GetResolution().y() * free_terrain_grid_->GetSize().y()) / 2.0f; 84 | grid_origin.z() = p.z - (free_terrain_grid_->GetResolution().z() * free_terrain_grid_->GetSize().z()) / 2.0f; 85 | free_terrain_grid_->SetOrigin(grid_origin); 86 | grid_center_ = p; 87 | } 88 | 89 | /* define inline functions */ 90 | inline void InitNodesStates(const NodePtrStack& graph) { 91 | for (const auto& node_ptr : graph) { 92 | node_ptr->gscore = FARUtil::kINF; 93 | node_ptr->fgscore = FARUtil::kINF; 94 | node_ptr->is_traversable = false; 95 | node_ptr->is_free_traversable = false; 96 | node_ptr->parent = NULL; 97 | node_ptr->free_parent = NULL; 98 | } 99 | } 100 | 101 | inline void RecordPathInfo(const NodePtrStack& global_path) { 102 | if (global_path.size() < 2) { 103 | if (FARUtil::IsDebug) ROS_ERROR("GP: recording path for momontum fails, planning path is empty"); 104 | return; 105 | } 106 | recorded_path_ = global_path; 107 | next_waypoint_ = global_path[1]->position; 108 | last_waypoint_dist_ = (odom_node_ptr_->position - next_waypoint_).norm(); 109 | is_global_path_init_ = true; 110 | path_momentum_counter_ = 0; 111 | last_planning_odom_ = odom_node_ptr_->position; 112 | } 113 | 114 | inline bool IsResetBlockStatus(const NavNodePtr& node_ptr, const NavNodePtr& goal_ptr) { 115 | if (node_ptr->is_odom || (node_ptr->is_near_nodes && (!node_ptr->is_finalized || node_ptr->is_frontier))) return true; 116 | if (!FARUtil::IsStaticEnv && node_ptr->is_near_nodes) return true; 117 | const auto it = node_ptr->edge_votes.find(goal_ptr->id); 118 | if (node_ptr->is_near_nodes && it != node_ptr->edge_votes.end() && it->second.size() < gp_params_.votes_size) { 119 | return true; 120 | } 121 | return false; 122 | } 123 | 124 | inline float EulerCost(const NavNodePtr& current_node, 125 | const NavNodePtr& neighbor_node) 126 | { 127 | return (current_node->position - neighbor_node->position).norm(); 128 | } 129 | 130 | inline void GoalReset() { 131 | origin_goal_pos_ = Point3D(0,0,0); 132 | is_goal_in_freespace_ = false; 133 | if (goal_node_ptr_ != NULL) { 134 | if (!is_use_internav_goal_) DynamicGraph::ClearGoalNodeInGraph(goal_node_ptr_); 135 | else goal_node_ptr_->is_goal = false; 136 | } 137 | goal_node_ptr_ = NULL; 138 | } 139 | 140 | public: 141 | 142 | GraphPlanner() = default; 143 | ~GraphPlanner() = default; 144 | 145 | void Init(const ros::NodeHandle& nh, const GraphPlannerParams& params); 146 | 147 | 148 | /** 149 | * Update Global Graph 150 | * @param vgraph current graph 151 | */ 152 | inline void UpdaetVGraph(const NodePtrStack& vgraph) {current_graph_ = vgraph;}; 153 | 154 | /** 155 | * Update Global Graph Traversability Status and gscores 156 | * @param graph current graph 157 | * @param odom_node_ptr current odom node ptr 158 | * @param goal_ptr current goal node ptr 159 | */ 160 | void UpdateGraphTraverability(const NavNodePtr& odom_node_ptr, const NavNodePtr& goal_ptr); 161 | 162 | /** 163 | * Generate path to goal based on traversibility result 164 | * @param goal_ptr current goal node 165 | * @param global_path(return) return the global path from odom node position 166 | * @param _nav_node_ptr(return) current navigation waypoint 167 | * @param _goal_p(return) goal position after free space adjust 168 | * @param _is_fail(return) whether the planner fails to find the path 169 | * @param _is_succeed(return) whether the vehicle has reached the goal 170 | * @param _is_free_nav(return) the attemptable navigation status (True)->Non-attempts 171 | * @return whether or not planning success -> publish a valid path for navigation 172 | */ 173 | 174 | bool PathToGoal(const NavNodePtr& goal_ptr, 175 | NodePtrStack& global_path, 176 | NavNodePtr& _nav_node_ptr, 177 | Point3D& _goal_p, 178 | bool& _is_fails, 179 | bool& _is_succeed, 180 | bool& _is_free_nav); 181 | 182 | /** 183 | * @brief Update connectivity between goal node and navigation graph 184 | * @param goal_ptr new create goal node pointer 185 | * @param updated_graph current updated navigation graph 186 | */ 187 | 188 | void UpdateGoalNavNodeConnects(const NavNodePtr& goal_ptr); 189 | 190 | /** 191 | * Graph planner goal update API 192 | * @param goal goal position 193 | */ 194 | void UpdateGoal(const Point3D& goal); 195 | 196 | 197 | /** 198 | * @brief Update free terrian grid for re-selecting goal position into free space 199 | * @param center current center of grid 200 | * @param obsCloudIn Obstalce point cloud input 201 | * @param freeCloudIn Free point cloud input 202 | */ 203 | void UpdateFreeTerrainGrid(const Point3D& center, 204 | const PointCloudPtr& obsCloudIn, 205 | const PointCloudPtr& freeCloudIn); 206 | 207 | /** 208 | * @brief Adjust goal position based on terrain and polygons 209 | * @param goal_ptr current goal node pointer 210 | * @param is_adjust_height whether or not adjust height of goal -> (False if multi layer planning) 211 | */ 212 | void ReEvaluateGoalPosition(const NavNodePtr& goal_ptr, const bool& is_adjust_height); 213 | 214 | /** 215 | * @brief Reset internal values and containers 216 | */ 217 | inline void ResetPlannerInternalValues() { 218 | goal_node_ptr_ = NULL; 219 | odom_node_ptr_ = NULL; 220 | 221 | is_goal_init_ = false; 222 | is_use_internav_goal_ = false; 223 | is_global_path_init_ = false; 224 | is_free_nav_goal_ = false; 225 | is_goal_in_freespace_ = false; 226 | 227 | current_graph_.clear(); 228 | recorded_path_.clear(); 229 | path_momentum_counter_ = 0; 230 | last_waypoint_dist_ = 0.0; 231 | origin_goal_pos_ = Point3D(0,0,0); 232 | next_waypoint_ = Point3D(0,0,0); 233 | last_planning_odom_ = Point3D(0,0,0); 234 | } 235 | 236 | const NavNodePtr& GetGoalNodePtr() const { return goal_node_ptr_;}; 237 | 238 | Point3D GetOriginNodePos(const bool& is_adjusted_z) const { 239 | if (goal_node_ptr_ == NULL) return Point3D(0,0,0); 240 | if (!is_adjusted_z) return origin_goal_pos_; 241 | else { 242 | return Point3D(origin_goal_pos_.x, 243 | origin_goal_pos_.y, 244 | goal_node_ptr_->position.z); 245 | } 246 | } 247 | 248 | }; 249 | 250 | #endif -------------------------------------------------------------------------------- /src/far_planner/include/far_planner/grid.h: -------------------------------------------------------------------------------- 1 | #ifndef GRID_UTIL_H 2 | #define GRID_UTIL_H 3 | 4 | /** 5 | * @file grid.h 6 | * @author Chao Cao (ccao1@andrew.cmu.edu) 7 | * @brief Class that implements a 3D grid 8 | * @version 0.1 9 | * @date 2021-01-26 10 | * 11 | * @copyright Copyright (c) 2021 12 | * 13 | */ 14 | #pragma once 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | namespace grid_ns 21 | { 22 | template 23 | class Grid 24 | { 25 | public: 26 | explicit Grid(const Eigen::Vector3i& size, _T init_value, const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), 27 | const Eigen::Vector3d& resolution = Eigen::Vector3d(1, 1, 1), int dimension = 3) 28 | { 29 | // MY_ASSERT(size.x() > 0); 30 | // MY_ASSERT(size.y() > 0); 31 | // MY_ASSERT(size.z() > 0); 32 | 33 | origin_ = origin; 34 | size_ = size; 35 | resolution_ = resolution; 36 | dimension_ = dimension; 37 | 38 | for (int i = 0; i < dimension_; i++) 39 | { 40 | resolution_inv_(i) = 1.0 / resolution_(i); 41 | } 42 | cell_number_ = size_.x() * size_.y() * size_.z(); 43 | cells_.resize(cell_number_), subs_.resize(cell_number_); 44 | for (int i = 0; i < cell_number_; i++) 45 | { 46 | cells_[i] = init_value; 47 | subs_[i] = ind2sub_(i); 48 | } 49 | } 50 | 51 | virtual ~Grid() = default; 52 | 53 | int GetCellNumber() const 54 | { 55 | return cell_number_; 56 | } 57 | 58 | Eigen::Vector3i GetSize() const 59 | { 60 | return size_; 61 | } 62 | 63 | Eigen::Vector3d GetOrigin() const 64 | { 65 | return origin_; 66 | } 67 | 68 | void SetOrigin(const Eigen::Vector3d& origin) 69 | { 70 | origin_ = origin; 71 | } 72 | 73 | void ReInitGrid(const _T& init_value) 74 | { 75 | std::fill(cells_.begin(), cells_.end(), init_value); 76 | } 77 | 78 | void SetResolution(const Eigen::Vector3d& resolution) 79 | { 80 | resolution_ = resolution; 81 | for (int i = 0; i < dimension_; i++) 82 | { 83 | resolution_inv_(i) = 1.0 / resolution(i); 84 | } 85 | } 86 | 87 | Eigen::Vector3d GetResolution() const 88 | { 89 | return resolution_; 90 | } 91 | 92 | Eigen::Vector3d GetResolutionInv() const 93 | { 94 | return resolution_inv_; 95 | } 96 | 97 | bool InRange(int x, int y, int z) const 98 | { 99 | return InRange(Eigen::Vector3i(x, y, z)); 100 | } 101 | 102 | bool InRange(const Eigen::Vector3i& sub) const 103 | { 104 | bool in_range = true; 105 | for (int i = 0; i < dimension_; i++) 106 | { 107 | in_range &= sub(i) >= 0 && sub(i) < size_(i); 108 | } 109 | return in_range; 110 | } 111 | 112 | bool InRange(int ind) const 113 | { 114 | return ind >= 0 && ind < cell_number_; 115 | } 116 | 117 | Eigen::Vector3i Ind2Sub(int ind) const 118 | { 119 | // MY_ASSERT(InRange(ind)); 120 | return subs_[ind]; 121 | } 122 | 123 | int Sub2Ind(int x, int y, int z) const 124 | { 125 | return x + (y * size_.x()) + (z * size_.x() * size_.y()); 126 | } 127 | 128 | int Sub2Ind(const Eigen::Vector3i& sub) const 129 | { 130 | // MY_ASSERT(InRange(sub)); 131 | return Sub2Ind(sub.x(), sub.y(), sub.z()); 132 | } 133 | 134 | Eigen::Vector3d Sub2Pos(int x, int y, int z) const 135 | { 136 | return Sub2Pos(Eigen::Vector3i(x, y, z)); 137 | } 138 | 139 | Eigen::Vector3d Sub2Pos(const Eigen::Vector3i& sub) const 140 | { 141 | Eigen::Vector3d pos(0, 0, 0); 142 | for (int i = 0; i < dimension_; i++) 143 | { 144 | pos(i) = origin_(i) + sub(i) * resolution_(i) + resolution_(i) / 2.0; 145 | } 146 | return pos; 147 | } 148 | 149 | Eigen::Vector3d Ind2Pos(int ind) const 150 | { 151 | // MY_ASSERT(InRange(ind)); 152 | return Sub2Pos(Ind2Sub(ind)); 153 | } 154 | 155 | Eigen::Vector3i Pos2Sub(double x, double y, double z) const 156 | { 157 | return Pos2Sub(Eigen::Vector3d(x, y, z)); 158 | } 159 | 160 | Eigen::Vector3i Pos2Sub(const Eigen::Vector3d& pos) const 161 | { 162 | Eigen::Vector3i sub(0, 0, 0); 163 | for (int i = 0; i < dimension_; i++) 164 | { 165 | sub(i) = pos(i) - origin_(i) > -1e-7 ? static_cast((pos(i) - origin_(i)) * resolution_inv_(i)) : -1; 166 | } 167 | return sub; 168 | } 169 | 170 | int Pos2Ind(const Eigen::Vector3d& pos) const 171 | { 172 | return Sub2Ind(Pos2Sub(pos)); 173 | } 174 | 175 | _T& GetCell(int x, int y, int z) 176 | { 177 | return GetCell(Eigen::Vector3i(x, y, z)); 178 | } 179 | 180 | _T& GetCell(const Eigen::Vector3i& sub) 181 | { 182 | // MY_ASSERT(InRange(sub)); 183 | int index = Sub2Ind(sub); 184 | return cells_[index]; 185 | } 186 | 187 | _T& GetCell(int index) 188 | { 189 | // MY_ASSERT(InRange(index)); 190 | return cells_[index]; 191 | } 192 | 193 | _T GetCellValue(int x, int y, int z) const 194 | { 195 | int index = Sub2Ind(x, y, z); 196 | return cells_[index]; 197 | } 198 | 199 | _T GetCellValue(const Eigen::Vector3i& sub) const 200 | { 201 | // MY_ASSERT(InRange(sub)); 202 | return GetCellValue(sub.x(), sub.y(), sub.z()); 203 | } 204 | 205 | _T GetCellValue(int index) const 206 | { 207 | // MY_ASSERT(InRange(index)); 208 | return cells_[index]; 209 | } 210 | 211 | void SetCellValue(int x, int y, int z, _T value) 212 | { 213 | int index = Sub2Ind(x, y, z); 214 | cells_[index] = value; 215 | } 216 | 217 | void SetCellValue(const Eigen::Vector3i& sub, _T value) 218 | { 219 | // MY_ASSERT(InRange(sub)); 220 | SetCellValue(sub.x(), sub.y(), sub.z(), value); 221 | } 222 | 223 | void SetCellValue(int index, const _T& value) 224 | { 225 | // MY_ASSERT(InRange(index)); 226 | cells_[index] = value; 227 | } 228 | 229 | void RayTraceSubs(const Eigen::Vector3i& start_sub, 230 | const Eigen::Vector3i& end_sub, 231 | std::vector& subs) 232 | { 233 | subs.clear(); 234 | const Eigen::Vector3i diff_sub = end_sub - start_sub; 235 | const double max_dist = diff_sub.squaredNorm(); 236 | const int step_x = signum(diff_sub.x()); 237 | const int step_y = signum(diff_sub.y()); 238 | const int step_z = signum(diff_sub.z()); 239 | const double t_delta_x = step_x == 0 ? DBL_MAX : (double)step_x / (double)diff_sub.x(); 240 | const double t_delta_y = step_y == 0 ? DBL_MAX : (double)step_y / (double)diff_sub.y(); 241 | const double t_delta_z = step_z == 0 ? DBL_MAX : (double)step_z / (double)diff_sub.z(); 242 | 243 | 244 | double t_max_x = step_x == 0 ? DBL_MAX : intbound(start_sub.x(), diff_sub.x()); 245 | double t_max_y = step_y == 0 ? DBL_MAX : intbound(start_sub.y(), diff_sub.y()); 246 | double t_max_z = step_z == 0 ? DBL_MAX : intbound(start_sub.z(), diff_sub.z()); 247 | double dist = 0; 248 | Eigen::Vector3i cur_sub = start_sub; 249 | 250 | while (InRange(cur_sub)) { 251 | subs.push_back(cur_sub); 252 | dist = (cur_sub - start_sub).squaredNorm(); 253 | if (cur_sub == end_sub || dist > max_dist) 254 | { 255 | return; 256 | } 257 | if (t_max_x < t_max_y) 258 | { 259 | if (t_max_x < t_max_z) 260 | { 261 | cur_sub.x() += step_x; 262 | t_max_x += t_delta_x; 263 | } 264 | else 265 | { 266 | cur_sub.z() += step_z; 267 | t_max_z += t_delta_z; 268 | } 269 | } 270 | else 271 | { 272 | if (t_max_y < t_max_z) 273 | { 274 | cur_sub.y() += step_y; 275 | t_max_y += t_delta_y; 276 | } 277 | else 278 | { 279 | cur_sub.z() += step_z; 280 | t_max_z += t_delta_z; 281 | } 282 | } 283 | } 284 | } 285 | 286 | private: 287 | Eigen::Vector3d origin_; 288 | Eigen::Vector3i size_; 289 | Eigen::Vector3d resolution_; 290 | Eigen::Vector3d resolution_inv_; 291 | std::vector<_T> cells_; 292 | std::vector subs_; 293 | int cell_number_; 294 | int dimension_; 295 | 296 | Eigen::Vector3i ind2sub_(int ind) const 297 | { 298 | // MY_ASSERT(InRange(ind)); 299 | Eigen::Vector3i sub; 300 | sub.z() = ind / (size_.x() * size_.y()); 301 | ind -= (sub.z() * size_.x() * size_.y()); 302 | sub.y() = ind / size_.x(); 303 | sub.x() = ind % size_.x(); 304 | return sub; 305 | } 306 | 307 | // Math Helper functions 308 | int signum(const int& x) 309 | { 310 | return x == 0 ? 0 : x < 0 ? -1 : 1; 311 | } 312 | double mod(const double& value, const double& modulus) 313 | { 314 | return std::fmod(std::fmod(value, modulus) + modulus, modulus); 315 | } 316 | double intbound(double s, double ds) 317 | { 318 | // Find the smallest positive t such that s+t*ds is an integer. 319 | if (ds < 0) 320 | { 321 | return intbound(-s, -ds); 322 | } 323 | else 324 | { 325 | s = mod(s, 1); 326 | // problem is now s+t*ds = 1 327 | return (1 - s) / ds; 328 | } 329 | } 330 | 331 | }; 332 | } // namespace grid_ns 333 | 334 | #endif -------------------------------------------------------------------------------- /src/far_planner/include/far_planner/intersection.h: -------------------------------------------------------------------------------- 1 | #ifndef INTERSECTION_H 2 | #define INTERSECTION_H 3 | 4 | // A C++ program to check if two given line segments intersect 5 | // See https://www.geeksforgeeks.org/orientation-3-ordered-points/ 6 | 7 | #include 8 | #include 9 | using namespace std; 10 | using namespace cv; 11 | 12 | // Given three colinear points p, q, r, the function checks if 13 | // point q lies on line segment 'pr' 14 | 15 | namespace POLYOPS { 16 | 17 | static bool onSegment(Point2f p, Point2f q, Point2f r) 18 | { 19 | if (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) && 20 | q.y <= max(p.y, r.y) && q.y >= min(p.y, r.y)) 21 | return true; 22 | 23 | return false; 24 | } 25 | 26 | // To find orientation of ordered triplet (p, q, r). 27 | // The function returns following values 28 | // 0 --> p, q and r are colinear 29 | // 1 --> Clockwise 30 | // 2 --> Counterclockwise 31 | static int orientation(Point2f p, Point2f q, Point2f r) 32 | { 33 | // for details of below formula. 34 | double val = (q.y - p.y) * (r.x - q.x) - 35 | (q.x - p.x) * (r.y - q.y); 36 | 37 | if (abs(val) < 1e-7) return 0; // colinear 38 | 39 | return (val > 0)? 1: 2; // clock or counterclock wise 40 | } 41 | 42 | // The main function that returns true if line segment 'p1q1' 43 | // and 'p2q2' intersect. 44 | static bool doIntersect(Point2f p1, Point2f q1, Point2f p2, Point2f q2) 45 | { 46 | // Find the four orientations needed for general and 47 | // special cases 48 | int o1 = orientation(p1, q1, p2); 49 | int o2 = orientation(p1, q1, q2); 50 | int o3 = orientation(p2, q2, p1); 51 | int o4 = orientation(p2, q2, q1); 52 | 53 | // General case 54 | if (o1 != o2 && o3 != o4) 55 | return true; 56 | 57 | // Special Cases 58 | // p1, q1 and p2 are colinear and p2 lies on segment p1q1 59 | if (o1 == 0 && onSegment(p1, p2, q1)) return true; 60 | 61 | // p1, q1 and q2 are colinear and q2 lies on segment p1q1 62 | if (o2 == 0 && onSegment(p1, q2, q1)) return true; 63 | 64 | // p2, q2 and p1 are colinear and p1 lies on segment p2q2 65 | if (o3 == 0 && onSegment(p2, p1, q2)) return true; 66 | 67 | // p2, q2 and q1 are colinear and q1 lies on segment p2q2 68 | if (o4 == 0 && onSegment(p2, q1, q2)) return true; 69 | 70 | return false; // Doesn't fall in any of the above cases 71 | } 72 | } 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /src/far_planner/include/far_planner/map_handler.h: -------------------------------------------------------------------------------- 1 | #ifndef MAP_HANDLER_H 2 | #define MAP_HANDLER_H 3 | 4 | #include "utility.h" 5 | 6 | enum CloudType { 7 | FREE_CLOUD = 0, 8 | OBS_CLOUD = 1 9 | }; 10 | 11 | struct MapHandlerParams { 12 | MapHandlerParams() = default; 13 | float sensor_range; 14 | float floor_height; 15 | float cell_length; 16 | float cell_height; 17 | float grid_max_length; 18 | float grid_max_height; 19 | // local terrain height map 20 | float height_voxel_dim; 21 | }; 22 | 23 | class MapHandler { 24 | 25 | public: 26 | MapHandler() = default; 27 | ~MapHandler() = default; 28 | 29 | void Init(const MapHandlerParams& params); 30 | void SetMapOrigin(const Point3D& robot_pos); 31 | 32 | void UpdateRobotPosition(const Point3D& odom_pos); 33 | 34 | void AdjustNodesHeight(const NodePtrStack& nodes); 35 | 36 | void AdjustCTNodeHeight(const CTNodeStack& ctnodes); 37 | 38 | static float TerrainHeightOfPoint(const Point3D& p, 39 | bool& is_matched, 40 | const bool& is_search); 41 | 42 | static bool IsNavPointOnTerrainNeighbor(const Point3D& p, const bool& is_extend); 43 | 44 | static float NearestTerrainHeightofNavPoint(const Point3D& point, bool& is_associated); 45 | 46 | /** 47 | * @brief Calculate the terrain height of a given point and radius around it 48 | * @param p A given position 49 | * @param radius The radius distance around the given posiitn p 50 | * @param minH[out] The mininal terrain height in the radius 51 | * @param maxH[out] The maximal terrain height in the radius 52 | * @param is_match[out] Whether or not find terrain association in radius 53 | * @return The average terrain height 54 | */ 55 | template 56 | static inline float NearestHeightOfRadius(const Position& p, const float& radius, float& minH, float& maxH, bool& is_matched) { 57 | std::vector pIdxK; 58 | std::vector pdDistK; 59 | PCLPoint pcl_p; 60 | pcl_p.x = p.x, pcl_p.y = p.y, pcl_p.z = 0.0f, pcl_p.intensity = 0.0f; 61 | minH = maxH = p.z; 62 | is_matched = false; 63 | if (kdtree_terrain_clould_->radiusSearch(pcl_p, radius, pIdxK, pdDistK) > 0) { 64 | float avgH = kdtree_terrain_clould_->getInputCloud()->points[pIdxK[0]].intensity; 65 | minH = maxH = avgH; 66 | for (int i=1; igetInputCloud()->points[pIdxK[i]].intensity; 68 | if (temp < minH) minH = temp; 69 | if (temp > maxH) maxH = temp; 70 | avgH += temp; 71 | } 72 | avgH /= (float)pIdxK.size(); 73 | is_matched = true; 74 | return avgH; 75 | } 76 | return p.z; 77 | } 78 | 79 | /** Update global cloud grid with incoming clouds 80 | * @param CloudInOut incoming cloud ptr and output valid in range points 81 | */ 82 | void UpdateObsCloudGrid(const PointCloudPtr& obsCloudInOut); 83 | void UpdateFreeCloudGrid(const PointCloudPtr& freeCloudIn); 84 | void UpdateTerrainHeightGrid(const PointCloudPtr& freeCloudIn, const PointCloudPtr& terrainHeightOut); 85 | 86 | /** Extract Surrounding Free & Obs clouds 87 | * @param SurroundCloudOut output surrounding cloud ptr 88 | */ 89 | void GetSurroundObsCloud(const PointCloudPtr& obsCloudOut); 90 | void GetSurroundFreeCloud(const PointCloudPtr& freeCloudOut); 91 | 92 | /** Extract Surrounding Free & Obs clouds 93 | * @param center the position of the grid that want to extract 94 | * @param cloudOut output cloud ptr 95 | * @param type choose free or obstacle cloud for extraction 96 | * @param is_large whether or not using the surrounding cells 97 | */ 98 | void GetCloudOfPoint(const Point3D& center, 99 | const PointCloudPtr& CloudOut, 100 | const CloudType& type, 101 | const bool& is_large); 102 | 103 | /** 104 | * Get neihbor cells center positions 105 | * @param neighbor_centers[out] neighbor centers stack 106 | */ 107 | void GetNeighborCeilsCenters(PointStack& neighbor_centers); 108 | 109 | /** 110 | * Get neihbor cells center positions 111 | * @param occupancy_centers[out] occupanied cells center stack 112 | */ 113 | void GetOccupancyCeilsCenters(PointStack& occupancy_centers); 114 | 115 | /** 116 | * Remove pointcloud from grid map 117 | * @param obsCloud obstacle cloud points that need to be removed 118 | */ 119 | void RemoveObsCloudFromGrid(const PointCloudPtr& obsCloud); 120 | 121 | /** 122 | * @brief Reset Current Grip Map Clouds 123 | */ 124 | void ResetGripMapCloud(); 125 | 126 | /** 127 | * @brief Clear the cells that from the robot position to the given position 128 | * @param point Give point location 129 | */ 130 | void ClearObsCellThroughPosition(const Point3D& point); 131 | 132 | private: 133 | MapHandlerParams map_params_; 134 | int neighbor_Lnum_, neighbor_Hnum_; 135 | Eigen::Vector3i robot_cell_sub_; 136 | int INFLATE_N; 137 | bool is_init_ = false; 138 | PointCloudPtr flat_terrain_cloud_; 139 | static PointKdTreePtr kdtree_terrain_clould_; 140 | 141 | template 142 | static inline float NearestHeightOfPoint(const Position& p, float& dist_square) { 143 | // Find the nearest node in graph 144 | std::vector pIdxK(1); 145 | std::vector pdDistK(1); 146 | PCLPoint pcl_p; 147 | dist_square = FARUtil::kINF; 148 | pcl_p.x = p.x, pcl_p.y = p.y, pcl_p.z = 0.0f, pcl_p.intensity = 0.0f; 149 | if (kdtree_terrain_clould_->nearestKSearch(pcl_p, 1, pIdxK, pdDistK) > 0) { 150 | pcl_p = kdtree_terrain_clould_->getInputCloud()->points[pIdxK[0]]; 151 | dist_square = pdDistK[0]; 152 | return pcl_p.intensity; 153 | } 154 | return p.z; 155 | } 156 | 157 | void SetTerrainHeightGridOrigin(const Point3D& robot_pos); 158 | 159 | void TraversableAnalysis(const PointCloudPtr& terrainHeightOut); 160 | 161 | inline void AssignFlatTerrainCloud(const PointCloudPtr& terrainRef, PointCloudPtr& terrainFlatOut) { 162 | const int N = terrainRef->size(); 163 | terrainFlatOut->resize(N); 164 | for (int i = 0; ipoints[i]; 166 | pcl_p.intensity = pcl_p.z, pcl_p.z = 0.0f; 167 | terrainFlatOut->points[i] = pcl_p; 168 | } 169 | } 170 | 171 | inline void Expansion2D(const Eigen::Vector3i& csub, std::vector& subs, const int& n) { 172 | subs.clear(); 173 | for (int ix=-n; ix<=n; ix++) { 174 | for (int iy=-n; iy<=n; iy++) { 175 | Eigen::Vector3i sub = csub; 176 | sub.x() += ix, sub.y() += iy; 177 | subs.push_back(sub); 178 | } 179 | } 180 | } 181 | 182 | void ObsNeighborCloudWithTerrain(std::unordered_set& neighbor_obs, 183 | std::unordered_set& extend_terrain_obs); 184 | 185 | std::unordered_set neighbor_free_indices_; // surrounding free cloud grid indices stack 186 | static std::unordered_set neighbor_obs_indices_; // surrounding obs cloud grid indices stack 187 | static std::unordered_set extend_obs_indices_; // extended surrounding obs cloud grid indices stack 188 | 189 | std::vector global_visited_induces_; 190 | std::vector util_obs_modified_list_; 191 | std::vector util_free_modified_list_; 192 | std::vector util_remove_check_list_; 193 | static std::vector terrain_grid_occupy_list_; 194 | static std::vector terrain_grid_traverse_list_; 195 | 196 | 197 | static std::unique_ptr> world_free_cloud_grid_; 198 | static std::unique_ptr> world_obs_cloud_grid_; 199 | static std::unique_ptr>> terrain_height_grid_; 200 | 201 | }; 202 | 203 | #endif -------------------------------------------------------------------------------- /src/far_planner/include/far_planner/node_struct.h: -------------------------------------------------------------------------------- 1 | #ifndef NODE_STRUCT_H 2 | #define NODE_STRUCT_H 3 | 4 | #include "point_struct.h" 5 | 6 | enum NodeType { 7 | NOT_DEFINED = 0, 8 | GROUND = 1, 9 | AIR = 2 10 | }; 11 | 12 | enum NodeFreeDirect { 13 | UNKNOW = 0, 14 | CONVEX = 1, 15 | CONCAVE = 2, 16 | PILLAR = 3 17 | }; 18 | 19 | typedef std::pair PointPair; 20 | 21 | namespace LiDARMODEL { 22 | /* array resolution: 1 degree */ 23 | static const int kHorizontalFOV = 360; 24 | static const int kVerticalFOV = 31; 25 | static const float kAngleResX = 0.2; 26 | static const float kAngleResY = 2.0; 27 | } // NODEPARAMS 28 | 29 | struct Polygon 30 | { 31 | Polygon() = default; 32 | std::size_t N; 33 | std::vector vertices; 34 | bool is_robot_inside; 35 | bool is_pillar; 36 | float perimeter; 37 | }; 38 | 39 | typedef std::shared_ptr PolygonPtr; 40 | typedef std::vector PolygonStack; 41 | 42 | struct CTNode 43 | { 44 | CTNode() = default; 45 | Point3D position; 46 | bool is_global_match; 47 | bool is_contour_necessary; 48 | bool is_ground_associate; 49 | std::size_t nav_node_id; 50 | NodeFreeDirect free_direct; 51 | 52 | PointPair surf_dirs; 53 | PolygonPtr poly_ptr; 54 | std::shared_ptr front; 55 | std::shared_ptr back; 56 | 57 | std::vector> connect_nodes; 58 | }; 59 | 60 | typedef std::shared_ptr CTNodePtr; 61 | typedef std::vector CTNodeStack; 62 | 63 | struct NavNode 64 | { 65 | NavNode() = default; 66 | std::size_t id; 67 | Point3D position; 68 | PointPair surf_dirs; 69 | std::deque pos_filter_vec; 70 | std::deque surf_dirs_vec; 71 | CTNodePtr ctnode; 72 | bool is_active; 73 | bool is_block_frontier; 74 | bool is_contour_match; 75 | bool is_odom; 76 | bool is_goal; 77 | bool is_near_nodes; 78 | bool is_wide_near; 79 | bool is_merged; 80 | bool is_covered; 81 | bool is_frontier; 82 | bool is_finalized; 83 | bool is_navpoint; 84 | bool is_boundary; 85 | int clear_dumper_count; 86 | std::deque frontier_votes; 87 | std::unordered_set invalid_boundary; 88 | std::vector> connect_nodes; 89 | std::vector> poly_connects; 90 | std::vector> contour_connects; 91 | std::unordered_map> contour_votes; 92 | std::unordered_map> edge_votes; 93 | std::vector> potential_contours; 94 | std::vector> potential_edges; 95 | std::vector> trajectory_connects; 96 | std::unordered_map trajectory_votes; 97 | std::unordered_map terrain_votes; 98 | NodeType node_type; 99 | NodeFreeDirect free_direct; 100 | // planner members 101 | bool is_block_to_goal; 102 | bool is_traversable; 103 | bool is_free_traversable; 104 | float gscore, fgscore; 105 | std::shared_ptr parent; 106 | std::shared_ptr free_parent; 107 | 108 | }; 109 | 110 | typedef std::shared_ptr NavNodePtr; 111 | typedef std::pair NavEdge; 112 | 113 | struct nodeptr_equal 114 | { 115 | bool operator()(const NavNodePtr& n1, const NavNodePtr& n2) const 116 | { 117 | return n1->id == n2->id; 118 | } 119 | }; 120 | 121 | struct navedge_hash 122 | { 123 | std::size_t operator() (const NavEdge& nav_edge) const 124 | { 125 | return boost::hash>()({nav_edge.first->id, nav_edge.second->id}); 126 | } 127 | }; 128 | 129 | struct nodeptr_hash 130 | { 131 | std::size_t operator() (const NavNodePtr& n_ptr) const 132 | { 133 | return std::hash()(n_ptr->id); 134 | } 135 | }; 136 | 137 | struct nodeptr_gcomp 138 | { 139 | bool operator()(const NavNodePtr& n1, const NavNodePtr& n2) const 140 | { 141 | return n1->gscore > n2->gscore; 142 | } 143 | }; 144 | 145 | struct nodeptr_fgcomp 146 | { 147 | bool operator()(const NavNodePtr& n1, const NavNodePtr& n2) const 148 | { 149 | return n1->fgscore > n2->fgscore; 150 | } 151 | }; 152 | 153 | struct nodeptr_icomp 154 | { 155 | bool operator()(const NavNodePtr& n1, const NavNodePtr& n2) const 156 | { 157 | return n1->position.intensity < n2->position.intensity; 158 | } 159 | }; 160 | 161 | #endif 162 | -------------------------------------------------------------------------------- /src/far_planner/include/far_planner/planner_visualizer.h: -------------------------------------------------------------------------------- 1 | #ifndef PLANNER_VISUALIZER_H 2 | #define PLANNER_VISUALIZER_H 3 | 4 | #include "utility.h" 5 | #include "contour_graph.h" 6 | #include 7 | #include 8 | 9 | typedef visualization_msgs::Marker Marker; 10 | typedef visualization_msgs::MarkerArray MarkerArray; 11 | 12 | 13 | enum VizColor { 14 | RED = 0, 15 | ORANGE = 1, 16 | BLACK = 2, 17 | YELLOW = 3, 18 | BLUE = 4, 19 | GREEN = 5, 20 | EMERALD = 6, 21 | WHITE = 7, 22 | MAGNA = 8, 23 | PURPLE = 9 24 | }; 25 | 26 | class DPVisualizer { 27 | private: 28 | ros::NodeHandle nh_; 29 | // Utility Cloud 30 | PointCloudPtr point_cloud_ptr_; 31 | // rviz publisher 32 | ros::Publisher viz_node_pub_, viz_path_pub_, viz_poly_pub_, viz_graph_pub_; 33 | ros::Publisher viz_contour_pub_, viz_map_pub_, viz_view_extend; 34 | 35 | public: 36 | DPVisualizer() = default; 37 | ~DPVisualizer() = default; 38 | 39 | void Init(const ros::NodeHandle& nh); 40 | 41 | void VizNodes(const NodePtrStack& node_stack, 42 | const std::string& ns, 43 | const VizColor& color, 44 | const float scale=0.75f, 45 | const float alpha=0.75f); 46 | 47 | void VizGlobalPolygons(const std::vector& contour_pairs, 48 | const std::vector& unmatched_pairs); 49 | 50 | void VizViewpointExtend(const NavNodePtr& ori_nav_ptr, const Point3D& extend_point); 51 | 52 | // True for non-attempts path 53 | void VizPath(const NodePtrStack& global_path, const bool& is_free_nav=false); 54 | 55 | void VizMapGrids(const PointStack& neighbor_centers, 56 | const PointStack& occupancy_centers, 57 | const float& ceil_length, 58 | const float& ceil_height); 59 | 60 | void VizContourGraph(const CTNodeStack& contour_graph); 61 | 62 | void VizPoint3D(const Point3D& point, 63 | const std::string& ns, 64 | const VizColor& color, 65 | const float scale=1.0f, 66 | const float alpha=0.9f); 67 | 68 | void VizGraph(const NodePtrStack& graph); 69 | void VizPointCloud(const ros::Publisher& viz_pub, 70 | const PointCloudPtr& pc); 71 | 72 | static void SetMarker(const VizColor& color, 73 | const std::string& ns, 74 | const float& scale, 75 | const float& alpha, 76 | Marker& scan_marker, 77 | const float& scale_ratio=FARUtil::kVizRatio); 78 | 79 | static void SetColor(const VizColor& color, const float& alpha, Marker& scan_marker); 80 | 81 | }; 82 | 83 | #endif -------------------------------------------------------------------------------- /src/far_planner/include/far_planner/point_struct.h: -------------------------------------------------------------------------------- 1 | #ifndef POINT_STRUCT_H 2 | #define POINT_STRUCT_H 3 | 4 | #define EPSILON 1e-7f 5 | 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 | 19 | // ROS message support 20 | #include 21 | 22 | 23 | typedef pcl::PointXYZI PCLPoint; 24 | typedef pcl::PointCloud PointCloud; 25 | typedef pcl::PointCloud::Ptr PointCloudPtr; 26 | 27 | struct Point3D { 28 | float x, y, z; 29 | float intensity; 30 | Point3D() = default; 31 | Point3D(float _x, float _y, float _z):\ 32 | x(_x), y(_y), z(_z), intensity(0) {} 33 | Point3D(float _x, float _y, float _z, float _i):\ 34 | x(_x), y(_y), z(_z), intensity(_i) {} 35 | Point3D(Eigen::Vector3f vec):\ 36 | x(vec(0)), y(vec(1)), z(vec(2)), intensity(0) {} 37 | Point3D(Eigen::Vector3d vec):\ 38 | x(vec(0)), y(vec(1)), z(vec(2)), intensity(0) {} 39 | Point3D(PCLPoint pt):\ 40 | x(pt.x), y(pt.y), z(pt.z), intensity(pt.intensity) {} 41 | 42 | bool operator ==(const Point3D& pt) const 43 | { 44 | return fabs(x - pt.x) < EPSILON && 45 | fabs(y - pt.y) < EPSILON && 46 | fabs(z - pt.z) < EPSILON; 47 | } 48 | 49 | bool operator !=(const Point3D& pt) const 50 | { 51 | return fabs(x - pt.x) > EPSILON || 52 | fabs(y - pt.y) > EPSILON || 53 | fabs(z - pt.z) > EPSILON; 54 | } 55 | 56 | float operator *(const Point3D& pt) const 57 | { 58 | return x * pt.x + y * pt.y + z * pt.z; 59 | } 60 | 61 | Point3D operator *(const float factor) const 62 | { 63 | return Point3D(x*factor, y*factor, z*factor); 64 | } 65 | 66 | Point3D operator /(const float factor) const 67 | { 68 | return Point3D(x/factor, y/factor, z/factor); 69 | } 70 | 71 | Point3D operator +(const Point3D& pt) const 72 | { 73 | return Point3D(x+pt.x, y+pt.y, z+pt.z); 74 | } 75 | template 76 | Point3D operator -(const Point& pt) const 77 | { 78 | return Point3D(x-pt.x, y-pt.y, z-pt.z); 79 | } 80 | Point3D operator -() const 81 | { 82 | return Point3D(-x, -y, -z); 83 | } 84 | float norm() const 85 | { 86 | return std::hypotf(x, std::hypotf(y,z)); 87 | }; 88 | 89 | float norm_flat() const 90 | { 91 | return std::hypotf(x, y); 92 | }; 93 | 94 | Point3D normalize() const 95 | { 96 | const float n = std::hypotf(x, std::hypotf(y,z)); 97 | if (n > EPSILON) { 98 | return Point3D(x/n, y/n, z/n); 99 | } else { 100 | // DEBUG 101 | // ROS_WARN("Point3D: vector normalization fails, vector norm is too small."); 102 | return Point3D(0,0,0); 103 | } 104 | }; 105 | 106 | Point3D normalize_flat() const 107 | { 108 | const float n = std::hypotf(x, y); 109 | if (n > EPSILON) { 110 | return Point3D(x/n, y/n, 0.0f); 111 | } else { 112 | // DEBUG 113 | // ROS_WARN("Point3D: flat vector normalization fails, vector norm is too small."); 114 | return Point3D(0,0,0); 115 | } 116 | }; 117 | 118 | float norm_dot(Point3D p) const 119 | { 120 | const float n1 = std::hypotf(x, std::hypotf(y,z)); 121 | const float n2 = std::hypotf(p.x, std::hypotf(p.y,p.z)); 122 | if (n1 < EPSILON || n2 < EPSILON) { 123 | // DEBUG 124 | // ROS_WARN("Point3D: vector norm dot fails, vector norm is too small."); 125 | return 0.f; 126 | } 127 | const float dot_value = (x * p.x + y * p.y + z * p.z) / (n1 * n2); 128 | return std::min(std::max(-1.0f, dot_value), 1.0f); 129 | }; 130 | 131 | float norm_flat_dot(Point3D p) const 132 | { 133 | const float n1 = std::hypotf(x, y); 134 | const float n2 = std::hypotf(p.x, p.y); 135 | if (n1 < EPSILON || n2 < EPSILON) { 136 | // DEBUG 137 | // ROS_WARN("Point3D: flat vector norm dot fails, vector norm is too small."); 138 | return 0.f; 139 | } 140 | const float dot_value = (x * p.x + y * p.y) / (n1 * n2); 141 | return std::min(std::max(-1.0f, dot_value), 1.0f); 142 | }; 143 | 144 | std::string ToString() const 145 | { 146 | return "x = " + std::to_string(x).substr(0,6) + "; " + 147 | "y = " + std::to_string(y).substr(0,6) + "; " + 148 | "z = " + std::to_string(z).substr(0,6) + " "; 149 | }; 150 | 151 | friend std::ostream& operator<<(std::ostream& os, const Point3D& p) 152 | { 153 | os << "["<< p.ToString() << "] "; 154 | return os; 155 | } 156 | }; 157 | 158 | struct point_hash 159 | { 160 | std::size_t operator() (const Point3D& p) const 161 | { 162 | std::size_t seed = 0; 163 | boost::hash_combine(seed, p.x); 164 | boost::hash_combine(seed, p.y); 165 | boost::hash_combine(seed, p.z); 166 | return seed; 167 | } 168 | }; 169 | 170 | struct point_comp 171 | { 172 | bool operator()(const Point3D& p1, const Point3D& p2) const 173 | { 174 | return p1 == p2; 175 | } 176 | }; 177 | 178 | struct intensity_comp 179 | { 180 | bool operator()(const Point3D& p1, const Point3D& p2) const 181 | { 182 | return p1.intensity < p2.intensity; 183 | } 184 | }; 185 | 186 | #endif -------------------------------------------------------------------------------- /src/far_planner/include/far_planner/scan_handler.h: -------------------------------------------------------------------------------- 1 | #ifndef SCAN_HANDLER_H 2 | #define SCAN_HANDLER_H 3 | 4 | #include "utility.h" 5 | 6 | struct ScanHandlerParams { 7 | ScanHandlerParams() = default; 8 | float terrain_range; 9 | float voxel_size; 10 | float ceil_height; 11 | }; 12 | 13 | enum GridStatus { 14 | EMPTY = 0, 15 | SCAN = 1, 16 | OBS = 2, 17 | RAY = 3, 18 | }; 19 | 20 | class ScanHandler { 21 | 22 | public: 23 | ScanHandler() = default; 24 | ~ScanHandler() = default; 25 | 26 | void Init(const ScanHandlerParams& params); 27 | 28 | void UpdateRobotPosition(const Point3D& odom_pos); 29 | 30 | void SetCurrentScanCloud(const PointCloudPtr& scanCloudIn, const PointCloudPtr& freeCloudIn); 31 | 32 | void SetSurroundObsCloud(const PointCloudPtr& obsCloudIn, 33 | const bool& is_fiWlter_cloud=false); 34 | 35 | void ExtractDyObsCloud(const PointCloudPtr& cloudIn, const PointCloudPtr& dyObsCloudOut); 36 | 37 | void GridVisualCloud(const PointCloudPtr& cloudOut, const GridStatus& type); 38 | 39 | void ReInitGrids(); 40 | 41 | inline PCLPoint Ind2PCLPoint(const int& ind) { 42 | PCLPoint pcl_p; 43 | Eigen::Vector3d pos = voxel_grids_->Ind2Pos(ind); 44 | pcl_p.x = pos.x(), pcl_p.y = pos.y(), pcl_p.z = pos.z(); 45 | return pcl_p; 46 | } 47 | 48 | private: 49 | ScanHandlerParams scan_params_; 50 | Eigen::Vector3i center_sub_; 51 | int row_num_, col_num_, level_num_; 52 | bool is_grids_init_ = false; 53 | PCLPoint center_p_; 54 | // Set resolution for Velodyne LiDAR PUCK: https://www.amtechs.co.jp/product/VLP-16-Puck.pdf 55 | const float ANG_RES_Y = 2.0f/180.0f * M_PI; // vertical resolution 2 degree 56 | const float ANG_RES_X = 0.5f/180.0f * M_PI; // horizontal resolution 0.5 degree 57 | std::unique_ptr> voxel_grids_; 58 | 59 | void SetMapOrigin(const Point3D& ori_robot_pos); 60 | void SetRayCloud(const Eigen::Vector3i& point_sub); 61 | 62 | }; 63 | 64 | 65 | #endif -------------------------------------------------------------------------------- /src/far_planner/include/far_planner/terrain_planner.h: -------------------------------------------------------------------------------- 1 | #ifndef TERRIAN_PLANNER_H 2 | #define TERRIAN_PLANNER_H 3 | 4 | #include "utility.h" 5 | #include "planner_visualizer.h" 6 | 7 | struct TerrainPlannerParams { 8 | TerrainPlannerParams() = default; 9 | std::string world_frame; 10 | float radius; 11 | float voxel_size; 12 | int inflate_size; 13 | }; 14 | 15 | struct TerrainNode { 16 | TerrainNode() = default; 17 | int id; 18 | Point3D position; 19 | bool is_occupied; 20 | float fscore; 21 | float gscore; 22 | std::shared_ptr parent; 23 | }; 24 | 25 | typedef std::shared_ptr TerrainNodePtr; 26 | typedef std::vector TerrainNodeStack; 27 | 28 | 29 | struct TNodeptr_fcomp 30 | { 31 | bool operator()(const TerrainNodePtr& n1, const TerrainNodePtr& n2) const 32 | { 33 | return n1->fscore > n2->fscore; 34 | } 35 | }; 36 | 37 | class TerrainPlanner { 38 | 39 | public: 40 | TerrainPlanner() = default; 41 | ~TerrainPlanner() = default; 42 | 43 | void Init(const ros::NodeHandle& nh, const TerrainPlannerParams& params); 44 | 45 | void UpdateCenterNode(const NavNodePtr& node_ptr); 46 | 47 | void SetLocalTerrainObsCloud(const PointCloudPtr& obsCloudIn); 48 | 49 | bool PlanPathFromPToP(const Point3D& from_p, const Point3D& to_p, PointStack& path); 50 | 51 | inline bool PlanPathFromNodeToNode(const NavNodePtr& node_from, const NavNodePtr& node_to, PointStack& path) { 52 | return PlanPathFromPToP(node_from->position, node_to->position, path); 53 | } 54 | 55 | inline bool IsPointOccupy(const Point3D& p) { 56 | if (!is_grids_init_) return false; 57 | const Eigen::Vector3i sub = terrain_grids_->Pos2Sub(p.x, p.y, center_pos_.z); 58 | if (terrain_grids_->InRange(sub) && terrain_grids_->GetCell(sub)->is_occupied) { 59 | return true; 60 | } 61 | return false; 62 | } 63 | 64 | void VisualPaths(); 65 | 66 | private: 67 | ros::NodeHandle nh_; 68 | TerrainPlannerParams tp_params_; 69 | int row_num_, col_num_; 70 | bool is_grids_init_ = false; 71 | NavNodePtr center_node_prt_ = NULL; 72 | Point3D center_pos_; 73 | std::vector viz_path_stack_; 74 | 75 | ros::Publisher local_path_pub_, terrain_map_pub_; 76 | 77 | std::unique_ptr> terrain_grids_; 78 | 79 | void ExtractPath(const TerrainNodePtr& end_ptr, PointStack& path); 80 | 81 | void GridVisualCloud(); 82 | 83 | inline float EulerCost(const TerrainNodePtr& node_ptr1, const TerrainNodePtr& node_ptr2) { 84 | return (node_ptr1->position - node_ptr2->position).norm(); 85 | } 86 | 87 | inline void AllocateGridNodes() { 88 | for (std::size_t i=0; iGetCellNumber(); i++) { 89 | terrain_grids_->GetCell(i) = std::make_shared(); 90 | terrain_grids_->GetCell(i)->id = i; 91 | } 92 | } 93 | 94 | inline void ResetGridsOccupancy() { 95 | if (!is_grids_init_) return; 96 | for (std::size_t i=0; iGetCellNumber(); i++) { 97 | terrain_grids_->GetCell(i)->is_occupied = false; 98 | } 99 | } 100 | 101 | inline void ResetGridsPositions() { 102 | if (!is_grids_init_) return; 103 | for (std::size_t i=0; iGetCellNumber(); i++) { 104 | terrain_grids_->GetCell(i)->position = Point3D(terrain_grids_->Ind2Pos(i)); 105 | } 106 | } 107 | 108 | inline void ResetGridsPlanStatus() { 109 | if (!is_grids_init_) return; 110 | for (std::size_t i=0; iGetCellNumber(); i++) { 111 | terrain_grids_->GetCell(i)->fscore = FARUtil::kINF; 112 | terrain_grids_->GetCell(i)->gscore = FARUtil::kINF; 113 | terrain_grids_->GetCell(i)->parent = NULL; 114 | } 115 | } 116 | 117 | inline Point3D Ind2Point3D(const int& ind) { 118 | return terrain_grids_->GetCell(ind)->position; 119 | } 120 | 121 | inline PCLPoint Ind2PCLPoint(const int& ind) { 122 | return FARUtil::Point3DToPCLPoint(Ind2Point3D(ind)); 123 | } 124 | 125 | }; 126 | 127 | 128 | #endif -------------------------------------------------------------------------------- /src/far_planner/include/far_planner/time_measure.h: -------------------------------------------------------------------------------- 1 | #ifndef TIME_MEASURE_H 2 | #define TIME_MEASURE_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | using namespace chrono; 10 | typedef high_resolution_clock Clock; 11 | 12 | typedef unordered_map> TimerInstant; 13 | 14 | class TimeMeasure { 15 | private: 16 | TimerInstant timer_stack_; 17 | public: 18 | TimeMeasure() = default; 19 | ~TimeMeasure() = default; 20 | 21 | inline void start_time(const string& timer_name, const bool& is_reset=false) { 22 | const auto it = timer_stack_.find(timer_name); 23 | const auto start_time = Clock::now(); 24 | if (it == timer_stack_.end()) { 25 | timer_stack_.insert({timer_name, start_time}); 26 | } else if (is_reset) { 27 | it->second = start_time; 28 | } 29 | } 30 | 31 | inline double end_time(const string& timer_name, const bool& is_output=true) { 32 | const auto it = timer_stack_.find(timer_name); 33 | if (it != timer_stack_.end()) { 34 | const auto end_time = Clock::now(); 35 | const auto duration = duration_cast(end_time - it->second); 36 | const double time_duration = duration.count() / 1000.0; 37 | if (is_output) std::cout<<" "<(cur_time - it->second); 49 | const double time_duration = duration.count() / 1000.0; 50 | return time_duration; 51 | } 52 | return -1.0; 53 | } 54 | }; 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /src/far_planner/launch/far_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /src/far_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | far_planner 3 | 0.0.1 4 | FAR Planner 5 | Fan Yang 6 | BSD 7 | 8 | catkin 9 | 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | visibility_graph_msg 14 | pcl_ros 15 | 16 | roscpp 17 | std_msgs 18 | sensor_msgs 19 | visibility_graph_msg 20 | pcl_ros 21 | 22 | -------------------------------------------------------------------------------- /src/far_planner/src/contour_detector.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FAR Planner 3 | * Copyright (C) 2021 Fan Yang - All rights reserved 4 | * fanyang2@andrew.cmu.edu, 5 | */ 6 | 7 | #include "far_planner/contour_detector.h" 8 | 9 | // const static int BLUR_SIZE = 10; 10 | 11 | /***************************************************************************************/ 12 | 13 | void ContourDetector::Init(const ContourDetectParams& params) { 14 | cd_params_ = params; 15 | /* Allocate Pointcloud pointer memory */ 16 | new_corners_cloud_ = PointCloudPtr(new pcl::PointCloud()); 17 | // Init projection cv Mat 18 | MAT_SIZE = std::ceil(cd_params_.sensor_range * 2.0f / cd_params_.voxel_dim); 19 | if (MAT_SIZE % 2 == 0) MAT_SIZE ++; 20 | MAT_RESIZE = MAT_SIZE * (int)cd_params_.kRatio; 21 | CMAT = MAT_SIZE / 2, CMAT_RESIZE = MAT_RESIZE / 2; 22 | img_mat_ = cv::Mat::zeros(MAT_SIZE, MAT_SIZE, CV_32FC1); 23 | img_counter_ = 0; 24 | odom_node_ptr_ = NULL; 25 | refined_contours_.clear(), refined_hierarchy_.clear(); 26 | DIST_LIMIT = cd_params_.kRatio * 1.5f; 27 | ALIGN_ANGLE_COS = cos(FARUtil::kAcceptAlign / 2.0f); 28 | VOXEL_DIM_INV = 1.0f / cd_params_.voxel_dim; 29 | } 30 | 31 | void ContourDetector::BuildTerrainImgAndExtractContour(const NavNodePtr& odom_node_ptr, 32 | const PointCloudPtr& surround_cloud, 33 | std::vector& realworl_contour) { 34 | CVPointStack cv_corners; 35 | PointStack corner_vec; 36 | this->UpdateOdom(odom_node_ptr); 37 | this->ResetImgMat(img_mat_); 38 | this->UpdateImgMatWithCloud(surround_cloud, img_mat_); 39 | this->ExtractContourFromImg(img_mat_, refined_contours_, realworl_contour); 40 | } 41 | 42 | void ContourDetector::UpdateImgMatWithCloud(const PointCloudPtr& pc, cv::Mat& img_mat) { 43 | int row_idx, col_idx, inf_row, inf_col; 44 | const std::vector inflate_vec{-1, 0, 1}; 45 | for (const auto& pcl_p : pc->points) { 46 | this->PointToImgSub(pcl_p, odom_pos_, row_idx, col_idx, false, false); 47 | if (!this->IsIdxesInImg(row_idx, col_idx)) continue; 48 | for (const auto& dr : inflate_vec) { 49 | for (const auto& dc : inflate_vec) { 50 | inf_row = row_idx+dr, inf_col = col_idx+dc; 51 | if (this->IsIdxesInImg(inf_row, inf_col)) { 52 | img_mat.at(inf_row, inf_col) += 1.0; 53 | } 54 | } 55 | } 56 | } 57 | if (!FARUtil::IsStaticEnv) { 58 | cv::threshold(img_mat, img_mat, cd_params_.kThredValue, 1.0, cv::ThresholdTypes::THRESH_BINARY); 59 | } 60 | if (cd_params_.is_save_img) this->SaveCurrentImg(img_mat); 61 | } 62 | 63 | void ContourDetector::ResizeAndBlurImg(const cv::Mat& img, cv::Mat& Rimg) { 64 | img.convertTo(Rimg, CV_8UC1, 255); 65 | cv::resize(Rimg, Rimg, cv::Size(), cd_params_.kRatio, cd_params_.kRatio, 66 | cv::InterpolationFlags::INTER_LINEAR); 67 | //cv::morphologyEx(Rimg, Rimg, cv::MORPH_OPEN, getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3))); 68 | cv::boxFilter(Rimg, Rimg, -1, cv::Size(cd_params_.kBlurSize, cd_params_.kBlurSize), cv::Point2i(-1, -1), false); 69 | //cv::morphologyEx(Rimg, Rimg, cv::MORPH_CLOSE, getStructuringElement(cv::MORPH_RECT, cv::Size(cd_params_.kBlurSize+2, cd_params_.kBlurSize+2))); 70 | } 71 | 72 | void ContourDetector::ExtractContourFromImg(const cv::Mat& img, 73 | std::vector& img_contours, 74 | std::vector& realworld_contour) 75 | { 76 | cv::Mat Rimg; 77 | this->ResizeAndBlurImg(img, Rimg); 78 | this->ExtractRefinedContours(Rimg, img_contours); 79 | this->ConvertContoursToRealWorld(img_contours, realworld_contour); 80 | } 81 | 82 | void ContourDetector::ConvertContoursToRealWorld(const std::vector& ori_contours, 83 | std::vector& realWorld_contours) 84 | { 85 | const std::size_t C_N = ori_contours.size(); 86 | realWorld_contours.clear(), realWorld_contours.resize(C_N); 87 | for (std::size_t i=0; iConvertCVToPoint3DVector(cv_contour, realWorld_contours[i], true); 90 | } 91 | } 92 | 93 | 94 | void ContourDetector::ShowCornerImage(const cv::Mat& img_mat, 95 | const PointCloudPtr& pc) { 96 | cv::Mat dst = cv::Mat::zeros(MAT_RESIZE, MAT_RESIZE, CV_8UC3); 97 | const int circle_size = (int)(cd_params_.kRatio*1.5); 98 | for (std::size_t i=0; isize(); i++) { 99 | cv::Point2f cv_p = this->ConvertPoint3DToCVPoint(pc->points[i], odom_pos_, true); 100 | cv::circle(dst, cv_p, circle_size, cv::Scalar(128,128,128), -1); 101 | 102 | } 103 | // show free odom point 104 | cv::circle(dst, free_odom_resized_, circle_size, cv::Scalar(0,0,255), -1); 105 | std::vector> round_contours; 106 | this->RoundContours(refined_contours_, round_contours); 107 | for(std::size_t idx=0; idx& refined_contours) 117 | { 118 | 119 | std::vector> raw_contours; 120 | refined_contours.clear(), refined_hierarchy_.clear(); 121 | cv::findContours(imgIn, raw_contours, refined_hierarchy_, 122 | cv::RetrievalModes::RETR_TREE, 123 | cv::ContourApproximationModes::CHAIN_APPROX_TC89_L1); 124 | 125 | refined_contours.resize(raw_contours.size()); 126 | for (std::size_t i=0; iTopoFilterContours(refined_contours); 131 | this->AdjecentDistanceFilter(refined_contours); 132 | } 133 | 134 | void ContourDetector::AdjecentDistanceFilter(std::vector& contoursInOut) { 135 | /* filter out vertices that are overlapped with neighbor */ 136 | std::unordered_set remove_idxs; 137 | for (std::size_t i=0; i DIST_LIMIT) { 144 | /** Reduce wall nodes */ 145 | RemoveWallConnection(contoursInOut[i], p, refined_idx); 146 | contoursInOut[i][refined_idx] = p; 147 | refined_idx ++; 148 | } 149 | } 150 | /** Reduce wall nodes */ 151 | RemoveWallConnection(contoursInOut[i], contoursInOut[i][0], refined_idx); 152 | contoursInOut[i].resize(refined_idx); 153 | if (refined_idx > 1 && FARUtil::PixelDistance(contoursInOut[i].front(), contoursInOut[i].back()) < DIST_LIMIT) { 154 | contoursInOut[i].pop_back(); 155 | } 156 | if (contoursInOut[i].size() < 3) remove_idxs.insert(i); 157 | } 158 | if (!remove_idxs.empty()) { // clear contour with vertices size less that 3 159 | std::vector temp_contours = contoursInOut; 160 | contoursInOut.clear(); 161 | for (int i=0; i& contoursInOut) { 169 | std::unordered_set remove_idxs; 170 | for (int i=0; i temp_contours = contoursInOut; 181 | contoursInOut.clear(); 182 | for (int i=0; i("/robot_vgraph", 5); 17 | graph_sub_ = nh_.subscribe("/decoded_vgraph", 5, &GraphMsger::GraphCallBack, this); 18 | 19 | global_graph_.clear(); 20 | nodes_cloud_ptr_ = PointCloudPtr(new pcl::PointCloud()); 21 | kdtree_graph_cloud_ = PointKdTreePtr(new pcl::KdTreeFLANN()); 22 | kdtree_graph_cloud_->setSortedResults(false); 23 | } 24 | 25 | void GraphMsger::EncodeGraph(const NodePtrStack& graphIn, visibility_graph_msg::Graph& graphOut) { 26 | graphOut.nodes.clear(); 27 | const std::string frame_id = graphOut.header.frame_id; 28 | for (const auto& node_ptr : graphIn) { 29 | if (node_ptr->connect_nodes.empty() || !IsEncodeType(node_ptr)) continue; 30 | visibility_graph_msg::Node msg_node; 31 | msg_node.header.frame_id = frame_id; 32 | msg_node.position = FARUtil::Point3DToGeoMsgPoint(node_ptr->position); 33 | msg_node.id = node_ptr->id; 34 | msg_node.FreeType = static_cast(node_ptr->free_direct); 35 | msg_node.is_covered = node_ptr->is_covered; 36 | msg_node.is_frontier = node_ptr->is_frontier; 37 | msg_node.is_navpoint = node_ptr->is_navpoint; 38 | msg_node.is_boundary = node_ptr->is_boundary; 39 | msg_node.surface_dirs.clear(); 40 | msg_node.surface_dirs.push_back(FARUtil::Point3DToGeoMsgPoint(node_ptr->surf_dirs.first)); 41 | msg_node.surface_dirs.push_back(FARUtil::Point3DToGeoMsgPoint(node_ptr->surf_dirs.second)); 42 | // Encode connections 43 | msg_node.connect_nodes.clear(); 44 | for (const auto& cnode_ptr : node_ptr->connect_nodes) { 45 | if (!IsEncodeType(cnode_ptr)) continue; 46 | msg_node.connect_nodes.push_back(cnode_ptr->id); 47 | } 48 | msg_node.poly_connects.clear(); 49 | for (const auto& cnode_ptr : node_ptr->poly_connects) { 50 | if (!IsEncodeType(cnode_ptr)) continue; 51 | msg_node.poly_connects.push_back(cnode_ptr->id); 52 | } 53 | msg_node.contour_connects.clear(); 54 | for (const auto& cnode_ptr : node_ptr->contour_connects) { 55 | if (!IsEncodeType(cnode_ptr)) continue; 56 | msg_node.contour_connects.push_back(cnode_ptr->id); 57 | } 58 | msg_node.trajectory_connects.clear(); 59 | for (const auto& cnode_ptr : node_ptr->trajectory_connects) { 60 | if (!IsEncodeType(cnode_ptr)) continue; 61 | msg_node.trajectory_connects.push_back(cnode_ptr->id); 62 | } 63 | graphOut.nodes.push_back(msg_node); 64 | } 65 | graphOut.size = graphOut.nodes.size(); 66 | } 67 | 68 | void GraphMsger::UpdateGlobalGraph(const NodePtrStack& graph) { 69 | global_graph_ = graph; 70 | // Update Graph Node KdTree 71 | if (global_graph_.empty()) { 72 | FARUtil::ClearKdTree(nodes_cloud_ptr_, kdtree_graph_cloud_); 73 | } else { 74 | nodes_cloud_ptr_->resize(global_graph_.size()); 75 | std::size_t idx = 0; 76 | for (const auto node_ptr : global_graph_) { 77 | if (node_ptr->is_odom || FARUtil::IsOutsideGoal(node_ptr)) continue; 78 | PCLPoint pcl_p = FARUtil::Point3DToPCLPoint(node_ptr->position); 79 | pcl_p.intensity = node_ptr->id; 80 | nodes_cloud_ptr_->points[idx] = pcl_p; 81 | idx ++; 82 | } 83 | kdtree_graph_cloud_->setInputCloud(nodes_cloud_ptr_); 84 | } 85 | this->PublishGlobalGraph(global_graph_); 86 | } 87 | 88 | void GraphMsger::PublishGlobalGraph(const NodePtrStack& graphIn) { 89 | visibility_graph_msg::Graph graph_msg; 90 | graph_msg.header.frame_id = gm_params_.frame_id; 91 | graph_msg.robot_id = gm_params_.robot_id; 92 | this->EncodeGraph(graphIn, graph_msg); 93 | graph_pub_.publish(graph_msg); 94 | } 95 | 96 | void GraphMsger::GraphCallBack(const visibility_graph_msg::GraphConstPtr& msg) { 97 | if (msg->nodes.empty()) return; 98 | NodePtrStack decoded_nodes; 99 | const std::size_t robot_id = msg->robot_id; 100 | const visibility_graph_msg::Graph graph_msg = *msg; 101 | IdxMap nodeIdx_idx_map; 102 | // Create nav nodes for decoded graph 103 | decoded_nodes.clear(); 104 | for (std::size_t i=0; i connect_idxs, poly_idxs, contour_idxs, traj_idxs; 117 | for (std::size_t i=0; iis_active || !cnode_ptr->is_active || (node_ptr->is_boundary && cnode_ptr->is_boundary))) { 126 | DynamicGraph::AddEdge(node_ptr, cnode_ptr); 127 | } 128 | } 129 | // poly connections 130 | for (const auto& cid : poly_idxs) { 131 | cnode_ptr = IdToNodePtr(cid, nodeIdx_idx_map, decoded_nodes); 132 | if (cnode_ptr != NULL && (!node_ptr->is_active || !cnode_ptr->is_active || (node_ptr->is_boundary && cnode_ptr->is_boundary))) { 133 | DynamicGraph::FillPolygonEdgeConnect(node_ptr, cnode_ptr, gm_params_.votes_size); 134 | } 135 | } 136 | // contour connections 137 | for (const auto& cid : contour_idxs) { 138 | cnode_ptr = IdToNodePtr(cid, nodeIdx_idx_map, decoded_nodes); 139 | if (cnode_ptr != NULL && (!node_ptr->is_active || !cnode_ptr->is_active || (node_ptr->is_boundary && cnode_ptr->is_boundary))) { 140 | DynamicGraph::FillContourConnect(node_ptr, cnode_ptr, gm_params_.votes_size); 141 | } 142 | } 143 | // trajectory connection 144 | for (const auto& cid : traj_idxs) { 145 | cnode_ptr = IdToNodePtr(cid, nodeIdx_idx_map, decoded_nodes); 146 | if (cnode_ptr != NULL && (!node_ptr->is_active || !cnode_ptr->is_active || (node_ptr->is_boundary && cnode_ptr->is_boundary))) { 147 | DynamicGraph::FillTrajConnect(node_ptr, cnode_ptr); 148 | } 149 | } 150 | } 151 | } 152 | 153 | NavNodePtr GraphMsger::NearestNodePtrOnGraph(const Point3D p, const float radius) { 154 | if (global_graph_.empty()) return NULL; 155 | // Find the nearest node in graph 156 | std::vector pIdxK(1); 157 | std::vector pdDistK(1); 158 | PCLPoint pcl_p = FARUtil::Point3DToPCLPoint(p); 159 | if (kdtree_graph_cloud_->nearestKSearch(pcl_p, 1, pIdxK, pdDistK) > 0) { 160 | if (pdDistK[0] < radius) { 161 | const PCLPoint graph_p = nodes_cloud_ptr_->points[pIdxK[0]]; 162 | const std::size_t node_id = static_cast(graph_p.intensity); 163 | return DynamicGraph::MappedNavNodeFromId(node_id); 164 | } 165 | } 166 | return NULL; 167 | } 168 | 169 | void GraphMsger::CreateDecodedNavNode(const visibility_graph_msg::Node& vnode, NavNodePtr& node_ptr) { 170 | const Point3D p = Point3D(vnode.position.x, vnode.position.y, vnode.position.z); 171 | const bool is_covered = vnode.is_covered == 0 ? false : true; 172 | const bool is_frontier = vnode.is_frontier == 0 ? false : true; 173 | const bool is_navpoint = vnode.is_navpoint == 0 ? false : true; 174 | const bool is_boundary = vnode.is_boundary == 0 ? false : true; 175 | DynamicGraph::CreateNavNodeFromPoint(p, node_ptr, false, is_navpoint, false, is_boundary); 176 | node_ptr->is_active = false; 177 | /* Assign relative values */ 178 | node_ptr->is_covered = is_covered; 179 | node_ptr->is_frontier = is_frontier; 180 | DynamicGraph::FillFrontierVotes(node_ptr, is_frontier); 181 | // positions 182 | node_ptr->is_finalized = true; 183 | const std::deque pos_queue(gm_params_.pool_size, p); 184 | node_ptr->pos_filter_vec = pos_queue; 185 | const PointPair surf_pair = {Point3D(vnode.surface_dirs[0].x, vnode.surface_dirs[0].y, vnode.surface_dirs[0].z), 186 | Point3D(vnode.surface_dirs[1].x, vnode.surface_dirs[1].y, vnode.surface_dirs[1].z)}; 187 | // surf directions 188 | node_ptr->free_direct = static_cast(vnode.FreeType); 189 | node_ptr->surf_dirs = surf_pair; 190 | const std::deque surf_queue(gm_params_.pool_size, surf_pair); 191 | node_ptr->surf_dirs_vec = surf_queue; 192 | } 193 | 194 | void GraphMsger::ExtractConnectIdxs(const visibility_graph_msg::Node& node, 195 | IdxStack& connect_idxs, 196 | IdxStack& poly_idxs, 197 | IdxStack& contour_idxs, 198 | IdxStack& traj_idxs) 199 | { 200 | connect_idxs.clear(), poly_idxs.clear(), contour_idxs.clear(), traj_idxs.clear(); 201 | for (const auto& cid : node.connect_nodes) { 202 | connect_idxs.push_back((std::size_t)cid); 203 | } 204 | for (const auto& cid : node.poly_connects) { 205 | poly_idxs.push_back((std::size_t)cid); 206 | } 207 | for (const auto& cid : node.contour_connects) { 208 | contour_idxs.push_back((std::size_t)cid); 209 | } 210 | for (const auto& cid : node.trajectory_connects) { 211 | traj_idxs.push_back((std::size_t)cid); 212 | } 213 | } 214 | 215 | -------------------------------------------------------------------------------- /src/far_planner/src/scan_handler.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Dynamic Route Planner 3 | * Copyright (C) 2021 Fan Yang - All rights reserved 4 | * fanyang2@andrew.cmu.edu, 5 | */ 6 | 7 | 8 | #include "far_planner/scan_handler.h" 9 | 10 | /***************************************************************************************/ 11 | 12 | const char INIT_BIT = char(0); // 0000 13 | const char SCAN_BIT = char(1); // 0001 14 | const char OBS_BIT = char(2); // 0010 15 | const char RAY_BIT = char(4); // 0100 16 | 17 | void ScanHandler::Init(const ScanHandlerParams& params) { 18 | scan_params_ = params; 19 | row_num_ = std::ceil(scan_params_.terrain_range * 2.0f / scan_params_.voxel_size); 20 | if (row_num_ % 2 == 0) row_num_ ++; 21 | col_num_ = row_num_; 22 | level_num_ = std::ceil(scan_params_.ceil_height * 2.0f / scan_params_.voxel_size); 23 | if (level_num_ % 2 == 0) level_num_ ++; 24 | Eigen::Vector3i grid_size(row_num_, col_num_, level_num_); 25 | Eigen::Vector3d grid_origin(0,0,0); 26 | Eigen::Vector3d grid_resolution(scan_params_.voxel_size, scan_params_.voxel_size, scan_params_.voxel_size); 27 | voxel_grids_ = std::make_unique>(grid_size, INIT_BIT, grid_origin, grid_resolution, 3); 28 | is_grids_init_ = true; 29 | } 30 | 31 | void ScanHandler::ReInitGrids() { 32 | if (!is_grids_init_) return; 33 | voxel_grids_->ReInitGrid(INIT_BIT); 34 | } 35 | 36 | void ScanHandler::UpdateRobotPosition(const Point3D& robot_pos) { 37 | if (!is_grids_init_) return; 38 | Eigen::Vector3d grid_origin; 39 | grid_origin.x() = robot_pos.x - (scan_params_.voxel_size * row_num_) / 2.0f; 40 | grid_origin.y() = robot_pos.y - (scan_params_.voxel_size * col_num_) / 2.0f; 41 | grid_origin.z() = robot_pos.z - (scan_params_.voxel_size * level_num_) / 2.0f; 42 | voxel_grids_->SetOrigin(grid_origin); 43 | center_sub_ = voxel_grids_->Pos2Sub(Eigen::Vector3d(robot_pos.x, robot_pos.y, robot_pos.z)); 44 | center_p_ = FARUtil::Point3DToPCLPoint(robot_pos); 45 | } 46 | 47 | void ScanHandler::SetCurrentScanCloud(const PointCloudPtr& scanCloudIn, const PointCloudPtr& freeCloudIn) { 48 | if (!is_grids_init_ || scanCloudIn->empty()) return; 49 | // remove free scan points 50 | PointCloudPtr copyObsScanCloud(new pcl::PointCloud()); 51 | pcl::copyPointCloud(*scanCloudIn, *copyObsScanCloud); 52 | FARUtil::RemoveOverlapCloud(copyObsScanCloud, freeCloudIn, true); 53 | for (const auto& point : copyObsScanCloud->points) { // assign obstacle scan voxels 54 | const float r = pcl::euclideanDistance(point, center_p_); 55 | const int L = static_cast(std::ceil((r * ANG_RES_X)/scan_params_.voxel_size/2.0f))+FARUtil::kObsInflate; 56 | const int N = static_cast(std::ceil((r * ANG_RES_Y)/scan_params_.voxel_size/2.0f)); 57 | Eigen::Vector3i c_sub = voxel_grids_->Pos2Sub(Eigen::Vector3d(point.x, point.y, point.z)); 58 | for (int i = -L; i <= L; i++) { 59 | for (int j = -L; j <= L; j++) { 60 | for (int k = -N; k <= N; k++) { 61 | Eigen::Vector3i sub; 62 | sub.x() = c_sub.x() + i, sub.y() = c_sub.y() + j, sub.z() = c_sub.z() + k; 63 | if (voxel_grids_->InRange(sub)) { 64 | const int ind = voxel_grids_->Sub2Ind(sub); 65 | voxel_grids_->GetCell(ind) = voxel_grids_->GetCell(ind) | SCAN_BIT; 66 | } 67 | } 68 | } 69 | } 70 | } 71 | for (const auto& point : scanCloudIn->points) { 72 | Eigen::Vector3i sub = voxel_grids_->Pos2Sub(Eigen::Vector3d(point.x, point.y, point.z)); 73 | this->SetRayCloud(sub); 74 | } 75 | } 76 | 77 | void ScanHandler::SetSurroundObsCloud(const PointCloudPtr& obsCloudIn, const bool& is_filter_cloud) { 78 | if (!is_grids_init_ || obsCloudIn->empty()) return; 79 | if (is_filter_cloud) FARUtil::FilterCloud(obsCloudIn, scan_params_.voxel_size); 80 | for (const auto& point : obsCloudIn->points) { 81 | Eigen::Vector3i sub = voxel_grids_->Pos2Sub(Eigen::Vector3d(point.x, point.y, point.z)); 82 | if (!voxel_grids_->InRange(sub)) continue; 83 | const int ind = voxel_grids_->Sub2Ind(sub); 84 | voxel_grids_->GetCell(ind) = voxel_grids_->GetCell(ind) | OBS_BIT; 85 | } 86 | } 87 | 88 | void ScanHandler::ExtractDyObsCloud(const PointCloudPtr& cloudIn, const PointCloudPtr& dyObsCloudOut) { 89 | if (!is_grids_init_ || cloudIn->empty()) return; 90 | dyObsCloudOut->clear(); 91 | for (const auto& point : cloudIn->points) { 92 | Eigen::Vector3i sub = voxel_grids_->Pos2Sub(Eigen::Vector3d(point.x, point.y, point.z)); 93 | if (!voxel_grids_->InRange(sub)) continue; 94 | const int ind = voxel_grids_->Sub2Ind(sub); 95 | const char cell_c = voxel_grids_->GetCell(ind); 96 | if ((cell_c | RAY_BIT) == cell_c) { 97 | dyObsCloudOut->points.push_back(point); 98 | } 99 | } 100 | } 101 | 102 | void ScanHandler::SetRayCloud(const Eigen::Vector3i& point_sub) { 103 | Eigen::Vector3i dir_sub = point_sub - center_sub_; 104 | if (dir_sub.squaredNorm() < 1.0) return; 105 | Eigen::Vector3i steps_sub; 106 | steps_sub.x() = abs(dir_sub.x()), steps_sub.y() = abs(dir_sub.y()), steps_sub.z() = abs(dir_sub.z()); 107 | Eigen::Vector3i sign_sub; 108 | sign_sub.x() = FARUtil::Signum(dir_sub.x()), sign_sub.y() = FARUtil::Signum(dir_sub.y()), sign_sub.z() = FARUtil::Signum(dir_sub.z()); 109 | if (steps_sub.x() >= std::max(steps_sub.y(), steps_sub.z())) { 110 | const int N = steps_sub.x(); 111 | const float rx2y = (float)steps_sub.y() / (float)steps_sub.x(); 112 | const float rx2z = (float)steps_sub.z() / (float)steps_sub.x(); 113 | for (int i=1; iInRange(csub)) break; 119 | const int ind = voxel_grids_->Sub2Ind(csub); 120 | const char cell_c = voxel_grids_->GetCell(ind); 121 | if ((cell_c | SCAN_BIT) == cell_c) break; // hit scan point cell 122 | voxel_grids_->GetCell(ind) = cell_c | RAY_BIT; 123 | } 124 | } else if (steps_sub.y() >= std::max(steps_sub.x(), steps_sub.z())) { 125 | const int N = steps_sub.y(); 126 | const float ry2x = (float)steps_sub.x() / (float)steps_sub.y(); 127 | const float ry2z = (float)steps_sub.z() / (float)steps_sub.y(); 128 | for (int i=1; iInRange(csub)) break; 134 | const int ind = voxel_grids_->Sub2Ind(csub); 135 | const char cell_c = voxel_grids_->GetCell(ind); 136 | if ((cell_c | SCAN_BIT) == cell_c) break; // hit scan point cell 137 | voxel_grids_->GetCell(ind) = cell_c | RAY_BIT; 138 | } 139 | } else { 140 | const int N = steps_sub.z(); 141 | const float rz2x = (float)steps_sub.x() / (float)steps_sub.z(); 142 | const float rz2y = (float)steps_sub.y() / (float)steps_sub.z(); 143 | for (int i=1; iInRange(csub)) break; 149 | const int ind = voxel_grids_->Sub2Ind(csub); 150 | const char cell_c = voxel_grids_->GetCell(ind); 151 | if ((cell_c | SCAN_BIT) == cell_c) break; // hit scan point cell 152 | voxel_grids_->GetCell(ind) = cell_c | RAY_BIT; 153 | } 154 | } 155 | } 156 | 157 | void ScanHandler::GridVisualCloud(const PointCloudPtr& cloudOut, const GridStatus& type) { 158 | if (!is_grids_init_) return; 159 | const int N = voxel_grids_->GetCellNumber(); 160 | cloudOut->clear(); 161 | char RType; 162 | switch (type) { 163 | case GridStatus::SCAN: 164 | RType = SCAN_BIT; 165 | break; 166 | case GridStatus::OBS: 167 | RType = OBS_BIT; 168 | break; 169 | case GridStatus::RAY: 170 | RType = RAY_BIT; 171 | break; 172 | default: 173 | RType = SCAN_BIT; 174 | break; 175 | } 176 | for (int ind=0; indGetCell(ind); 178 | if ((c_type | RType) == c_type) { 179 | cloudOut->points.push_back(this->Ind2PCLPoint(ind)); 180 | } 181 | } 182 | } -------------------------------------------------------------------------------- /src/far_planner/src/terrain_planner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FAR Planner 3 | * Copyright (C) 2021 Fan Yang - All rights reserved 4 | * fanyang2@andrew.cmu.edu, 5 | */ 6 | 7 | 8 | #include "far_planner/terrain_planner.h" 9 | 10 | /***************************************************************************************/ 11 | 12 | void TerrainPlanner::Init(const ros::NodeHandle& nh, const TerrainPlannerParams& params) { 13 | nh_ = nh; 14 | tp_params_ = params; 15 | row_num_ = std::ceil((FARUtil::kLocalPlanRange + tp_params_.radius) * 2.0f / tp_params_.voxel_size); 16 | col_num_ = row_num_; 17 | TerrainNodePtr init_terrain_node_ptr = NULL; 18 | Eigen::Vector3i grid_size(row_num_, col_num_, 1); 19 | Eigen::Vector3d grid_origin(0,0,0); 20 | Eigen::Vector3d grid_resolution(tp_params_.voxel_size, tp_params_.voxel_size, tp_params_.voxel_size); 21 | terrain_grids_ = std::make_unique>(grid_size, init_terrain_node_ptr, grid_origin, grid_resolution, 3); 22 | this->AllocateGridNodes(); 23 | viz_path_stack_.clear(); 24 | 25 | local_path_pub_ = nh_.advertise("/local_terrain_path_debug", 5); 26 | terrain_map_pub_ = nh_.advertise("/local_terrain_map_debug", 5); 27 | } 28 | 29 | void TerrainPlanner::UpdateCenterNode(const NavNodePtr& node_ptr) { 30 | if (node_ptr == NULL) return; 31 | center_node_prt_ = node_ptr, center_pos_ = node_ptr->position; 32 | Eigen::Vector3d grid_origin; 33 | grid_origin.x() = center_pos_.x - (tp_params_.voxel_size * row_num_) / 2.0f; 34 | grid_origin.y() = center_pos_.y - (tp_params_.voxel_size * col_num_) / 2.0f; 35 | grid_origin.z() = center_pos_.z - (tp_params_.voxel_size * 1.0f) / 2.0f; 36 | terrain_grids_->SetOrigin(grid_origin); 37 | is_grids_init_ = true; 38 | this->ResetGridsPositions(); 39 | this->ResetGridsOccupancy(); 40 | } 41 | 42 | void TerrainPlanner::SetLocalTerrainObsCloud(const PointCloudPtr& obsCloudIn) { 43 | if (!is_grids_init_ || obsCloudIn->empty()) return; 44 | const int N_IF = tp_params_.inflate_size; 45 | for (const auto& point : obsCloudIn->points) { 46 | Eigen::Vector3i c_sub = terrain_grids_->Pos2Sub(Eigen::Vector3d(point.x, point.y, center_pos_.z)); 47 | for (int i = -N_IF; i <= N_IF; i++) { 48 | for (int j = -N_IF; j <= N_IF; j++) { 49 | Eigen::Vector3i sub; 50 | sub.x() = c_sub.x() + i, sub.y() = c_sub.y() + j, sub.z() = 0; 51 | if (terrain_grids_->InRange(sub)) { 52 | const int ind = terrain_grids_->Sub2Ind(sub); 53 | terrain_grids_->GetCell(ind)->is_occupied = true; 54 | } 55 | } 56 | } 57 | } 58 | /* visualize terrain planner map and path */ 59 | this->GridVisualCloud(); 60 | } 61 | 62 | void TerrainPlanner::GridVisualCloud() { 63 | if (!is_grids_init_) return; 64 | PointCloudPtr temp_cloud_ptr(new pcl::PointCloud()); 65 | const int N = terrain_grids_->GetCellNumber(); 66 | for (int ind=0; indGetCell(ind)->is_occupied) { 68 | temp_cloud_ptr->points.push_back(this->Ind2PCLPoint(ind)); 69 | } 70 | } 71 | sensor_msgs::PointCloud2 msg_pc; 72 | pcl::toROSMsg(*temp_cloud_ptr, msg_pc); 73 | msg_pc.header.frame_id = tp_params_.world_frame; 74 | msg_pc.header.stamp = ros::Time::now(); 75 | terrain_map_pub_.publish(msg_pc); 76 | } 77 | 78 | bool TerrainPlanner::PlanPathFromPToP(const Point3D& from_p, const Point3D& to_p, PointStack& path) { 79 | path.clear(); 80 | if (!is_grids_init_) return false; 81 | this->ResetGridsPlanStatus(); 82 | const Eigen::Vector3d start_pos(from_p.x, from_p.y, center_pos_.z); 83 | const Eigen::Vector3d end_pos(to_p.x, to_p.y, center_pos_.z); 84 | const Eigen::Vector3i start_sub = terrain_grids_->Pos2Sub(start_pos); 85 | const Eigen::Vector3i end_sub = terrain_grids_->Pos2Sub(end_pos); 86 | if (!terrain_grids_->InRange(start_sub) || !terrain_grids_->InRange(end_sub)) { 87 | if (FARUtil::IsDebug) ROS_WARN("TP: two interval navigation nodes are not in terrain planning range."); 88 | return false; 89 | } 90 | const TerrainNodePtr start_node_ptr = terrain_grids_->GetCell(terrain_grids_->Sub2Ind(start_sub)); 91 | const TerrainNodePtr end_node_ptr = terrain_grids_->GetCell(terrain_grids_->Sub2Ind(end_sub)); 92 | const Point3D unit_axial = (to_p - from_p).normalize(); 93 | const float ndist = (to_p - from_p).norm(); 94 | 95 | // Lambda function 96 | auto InCylinder = [&](const TerrainNodePtr& tnode_ptr) { 97 | const Point3D vec = tnode_ptr->position - from_p; 98 | float proj_scalar = vec * unit_axial; 99 | if (proj_scalar < - FARUtil::kNavClearDist || proj_scalar > ndist + FARUtil::kNavClearDist) { 100 | return false; 101 | } 102 | const Point3D vec_axial = unit_axial * proj_scalar; 103 | if ((vec - vec_axial).norm() > tp_params_.radius) { 104 | return false; 105 | } 106 | return true; 107 | }; 108 | 109 | std::array dx = {1, 1, 0,-1,-1, 0, 1,-1}; 110 | std::array dy = {0, 1, 1, 0, 1,-1,-1,-1}; 111 | start_node_ptr->gscore = 0.0; 112 | std::unordered_set open_set; 113 | std::priority_queue, TNodeptr_fcomp> open_queue; 114 | std::unordered_set close_set; 115 | open_queue.push(start_node_ptr); 116 | open_set.insert(start_node_ptr->id); 117 | while (true) { 118 | if (open_set.empty()) { 119 | break; 120 | } 121 | const TerrainNodePtr current = open_queue.top(); 122 | if (current == end_node_ptr) { 123 | this->ExtractPath(end_node_ptr, path); 124 | break; 125 | } 126 | open_queue.pop(); 127 | open_set.erase(current->id); 128 | close_set.insert(current->id); 129 | for (int i=0; i<8; i++) { 130 | Eigen::Vector3i csub = terrain_grids_->Ind2Sub(current->id); 131 | csub.x() += dx[i], csub.y() += dy[i], csub.z() = 0; 132 | if (!terrain_grids_->InRange(csub)) continue; 133 | const TerrainNodePtr neighbor = terrain_grids_->GetCell(terrain_grids_->Sub2Ind(csub)); 134 | if (close_set.count(neighbor->id) || (neighbor->is_occupied && neighbor != end_node_ptr) || !InCylinder(neighbor)) continue; 135 | const float temp_gscore = current->gscore + this->EulerCost(current, neighbor); 136 | if (temp_gscore < neighbor->gscore) { 137 | neighbor->parent = current; 138 | neighbor->gscore = temp_gscore; 139 | neighbor->fscore = temp_gscore + this->EulerCost(neighbor, end_node_ptr); 140 | if (!open_set.count(neighbor->id)) { 141 | open_queue.push(neighbor); 142 | open_set.insert(neighbor->id); 143 | } 144 | } 145 | } 146 | } 147 | if (!path.empty()) { 148 | viz_path_stack_.push_back(path); 149 | } 150 | return !path.empty(); 151 | } 152 | 153 | 154 | void TerrainPlanner::ExtractPath(const TerrainNodePtr& end_ptr, PointStack& path) { 155 | path.clear(); 156 | TerrainNodePtr cur_ptr = end_ptr; 157 | while (true) { 158 | path.push_back(cur_ptr->position); 159 | if (cur_ptr->parent != NULL) { 160 | cur_ptr = cur_ptr->parent; 161 | } else { 162 | break; 163 | } 164 | } 165 | std::reverse(path.begin(), path.end()); 166 | } 167 | 168 | void TerrainPlanner::VisualPaths() { 169 | Marker terrain_paths_marker; 170 | terrain_paths_marker.type = Marker::LINE_LIST; 171 | DPVisualizer::SetMarker(VizColor::ORANGE, "terrain_path", 0.3f, 0.85f, terrain_paths_marker); 172 | 173 | auto DrawPath = [&](const PointStack& path) { 174 | if (path.size() < 2) return; 175 | geometry_msgs::Point last_p = FARUtil::Point3DToGeoMsgPoint(path[0]); 176 | for (int i=1; i 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "rviz/display_context.h" 13 | #include "rviz/properties/string_property.h" 14 | #include "rviz/default_plugin/tools/pose_tool.h" 15 | 16 | namespace rviz 17 | { 18 | class StringProperty; 19 | 20 | class GoalPointTool : public PoseTool 21 | { 22 | Q_OBJECT 23 | public: 24 | GoalPointTool(); 25 | virtual ~GoalPointTool() 26 | { 27 | } 28 | virtual void onInitialize(); 29 | 30 | protected: 31 | virtual void odomHandler(const nav_msgs::Odometry::ConstPtr& odom); 32 | virtual void onPoseSet(double x, double y, double theta); 33 | 34 | private Q_SLOTS: 35 | void updateTopic(); 36 | 37 | private: 38 | float vehicle_z; 39 | 40 | ros::NodeHandle nh_; 41 | ros::Subscriber sub_; 42 | ros::Publisher pub_; 43 | ros::Publisher pub_joy_; 44 | 45 | StringProperty* topic_property_; 46 | }; 47 | } 48 | 49 | #endif // GOALPOINT_RVIZ_PLUGIN_WAYPOINT_TOOL_H 50 | -------------------------------------------------------------------------------- /src/goalpoint_rviz_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | goalpoint_rviz_plugin 4 | 0.0.1 5 | Route Planner Goalpoint RVIZ Plugin 6 | Chao Cao 7 | BSD 8 | 9 | catkin 10 | 11 | roscpp 12 | rospy 13 | rviz 14 | qtbase5-dev 15 | 16 | roscpp 17 | rospy 18 | rviz 19 | 20 | roscpp 21 | rospy 22 | rviz 23 | libqt5-core 24 | libqt5-gui 25 | libqt5-widgets 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /src/goalpoint_rviz_plugin/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A tool for sending goalpoint to FAR route planner 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/goalpoint_rviz_plugin/src/goalpoint_tool.cpp: -------------------------------------------------------------------------------- 1 | #include "goalpoint_tool.h" 2 | 3 | namespace rviz 4 | { 5 | GoalPointTool::GoalPointTool() 6 | { 7 | shortcut_key_ = 'w'; 8 | 9 | topic_property_ = new StringProperty("Topic", "goalpoint", "The topic on which to publish goal waypiont for dynamic route planner.", 10 | getPropertyContainer(), SLOT(updateTopic()), this); 11 | } 12 | 13 | void GoalPointTool::onInitialize() 14 | { 15 | PoseTool::onInitialize(); 16 | setName("Goalpoint"); 17 | updateTopic(); 18 | vehicle_z = 0; 19 | } 20 | 21 | void GoalPointTool::updateTopic() 22 | { 23 | sub_ = nh_.subscribe ("/state_estimation", 5, &GoalPointTool::odomHandler, this); 24 | pub_ = nh_.advertise("/goal_point", 5); 25 | pub_joy_ = nh_.advertise("/joy", 5); 26 | } 27 | 28 | void GoalPointTool::odomHandler(const nav_msgs::Odometry::ConstPtr& odom) 29 | { 30 | vehicle_z = odom->pose.pose.position.z; 31 | } 32 | 33 | void GoalPointTool::onPoseSet(double x, double y, double theta) 34 | { 35 | sensor_msgs::Joy joy; 36 | 37 | joy.axes.push_back(0); 38 | joy.axes.push_back(0); 39 | joy.axes.push_back(-1.0); 40 | joy.axes.push_back(0); 41 | joy.axes.push_back(1.0); 42 | joy.axes.push_back(1.0); 43 | joy.axes.push_back(0); 44 | joy.axes.push_back(0); 45 | 46 | joy.buttons.push_back(0); 47 | joy.buttons.push_back(0); 48 | joy.buttons.push_back(0); 49 | joy.buttons.push_back(0); 50 | joy.buttons.push_back(0); 51 | joy.buttons.push_back(0); 52 | joy.buttons.push_back(0); 53 | joy.buttons.push_back(1); 54 | joy.buttons.push_back(0); 55 | joy.buttons.push_back(0); 56 | joy.buttons.push_back(0); 57 | 58 | joy.header.stamp = ros::Time::now(); 59 | joy.header.frame_id = "goalpoint_tool"; 60 | 61 | geometry_msgs::PointStamped goal_point; 62 | goal_point.header.frame_id = "map"; 63 | goal_point.header.stamp = ros::Time::now(); 64 | goal_point.point.x = x; 65 | goal_point.point.y = y; 66 | goal_point.point.z = vehicle_z; 67 | 68 | pub_.publish(goal_point); 69 | usleep(10000); 70 | pub_.publish(goal_point); 71 | 72 | usleep(10000); // set to 1000000us (1s) on real robot 73 | pub_joy_.publish(joy); 74 | } 75 | } // end namespace rviz 76 | 77 | #include 78 | PLUGINLIB_EXPORT_CLASS(rviz::GoalPointTool, rviz::Tool) 79 | -------------------------------------------------------------------------------- /src/graph_decoder/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(graph_decoder) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | set(CMAKE_BUILD_TYPE Release) 7 | set(BUILD_STATIC_LIBS ON) 8 | set(BUILD_SHARED_LIBS OFF) 9 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 10 | 11 | ## Find catkin macros and libraries 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | std_msgs 15 | visibility_graph_msg 16 | pcl_ros 17 | ) 18 | 19 | ################################### 20 | ## catkin specific configuration ## 21 | ################################### 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS 25 | roscpp 26 | std_msgs 27 | pcl_ros 28 | ) 29 | 30 | ########### 31 | ## Build ## 32 | ########### 33 | 34 | ## Specify additional locations of header files 35 | ## Your package locations should be listed before other locations 36 | include_directories( 37 | ${catkin_INCLUDE_DIRS} 38 | ${PCL_INCLUDE_DIRS} 39 | "${PROJECT_SOURCE_DIR}/include" 40 | /usr/local/include # Location when using 'make system_install' 41 | /usr/include # More usual location (e.g. when installing using a package) 42 | ) 43 | 44 | ## Specify additional locations for library files 45 | link_directories( 46 | /usr/local/lib # Location when using 'make system_install' 47 | /usr/lib # More usual location (e.g. when installing using a package) 48 | ) 49 | 50 | ## Depedent cpp files 51 | set(SOURCES ${SOURCES} 52 | src/decoder_node.cpp 53 | ) 54 | 55 | ## Declare executables 56 | add_executable(${PROJECT_NAME} ${SOURCES}) 57 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 58 | 59 | ## Specify libraries to link a library or executable target against 60 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 61 | 62 | install(TARGETS ${PROJECT_NAME} 63 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 64 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 65 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 66 | ) 67 | 68 | install(DIRECTORY launch/ 69 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 70 | ) 71 | -------------------------------------------------------------------------------- /src/graph_decoder/config/default.yaml: -------------------------------------------------------------------------------- 1 | # Graph Decoder Default Params 2 | world_frame : map 3 | visual_scale_ratio : 0.5 -------------------------------------------------------------------------------- /src/graph_decoder/decoder.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 549 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Class: rviz/MarkerArray 38 | Enabled: true 39 | Marker Topic: /graph_decoder_viz 40 | Name: MarkerArray 41 | Namespaces: 42 | {} 43 | Queue Size: 100 44 | Value: true 45 | Enabled: true 46 | Global Options: 47 | Background Color: 0; 0; 0 48 | Default Light: true 49 | Fixed Frame: map 50 | Frame Rate: 30 51 | Name: root 52 | Tools: 53 | - Class: rviz/Interact 54 | Hide Inactive Objects: true 55 | - Class: rviz/MoveCamera 56 | - Class: rviz/Select 57 | - Class: rviz/FocusCamera 58 | - Class: rviz/Measure 59 | - Class: rviz/SetInitialPose 60 | Theta std deviation: 0.2617993950843811 61 | Topic: /initialpose 62 | X std deviation: 0.5 63 | Y std deviation: 0.5 64 | - Class: rviz/SetGoal 65 | Topic: /move_base_simple/goal 66 | - Class: rviz/PublishPoint 67 | Single click: true 68 | Topic: /clicked_point 69 | Value: true 70 | Views: 71 | Current: 72 | Class: rviz/Orbit 73 | Distance: 10 74 | Enable Stereo Rendering: 75 | Stereo Eye Separation: 0.05999999865889549 76 | Stereo Focal Distance: 1 77 | Swap Stereo Eyes: false 78 | Value: false 79 | Focal Point: 80 | X: 0 81 | Y: 0 82 | Z: 0 83 | Focal Shape Fixed Size: true 84 | Focal Shape Size: 0.05000000074505806 85 | Invert Z Axis: false 86 | Name: Current View 87 | Near Clip Distance: 0.009999999776482582 88 | Pitch: 0.785398006439209 89 | Target Frame: 90 | Value: Orbit (rviz) 91 | Yaw: 0.785398006439209 92 | Saved: ~ 93 | Window Geometry: 94 | Displays: 95 | collapsed: true 96 | Height: 1385 97 | Hide Left Dock: true 98 | Hide Right Dock: true 99 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006970000003efc0100000002fb0000000800540069006d0065010000000000000697000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000697000004cb00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 100 | Selection: 101 | collapsed: false 102 | Time: 103 | collapsed: false 104 | Tool Properties: 105 | collapsed: false 106 | Views: 107 | collapsed: true 108 | Width: 1687 109 | X: 1753 110 | Y: 27 111 | -------------------------------------------------------------------------------- /src/graph_decoder/include/graph_decoder/decoder_node.h: -------------------------------------------------------------------------------- 1 | #ifndef GRAPH_DECODER_H 2 | #define GRAPH_DECODER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include "graph_decoder/point_struct.h" 19 | 20 | typedef visualization_msgs::Marker Marker; 21 | typedef visualization_msgs::MarkerArray MarkerArray; 22 | 23 | typedef std::pair PointPair; 24 | 25 | enum VizColor { 26 | RED = 0, 27 | ORANGE = 1, 28 | BLACK = 2, 29 | YELLOW = 3, 30 | BLUE = 4, 31 | GREEN = 5, 32 | EMERALD = 6, 33 | WHITE = 7, 34 | MAGNA = 8, 35 | PURPLE = 9 36 | }; 37 | 38 | enum NodeFreeDirect { 39 | UNKNOW = 0, 40 | CONVEX = 1, 41 | CONCAVE = 2, 42 | PILLAR = 3 43 | }; 44 | 45 | struct NavNode { 46 | NavNode() = default; 47 | std::size_t id; 48 | Point3D position; 49 | NodeFreeDirect free_direct; 50 | PointPair surf_dirs; 51 | bool is_covered; 52 | bool is_frontier; 53 | bool is_navpoint; 54 | bool is_boundary; 55 | std::vector connect_idxs; 56 | std::vector> connect_nodes; 57 | 58 | std::vector poly_idxs; 59 | std::vector> poly_connects; 60 | 61 | std::vector contour_idxs; 62 | std::vector> contour_connects; 63 | 64 | std::vector traj_idxs; 65 | std::vector> traj_connects; 66 | }; 67 | 68 | typedef std::shared_ptr NavNodePtr; 69 | typedef std::vector NodePtrStack; 70 | 71 | struct GraphDecoderParams { 72 | GraphDecoderParams() = default; 73 | std::string frame_id; 74 | float viz_scale_ratio; 75 | }; 76 | 77 | class GraphDecoder { 78 | public: 79 | GraphDecoder() = default; 80 | ~GraphDecoder() = default; 81 | 82 | void Init(); 83 | void Loop(); 84 | 85 | private: 86 | ros::NodeHandle nh; 87 | ros::Subscriber graph_sub_; 88 | ros::Subscriber save_graph_sub_, read_graph_sub_; 89 | ros::Publisher graph_pub_, graph_viz_pub_; 90 | 91 | ros::ServiceServer request_graph_service_; 92 | GraphDecoderParams gd_params_; 93 | NodePtrStack received_graph_; 94 | MarkerArray graph_marker_array_; 95 | std::size_t robot_id_; 96 | 97 | void LoadParmas(); 98 | 99 | void SetMarker(const VizColor& color, 100 | const std::string& ns, 101 | const float scale, 102 | const float alpha, 103 | Marker& scan_marker); 104 | 105 | void SetColor(const VizColor& color, const float alpha, Marker& scan_marker); 106 | 107 | void GraphCallBack(const visibility_graph_msg::GraphConstPtr& msg); 108 | 109 | void EncodeGraph(const NodePtrStack& graphIn, visibility_graph_msg::Graph& graphOut); 110 | 111 | void SaveGraphCallBack(const std_msgs::StringConstPtr& msg); 112 | 113 | void ReadGraphCallBack(const std_msgs::StringConstPtr& msg); 114 | 115 | bool SaveGraphService(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); 116 | 117 | bool ReadGraphFromFile(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); 118 | 119 | bool RequestGraphService(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); 120 | 121 | void VisualizeGraph(const NodePtrStack& graphIn); 122 | 123 | void CreateNavNode(std::string str, NavNodePtr& node_ptr); 124 | 125 | void CreateNavNode(const visibility_graph_msg::Node& msg, NavNodePtr& node_ptr); 126 | 127 | void AssignConnectNodes(const std::unordered_map& idxs_map, 128 | const NodePtrStack& graph, 129 | std::vector& node_idxs, 130 | std::vector& connects); 131 | 132 | template 133 | geometry_msgs::Point inline ToGeoMsgP(const Point& p) { 134 | geometry_msgs::Point geo_p; 135 | geo_p.x = p.x; 136 | geo_p.y = p.y; 137 | geo_p.z = p.z; 138 | return geo_p; 139 | } 140 | 141 | template 142 | bool IsTypeInStack(const T& elem, const std::vector& T_stack) { 143 | if (std::find(T_stack.begin(), T_stack.end(), elem) != T_stack.end()) { 144 | return true; 145 | } 146 | return false; 147 | } 148 | 149 | inline void ResetGraph(NodePtrStack& graphOut) { 150 | graphOut.clear(); 151 | } 152 | 153 | inline void ConvertGraphToMsg(const NodePtrStack& graph, visibility_graph_msg::Graph& graph_msg) { 154 | graph_msg.header.frame_id = gd_params_.frame_id; 155 | graph_msg.header.stamp = ros::Time::now(); 156 | graph_msg.robot_id = robot_id_; 157 | EncodeGraph(graph, graph_msg); 158 | } 159 | 160 | inline bool AddNodePtrToGraph(const NavNodePtr& node_ptr, NodePtrStack& graphOut) { 161 | if (node_ptr != NULL) { 162 | graphOut.push_back(node_ptr); 163 | return true; 164 | } 165 | return false; 166 | } 167 | 168 | }; 169 | 170 | 171 | #endif -------------------------------------------------------------------------------- /src/graph_decoder/include/graph_decoder/point_struct.h: -------------------------------------------------------------------------------- 1 | #ifndef POINT_STRUCT_H 2 | #define POINT_STRUCT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "Eigen/Core" 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // ROS message support 18 | #include 19 | 20 | 21 | typedef pcl::PointXYZI PCLPoint; 22 | typedef pcl::PointCloud PointCloud; 23 | typedef pcl::PointCloud::Ptr PointCloudPtr; 24 | 25 | struct Point3D { 26 | float x, y, z; 27 | float intensity; 28 | Point3D() = default; 29 | Point3D(float _x, float _y, float _z):\ 30 | x(_x), y(_y), z(_z), intensity(0) {} 31 | Point3D(float _x, float _y, float _z, float _i):\ 32 | x(_x), y(_y), z(_z), intensity(_i) {} 33 | Point3D(Eigen::Vector3f vec):\ 34 | x(vec(0)), y(vec(1)), z(vec(2)), intensity(0) {} 35 | Point3D(Eigen::Vector3d vec):\ 36 | x(vec(0)), y(vec(1)), z(vec(2)), intensity(0) {} 37 | Point3D(PCLPoint pt):\ 38 | x(pt.x), y(pt.y), z(pt.z), intensity(pt.intensity) {} 39 | 40 | bool operator ==(const Point3D& pt) const 41 | { 42 | return x == pt.x && y == pt.y && z == pt.z; 43 | } 44 | 45 | bool operator !=(const Point3D& pt) const 46 | { 47 | return x != pt.x || y != pt.y || z != pt.z; 48 | } 49 | 50 | float operator *(const Point3D& pt) const 51 | { 52 | return x * pt.x + y * pt.y + z * pt.z; 53 | } 54 | 55 | Point3D operator *(const float factor) const 56 | { 57 | return Point3D(x*factor, y*factor, z*factor); 58 | } 59 | 60 | Point3D operator /(const float factor) const 61 | { 62 | return Point3D(x/factor, y/factor, z/factor); 63 | } 64 | 65 | Point3D operator +(const Point3D& pt) const 66 | { 67 | return Point3D(x+pt.x, y+pt.y, z+pt.z); 68 | } 69 | template 70 | Point3D operator -(const Point& pt) const 71 | { 72 | return Point3D(x-pt.x, y-pt.y, z-pt.z); 73 | } 74 | Point3D operator -() const 75 | { 76 | return Point3D(-x, -y, -z); 77 | } 78 | float norm() const 79 | { 80 | return std::hypotf(x, std::hypotf(y,z)); 81 | }; 82 | 83 | float norm_flat() const 84 | { 85 | return std::hypotf(x, y); 86 | }; 87 | 88 | Point3D normalize() const 89 | { 90 | const float n = std::hypotf(x, std::hypotf(y,z)); 91 | if (n > 1e-7f) { 92 | return Point3D(x/n, y/n, z/n); 93 | } else { 94 | // DEBUG 95 | // ROS_WARN("Point3D: vector normalization fails, vector norm is too small."); 96 | return Point3D(0,0,0); 97 | } 98 | }; 99 | 100 | Point3D normalize_flat() const 101 | { 102 | const float n = std::hypotf(x, y); 103 | if (n > 1e-7f) { 104 | return Point3D(x/n, y/n, 0.0f); 105 | } else { 106 | // DEBUG 107 | // ROS_WARN("Point3D: flat vector normalization fails, vector norm is too small."); 108 | return Point3D(0,0,0); 109 | } 110 | }; 111 | 112 | float norm_dot(Point3D p) const 113 | { 114 | const float n1 = std::hypotf(x, std::hypotf(y,z)); 115 | const float n2 = std::hypotf(p.x, std::hypotf(p.y,p.z)); 116 | if (n1 < 1e-7f || n2 < 1e-7f) { 117 | // DEBUG 118 | // ROS_WARN("Point3D: vector norm dot fails, vector norm is too small."); 119 | return 0.f; 120 | } 121 | const float dot_value = (x * p.x + y * p.y + z * p.z) / (n1 * n2); 122 | return std::min(std::max(-1.0f, dot_value), 1.0f); 123 | }; 124 | 125 | float norm_flat_dot(Point3D p) const 126 | { 127 | const float n1 = std::hypotf(x, y); 128 | const float n2 = std::hypotf(p.x, p.y); 129 | if (n1 < 1e-7f || n2 < 1e-7f) { 130 | // DEBUG 131 | // ROS_WARN("Point3D: flat vector norm dot fails, vector norm is too small."); 132 | return 0.f; 133 | } 134 | const float dot_value = (x * p.x + y * p.y) / (n1 * n2); 135 | return std::min(std::max(-1.0f, dot_value), 1.0f); 136 | }; 137 | 138 | std::string ToString() const 139 | { 140 | return "x = " + std::to_string(x).substr(0,6) + "; " + 141 | "y = " + std::to_string(y).substr(0,6) + "; " + 142 | "z = " + std::to_string(z).substr(0,6) + " "; 143 | }; 144 | 145 | friend std::ostream& operator<<(std::ostream& os, const Point3D& p) 146 | { 147 | os << "["<< p.ToString() << "] "; 148 | return os; 149 | } 150 | }; 151 | 152 | struct point_hash 153 | { 154 | std::size_t operator() (const Point3D& p) const 155 | { 156 | std::size_t seed = 0; 157 | boost::hash_combine(seed, p.x); 158 | boost::hash_combine(seed, p.y); 159 | boost::hash_combine(seed, p.z); 160 | return seed; 161 | } 162 | }; 163 | 164 | struct point_comp 165 | { 166 | bool operator()(const Point3D& p1, const Point3D& p2) const 167 | { 168 | return p1.x == p2.x && p1.y == p2.y && p1.z == p2.z; 169 | } 170 | }; 171 | 172 | struct intensity_comp 173 | { 174 | bool operator()(const Point3D& p1, const Point3D& p2) const 175 | { 176 | return p1.intensity < p2.intensity; 177 | } 178 | }; 179 | 180 | #endif -------------------------------------------------------------------------------- /src/graph_decoder/launch/decoder.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/graph_decoder/package.xml: -------------------------------------------------------------------------------- 1 | 2 | graph_decoder 3 | 0.0.1 4 | Visibility Graph Decoder 5 | Fan Yang 6 | BSD 7 | 8 | catkin 9 | 10 | roscpp 11 | std_msgs 12 | geometry_msgs 13 | visibility_graph_msg 14 | pcl_ros 15 | 16 | roscpp 17 | std_msgs 18 | geometry_msgs 19 | visibility_graph_msg 20 | pcl_ros 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/graph_decoder/rviz/decoder.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /MarkerArray1 10 | - /MarkerArray1/Namespaces1 11 | Splitter Ratio: 0.5 12 | Tree Height: 1154 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Class: rviz/MarkerArray 40 | Enabled: true 41 | Marker Topic: /graph_decoder_viz 42 | Name: MarkerArray 43 | Namespaces: 44 | angle_direct: false 45 | freespace_vertex: false 46 | freespace_vgraph: true 47 | global_vertex: false 48 | global_vgraph: false 49 | polygon_edge: true 50 | trajectory_vertex: true 51 | vertex_angle: true 52 | Queue Size: 100 53 | Value: true 54 | - Alpha: 0.10000000149011612 55 | Autocompute Intensity Bounds: true 56 | Autocompute Value Bounds: 57 | Max Value: 10 58 | Min Value: -10 59 | Value: true 60 | Axis: Z 61 | Channel Name: intensity 62 | Class: rviz/PointCloud2 63 | Color: 255; 255; 255 64 | Color Transformer: Intensity 65 | Decay Time: 0 66 | Enabled: true 67 | Invert Rainbow: false 68 | Max Color: 255; 255; 255 69 | Min Color: 0; 0; 0 70 | Name: OverallMap 71 | Position Transformer: XYZ 72 | Queue Size: 10 73 | Selectable: true 74 | Size (Pixels): 2 75 | Size (m): 0.009999999776482582 76 | Style: Points 77 | Topic: /overall_map 78 | Unreliable: false 79 | Use Fixed Frame: true 80 | Use rainbow: true 81 | Value: true 82 | Enabled: true 83 | Global Options: 84 | Background Color: 0; 0; 0 85 | Default Light: true 86 | Fixed Frame: map 87 | Frame Rate: 30 88 | Name: root 89 | Tools: 90 | - Class: rviz/Interact 91 | Hide Inactive Objects: true 92 | - Class: rviz/MoveCamera 93 | - Class: rviz/Select 94 | - Class: rviz/FocusCamera 95 | - Class: rviz/Measure 96 | - Class: rviz/SetInitialPose 97 | Theta std deviation: 0.2617993950843811 98 | Topic: /initialpose 99 | X std deviation: 0.5 100 | Y std deviation: 0.5 101 | - Class: rviz/SetGoal 102 | Topic: /move_base_simple/goal 103 | - Class: rviz/PublishPoint 104 | Single click: true 105 | Topic: /clicked_point 106 | Value: true 107 | Views: 108 | Current: 109 | Class: rviz/Orbit 110 | Distance: 54.439945220947266 111 | Enable Stereo Rendering: 112 | Stereo Eye Separation: 0.05999999865889549 113 | Stereo Focal Distance: 1 114 | Swap Stereo Eyes: false 115 | Value: false 116 | Focal Point: 117 | X: 10.719555854797363 118 | Y: -1.3865398168563843 119 | Z: 0.5914081335067749 120 | Focal Shape Fixed Size: true 121 | Focal Shape Size: 0.05000000074505806 122 | Invert Z Axis: false 123 | Name: Current View 124 | Near Clip Distance: 0.009999999776482582 125 | Pitch: 1.5697963237762451 126 | Target Frame: 127 | Value: Orbit (rviz) 128 | Yaw: 4.693587303161621 129 | Saved: ~ 130 | Window Geometry: 131 | Displays: 132 | collapsed: true 133 | Height: 1383 134 | Hide Left Dock: true 135 | Hide Right Dock: true 136 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000050dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000050d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006970000003efc0100000002fb0000000800540069006d0065000000000000000697000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000006970000050d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 137 | Selection: 138 | collapsed: false 139 | Time: 140 | collapsed: false 141 | Tool Properties: 142 | collapsed: false 143 | Views: 144 | collapsed: true 145 | Width: 1687 146 | X: 1753 147 | Y: 27 148 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(teleop_rviz_plugin) 3 | find_package(catkin REQUIRED COMPONENTS rviz) 4 | catkin_package() 5 | include_directories(${catkin_INCLUDE_DIRS}) 6 | link_directories(${catkin_LIBRARY_DIRS}) 7 | 8 | set(CMAKE_AUTOMOC ON) 9 | 10 | if(rviz_QT_VERSION VERSION_LESS "5") 11 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 12 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 13 | include(${QT_USE_FILE}) 14 | else() 15 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 16 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 17 | set(QT_LIBRARIES Qt5::Widgets) 18 | endif() 19 | 20 | add_definitions(-DQT_NO_KEYWORDS) 21 | 22 | set(SRC_FILES 23 | src/drive_widget.cpp 24 | src/teleop_panel.cpp 25 | ) 26 | 27 | add_library(${PROJECT_NAME} ${SRC_FILES}) 28 | 29 | target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) 30 | 31 | install(TARGETS 32 | ${PROJECT_NAME} 33 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 35 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 36 | ) 37 | 38 | install(FILES 39 | plugin_description.xml 40 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 41 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | teleop_rviz_plugin 3 | 0.0.1 4 | 5 | Teleop RVIZ Plugin 6 | 7 | Ji Zhang 8 | BSD 9 | 10 | catkin 11 | 12 | qtbase5-dev 13 | rviz 14 | 15 | libqt5-core 16 | libqt5-gui 17 | libqt5-widgets 18 | rviz 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A panel for driving diff-drive style robot base control. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/src/drive_widget.cpp: -------------------------------------------------------------------------------- 1 | #include "drive_widget.h" 2 | 3 | namespace teleop_rviz_plugin 4 | { 5 | 6 | DriveWidget::DriveWidget( QWidget* parent ) 7 | : QWidget( parent ) 8 | , linear_velocity_( 0 ) 9 | , angular_velocity_( 0 ) 10 | , linear_scale_( 1 ) 11 | , angular_scale_( 1 ) 12 | , x_mouse_( 0 ) 13 | , y_mouse_( 0 ) 14 | , mouse_pressed_(false) 15 | { 16 | } 17 | 18 | void DriveWidget::paintEvent( QPaintEvent* event ) 19 | { 20 | QColor background; 21 | QColor crosshair; 22 | if( isEnabled() ) 23 | { 24 | background = Qt::white; 25 | crosshair = Qt::black; 26 | } 27 | else 28 | { 29 | background = Qt::lightGray; 30 | crosshair = Qt::darkGray; 31 | } 32 | 33 | int w = width(); 34 | int h = height(); 35 | int size = (( w > h ) ? h : w) - 1; 36 | int hpad = ( w - size ) / 2; 37 | int vpad = ( h - size ) / 2; 38 | 39 | QPainter painter( this ); 40 | painter.setBrush( background ); 41 | painter.setPen( crosshair ); 42 | 43 | painter.drawRect( QRect( hpad, vpad, size, size )); 44 | 45 | painter.drawLine( hpad, height() / 2, hpad + size, height() / 2 ); 46 | painter.drawLine( width() / 2, vpad, width() / 2, vpad + size ); 47 | 48 | if( isEnabled() && (angular_velocity_ != 0 || linear_velocity_ != 0 )) 49 | { 50 | QPen pen; 51 | pen.setWidth( size/150 ); 52 | pen.setColor( Qt::darkRed ); 53 | pen.setCapStyle( Qt::RoundCap ); 54 | pen.setJoinStyle( Qt::RoundJoin ); 55 | painter.setPen( pen ); 56 | 57 | QPointF joystick[ 2 ]; 58 | joystick[ 0 ].setX( w/2 ); 59 | joystick[ 0 ].setY( h/2 ); 60 | joystick[ 1 ].setX( x_mouse_ ); 61 | joystick[ 1 ].setY( y_mouse_ ); 62 | 63 | painter.drawPolyline( joystick, 2 ); 64 | painter.drawEllipse( x_mouse_ - 10, y_mouse_ - 10, 20, 20 ); 65 | } 66 | } 67 | 68 | void DriveWidget::mouseMoveEvent( QMouseEvent* event ) 69 | { 70 | sendVelocitiesFromMouse( event->x(), event->y(), width(), height() ); 71 | } 72 | 73 | void DriveWidget::mousePressEvent( QMouseEvent* event ) 74 | { 75 | sendVelocitiesFromMouse( event->x(), event->y(), width(), height() ); 76 | } 77 | 78 | void DriveWidget::leaveEvent( QEvent* event ) 79 | { 80 | stop(); 81 | } 82 | 83 | void DriveWidget::mouseReleaseEvent( QMouseEvent* event ) 84 | { 85 | stop(); 86 | } 87 | 88 | void DriveWidget::sendVelocitiesFromMouse( int x, int y, int width, int height ) 89 | { 90 | int size = (( width > height ) ? height : width ); 91 | int hpad = ( width - size ) / 2; 92 | int vpad = ( height - size ) / 2; 93 | 94 | x_mouse_ = x; 95 | if ( x_mouse_ < width / 2 - size / 2 ) x_mouse_ = width / 2 - size / 2; 96 | else if ( x_mouse_ > width / 2 + size / 2 ) x_mouse_ = width / 2 + size / 2; 97 | y_mouse_ = y; 98 | if ( y_mouse_ < height / 2 - size / 2 ) y_mouse_ = height / 2 - size / 2; 99 | else if ( y_mouse_ > height / 2 + size / 2 ) y_mouse_ = height / 2 + size / 2; 100 | 101 | linear_velocity_ = (1.0 - float( y - vpad ) / float( size / 2 )) * linear_scale_; 102 | if ( linear_velocity_ < -1.0 ) linear_velocity_ = -1.0; 103 | else if ( linear_velocity_ > 1.0 ) linear_velocity_ = 1.0; 104 | if ( fabs( linear_velocity_ ) < 0.1 ) linear_velocity_ = 0; 105 | angular_velocity_ = ( 1.0 - float( x - hpad ) / float( size / 2 )) * angular_scale_; 106 | if ( angular_velocity_ < -1.0 ) angular_velocity_ = -1.0; 107 | else if ( angular_velocity_ > 1.0 ) angular_velocity_ = 1.0; 108 | 109 | mouse_pressed_ = true; 110 | 111 | Q_EMIT outputVelocity( linear_velocity_, angular_velocity_, mouse_pressed_ ); 112 | update(); 113 | } 114 | 115 | void DriveWidget::stop() 116 | { 117 | linear_velocity_ = 0; 118 | angular_velocity_ = 0; 119 | mouse_pressed_ = false; 120 | 121 | Q_EMIT outputVelocity( linear_velocity_, angular_velocity_, mouse_pressed_ ); 122 | update(); 123 | } 124 | 125 | } 126 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/src/drive_widget.h: -------------------------------------------------------------------------------- 1 | #ifndef DRIVE_WIDGET_H 2 | #define DRIVE_WIDGET_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace teleop_rviz_plugin 13 | { 14 | 15 | class DriveWidget: public QWidget 16 | { 17 | Q_OBJECT 18 | public: 19 | DriveWidget( QWidget* parent = 0 ); 20 | virtual void paintEvent( QPaintEvent* event ); 21 | virtual void mouseMoveEvent( QMouseEvent* event ); 22 | virtual void mousePressEvent( QMouseEvent* event ); 23 | virtual void mouseReleaseEvent( QMouseEvent* event ); 24 | virtual void leaveEvent( QEvent* event ); 25 | virtual QSize sizeHint() const { return QSize( 150, 150 ); } 26 | 27 | Q_SIGNALS: 28 | void outputVelocity( float linear, float angular, bool pressed ); 29 | 30 | protected: 31 | void sendVelocitiesFromMouse( int x, int y, int width, int height ); 32 | void stop(); 33 | 34 | float linear_velocity_; 35 | float angular_velocity_; 36 | float linear_scale_; 37 | float angular_scale_; 38 | float x_mouse_, y_mouse_; 39 | bool mouse_pressed_; 40 | }; 41 | 42 | } 43 | 44 | #endif // DRIVE_WIDGET_H 45 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/src/teleop_panel.cpp: -------------------------------------------------------------------------------- 1 | #include "drive_widget.h" 2 | #include "teleop_panel.h" 3 | 4 | namespace teleop_rviz_plugin 5 | { 6 | 7 | TeleopPanel::TeleopPanel( QWidget* parent ) 8 | : rviz::Panel( parent ) 9 | , linear_velocity_( 0 ) 10 | , angular_velocity_( 0 ) 11 | , mouse_pressed_( false ) 12 | , mouse_pressed_sent_( false ) 13 | { 14 | QVBoxLayout* layout = new QVBoxLayout; 15 | check_box_1_ = new QCheckBox( "Planning Attemptable", this ); 16 | check_box_1_->setCheckState( Qt::Checked ); 17 | layout->addWidget( check_box_1_ ); 18 | check_box_2_ = new QCheckBox( "Update Visibility Graph", this ); 19 | check_box_2_->setCheckState( Qt::Checked ); 20 | layout->addWidget( check_box_2_ ); 21 | push_button_1_ = new QPushButton( "Reset Visibility Graph", this ); 22 | layout->addWidget( push_button_1_ ); 23 | push_button_2_ = new QPushButton( "Resume Navigation to Goal", this ); 24 | layout->addWidget( push_button_2_ ); 25 | push_button_3_ = new QPushButton( "Read", this ); 26 | layout->addWidget( push_button_3_ ); 27 | push_button_4_ = new QPushButton( "Save", this ); 28 | layout->addWidget( push_button_4_ ); 29 | drive_widget_ = new DriveWidget; 30 | layout->addWidget( drive_widget_ ); 31 | setLayout( layout ); 32 | 33 | QTimer* output_timer = new QTimer( this ); 34 | 35 | connect( push_button_1_, SIGNAL( pressed() ), this, SLOT( pressButton1() )); 36 | connect( push_button_2_, SIGNAL( pressed() ), this, SLOT( pressButton2() )); 37 | connect( push_button_3_, SIGNAL( pressed() ), this, SLOT( pressButton3() )); 38 | connect( push_button_4_, SIGNAL( pressed() ), this, SLOT( pressButton4() )); 39 | connect( check_box_1_, SIGNAL( stateChanged(int) ), this, SLOT( clickBox1(int) )); 40 | connect( check_box_2_, SIGNAL( stateChanged(int) ), this, SLOT( clickBox2(int) )); 41 | connect( drive_widget_, SIGNAL( outputVelocity( float, float, bool )), this, SLOT( setVel( float, float, bool ))); 42 | connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() )); 43 | 44 | output_timer->start( 100 ); 45 | 46 | velocity_publisher_ = nh_.advertise( "/joy", 5 ); 47 | attemptable_publisher_ = nh_.advertise( "/planning_attemptable", 5 ); 48 | update_publisher_ = nh_.advertise( "/update_visibility_graph", 5 ); 49 | reset_publisher_ = nh_.advertise( "/reset_visibility_graph", 5 ); 50 | read_publisher_ = nh_.advertise( "/read_file_dir", 5 ); 51 | save_publisher_ = nh_.advertise( "/save_file_dir", 5 ); 52 | drive_widget_->setEnabled( true ); 53 | } 54 | 55 | void TeleopPanel::pressButton1() 56 | { 57 | if ( ros::ok() && velocity_publisher_ ) 58 | { 59 | std_msgs::Empty msg; 60 | reset_publisher_.publish(msg); 61 | } 62 | } 63 | 64 | void TeleopPanel::pressButton2() 65 | { 66 | if ( ros::ok() && velocity_publisher_ ) 67 | { 68 | sensor_msgs::Joy joy; 69 | 70 | joy.axes.push_back(0); 71 | joy.axes.push_back(0); 72 | joy.axes.push_back(-1.0); 73 | joy.axes.push_back(0); 74 | joy.axes.push_back(1.0); 75 | joy.axes.push_back(1.0); 76 | joy.axes.push_back(0); 77 | joy.axes.push_back(0); 78 | 79 | joy.buttons.push_back(0); 80 | joy.buttons.push_back(0); 81 | joy.buttons.push_back(0); 82 | joy.buttons.push_back(0); 83 | joy.buttons.push_back(0); 84 | joy.buttons.push_back(0); 85 | joy.buttons.push_back(0); 86 | joy.buttons.push_back(1); 87 | joy.buttons.push_back(0); 88 | joy.buttons.push_back(0); 89 | joy.buttons.push_back(0); 90 | 91 | joy.header.stamp = ros::Time::now(); 92 | joy.header.frame_id = "teleop_panel"; 93 | velocity_publisher_.publish( joy ); 94 | } 95 | } 96 | 97 | void TeleopPanel::pressButton3() 98 | { 99 | if ( ros::ok() && velocity_publisher_ ) 100 | { 101 | QString qFilename = QFileDialog::getOpenFileName(this, tr("Read File"), "/", tr("VGH - Visibility Graph Files (*.vgh)")); 102 | 103 | std::string filename = qFilename.toStdString(); 104 | std_msgs::String msg; 105 | msg.data = filename; 106 | read_publisher_.publish(msg); 107 | } 108 | } 109 | 110 | void TeleopPanel::pressButton4() 111 | { 112 | if ( ros::ok() && velocity_publisher_ ) 113 | { 114 | QString qFilename = QFileDialog::getSaveFileName(this, tr("Save File"), "/", tr("VGH - Visibility Graph Files (*.vgh)")); 115 | 116 | std::string filename = qFilename.toStdString(); 117 | if (filename != "") { 118 | int length = filename.length(); 119 | if (length < 4) { 120 | filename += ".vgh"; 121 | } else if (filename[length - 4] != '.' || filename[length - 3] != 'v' || filename[length - 2] != 'g' || filename[length - 1] != 'h') { 122 | filename += ".vgh"; 123 | } 124 | } 125 | std_msgs::String msg; 126 | msg.data = filename; 127 | save_publisher_.publish(msg); 128 | } 129 | } 130 | 131 | void TeleopPanel::clickBox1(int val) 132 | { 133 | if ( ros::ok() && attemptable_publisher_ ) 134 | { 135 | std_msgs::Bool msg; 136 | msg.data = bool(val); 137 | attemptable_publisher_.publish(msg); 138 | } 139 | } 140 | 141 | void TeleopPanel::clickBox2(int val) 142 | { 143 | if ( ros::ok() && update_publisher_ ) 144 | { 145 | std_msgs::Bool msg; 146 | msg.data = bool(val); 147 | update_publisher_.publish(msg); 148 | } 149 | } 150 | 151 | void TeleopPanel::setVel( float lin, float ang, bool pre ) 152 | { 153 | linear_velocity_ = lin; 154 | angular_velocity_ = ang; 155 | mouse_pressed_ = pre; 156 | } 157 | 158 | void TeleopPanel::sendVel() 159 | { 160 | if( ros::ok() && velocity_publisher_ && ( mouse_pressed_ || mouse_pressed_sent_ )) 161 | { 162 | sensor_msgs::Joy joy; 163 | 164 | joy.axes.push_back( 0 ); 165 | joy.axes.push_back( 0 ); 166 | joy.axes.push_back( 1.0 ); 167 | joy.axes.push_back( angular_velocity_ ); 168 | joy.axes.push_back( linear_velocity_ ); 169 | joy.axes.push_back( 1.0 ); 170 | joy.axes.push_back( 0 ); 171 | joy.axes.push_back( 0 ); 172 | 173 | joy.buttons.push_back( 0 ); 174 | joy.buttons.push_back( 0 ); 175 | joy.buttons.push_back( 0 ); 176 | joy.buttons.push_back( 0 ); 177 | joy.buttons.push_back( 0 ); 178 | joy.buttons.push_back( 0 ); 179 | joy.buttons.push_back( 0 ); 180 | joy.buttons.push_back( 1 ); 181 | joy.buttons.push_back( 0 ); 182 | joy.buttons.push_back( 0 ); 183 | joy.buttons.push_back( 0 ); 184 | 185 | joy.header.stamp = ros::Time::now(); 186 | joy.header.frame_id = "teleop_panel"; 187 | velocity_publisher_.publish( joy ); 188 | 189 | mouse_pressed_sent_ = mouse_pressed_; 190 | } 191 | } 192 | 193 | void TeleopPanel::save( rviz::Config config ) const 194 | { 195 | rviz::Panel::save( config ); 196 | } 197 | 198 | void TeleopPanel::load( const rviz::Config& config ) 199 | { 200 | rviz::Panel::load( config ); 201 | } 202 | 203 | } 204 | #include 205 | PLUGINLIB_EXPORT_CLASS( teleop_rviz_plugin::TeleopPanel,rviz::Panel ) 206 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/src/teleop_panel.h: -------------------------------------------------------------------------------- 1 | #ifndef TELEOP_PANEL_H 2 | #define TELEOP_PANEL_H 3 | 4 | #ifndef Q_MOC_RUN 5 | # include 6 | 7 | # include 8 | #endif 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | class QLineEdit; 26 | 27 | namespace teleop_rviz_plugin 28 | { 29 | 30 | class DriveWidget; 31 | 32 | class TeleopPanel: public rviz::Panel 33 | { 34 | Q_OBJECT 35 | public: 36 | TeleopPanel( QWidget* parent = 0 ); 37 | virtual void load( const rviz::Config& config ); 38 | virtual void save( rviz::Config config ) const; 39 | 40 | public Q_SLOTS: 41 | void setVel( float linear_velocity_, float angular_velocity_, bool mouse_pressed_ ); 42 | 43 | protected Q_SLOTS: 44 | void pressButton1(); 45 | void pressButton2(); 46 | void pressButton3(); 47 | void pressButton4(); 48 | void clickBox1(int val); 49 | void clickBox2(int val); 50 | void sendVel(); 51 | 52 | protected: 53 | DriveWidget* drive_widget_; 54 | ros::Publisher velocity_publisher_; 55 | ros::Publisher attemptable_publisher_; 56 | ros::Publisher update_publisher_; 57 | ros::Publisher reset_publisher_; 58 | ros::Publisher read_publisher_; 59 | ros::Publisher save_publisher_; 60 | ros::NodeHandle nh_; 61 | 62 | QPushButton *push_button_1_; 63 | QPushButton *push_button_2_; 64 | QPushButton *push_button_3_; 65 | QPushButton *push_button_4_; 66 | QCheckBox *check_box_1_; 67 | QCheckBox *check_box_2_; 68 | 69 | float linear_velocity_; 70 | float angular_velocity_; 71 | bool mouse_pressed_; 72 | bool mouse_pressed_sent_; 73 | }; 74 | 75 | } 76 | 77 | #endif // TELEOP_PANEL_H 78 | -------------------------------------------------------------------------------- /src/visibility_graph_msg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(visibility_graph_msg) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | std_msgs 7 | geometry_msgs 8 | message_generation 9 | ) 10 | 11 | add_message_files( 12 | FILES 13 | Node.msg 14 | Graph.msg 15 | ) 16 | 17 | generate_messages( 18 | DEPENDENCIES 19 | std_msgs 20 | geometry_msgs 21 | ) 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS message_runtime 25 | ) 26 | -------------------------------------------------------------------------------- /src/visibility_graph_msg/msg/Graph.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint16 robot_id 3 | visibility_graph_msg/Node[] nodes 4 | uint32 size -------------------------------------------------------------------------------- /src/visibility_graph_msg/msg/Node.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint32 id 3 | uint8 FreeType 4 | geometry_msgs/Point position 5 | geometry_msgs/Point[] surface_dirs 6 | bool is_covered 7 | bool is_frontier 8 | bool is_navpoint 9 | bool is_boundary 10 | uint32[] connect_nodes 11 | uint32[] poly_connects 12 | uint32[] contour_connects 13 | uint32[] trajectory_connects -------------------------------------------------------------------------------- /src/visibility_graph_msg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | visibility_graph_msg 4 | 0.0.1 5 | Visibility Graph Message 6 | Fan Yang 7 | BSD 8 | 9 | catkin 10 | 11 | roscpp 12 | std_msgs 13 | geometry_msgs 14 | message_generation 15 | 16 | 17 | roscpp 18 | std_msgs 19 | geometry_msgs 20 | message_runtime 21 | 22 | 23 | --------------------------------------------------------------------------------