├── .gitignore ├── SecondParty └── BorisMathLib │ ├── .gitignore │ ├── Geometry │ ├── Circle2D.h │ ├── Line2D.h │ ├── Circle2D.cpp │ └── Line2D.cpp │ ├── README.md │ ├── test │ ├── CMakeLists.txt │ ├── test_GeometryFuc.cpp │ ├── test_Spline.cpp │ ├── test_SplineCurve.cpp │ └── test_RS.cpp │ ├── CMakeLists.txt │ ├── Points │ ├── RefPointsGenerator.h │ ├── RefPointsGenerator.cpp │ └── PointsType.h │ ├── Interplot │ ├── SplineCurve.h │ ├── Spline.h │ ├── SplineCurve.cpp │ └── Spline.cpp │ ├── Curves │ ├── ReedsShepp.h │ └── ReedsShepp.cpp │ └── Vectors │ ├── Vector2D.h │ └── Vector2D.cpp ├── docs └── Pics │ ├── 1.png │ └── 2.png ├── maps ├── map_basic.png ├── map_demo.png ├── map_empty.png ├── map_large.png ├── map_maze.png ├── map_slalom.png ├── map_dead_end.png ├── map_parking.png ├── map_parking_lot.png └── map.yaml ├── Setup.sh ├── src ├── models │ ├── CMakeLists.txt │ ├── VehicleCircleShape.hpp │ ├── VehicleModel.cpp │ └── VehicleModel.h ├── utils │ ├── CMakeLists.txt │ ├── Timer.h │ └── Timer.cpp ├── pathsearcher │ ├── StateNode.cpp │ ├── CMakeLists.txt │ ├── StateNode.h │ ├── HybridAStar.h │ └── HybridAStar.cpp ├── CMakeLists.txt └── Maps │ ├── CMakeLists.txt │ ├── GridMap.h │ └── GridMap.cpp ├── README.md ├── test ├── CMakeLists.txt ├── models │ └── test_models_move.cpp ├── pathsearcher │ ├── test_HAS_global_dilate.cpp │ ├── test_HAS_functions.cpp │ └── test_HAS_search.cpp └── maps │ └── test_map_Dilate.cpp ├── CMakeLists.txt └── app └── PlanNode.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | build/ 3 | # SecondParty/ -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | build/ -------------------------------------------------------------------------------- /docs/Pics/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BorisVandermeer/Hybrid-A-Star/HEAD/docs/Pics/1.png -------------------------------------------------------------------------------- /docs/Pics/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BorisVandermeer/Hybrid-A-Star/HEAD/docs/Pics/2.png -------------------------------------------------------------------------------- /maps/map_basic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BorisVandermeer/Hybrid-A-Star/HEAD/maps/map_basic.png -------------------------------------------------------------------------------- /maps/map_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BorisVandermeer/Hybrid-A-Star/HEAD/maps/map_demo.png -------------------------------------------------------------------------------- /maps/map_empty.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BorisVandermeer/Hybrid-A-Star/HEAD/maps/map_empty.png -------------------------------------------------------------------------------- /maps/map_large.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BorisVandermeer/Hybrid-A-Star/HEAD/maps/map_large.png -------------------------------------------------------------------------------- /maps/map_maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BorisVandermeer/Hybrid-A-Star/HEAD/maps/map_maze.png -------------------------------------------------------------------------------- /maps/map_slalom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BorisVandermeer/Hybrid-A-Star/HEAD/maps/map_slalom.png -------------------------------------------------------------------------------- /maps/map_dead_end.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BorisVandermeer/Hybrid-A-Star/HEAD/maps/map_dead_end.png -------------------------------------------------------------------------------- /maps/map_parking.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BorisVandermeer/Hybrid-A-Star/HEAD/maps/map_parking.png -------------------------------------------------------------------------------- /maps/map_parking_lot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BorisVandermeer/Hybrid-A-Star/HEAD/maps/map_parking_lot.png -------------------------------------------------------------------------------- /Setup.sh: -------------------------------------------------------------------------------- 1 | mkdir SecondParty 2 | cd SecondParty 3 | git clone https://github.com/BorisVandermeer/BorisMathLib.git --depth=1 4 | -------------------------------------------------------------------------------- /maps/map.yaml: -------------------------------------------------------------------------------- 1 | image: map_maze.png 2 | resolution: 1 3 | origin: [ 0.0, 0.0, 0.0 ] 4 | occupied_thresh: 0.1 5 | free_thresh: 0.05 6 | negate: 0 -------------------------------------------------------------------------------- /src/models/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(Models) 3 | 4 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}) 5 | include_directories(${BORIS_MATH_INCLUDE_DIRS}) 6 | 7 | aux_source_directory(. VEHICLE_MODELS_SRC_FILES) 8 | add_library(VEHICLE_MODELS_LIB ${VEHICLE_MODELS_SRC_FILES}) 9 | 10 | SET(VEHICLE_MODELS_LIBRARIES VEHICLE_MODELS_LIB PARENT_SCOPE) 11 | SET(VEHICLE_MODELS_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR} PARENT_SCOPE) -------------------------------------------------------------------------------- /src/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(Utils) 3 | 4 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}) 5 | include_directories(${PLANNING_MATH_INCLUDE_DIRS}) 6 | 7 | aux_source_directory(. PLANNING_UTILS_SRC_FILES) 8 | add_library(PLANNING_UTILS_LIB ${PLANNING_UTILS_SRC_FILES}) 9 | 10 | SET(PLANNING_UTILS_LIBRARIES PLANNING_UTILS_LIB PARENT_SCOPE) 11 | SET(PLANNING_UTILS_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR} PARENT_SCOPE) -------------------------------------------------------------------------------- /src/pathsearcher/StateNode.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A implement of Planning node of Search algorithm 6 | * 7 | * Reference : 8 | * https://github.com/zm0612/Hybrid_A_Star 9 | * 10 | *********************************************************************/ 11 | 12 | 13 | namespace Planning 14 | { 15 | 16 | 17 | } // namespace Planning 18 | 19 | 20 | -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/Geometry/Circle2D.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A Simple implement of 2D Circles 6 | * 7 | *********************************************************************/ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | namespace Geometry{ 14 | 15 | Vectors::Vector2D solveCenterPointOfCircle(Vectors::Vector2D p1,Vectors::Vector2D p2, Vectors::Vector2D p3); 16 | 17 | } -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(math_test) 3 | 4 | add_subdirectory(models) 5 | add_subdirectory(Maps) 6 | add_subdirectory(utils) 7 | add_subdirectory(pathsearcher) 8 | 9 | SET(PLANNING_LIBRARIES 10 | ${BORIS_MATH_LIBRARIES} 11 | ${BORIS_MAP_LIBRARIES} ${VEHICLE_MODELS_LIBRARIES} 12 | ${PLANNING_UTILS_LIBRARIES} ${PATH_SEARCHER_LIBRARIES} 13 | PARENT_SCOPE) 14 | 15 | SET(PLANNING_INCLUDE_DIRS 16 | ${BORIS_MATH_INCLUDE_DIRS} ${PATH_SEARCHER_INCLUDE_DIRS} ${PLANNING_UTILS_INCLUDE_DIRS} 17 | ${BORIS_MAP_INCLUDE_DIRS} ${VEHICLE_MODELS_INCLUDE_DIRS} 18 | PARENT_SCOPE) 19 | -------------------------------------------------------------------------------- /src/Maps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(boris_maps) 3 | 4 | find_package(Eigen3 REQUIRED) 5 | 6 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}) 7 | include_directories(${BORIS_MATH_INCLUDE_DIRS}) 8 | include_directories(${VEHICLE_MODELS_INCLUDE_DIRS}) 9 | include_directories(${PLANNING_UTILS_INCLUDE_DIRS}) 10 | include_directories(${EIGEN3_INCLUDE_DIR}) 11 | 12 | aux_source_directory(. BORIS_MAP_SRC_FILES) 13 | 14 | add_library(BORIS_MAP_LIB ${BORIS_MAP_SRC_FILES}) 15 | 16 | SET(BORIS_MAP_LIBRARIES BORIS_MAP_LIB PARENT_SCOPE) 17 | SET(BORIS_MAP_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR} PARENT_SCOPE) -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/README.md: -------------------------------------------------------------------------------- 1 | # BorisMathLib 2 | 3 | ![Hits](https://hits.seeyoufarm.com/api/count/incr/badge.svg?url=https%3A%2F%2Fgithub.com%2FBorisVandermeer&count_bg=%2379C83D&title_bg=%23555555&icon=&icon_color=%23E7E7E7&title=hits&edge_flat=false) 4 | 5 | BorisVandermeer 的一些基础数学仓库 6 | 7 | ## 主要内容 8 | 9 | - 曲线模块：RS曲线 10 | - 插值模块：Spline 插值，Spline曲线 11 | - 点集模块：定义了公用的点的类型 12 | 13 | ## Run A Demo 14 | 15 | 以当前仓库为工程的顶层目录 16 | 17 | ```bash 18 | 19 | # build 20 | mkdir build 21 | cmake .. 22 | make 23 | 24 | # Run 25 | ./test/*** 26 | 27 | ``` 28 | 29 | ## How To Use 30 | 31 | 将仓库包含在工程内，使用${BORIS_MATH_LIBRARIES}、 ${BORIS_MATH_INCLUDE_DIRS}、 ${BORIS_MATH_FOUND}三个变量进行操作。 32 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Hybrid A Star 的实现 2 | 3 | ![Hits](https://hits.seeyoufarm.com/api/count/incr/badge.svg?url=https%3A%2F%2Fgithub.com%2FBorisVandermeer&count_bg=%2379C83D&title_bg=%23555555&icon=&icon_color=%23E7E7E7&title=hits&edge_flat=false) 4 | 5 | 一个Hybrid A Star 的基础实现。 6 | 使用多个圆描述车辆的碰撞，模拟阿克曼转向模拟车辆的运动。 7 | 8 | 用户可继承HybridAStar类，并且修改其中的虚函数以实现自己的Hybrid A Star 子类。 9 | 10 | ## 构建方式 11 | 12 | - 仓库显示使用的是OpenCV 4 实现的。仅用于显示。 13 | 14 | ``` bash 15 | # Setup 16 | bash ./Setup.sh 17 | 18 | # build 19 | mkdir build && cd build 20 | cmake .. 21 | make 22 | ``` 23 | 24 | ## Run A Demo 25 | 26 | ``` shell 27 | ./build/node 28 | ``` 29 | 30 | 用四次点击描述规划。第一次点击是选择起点位置，第二次是起点方向。第三次是终点位置，第四次是终点方向。 31 | 32 | 效果如下： 33 | 34 | ![效果图1](docs/Pics/1.png) 35 | ![效果图2](docs/Pics/2.png) 36 | -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | 3 | find_package(OpenCV REQUIRED) 4 | 5 | include_directories(${BORIS_MATH_INCLUDE_DIRS}) 6 | include_directories(${OpenCV_INCLUDE_DIRS}) 7 | 8 | add_executable(test_spline test_Spline.cpp) 9 | target_link_libraries(test_spline ${OpenCV_LIBS} ${BORIS_MATH_LIBRARIES}) 10 | 11 | add_executable(test_spline_curve test_SplineCurve.cpp) 12 | target_link_libraries(test_spline_curve ${OpenCV_LIBS} ${BORIS_MATH_LIBRARIES}) 13 | 14 | add_executable(test_rscurve test_RS.cpp) 15 | target_link_libraries(test_rscurve ${OpenCV_LIBS} ${BORIS_MATH_LIBRARIES}) 16 | 17 | add_executable(test_geometry_fuctions test_GeometryFuc.cpp) 18 | target_link_libraries(test_geometry_fuctions ${OpenCV_LIBS} ${BORIS_MATH_LIBRARIES}) -------------------------------------------------------------------------------- /src/pathsearcher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(path_searchers) 3 | 4 | find_package(Eigen3 REQUIRED) 5 | 6 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}) 7 | include_directories(${BORIS_MATH_INCLUDE_DIRS}) 8 | include_directories(${BORIS_MAP_INCLUDE_DIRS}) 9 | include_directories(${VEHICLE_MODELS_INCLUDE_DIRS}) 10 | include_directories(${PLANNING_UTILS_INCLUDE_DIRS}) 11 | include_directories(${EIGEN3_INCLUDE_DIR}) 12 | 13 | aux_source_directory(. PATH_SEARCHER_SRC_FILES) 14 | 15 | add_library(PATH_SEARCHER_LIB ${PATH_SEARCHER_SRC_FILES}) 16 | target_link_libraries(PATH_SEARCHER_LIB 17 | ${BORIS_MATH_LIBRARIES} 18 | ${BORIS_MAP_LIBRARIES} ${VEHICLE_MODELS_LIBRARIES}) 19 | 20 | SET(PATH_SEARCHER_LIBRARIES PATH_SEARCHER_LIB PARENT_SCOPE) 21 | SET(PATH_SEARCHER_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR} PARENT_SCOPE) -------------------------------------------------------------------------------- /src/utils/Timer.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A implement of Vehicle State. 6 | * 7 | *********************************************************************/ 8 | 9 | #ifndef _CPU_TIMER_H_ 10 | #define _CPU_TIMER_H_ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace Utils 18 | { 19 | class Timer{ 20 | public: 21 | Timer(){}; 22 | void Begin(); 23 | void Toc(const std::string &task_name); 24 | void clear(); 25 | double Stop(); 26 | 27 | std::vector> getData; 28 | 29 | 30 | private: 31 | std::chrono::time_point start_, end_; 32 | std::vector> data_; 33 | 34 | }; 35 | } // namespace Utils 36 | 37 | 38 | 39 | #endif -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/Geometry/Line2D.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A Simple implement of 2D Line 6 | * 7 | *********************************************************************/ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | namespace Geometry { 14 | 15 | class Line { 16 | public: 17 | typedef Vectors::Vector2D Vector2D; 18 | 19 | Line() = default; 20 | Line(Vector2D & _point, Vector2D & _direction):direction(_direction),point(_point){} 21 | Line(Vector2D const & _point, Vector2D const & _direction):direction(_direction),point(_point){} 22 | Vectors::Vector2D direction; 23 | Vectors::Vector2D point; 24 | }; 25 | 26 | Points::PosPoint2D GetIntersection(Line const & l1, Line const & l2); 27 | 28 | bool isParallel(Line const & l1, Line const & l2); 29 | 30 | } // namespace Geometry 31 | -------------------------------------------------------------------------------- /src/utils/Timer.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A implement of Vehicle State. 6 | * 7 | *********************************************************************/ 8 | 9 | #include 10 | 11 | namespace Utils 12 | { 13 | void Timer::Begin(){ 14 | this->clear(); 15 | start_ = std::chrono::system_clock::now(); 16 | } 17 | 18 | void Timer::Toc(const std::string &task_name){ 19 | end_ = std::chrono::system_clock::now(); 20 | std::chrono::duration use_time = end_ - start_; 21 | std::pair tocdata = {task_name,use_time.count()}; 22 | data_.push_back(tocdata); 23 | } 24 | 25 | void Timer::clear(){ 26 | data_.clear(); 27 | } 28 | 29 | double Timer::Stop() { 30 | end_ = std::chrono::system_clock::now(); 31 | std::chrono::duration use_time = end_ - start_; 32 | return use_time.count() * 1000.0; 33 | } 34 | 35 | 36 | } // namespace Utils -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(test) 3 | 4 | SET(CMAKE_BUILD_TYPE "Debug") 5 | SET(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g2 -ggdb") 6 | SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall") 7 | 8 | find_package(OpenCV) 9 | 10 | include_directories(${PLANNING_INCLUDE_DIRS}) 11 | include_directories(${OpenCV_INCLUDE_DIRS}) 12 | 13 | add_executable(test_map_gridmap_dilate maps/test_map_Dilate.cpp) 14 | target_link_libraries(test_map_gridmap_dilate ${OpenCV_LIBS} ${PLANNING_LIBRARIES}) 15 | 16 | add_executable(test_models_move_radius models/test_models_move.cpp) 17 | target_link_libraries(test_models_move_radius ${OpenCV_LIBS} ${PLANNING_LIBRARIES}) 18 | 19 | add_executable(test_searcher_HAS_dilate pathsearcher/test_HAS_global_dilate.cpp) 20 | target_link_libraries(test_searcher_HAS_dilate ${OpenCV_LIBS} ${PLANNING_LIBRARIES}) 21 | 22 | add_executable(test_searcher_HAS_functions pathsearcher/test_HAS_functions.cpp) 23 | target_link_libraries(test_searcher_HAS_functions ${OpenCV_LIBS} ${PLANNING_LIBRARIES}) 24 | 25 | add_executable(test_searcher_HAS_search pathsearcher/test_HAS_search.cpp) 26 | target_link_libraries(test_searcher_HAS_search ${OpenCV_LIBS} ${PLANNING_LIBRARIES}) 27 | 28 | -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/test/test_GeometryFuc.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | using Vectors::Vector2D; 9 | 10 | int main(){ 11 | // Test GetIntersection 12 | { 13 | Geometry::Line l1; 14 | Geometry::Line l2; 15 | Vector2D Intersection(3,5); 16 | double phi1 = 10,k1 = 10; 17 | double phi2 = 15,k2 = 40;; 18 | l1.point = Intersection + k1*Vector2D(cos(phi1),sin(phi1)); 19 | l1.direction = Vector2D(cos(phi1),sin(phi1)); 20 | l2.point = Intersection + k2*Vector2D(cos(phi2),sin(phi2)); 21 | l2.direction = Vector2D(cos(phi2),sin(phi2)); 22 | std::cout< 10 | #include 11 | 12 | #define A_PRIVATE_VALUE (1e-5) 13 | 14 | namespace Geometry{ 15 | 16 | Vectors::Vector2D solveCenterPointOfCircle(Vectors::Vector2D p1,Vectors::Vector2D p2, Vectors::Vector2D p3){ 17 | // Referance :: https://blog.csdn.net/liyuanbhu/article/details/52891868· 18 | double x1 = p1.x, y1 = p1.y; 19 | double x2 = p2.x, y2 = p2.y; 20 | double x3 = p3.x, y3 = p3.y; 21 | 22 | double a = x1 - x2; 23 | double b = y1 - y2; 24 | double c = x1 - x3; 25 | double d = y1 - y3; 26 | double e = ((x1 * x1 - x2 * x2) + (y1 * y1 - y2 * y2)) / 2.0; 27 | double f = ((x1 * x1 - x3 * x3) + (y1 * y1 - y3 * y3)) / 2.0; 28 | double det = b * c - a * d; 29 | 30 | if( fabs(det) < A_PRIVATE_VALUE){ 31 | return Vectors::Vector2D(NAN,NAN); 32 | } 33 | 34 | double x0 = -(d * e - b * f) / det; 35 | double y0 = -(a * f - c * e) / det; 36 | return Vectors::Vector2D(x0,y0); 37 | 38 | } 39 | 40 | } -------------------------------------------------------------------------------- /src/models/VehicleCircleShape.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A Descripe Vehicle with servel shapes 6 | * 7 | *********************************************************************/ 8 | 9 | #include 10 | 11 | namespace Models{ 12 | 13 | template 14 | class VehicleShapeCircles : private VehicleShape{ 15 | public: 16 | VehicleShapeCircles() = delete; 17 | VehicleShapeCircles(VehicleShape _shape); 18 | double Radius; 19 | std::vector CircleCenters; // Distance to rear axle center; 20 | }; 21 | 22 | template 23 | VehicleShapeCircles::VehicleShapeCircles(VehicleShape _shape){ 24 | len = _shape.len, wid=_shape.wid, reardist = _shape.reardist, wheelbase = _shape.wheelbase; 25 | const int &nums = N; 26 | CircleCenters.resize(nums); 27 | double stepsize = len/nums; 28 | Radius = std::sqrt(stepsize*stepsize+wid*wid)/2; 29 | CircleCenters[0] = stepsize/2-reardist; 30 | for(int i=1;i VehicleShape2Circles; 36 | typedef VehicleShapeCircles<3> VehicleShape3Circles; 37 | 38 | } // namespace Models -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.21.0) 2 | project(BorisMathLib VERSION 0.0.4) 3 | 4 | if( BorisMathLib_IS_TOP_LEVEL) 5 | SET(IS_TOP_DIR YES) 6 | SET(ENABLE_TEST YES) 7 | else() 8 | SET(IS_TOP_DIR NO) 9 | SET(ENABLE_TEST NO) 10 | endif() 11 | 12 | # Build Config 13 | if(IS_TOP_DIR) 14 | SET(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g2 -ggdb") 15 | SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall") 16 | endif() 17 | 18 | include_directories(.) 19 | 20 | aux_source_directory(Points SRC_FILES_1) 21 | aux_source_directory(Curves SRC_FILES_2) 22 | aux_source_directory(Interplot SRC_FILES_3) 23 | aux_source_directory(Vectors SRC_FILES_4) 24 | aux_source_directory(Geometry SRC_FILES_5) 25 | 26 | SET(BORIS_MATH_SRC_FILES ${SRC_FILES_1} ${SRC_FILES_2} ${SRC_FILES_3} ${SRC_FILES_4} ${SRC_FILES_5}) 27 | 28 | add_library(BORIS_MATH_LIB ${BORIS_MATH_SRC_FILES}) 29 | 30 | if(IS_TOP_DIR) 31 | SET(BORIS_MATH_VERSION BorisMathLib_VERSION) 32 | SET(BORIS_MATH_FOUND YES PARENT_SCOPE) 33 | SET(BORIS_MATH_LIBRARIES BORIS_MATH_LIB) 34 | SET(BORIS_MATH_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}) 35 | else() 36 | SET(BORIS_MATH_VERSION BorisMathLib_VERSION PARENT_SCOPE) 37 | SET(BORIS_MATH_FOUND YES PARENT_SCOPE) 38 | SET(BORIS_MATH_LIBRARIES BORIS_MATH_LIB PARENT_SCOPE) 39 | SET(BORIS_MATH_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR} PARENT_SCOPE) 40 | endif() 41 | 42 | if(ENABLE_TEST) 43 | add_subdirectory(test) 44 | endif() -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.21.0) 2 | project(BorisPlanner) 3 | 4 | # Top Level Check 5 | if( BorisPlanner_IS_TOP_LEVEL) 6 | SET(IS_TOP_DIR YES) 7 | SET(ENABLE_TEST YES) 8 | else() 9 | SET(IS_TOP_DIR NO) 10 | SET(ENABLE_TEST NO) 11 | endif() 12 | 13 | # Build Config 14 | if(IS_TOP_DIR) 15 | SET(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g2 -ggdb") 16 | SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall") 17 | endif() 18 | 19 | # <<<<< Find Dependencies <<<<< # 20 | 21 | if(NOT BORIS_MATH_FOUND) 22 | if(NOT EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/SecondParty) 23 | execute_process(COMMAND mkdir ${CMAKE_CURRENT_SOURCE_DIR}/SecondParty) 24 | endif() 25 | if(NOT EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/SecondParty/BorisMathLib) 26 | execute_process(COMMAND git clone git@github.com:BorisVandermeer/BorisMathLib.git --depth=1 27 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/SecondParty) 28 | endif() 29 | 30 | add_subdirectory(SecondParty/BorisMathLib) 31 | endif() 32 | # >>>>> Find Dependencies >>>>> # 33 | 34 | SET(BORIS_PLANNER_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}) 35 | 36 | add_subdirectory(src) 37 | 38 | # Build Tests 39 | if(ENABLE_TEST) 40 | add_subdirectory(test) 41 | endif() 42 | 43 | find_package(OpenCV 4 REQUIRED) 44 | aux_source_directory(app NODE_SRC) 45 | add_executable(node ${NODE_SRC} ) 46 | target_link_libraries(node ${PLANNING_LIBRARIES} ${OpenCV_LIBS}) 47 | target_include_directories(node PUBLIC ${PLANNING_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) 48 | -------------------------------------------------------------------------------- /src/pathsearcher/StateNode.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A implement of Planning node of Search algorithm 6 | * 7 | * Reference : 8 | * https://github.com/zm0612/Hybrid_A_Star 9 | * 10 | *********************************************************************/ 11 | #ifndef _PLANNING_STATE_NODE_H_ 12 | #define _PLANNING_STATE_NODE_H_ 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | namespace Planning 19 | { 20 | class StateNode 21 | { 22 | public: 23 | enum NODE_STATUS { 24 | NOT_VISITED = 0, IN_OPENSET = 1, IN_CLOSESET = 2 25 | }; 26 | 27 | enum DIRECTION { 28 | FORWARD = 0, BACKWARD = 1, NO = 3 29 | }; 30 | 31 | typedef Points::Pos2D Pos2d; 32 | typedef Points::GridPos2D GridPos; 33 | typedef Points::Pos2Ds Pos2ds; 34 | 35 | StateNode() = delete; 36 | explicit StateNode(const GridPos &grid_index) { 37 | status = NOT_VISITED; 38 | GridIdx = grid_index; 39 | parent_ptr = nullptr; 40 | } 41 | 42 | NODE_STATUS status; 43 | DIRECTION dir; 44 | 45 | GridPos GridIdx; 46 | int SteerIdx; 47 | 48 | Pos2d State; 49 | double Steering; 50 | 51 | double CostG, CostH; 52 | 53 | 54 | std::shared_ptr parent_ptr; 55 | typedef std::shared_ptr Ptr; 56 | 57 | Pos2ds intermediate_states_; 58 | }; 59 | 60 | } // namespace Planning 61 | 62 | #endif -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/Points/RefPointsGenerator.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * Generate States Form a referance Path 6 | * 7 | *********************************************************************/ 8 | #ifndef _MATH_GENERATE_STATES_FROM_CURVES_ 9 | #define _MATH_GENERATE_STATES_FROM_CURVES_ 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace PointsTools{ 16 | 17 | // Get reference points from RSCurve, save data to points; 18 | class PointsGenHandller{ 19 | public: 20 | typedef Points::Pos2D Pos2d; 21 | typedef Points::Pos2Ds RefPoses; 22 | typedef Points::PosPoint2D Point2d; 23 | typedef Points::PosPoint2Ds RefPoints; 24 | 25 | class StartPos2d : public Pos2d{ 26 | public: 27 | StartPos2d(){x=0,y=0,phi=0;} 28 | StartPos2d(double _x,double _y, double _phi){x=_x,y=_y,phi=_phi;} 29 | StartPos2d(Curves::RSCurveStateSpace::State state){x = state.x,y = state.y, phi = state.phi;} 30 | }; 31 | 32 | class StartPoint2d : public Point2d{ 33 | public: 34 | StartPoint2d(){x=0,y=0;} 35 | StartPoint2d(double _x,double _y, double _phi){x=_x,y=_y;} 36 | StartPoint2d(Curves::RSCurveStateSpace::State state){x = state.x,y = state.y;} 37 | }; 38 | 39 | Interplot::SplineCurve toSplineCurve(RefPoints & points) const; 40 | 41 | PointsGenHandller() = default; 42 | bool GetPoses(RefPoses & points, Curves::RSCurve const RSC, double stepsize,StartPos2d From = StartPos2d(0,0,0)); 43 | 44 | }; 45 | 46 | } // namespace Utils 47 | 48 | #endif -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/Interplot/SplineCurve.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A simple cubic spline path library without external 6 | * dependencies. 7 | * 8 | *********************************************************************/ 9 | 10 | #ifndef _INTERPLOATION_SPLINE_CURVE_H_ 11 | #define _INTERPLOATION_SPLINE_CURVE_H_ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace Interplot 18 | { 19 | class SplineCurve 20 | { 21 | public: 22 | typedef Spline::RefPoints RefPoints; 23 | typedef Points::PosPoint2D Point; 24 | typedef Points::Pos2D Pos2D; 25 | 26 | SplineCurve(){}; 27 | ~SplineCurve(){}; 28 | void setPoints(RefPoints refps); 29 | void setPoints(RefPoints refps,double FromHeading,double ToHeading); 30 | void rebuild(double stepsize); 31 | void setSplines(Spline _xs,Spline _ys); 32 | double getProjection(Point target,double max_s,double min_s,bool NewtownRefine = true, double gridsize = 0.5); 33 | double getDirectionalProjection(Pos2D target,double max_s,double min_s,bool NewtownRefine = true, double gridsize = 0.5); 34 | bool HasData() const {return data_flag;} 35 | Point operator()(double s) const; 36 | double getHeading(double s){return std::atan2(ys_.getDeriv(1,s),xs_.getDeriv(1,s));} 37 | 38 | public: 39 | double max_s; 40 | 41 | private: 42 | Spline xs_,ys_; 43 | bool data_flag = false; 44 | double getProjectionByNewton(Point target,double s_hint, double s_max); 45 | double getDirectionalProjectionByNewton(Pos2D target,double s_hint, double s_max); 46 | 47 | }; 48 | 49 | } // namespace Interplot 50 | 51 | #endif -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/Curves/ReedsShepp.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Reference : 5 | * ompl/src/base/spaces 6 | * 7 | *********************************************************************/ 8 | 9 | #ifndef _CURVES_REEDS_SHEPP_CURVE_H_ 10 | #define _CURVES_REEDS_SHEPP_CURVE_H_ 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | namespace Curves{ 17 | 18 | enum RSCurveSegmentType 19 | { 20 | RS_NOP = 0, 21 | RS_LEFT = 1, 22 | RS_STRAIGHT = 2, 23 | RS_RIGHT = 3 24 | }; 25 | 26 | class RSCurve{ 27 | public: 28 | RSCurve(){}; 29 | RSCurve( const RSCurveSegmentType *type, 30 | double t = std::numeric_limits::max(), double u = 0., double v = 0., 31 | double w = 0., double x = 0.,double _radius=1.0); 32 | double TotalLength() const {return totalLength_*radius;} 33 | 34 | const RSCurveSegmentType *type_; 35 | double length[5]; 36 | double radius = 1; 37 | 38 | private: 39 | double totalLength_ = std::numeric_limits::max(); 40 | 41 | }; 42 | 43 | class RSCurveStateSpace{ 44 | public: 45 | // RSCurveStateSpace() = delete; 46 | RSCurveStateSpace(double turningRadius = 1.0):radius_(turningRadius){}; 47 | static const RSCurveSegmentType RSCurveType[18][5]; 48 | typedef Points::Pos2D Pos2d; 49 | typedef Pos2d State; 50 | 51 | RSCurve RSCurveCalc(double x,double y,double phi) const; 52 | RSCurve RSCurveCalc(State & from, State & to) const; 53 | double getRadius() const {return radius_;}; 54 | void setRadius(double turningRadius){radius_ = turningRadius;}; 55 | protected: 56 | double radius_; 57 | }; 58 | 59 | } //namespace Curves 60 | 61 | #endif -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/Vectors/Vector2D.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A Simple implement of Vec2D 6 | * 7 | *********************************************************************/ 8 | 9 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | 15 | 16 | namespace Vectors { 17 | 18 | class Vector2D { 19 | public: 20 | Vector2D(); 21 | Vector2D(double _x, double _y){ x = _x, y = _y;} 22 | Vector2D(Points::PosPoint2D p):x(p.x),y(p.y){}; 23 | Vector2D operator-() const; 24 | double operator*(const Vector2D &vector) const; 25 | Vector2D operator*(double scalar) const; 26 | Vector2D operator/(double scalar) const; 27 | Vector2D operator+(const Vector2D &vector) const; 28 | Vector2D operator-(const Vector2D &vector) const; 29 | bool operator==(const Vector2D &vector) const; 30 | bool operator!=(const Vector2D &vector) const; 31 | Vector2D &operator*=(double scalar); 32 | Vector2D &operator/=(double scalar); 33 | Vector2D &operator+=(const Vector2D &vector); 34 | Vector2D &operator-=(const Vector2D &vector); 35 | 36 | public: 37 | double x,y; 38 | }; 39 | 40 | Vector2D operator*(double scalar, const Vector2D &vector); 41 | 42 | std::ostream &operator<<(std::ostream &stream,const Vector2D &vector); 43 | 44 | double abs(const Vector2D &vector); 45 | 46 | double absSq(const Vector2D &vector); 47 | 48 | double det(const Vector2D &vector1, const Vector2D &vector2); 49 | 50 | double leftOf(const Vector2D &vector1, const Vector2D &vector2,const Vector2D &vector3); 51 | 52 | Vector2D normalize(const Vector2D &vector); 53 | 54 | bool isParallel(Vector2D const & v1, Vector2D const & v2); 55 | 56 | } // namespace Vectors 57 | -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/Geometry/Line2D.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A Simple implement of 2D Line 6 | * 7 | *********************************************************************/ 8 | 9 | #include 10 | 11 | #include 12 | #define A_PRIVATE_VALUE (1e-5) 13 | 14 | namespace Geometry { 15 | 16 | Points::PosPoint2D GetIntersection(Line const & l1, Line const & l2){ 17 | /* 18 | * x = x1 + k1*dx1 \ \ T = dy1*dx2-dx1*dy2; 19 | * x = x2 + k2*dx2 ----\ x1 - x2 = k2*dx2 - k1*dx1 ----\ T*k2 = (dx1*(y1-y2)-dy1*(x1-x2)) 20 | * y = y1 + k1*dy1 ----/ y1 - y2 = k2*dy2 - k1*dy1 ----/ T*k1 = (dx2*(y2-y1)-dy2*(x2-x1)) 21 | * y = y2 + k2*dy2 / / 22 | */ 23 | double const & x1 = l1.point.x; double const & y1 = l1.point.y; 24 | double const & dx1 = l1.direction.x; double const & dy1 = l1.direction.y; 25 | double const & x2 = l2.point.x; double const & y2 = l2.point.y; 26 | double const & dx2 = l2.direction.x; double const & dy2 = l2.direction.y; 27 | 28 | double tmp = dy1*dx2-dx1*dy2; 29 | if(std::fabs(tmp) 10 | 11 | #include 12 | 13 | 14 | namespace Models{ 15 | 16 | static double Mod2Pi(const double &x){ 17 | double v = fmod(x, 2 * M_PI); 18 | if (v < -M_PI) { 19 | v += 2.0 * M_PI; 20 | } else if (v > M_PI) { 21 | v -= 2.0 * M_PI; 22 | } 23 | return v; 24 | } 25 | 26 | VehicleMoveModel::Pos2d VehicleMoveModel::MoveByRadius(Pos2d const &from, double const &radius, double const &dist) const{ 27 | double v = dist/radius; 28 | Pos2d ans; 29 | if(v>0){ 30 | ans.x = from.x + (std::sin(from.phi + v) - std::sin(from.phi))*radius; 31 | ans.y = from.y - (std::cos(from.phi + v) - std::cos(from.phi))*radius; 32 | ans.phi = Mod2Pi(from.phi + v); 33 | } else { 34 | ans.x = from.x - (std::sin(from.phi - v) - std::sin(from.phi))*radius; 35 | ans.y = from.y + (std::cos(from.phi - v) - std::cos(from.phi))*radius; 36 | ans.phi = Mod2Pi(from.phi + v); 37 | } 38 | 39 | return ans; 40 | } 41 | 42 | double VehicleMoveModel::Steer2Radius(double const &angle) const{ 43 | if(fabs(angle)<0.000001) return __DBL_MAX__; 44 | else return wheelbase/std::tan(angle); 45 | } 46 | 47 | VehicleMoveModel::Pos2d VehicleMoveModel::MoveBySteerting(Pos2d const &from, double const &angle, double const &dist) const{ 48 | if(fabs(angle)<0.000001) 49 | return Pos2d(from.x + dist*std::cos(from.phi),from.y + dist*std::sin(from.phi),from.phi+angle); 50 | double radius = wheelbase/std::tan(angle); 51 | return MoveByRadius(from,radius,dist); 52 | } 53 | 54 | } // namespace Models -------------------------------------------------------------------------------- /src/Maps/GridMap.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Author : BorisVandermeer 4 | * 5 | * Discription ： 6 | * A implement of GridMap 7 | * 8 | *********************************************************************/ 9 | 10 | #include 11 | #include 12 | 13 | #define GRIDMAP_IS_OBSTACLE 1 14 | #define GRIDMAP_NOT_OBSTACLE 0 15 | 16 | namespace Maps 17 | { 18 | namespace Utils{ 19 | typedef std::vector> MapDataType; 20 | void GridMapDilateDefaultkernel(MapDataType*,int ,int); 21 | } 22 | 23 | class GridMap{ 24 | public: 25 | GridMap() = delete; 26 | GridMap(size_t x, size_t y, double reslotion); 27 | ~GridMap(){}; 28 | 29 | size_t getSizeX() const {return x_size_;} 30 | size_t getSizeY() const {return y_size_;} 31 | double getMaxX() const {return x_upper_;} 32 | double getMaxY() const {return y_upper_;} 33 | 34 | void SetObstacleByPos(double const &pt_x, double const &pt_y); 35 | void SetObstacleByIdx(int const &x, int const &y); 36 | void ReSetObstacle(unsigned int x, unsigned int y); 37 | // True for isObs. 38 | bool HasObstacle(unsigned int x, unsigned int y) const; 39 | 40 | int operator()(int x,int y) const; 41 | double getReslotion() const {return reslotion_;} 42 | 43 | std::vector ToIdx(double x,double y){return {static_cast(x/reslotion_), static_cast(y/reslotion_)};} 44 | std::vector ToPosition(int x,int y){return {static_cast((x+0.5)*reslotion_), static_cast((y+0.5)/reslotion_)};} 45 | 46 | public : 47 | typedef std::vector> MapDataType; 48 | void Dilate(std::shared_ptr &DST, void (*kernel)(MapDataType*,int,int)=Utils::GridMapDilateDefaultkernel); 49 | 50 | private: 51 | 52 | int& operator()(int x,int y); 53 | MapDataType MapData; 54 | size_t x_size_,y_size_; 55 | double reslotion_; // meters per pix 56 | double x_upper_, y_upper_; 57 | }; 58 | 59 | } // namespace Planning 60 | -------------------------------------------------------------------------------- /src/models/VehicleModel.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A implement of Normal Vehicle Models 6 | * 7 | *********************************************************************/ 8 | 9 | #include 10 | #include 11 | 12 | namespace Models{ 13 | 14 | class VehicleDynamicState{ 15 | double x,y,heading; 16 | double v_par,v_vel; 17 | double a_par,a_vel; 18 | double phi; 19 | }; 20 | 21 | class VehicleShape{ 22 | public: 23 | VehicleShape(){}; 24 | VehicleShape(double _len, double _wid, double _reardist,double _wheelbase) 25 | :len(_len),wid(_wid),reardist(_reardist),wheelbase(_wheelbase){} 26 | ~VehicleShape(){}; 27 | 28 | /* 29 | * @param len vehicle length (a to c) 30 | * @param wid vehicle width (a to d) 31 | * @param reardist Length from rear axle to rear (a to b) 32 | * @param wheelbase lenth between front axle to rear axle 33 | * 34 | * b e 35 | * a ----------------- c 36 | * | | | | Front 37 | * | | | | 38 | * d ----------------- 39 | */ 40 | double len,wid,reardist,wheelbase; 41 | }; 42 | 43 | class VehicleMoveModel : public VehicleShape{ 44 | public : 45 | VehicleMoveModel(){}; 46 | VehicleMoveModel(double _len, double _wid, double _reardist,double _wheelbase){ 47 | len = _len,wid = _wid,reardist = _reardist,wheelbase = _wheelbase; 48 | } 49 | VehicleMoveModel(VehicleShape shape){ 50 | len = shape.len,wid = shape.wid,reardist = shape.reardist,wheelbase = shape.wheelbase; 51 | } 52 | 53 | typedef Points::Pos2D Pos2d; 54 | 55 | double Steer2Radius(double const &angle) const; 56 | Pos2d MoveByRadius(Pos2d const &from, double const &radius, double const &dist) const; 57 | Pos2d MoveBySteerting(Pos2d const &from, double const &angle, double const &dist) const; 58 | 59 | 60 | }; 61 | 62 | } // namespace Models 63 | -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/Points/RefPointsGenerator.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * Generate States Form a referance Path 6 | * 7 | *********************************************************************/ 8 | 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | namespace PointsTools{ 14 | 15 | Interplot::SplineCurve PointsGenHandller::toSplineCurve(RefPoints & points) const{ 16 | Interplot::SplineCurve ans; 17 | ans.setPoints(points); 18 | return ans; 19 | } 20 | 21 | bool PointsGenHandller::GetPoses(RefPoses & poses, Curves::RSCurve const RSC, double stepsize,StartPos2d From){ 22 | // double phi = 0; 23 | size_t num = RSC.TotalLength()/stepsize+1; 24 | double seglen = RSC.TotalLength()/RSC.radius; 25 | 26 | for(double i=0;i<=num;i++){ 27 | double v,seg = static_cast(i)*seglen/num; 28 | double tmpx = 0,tmpy = 0,tmpphi= From.phi; 29 | for(int j=0;j<5&&seg>0;j++){ 30 | if(RSC.length[j]<0.0){ 31 | v = std::max(-seg, RSC.length[j]); 32 | seg += v; 33 | } else { 34 | v = std::min(seg, RSC.length[j]); 35 | seg -= v; 36 | } 37 | 38 | switch(RSC.type_[j]){ 39 | case Curves::RSCurveSegmentType::RS_LEFT: 40 | tmpx = tmpx + std::sin(tmpphi + v) - std::sin(tmpphi); 41 | tmpy = tmpy - std::cos(tmpphi + v) + std::cos(tmpphi); 42 | tmpphi = tmpphi + v; 43 | break; 44 | case Curves::RSCurveSegmentType::RS_RIGHT: 45 | tmpx = tmpx - std::sin(tmpphi - v) + std::sin(tmpphi); 46 | tmpy = tmpy + std::cos(tmpphi - v) - std::cos(tmpphi); 47 | tmpphi = tmpphi - v; 48 | break; 49 | case Curves::RSCurveSegmentType::RS_STRAIGHT: 50 | tmpx = v * std::cos(tmpphi) + tmpx; 51 | tmpy = v * std::sin(tmpphi) + tmpy; 52 | break; 53 | case Curves::RSCurveSegmentType::RS_NOP: 54 | break; 55 | } 56 | } 57 | poses.x.push_back(tmpx*RSC.radius+From.x); 58 | poses.y.push_back(tmpy*RSC.radius+From.y); 59 | poses.phi.push_back(tmpphi); 60 | } 61 | 62 | return true; 63 | 64 | } 65 | }// namespace Utils -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/Points/PointsType.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * Describe common Points types 6 | * To disable These Types, use " #define _BORIS_POINTS_TYPE_DISABLE_ " 7 | * before where the header is included. Use " #ifdef _BORIS_POINTS_TYPE_ENABLED_ " 8 | * to check whether the types here is enabled. 9 | * 10 | * 11 | * 12 | *********************************************************************/ 13 | #ifndef _BORIS_POINTS_TYPE_DEF_H_ 14 | #define _BORIS_POINTS_TYPE_DEF_H_ 15 | 16 | #ifndef _BORIS_POINTS_TYPE_DISABLE_ 17 | #define _BORIS_POINTS_TYPE_ENABLED_ 18 | 19 | #include 20 | 21 | namespace Points{ 22 | template 23 | struct Point2d{ 24 | DataType x,y; 25 | Point2d() = default; 26 | Point2d(DataType _x, DataType _y):x(_x),y(_y){} 27 | }; 28 | 29 | template 30 | struct Point2ds{ 31 | std::vector x,y; 32 | Point2ds() = default; 33 | size_t size() const {return x.size();} 34 | void clear(){x.clear();y.clear();} 35 | void push_back(Point2d _pos){x.push_back(_pos.x);y.push_back(_pos.y);} 36 | void set(int idx, Point2d _pos){x[idx]=_pos.x,y[idx]=_pos.y;} 37 | Point2ds operator [](int idx){return Point2ds(x[idx],y[idx]);} 38 | }; 39 | 40 | template 41 | struct Point2dWithDir{ 42 | DataType x,y,phi; 43 | Point2dWithDir() = default; 44 | Point2dWithDir(DataType _x, DataType _y, DataType _phi):x(_x),y(_y),phi(_phi){} 45 | }; 46 | 47 | template 48 | struct Point2dWithDirs{ 49 | std::vector x,y,phi; 50 | Point2dWithDirs() = default; 51 | size_t size(){return x.size();} 52 | void clear(){x.clear();y.clear();phi.clear();}; 53 | void push_back(Point2dWithDir _pos){x.push_back(_pos.x);y.push_back(_pos.y);phi.push_back(_pos.phi);} 54 | void set(int idx, Point2dWithDir _pos){x[idx]=_pos.x,y[idx]=_pos.y,phi[idx]=_pos.phi;} 55 | Point2dWithDir operator [](int idx){return Point2dWithDir(x[idx],y[idx],phi[idx]);} 56 | }; 57 | 58 | // template 59 | // struct Point2dWithHeading{ DataType x,y,heading;}; 60 | 61 | template 62 | struct Point3d{ 63 | DataType x,y,z; 64 | Point3d() = default; 65 | Point3d(DataType _x, DataType _y, DataType _z):x(_x),y(_y),z(_z){} 66 | }; 67 | 68 | template 69 | struct Point3dWithDir{ 70 | DataType x,y,z,phi; 71 | Point3dWithDir() = default; 72 | Point3dWithDir(DataType _x, DataType _y, DataType _z,DataType _phi):x(_x),y(_y),z(_z),phi(_phi){} 73 | }; 74 | 75 | typedef Point2d GridPoint2D; 76 | typedef Point2d PosPoint2D; 77 | 78 | typedef Point2ds GridPoint2Ds; 79 | typedef Point2ds PosPoint2Ds; 80 | 81 | typedef Point2dWithDir GridPos2D; 82 | typedef Point2dWithDir Pos2D; 83 | 84 | typedef Point2dWithDirs GridPos2Ds; 85 | typedef Point2dWithDirs Pos2Ds; 86 | 87 | typedef Point3d GridPoint3D; 88 | typedef Point3d PosPoint3D; 89 | 90 | } // namespace Points 91 | 92 | #endif 93 | #endif 94 | -------------------------------------------------------------------------------- /test/models/test_models_move.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * An HAS Move Test 6 | * 7 | *********************************************************************/ 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #define IMG_HIGHT 800 19 | #define IMG_WIDTH 1200 20 | 21 | using namespace std; 22 | using namespace cv; 23 | 24 | const cv::Vec3b IMG_COLOR = {0,0,0}; 25 | cv::Mat img2show(IMG_HIGHT,IMG_WIDTH,CV_8UC3,Scalar(255,255,255)); 26 | 27 | void ClearImage() 28 | { 29 | for(int rowidx = 0; rowidx(rowidx,pixidx) = {255,255,255}; 34 | } 35 | } 36 | } 37 | static void dilate(Vec3b Color,int n,Mat & img) 38 | { 39 | Mat img_backup = img.clone(); 40 | int & rows = img_backup.rows; 41 | int & cols = img_backup.cols; 42 | for(int row = n;row(row+i,col+j) == Color){ 48 | img.at(row,col) = Color; 49 | flag = true; 50 | break; 51 | } 52 | } 53 | if(flag) break; 54 | } 55 | } 56 | } 57 | } 58 | 59 | double islbuttondown = false; 60 | void EventMouseClick(int event, int x, int y, int flags, void* ustc) 61 | { 62 | if(event == CV_EVENT_MOUSEMOVE && flags == CV_EVENT_FLAG_LBUTTON){ 63 | 64 | } 65 | if(event == CV_EVENT_RBUTTONDOWN){ 66 | 67 | } 68 | // if (event == CV_EVENT_RBUTTONDOWN) 69 | // { 70 | 71 | // } 72 | 73 | imshow("GridMap test",img2show); 74 | 75 | } 76 | 77 | int main() 78 | { 79 | Models::VehicleMoveModel Model(80,40,15,50); 80 | double stepsize = 1.0; 81 | 82 | for(int R = -400; R<=400; R+=20){ 83 | Points::Pos2D pos(IMG_WIDTH/2,IMG_HIGHT/2,-M_PI/2); 84 | img2show.at(pos.y,pos.x) = IMG_COLOR; 85 | for(double dist = stepsize;dist0&&pos.y>0&&pos.x(pos.y,pos.x) = IMG_COLOR; 89 | } 90 | } 91 | 92 | imshow("Vehicle Move test",img2show); 93 | while(1) 94 | { 95 | if(cv::waitKey(1) == 27) break; 96 | } 97 | 98 | for(double R = -M_PI/3; R<=M_PI/3; R+=M_PI/30){ 99 | if(fabs(R)<0.00001) R+=M_PI/30; 100 | Points::Pos2D pos(IMG_WIDTH/2,IMG_HIGHT/2,M_PI/2); 101 | img2show.at(pos.y,pos.x) = IMG_COLOR; 102 | for(double dist = stepsize;dist0&&pos.y>0&&pos.x(pos.y,pos.x) = IMG_COLOR; 106 | } 107 | } 108 | 109 | imshow("Vehicle Move test",img2show); 110 | 111 | while(1) 112 | { 113 | if(cv::waitKey(1) == 27) return 0; 114 | } 115 | } 116 | 117 | -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/Interplot/Spline.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A simple cubic spline interpolation library without external 6 | * dependencies. 7 | * 8 | * Reference : 9 | * https://blog.csdn.net/qq_21933647/article/details/103500428 10 | * 11 | *********************************************************************/ 12 | 13 | #ifndef _INTERPLOATION_SPLINE_H_ 14 | #define _INTERPLOATION_SPLINE_H_ 15 | 16 | #include 17 | #include 18 | 19 | namespace Interplot 20 | { 21 | 22 | namespace Utils 23 | { 24 | // Disposable Band Matrix class to solve "Ax=b" problems by LU decompose. 25 | class band_matrix { 26 | public: 27 | std::vector > m_upper; // upper band 28 | std::vector > m_lower; // lower & diag band 29 | bool decomposed_flag_ = false; 30 | public: 31 | band_matrix(){}; 32 | band_matrix(int dim, int n_u, int n_l); 33 | ~band_matrix(){}; 34 | bool isDecomposed() const {return decomposed_flag_;} 35 | void resize(int dim, int n_u, int n_l); // init with dim,n_u,n_l 36 | int dim() const; // matrix dimension 37 | int num_upper() const {return m_upper.size() - 1;} 38 | int num_lower() const {return m_lower.size() - 1;} 39 | // access operator 40 | double &operator()(int i, int j); // write 41 | double operator()(int i, int j) const; // read 42 | // we can store an additional diogonal (in m_lower) 43 | double &saved_diag(int i); 44 | double saved_diag(int i) const; 45 | void lu_decompose(); 46 | // solves Rx=y 47 | std::vector r_solve(const std::vector &b) const; 48 | // solves Ly=b 49 | std::vector l_solve(const std::vector &b) const; 50 | // solves Ax=y 51 | std::vector lu_solve(const std::vector &b); 52 | }; 53 | } // namespace Utils 54 | 55 | 56 | class Spline{ 57 | public: 58 | enum BoundType { 59 | FirstOrderDer = 1, 60 | SecondOrderDer = 2 61 | }; 62 | 63 | typedef Points::PosPoint2Ds RefPoints; 64 | 65 | public: 66 | // Use Natural Spline as Default 67 | Spline(): 68 | m_left(SecondOrderDer), 69 | m_right(SecondOrderDer), 70 | m_left_value(0.0), 71 | m_right_value(0.0), 72 | m_force_linear_extrapolation(false) {} 73 | 74 | void set_boundary(BoundType left, double left_value, BoundType right, 75 | double right_value, 76 | bool force_linear_extrapolation = false); 77 | // void set_points(const std::vector& x, const std::vector& y,bool cubic_spline = true); 78 | void set_points(const RefPoints &points, bool cubic_spline = true ); 79 | double getLastx(){return PointsData.x.back();} 80 | double operator()(double x) const; 81 | void clear(){PointsData.clear();}; 82 | double getDeriv(int order, double x) const; 83 | 84 | private: 85 | RefPoints PointsData; 86 | std::vector m_a, m_b, m_c; // spline coefficients 87 | double m_b0, m_c0; // for left extrapol 88 | BoundType m_left, m_right; 89 | double m_left_value, m_right_value; 90 | bool m_force_linear_extrapolation; 91 | }; 92 | 93 | } // namespace Interplot 94 | 95 | 96 | #endif -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/Vectors/Vector2D.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A Simple implement of Vec2D 6 | * 7 | *********************************************************************/ 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #define A_PRIVATE_VALUE (0.00001F) 15 | 16 | namespace Vectors { 17 | 18 | Vector2D::Vector2D(){x = 0.0F,y=0.0F;} 19 | 20 | Vector2D Vector2D::operator-() const { return Vector2D(-x, -y); } 21 | 22 | double Vector2D::operator*(const Vector2D &vector) const { 23 | return x * vector.x + y * vector.y; 24 | } 25 | 26 | Vector2D Vector2D::operator*(double scalar) const { 27 | return Vector2D(x * scalar, y * scalar); 28 | } 29 | 30 | Vector2D Vector2D::operator/(double scalar) const { 31 | const double invScalar = 1.0F / scalar; 32 | return Vector2D(x * invScalar, y * invScalar); 33 | } 34 | 35 | Vector2D Vector2D::operator+(const Vector2D &vector) const { 36 | return Vector2D(x + vector.x, y + vector.y); 37 | } 38 | 39 | Vector2D Vector2D::operator-(const Vector2D &vector) const { 40 | return Vector2D(x - vector.x, y - vector.y); 41 | } 42 | 43 | bool Vector2D::operator==(const Vector2D &vector) const { 44 | return x == vector.x && y == vector.y; 45 | } 46 | 47 | bool Vector2D::operator!=(const Vector2D &vector) const { 48 | return x != vector.x || y != vector.y; 49 | } 50 | 51 | Vector2D &Vector2D::operator*=(double scalar) { 52 | x *= scalar; 53 | y *= scalar; 54 | 55 | return *this; 56 | } 57 | 58 | Vector2D &Vector2D::operator/=(double scalar) { 59 | const double invScalar = 1.0F / scalar; 60 | x *= invScalar; 61 | y *= invScalar; 62 | 63 | return *this; 64 | } 65 | 66 | Vector2D &Vector2D::operator+=(const Vector2D &vector) { 67 | x += vector.x; 68 | y += vector.y; 69 | 70 | return *this; 71 | } 72 | 73 | Vector2D &Vector2D::operator-=(const Vector2D &vector) { 74 | x -= vector.x; 75 | y -= vector.y; 76 | 77 | return *this; 78 | } 79 | 80 | Vector2D operator*(double scalar, const Vector2D &vector) { 81 | return Vector2D(scalar * vector.x, scalar * vector.y); 82 | } 83 | 84 | std::ostream &operator<<(std::ostream &stream, const Vector2D &vector) { 85 | stream << "(" << vector.x << "," << vector.y << ")"; 86 | 87 | return stream; 88 | } 89 | 90 | double abs(const Vector2D &vector) { return std::sqrt(vector * vector); } 91 | 92 | double absSq(const Vector2D &vector) { return vector * vector; } 93 | 94 | double det(const Vector2D &vector1, const Vector2D &vector2) { 95 | return vector1.x * vector2.y - vector1.y * vector2.x; 96 | } 97 | 98 | double leftOf(const Vector2D &vector1, const Vector2D &vector2, 99 | const Vector2D &vector3) { 100 | return det(vector1 - vector3, vector2 - vector1); 101 | } 102 | 103 | Vector2D normalize(const Vector2D &vector) { return vector / abs(vector); } 104 | 105 | bool isParallel(Vector2D const & v1, Vector2D const & v2){ 106 | Vector2D v11 = v1; 107 | Vector2D v22 = v2; 108 | while(fabs(v11.x)+fabs(v11.y)<1) v11 = v11+v11; 109 | while(fabs(v22.x)+fabs(v22.y)<1) v22 = v22+v22; 110 | if(fabs(det(v1,v2)) 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #define IMG_HIGHT 900 18 | #define IMG_WIDTH 1200 19 | 20 | const cv::Vec3b IMG_COLOR = {0,0,0}; 21 | 22 | using namespace std; 23 | using namespace cv; 24 | 25 | cv::Mat img(IMG_HIGHT,IMG_WIDTH,CV_8UC3,Scalar(255,255,255));; 26 | enum mode{ 27 | launch = 0, picking = 1, showing = 2 28 | }; 29 | mode curmode; 30 | vector x_s; 31 | vector y_s; 32 | 33 | Interplot::Spline sp; 34 | 35 | 36 | 37 | void ClearImage() 38 | { 39 | for(int rowidx = 0; rowidx(rowidx,pixidx) = {255,255,255}; 44 | } 45 | } 46 | } 47 | 48 | static void dilate(Vec3b Color,int n,Mat & img) 49 | { 50 | Mat img_backup = img.clone(); 51 | int & rows = img_backup.rows; 52 | int & cols = img_backup.cols; 53 | for(int row = n;row(row+i,col+j)[0]==Color[0] 63 | // && img_backup.at(row+i,col+j)[1]==Color[1] 64 | // && img_backup.at(row+i,col+j)[2]==Color[2]) 65 | if(img_backup.at(row+i,col+j) == Color) 66 | { 67 | img.at(row,col) = Color; 68 | flag = true; 69 | break; 70 | } 71 | } 72 | if(flag) break; 73 | } 74 | } 75 | } 76 | } 77 | 78 | void EventMouseClick(int event, int x, int y, int flags, void* ustc) 79 | { 80 | if (event == CV_EVENT_LBUTTONDOWN) 81 | { 82 | cout<<"CV_EVENT_LBUTTONDOWN"<x_s.back()) { 91 | x_s.push_back(x);y_s.push_back(y);} 92 | for(int i=0,maxi=x_s.size();i(y_s[i],x_s[i]) = IMG_COLOR; 95 | } 96 | dilate(IMG_COLOR,4, img); 97 | imshow("Spline test",img); 98 | } 99 | if (event == CV_EVENT_RBUTTONDOWN) 100 | { 101 | cout<<"CV_EVENT_RBUTTONDOWN"<2){ 104 | Interplot::Spline::RefPoints points; 105 | points.x = x_s; 106 | points.y = y_s; 107 | 108 | sp.set_points(points); 109 | } 110 | 111 | for(int i=0;i0&&sp(i)(tmp,i) = IMG_COLOR; 116 | } 117 | } 118 | 119 | dilate(IMG_COLOR,4, img); 120 | imshow("Spline test",img); 121 | } 122 | 123 | } 124 | 125 | int main() 126 | { 127 | 128 | curmode = launch; 129 | imshow("Spline test",img); 130 | cvSetMouseCallback("Spline test", EventMouseClick); 131 | while(1) 132 | { 133 | if(cv::waitKey(1) == 27) return 0; 134 | } 135 | } -------------------------------------------------------------------------------- /test/pathsearcher/test_HAS_global_dilate.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * An HAS Dilate Kernal Test 6 | * 7 | *********************************************************************/ 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #define IMG_HIGHT 800 19 | #define IMG_WIDTH 1200 20 | 21 | using namespace std; 22 | using namespace cv; 23 | 24 | const cv::Vec3b IMG_COLOR = {0,0,0}; 25 | cv::Mat img2show(IMG_HIGHT,IMG_WIDTH,CV_8UC3,Scalar(255,255,255)); 26 | cv::Mat img4data(IMG_HIGHT,IMG_WIDTH,CV_8UC3,Scalar(255,255,255)); 27 | 28 | shared_ptr ptr1; 29 | shared_ptr ptr2; 30 | shared_ptr ptr3; 31 | 32 | 33 | void ClearImage() 34 | { 35 | for(int rowidx = 0; rowidx(rowidx,pixidx) = {255,255,255}; 40 | } 41 | } 42 | } 43 | static void dilate(Vec3b Color,int n,Mat & img) 44 | { 45 | Mat img_backup = img.clone(); 46 | int & rows = img_backup.rows; 47 | int & cols = img_backup.cols; 48 | for(int row = n;row(row+i,col+j) == Color){ 54 | img.at(row,col) = Color; 55 | flag = true; 56 | break; 57 | } 58 | } 59 | if(flag) break; 60 | } 61 | } 62 | } 63 | } 64 | 65 | static void dilate(Vec3b Color,int n,Mat &img , Mat &img2) 66 | { 67 | int & rows = img.rows; 68 | int & cols = img.cols; 69 | for(int row = n;row(row+i,col+j) == Color){ 75 | img2.at(row,col) = Color; 76 | flag = true; 77 | break; 78 | } 79 | } 80 | if(flag) break; 81 | } 82 | } 83 | } 84 | } 85 | 86 | void ShowGridMap(shared_ptr ptr){ 87 | ClearImage(); 88 | // cout<getSizeX()<<' '<getSizeY()<getSizeX();x++) for(int y=0;ygetSizeY();y++){ 90 | if((*ptr).HasObstacle(x,y)) img2show.at(y,x) = IMG_COLOR; 91 | } 92 | } 93 | 94 | double islbuttondown = false; 95 | void EventMouseClick(int event, int x, int y, int flags, void* ustc) 96 | { 97 | if(event == CV_EVENT_MOUSEMOVE && flags == CV_EVENT_FLAG_LBUTTON){ 98 | img2show.at(y,x) = IMG_COLOR; 99 | ptr1->SetObstacleByIdx((unsigned int)x,y); 100 | ptr2 = ptr1; 101 | } 102 | if(event == CV_EVENT_RBUTTONDOWN){ 103 | ClearImage(); 104 | ptr3 = ptr2; 105 | ptr3->Dilate(ptr2,Planning::Searcher::HybridAStarGlobal::DilateKernal); 106 | ShowGridMap(ptr3); 107 | } 108 | // if (event == CV_EVENT_RBUTTONDOWN) 109 | // { 110 | 111 | // } 112 | 113 | imshow("GridMap test",img2show); 114 | 115 | } 116 | 117 | int main() 118 | { 119 | ptr1 = make_shared(IMG_WIDTH,IMG_HIGHT,1); 120 | imshow("GridMap test",img2show); 121 | cvSetMouseCallback("GridMap test", EventMouseClick); 122 | while(1) 123 | { 124 | if(cv::waitKey(1) == 27) return 0; 125 | } 126 | } 127 | 128 | -------------------------------------------------------------------------------- /test/maps/test_map_Dilate.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * An GridMap Dilate Test 6 | * 7 | *********************************************************************/ 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #define IMG_HIGHT 800 19 | #define IMG_WIDTH 1200 20 | 21 | using namespace std; 22 | using namespace cv; 23 | 24 | const cv::Vec3b IMG_COLOR = {0,0,0}; 25 | cv::Mat img2show(IMG_HIGHT,IMG_WIDTH,CV_8UC3,Scalar(255,255,255)); 26 | cv::Mat img4data(IMG_HIGHT,IMG_WIDTH,CV_8UC3,Scalar(255,255,255)); 27 | 28 | shared_ptr ptr1; 29 | shared_ptr ptr2; 30 | shared_ptr ptr3; 31 | 32 | 33 | void ClearImage() 34 | { 35 | for(int rowidx = 0; rowidx(rowidx,pixidx) = {255,255,255}; 40 | } 41 | } 42 | } 43 | static void __attribute__((unused)) dilate(Vec3b Color,int n,Mat & img) 44 | { 45 | Mat img_backup = img.clone(); 46 | int & rows = img_backup.rows; 47 | int & cols = img_backup.cols; 48 | for(int row = n;row(row+i,col+j) == Color){ 54 | img.at(row,col) = Color; 55 | flag = true; 56 | break; 57 | } 58 | } 59 | if(flag) break; 60 | } 61 | } 62 | } 63 | } 64 | 65 | static void __attribute__((unused)) dilate(Vec3b Color,int n,Mat &img , Mat &img2) 66 | { 67 | int & rows = img.rows; 68 | int & cols = img.cols; 69 | for(int row = n;row(row+i,col+j) == Color){ 75 | img2.at(row,col) = Color; 76 | flag = true; 77 | break; 78 | } 79 | } 80 | if(flag) break; 81 | } 82 | } 83 | } 84 | } 85 | 86 | void ShowGridMap(shared_ptr ptr){ 87 | ClearImage(); 88 | // cout<getSizeX()<<' '<getSizeY()<getSizeX() ;xgetSizeY();y(y,x) = IMG_COLOR; 92 | } 93 | } 94 | 95 | double islbuttondown = false; 96 | void EventMouseClick(int event, int x, int y, int flags, void* ustc) 97 | { 98 | if(event == CV_EVENT_MOUSEMOVE && flags == CV_EVENT_FLAG_LBUTTON){ 99 | img2show.at(y,x) = IMG_COLOR; 100 | ptr1->SetObstacleByIdx((unsigned int)x,y); 101 | ptr2 = ptr1; 102 | } 103 | if(event == CV_EVENT_RBUTTONDOWN){ 104 | ClearImage(); 105 | ptr3 = ptr2; 106 | ptr3->Dilate(ptr2); 107 | ShowGridMap(ptr3); 108 | } 109 | // if (event == CV_EVENT_RBUTTONDOWN) 110 | // { 111 | 112 | // } 113 | 114 | imshow("GridMap test",img2show); 115 | 116 | } 117 | 118 | 119 | int main() 120 | { 121 | ptr1 = make_shared(IMG_WIDTH,IMG_HIGHT,1); 122 | imshow("GridMap test",img2show); 123 | cvSetMouseCallback("GridMap test", EventMouseClick); 124 | while(1) 125 | { 126 | if(cv::waitKey(1) == 27) return 0; 127 | } 128 | } 129 | 130 | -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/test/test_SplineCurve.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * An OpenCV Spline Test 6 | * 7 | *********************************************************************/ 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #define IMG_HIGHT 900 17 | #define IMG_WIDTH 1200 18 | 19 | const cv::Vec3b IMG_COLOR = {0,0,0}; 20 | const cv::Vec3b IMG_COLOR2 = {255,0,0}; 21 | 22 | using namespace std; 23 | using namespace cv; 24 | 25 | cv::Mat img(IMG_HIGHT,IMG_WIDTH,CV_8UC3,Scalar(255,255,255));; 26 | enum mode{ 27 | launch = 0, picking = 1, showing = 2, picking2 = 3 28 | }; 29 | mode curmode; 30 | Points::PosPoint2Ds pts; 31 | vector &x_s = pts.x; 32 | vector &y_s = pts.y; 33 | 34 | Interplot::SplineCurve sp; 35 | 36 | void ClearImage() 37 | { 38 | for(int rowidx = 0; rowidx(rowidx,pixidx) = {255,255,255}; 43 | } 44 | } 45 | } 46 | 47 | static void dilate(Vec3b Color,int n,Mat & img) 48 | { 49 | Mat img_backup = img.clone(); 50 | int & rows = img_backup.rows; 51 | int & cols = img_backup.cols; 52 | for(int row = n;row(row+i,col+j)[0]==Color[0] 62 | // && img_backup.at(row+i,col+j)[1]==Color[1] 63 | // && img_backup.at(row+i,col+j)[2]==Color[2]) 64 | if(img_backup.at(row+i,col+j) == Color) 65 | { 66 | img.at(row,col) = Color; 67 | flag = true; 68 | break; 69 | } 70 | } 71 | if(flag) break; 72 | } 73 | } 74 | } 75 | } 76 | 77 | int MBUTTON_CNT = 0; 78 | Points::Pos2D target; 79 | void EventMouseClick(int event, int x, int y, int flags, void* ustc) 80 | { 81 | if (event == EVENT_LBUTTONDOWN) 82 | { 83 | cout<<"EVENT_LBUTTONDOWN"<(y_s[i],x_s[i]) = IMG_COLOR; 95 | } 96 | dilate(IMG_COLOR,4, img); 97 | imshow("Spline test",img); 98 | } 99 | if (event == EVENT_RBUTTONDOWN) 100 | { 101 | cout<<"EVENT_RBUTTONDOWN"<2){ 104 | sp.setPoints(pts); 105 | } 106 | 107 | for(int s = 0;s(s)); 109 | if(ans.x>0&&ans.x0&&ans.y(ans.y,ans.x) = IMG_COLOR; 111 | } 112 | } 113 | 114 | dilate(IMG_COLOR,4, img); 115 | imshow("Spline test",img); 116 | } 117 | if (event == EVENT_MBUTTONDOWN) 118 | { 119 | if(curmode == showing){ 120 | target.x = x,target.y=y; 121 | curmode = picking2; 122 | } 123 | else if(curmode == picking2){ 124 | target.phi = atan2(y-target.y,x-target.x); 125 | auto s = sp.getDirectionalProjection(target,sp.max_s,0); 126 | Points::PosPoint2D ans = sp(s); 127 | if(ans.x>0&&ans.x0&&ans.y(ans.y,ans.x) = IMG_COLOR2; 129 | dilate(IMG_COLOR2,4, img); 130 | imshow("Spline test",img); 131 | } 132 | curmode = showing; 133 | } 134 | 135 | } 136 | } 137 | 138 | int main() 139 | { 140 | curmode = launch; 141 | imshow("Spline test",img); 142 | setMouseCallback("Spline test", EventMouseClick); 143 | while(1) 144 | { 145 | if(cv::waitKey(1) == 27) return 0; 146 | } 147 | } -------------------------------------------------------------------------------- /src/pathsearcher/HybridAStar.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A implement of Hybrid A Star algorithm. 6 | * Use " #define _SEARCHER_HAS_DEBUG_MODE_ " to Enable a friedn DEBUG function. 7 | * 8 | * Reference : 9 | * https://github.com/zm0612/Hybrid_A_Star 10 | * 11 | *********************************************************************/ 12 | #ifndef _PLANNING_HYBRID_A_STAR_H_ 13 | #define _PLANNING_HYBRID_A_STAR_H_ 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace Planning 26 | { 27 | 28 | namespace Searcher 29 | { 30 | namespace HybridAStarGlobal{ 31 | extern int Radius; // Pix Rad 32 | void DilateKernal(Maps::GridMap::MapDataType * pData, int x, int y); 33 | } 34 | 35 | class HybridAStar { 36 | public: 37 | struct HybridAStarSearchConfig{ 38 | double Segment_Len; 39 | double OneShotDist; 40 | double Penalty_SteeringChange; 41 | double Penalty_Steering; 42 | double Penalty_Reversing ; 43 | }; 44 | 45 | struct VehicleDynamicConfig{ 46 | double Vehicle_Move_Stepsize; 47 | double Vehicle_Max_Steering; 48 | double StateResolution; 49 | int AngularGridSize; 50 | int Vehicle_Steering_disnum; 51 | double RSSteering; 52 | }; 53 | 54 | typedef Models::VehicleShape VehicleShape; 55 | typedef Maps::GridMap SearchMap; 56 | 57 | // Should be Called Before SetMap, and Dynamic. 58 | void SetVehicleShape(VehicleShape const &data); 59 | void SetVehicleDynamic(VehicleDynamicConfig const &data); 60 | void SetSearchConfig(HybridAStarSearchConfig &config); 61 | void SetMap(std::shared_ptr& map_ptr,double state_grid_resolution); 62 | 63 | 64 | typedef StateNode::GridPos GridPos; 65 | typedef StateNode::Pos2d Pos2d; 66 | typedef StateNode::Pos2ds Pos2ds; 67 | 68 | bool Search( const Pos2d & start, const Pos2d & target); 69 | // bool Search( const GridPos & start, const GridPos & target); 70 | 71 | Pos2ds GetResult(); 72 | void Reset(); 73 | 74 | private: 75 | GridPos State2StateIndex(Pos2d state); 76 | virtual double ComputeH(StateNode::Ptr const &pStart); 77 | virtual double ComputeH(StateNode::Ptr const &pStart, StateNode::Ptr const &pTarget); 78 | virtual double ComputeG(StateNode::Ptr const &pStart, StateNode::Ptr const &pNeighbor); 79 | virtual bool HasCollision(Pos2d const & State); 80 | virtual bool Expand(StateNode::Ptr const &pCurrent, StateNode::Ptr const &pTarget); 81 | virtual void GetNeighborNodes(const StateNode::Ptr &CurrentPtr, std::vector &NeighborIdxs); 82 | 83 | StateNode::Ptr From = nullptr; 84 | StateNode::Ptr To = nullptr; 85 | std::vector>> StateNodeMap; 86 | 87 | 88 | /* Tools*/ 89 | PointsTools::PointsGenHandller PointsHandler; 90 | 91 | /* Vehicle Config*/ 92 | private: 93 | typedef Models::VehicleShapeCircles<2> VehicleShape_; 94 | typedef Models::VehicleMoveModel VehicleDynamicModel; 95 | std::shared_ptr DilatedMap = nullptr; 96 | VehicleShape_ VehicleData = VehicleShape(4.8,1.8,1.0,2.8); 97 | VehicleDynamicModel ModelData = VehicleShape(4.8,1.8,1.0,2.8); 98 | 99 | double Vehicle_Move_Stepsize; 100 | double Vehicle_Max_Steering; 101 | int Vehicle_Steering_disnum; 102 | 103 | /* Search Penalty Config*/ 104 | private : 105 | double Segment_Len; 106 | double OneShotDist; 107 | double Penalty_SteeringChange = 1.0; 108 | double Penalty_Steering; 109 | double Penalty_Reversing; 110 | 111 | 112 | /* Search Curves Config*/ 113 | private: 114 | typedef Curves::RSCurveStateSpace PathSpace; 115 | std::shared_ptr RSCurveSpace; 116 | 117 | /*Map Config*/ 118 | private: 119 | std::shared_ptr MapData = nullptr; 120 | size_t MAP_GRID_SIZE_X_; 121 | size_t MAP_GRID_SIZE_Y_; 122 | double MAP_GRID_RESOLUTION_; //meters per pix 123 | 124 | /* Search Map Config */ 125 | private: 126 | double STATE_GRID_RESOLUTION_; //meters per pix 127 | double ANGULAR_RESOLUTION_; 128 | size_t STATE_GRID_SIZE_X_; 129 | size_t STATE_GRID_SIZE_Y_; 130 | size_t STATE_GRID_SIZE_PHI_; // 0 for -pi max for pi-res 131 | 132 | #ifdef _SEARCHER_HAS_DEBUG_MODE_ 133 | friend void HAS_DEBUG_FUNCTION(); 134 | 135 | #endif 136 | 137 | }; 138 | #ifdef _SEARCHER_HAS_DEBUG_MODE_ 139 | void HAS_DEBUG_FUNCTION(); 140 | #endif 141 | 142 | } // namespace Searcher 143 | 144 | } // namespace Planning 145 | 146 | 147 | #endif -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/test/test_RS.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * An OpenCV RSCurve Test 6 | * 7 | *********************************************************************/ 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #define IMG_HIGHT 900 19 | #define IMG_WIDTH 1200 20 | 21 | using namespace std; 22 | using namespace cv; 23 | 24 | const cv::Vec3b IMG_COLOR = {0,0,0}; 25 | cv::Mat img(IMG_HIGHT,IMG_WIDTH,CV_8UC3,Scalar(255,255,255)); 26 | 27 | int clickcounter = 0; 28 | double Radius = 40; 29 | Curves::RSCurveStateSpace::State From; 30 | Curves::RSCurveStateSpace::State To; 31 | Curves::RSCurveStateSpace RSPSS; 32 | 33 | PointsTools::PointsGenHandller PGH; 34 | 35 | void __attribute__((unused)) ClearImage() 36 | { 37 | for(int rowidx = 0; rowidx(rowidx,pixidx) = {255,255,255}; 42 | } 43 | } 44 | } 45 | 46 | static void __attribute__((unused)) dilate(Vec3b Color,int n,Mat & img) 47 | { 48 | Mat img_backup = img.clone(); 49 | int & rows = img_backup.rows; 50 | int & cols = img_backup.cols; 51 | for(int row = n;row(row+i,col+j) == Color){ 57 | img.at(row,col) = Color; 58 | flag = true; 59 | break; 60 | } 61 | } 62 | if(flag) break; 63 | } 64 | } 65 | } 66 | } 67 | 68 | void EventMouseClick(int event, int x, int y, int flags, void* ustc) 69 | { 70 | Curves::RSCurve s; 71 | Points::Pos2Ds poses; 72 | // cout<(From.y,From.x) = IMG_COLOR; 83 | dilate(IMG_COLOR,4,img); 84 | break; 85 | case 1: 86 | From.phi = atan2(y-From.y,x-From.x); 87 | ClearImage(); 88 | img.at(From.y,From.x) = IMG_COLOR; 89 | img.at(From.y+30*sin(From.phi),From.x+30*cos(From.phi)) = IMG_COLOR; 90 | dilate(IMG_COLOR,4,img); 91 | clickcounter++; 92 | break; 93 | case 2: 94 | To.x = x; 95 | To.y = y; 96 | ClearImage(); 97 | img.at(From.y,From.x) = IMG_COLOR; 98 | img.at(From.y+30*sin(From.phi),From.x+30*cos(From.phi)) = IMG_COLOR; 99 | img.at(To.y,To.x) = IMG_COLOR; 100 | dilate(IMG_COLOR,4,img); 101 | clickcounter++; 102 | break; 103 | case 3: 104 | To.phi = atan2(y-To.y,x-To.x); 105 | ClearImage(); 106 | img.at(From.y,From.x) = IMG_COLOR; 107 | img.at(From.y+30*sin(From.phi),From.x+30*cos(From.phi)) = IMG_COLOR; 108 | img.at(To.y,To.x) = IMG_COLOR; 109 | img.at(To.y+30*sin(To.phi),To.x+30*cos(To.phi)) = IMG_COLOR; 110 | dilate(IMG_COLOR,4,img); 111 | cout<0&&poses.x[i]>0&&poses.x[i](poses.y[i],poses.x[i]) = IMG_COLOR; 119 | } 120 | dilate(IMG_COLOR,2,img); 121 | clickcounter++; 122 | break; 123 | default: 124 | break; 125 | } 126 | imshow("RSCurve test",img); 127 | } 128 | if (event == CV_EVENT_RBUTTONDOWN) 129 | { 130 | cout<<"CV_EVENT_RBUTTONDOWN"<>Radius; 141 | cout< 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | using namespace std; 17 | 18 | namespace Maps{ 19 | 20 | GridMap::GridMap(size_t x, size_t y, double reslotion) 21 | { 22 | assert((x>0)&&(y>0)); 23 | std::vector tmp(y,GRIDMAP_NOT_OBSTACLE); 24 | MapData.resize(x,tmp); 25 | x_size_ = x;y_size_ = y; 26 | x_upper_ = x*reslotion; 27 | y_upper_ = y*reslotion; 28 | reslotion_ = reslotion; 29 | } 30 | 31 | void GridMap::SetObstacleByIdx(int const &x, int const &y) 32 | { 33 | MapData[x][y] = GRIDMAP_IS_OBSTACLE; 34 | } 35 | 36 | void GridMap::SetObstacleByPos(double const &x, double const &y) 37 | { 38 | unsigned int idx_x = x/reslotion_; 39 | unsigned int idx_y = y/reslotion_; 40 | SetObstacleByIdx(idx_x, idx_y); 41 | } 42 | 43 | void GridMap::ReSetObstacle(unsigned int x, unsigned int y) 44 | { 45 | MapData[x][y] = GRIDMAP_NOT_OBSTACLE; 46 | } 47 | 48 | 49 | bool GridMap::HasObstacle(unsigned int x, unsigned int y) const 50 | { 51 | if(MapData[x][y] == GRIDMAP_IS_OBSTACLE) return true; 52 | return false; 53 | } 54 | 55 | int GridMap::operator()(int x,int y) const 56 | { 57 | return MapData[x][y]; 58 | } 59 | 60 | int& GridMap::operator()(int x,int y) 61 | { 62 | return MapData[x][y]; 63 | } 64 | 65 | void Utils::GridMapDilateDefaultkernel(GridMap::MapDataType *pData,int x, int y){ 66 | int size_x = pData->size(); 67 | int size_y = pData->front().size(); 68 | 69 | if(x==0&&y==0){ 70 | (*pData)[x ][y ] = GRIDMAP_IS_OBSTACLE; (*pData)[x+1][y ] = GRIDMAP_IS_OBSTACLE; 71 | (*pData)[x ][y+1] = GRIDMAP_IS_OBSTACLE; (*pData)[x+1][y+1] = GRIDMAP_IS_OBSTACLE; 72 | } 73 | else if(x==size_x-1&&y==0){ 74 | (*pData)[x-1][y ] = GRIDMAP_IS_OBSTACLE; (*pData)[x ][y ] = GRIDMAP_IS_OBSTACLE; 75 | (*pData)[x-1][y+1] = GRIDMAP_IS_OBSTACLE; (*pData)[x ][y+1] = GRIDMAP_IS_OBSTACLE; 76 | } 77 | else if(x==0&&y==size_y-1){ 78 | (*pData)[x ][y-1] = GRIDMAP_IS_OBSTACLE; (*pData)[x+1][y-1] = GRIDMAP_IS_OBSTACLE; 79 | (*pData)[x ][y ] = GRIDMAP_IS_OBSTACLE; (*pData)[x+1][y ] = GRIDMAP_IS_OBSTACLE; 80 | } 81 | else if(x==size_x-1&&y==size_y-1){ 82 | (*pData)[x-1][y-1] = GRIDMAP_IS_OBSTACLE; (*pData)[x ][y-1] = GRIDMAP_IS_OBSTACLE; 83 | (*pData)[x-1][y ] = GRIDMAP_IS_OBSTACLE; (*pData)[x ][y ] = GRIDMAP_IS_OBSTACLE; 84 | } 85 | else if(x==0){ 86 | (*pData)[x ][y-1] = GRIDMAP_IS_OBSTACLE; (*pData)[x+1][y-1] = GRIDMAP_IS_OBSTACLE; 87 | (*pData)[x ][y ] = GRIDMAP_IS_OBSTACLE; (*pData)[x+1][y ] = GRIDMAP_IS_OBSTACLE; 88 | (*pData)[x ][y+1] = GRIDMAP_IS_OBSTACLE; (*pData)[x+1][y+1] = GRIDMAP_IS_OBSTACLE; 89 | } 90 | else if(x==size_x-1){ 91 | (*pData)[x-1][y-1] = GRIDMAP_IS_OBSTACLE; (*pData)[x ][y-1] = GRIDMAP_IS_OBSTACLE; 92 | (*pData)[x-1][y ] = GRIDMAP_IS_OBSTACLE; (*pData)[x ][y ] = GRIDMAP_IS_OBSTACLE; 93 | (*pData)[x-1][y+1] = GRIDMAP_IS_OBSTACLE; (*pData)[x ][y+1] = GRIDMAP_IS_OBSTACLE; 94 | } 95 | else if(y==0){ 96 | (*pData)[x-1][y ] = GRIDMAP_IS_OBSTACLE; (*pData)[x ][y ] = GRIDMAP_IS_OBSTACLE; (*pData)[x+1][y ] = GRIDMAP_IS_OBSTACLE; 97 | (*pData)[x-1][y+1] = GRIDMAP_IS_OBSTACLE; (*pData)[x ][y+1] = GRIDMAP_IS_OBSTACLE; (*pData)[x+1][y+1] = GRIDMAP_IS_OBSTACLE; 98 | } 99 | else if(y==size_y-1){ 100 | (*pData)[x-1][y-1] = GRIDMAP_IS_OBSTACLE; (*pData)[x ][y-1] = GRIDMAP_IS_OBSTACLE; (*pData)[x+1][y-1] = GRIDMAP_IS_OBSTACLE; 101 | (*pData)[x-1][y ] = GRIDMAP_IS_OBSTACLE; (*pData)[x ][y ] = GRIDMAP_IS_OBSTACLE; (*pData)[x+1][y ] = GRIDMAP_IS_OBSTACLE; 102 | } 103 | else { 104 | (*pData)[x-1][y-1] = GRIDMAP_IS_OBSTACLE; (*pData)[x ][y-1] = GRIDMAP_IS_OBSTACLE; (*pData)[x+1][y-1] = GRIDMAP_IS_OBSTACLE; 105 | (*pData)[x-1][y ] = GRIDMAP_IS_OBSTACLE; (*pData)[x ][y ] = GRIDMAP_IS_OBSTACLE; (*pData)[x+1][y ] = GRIDMAP_IS_OBSTACLE; 106 | (*pData)[x-1][y+1] = GRIDMAP_IS_OBSTACLE; (*pData)[x ][y+1] = GRIDMAP_IS_OBSTACLE; (*pData)[x+1][y+1] = GRIDMAP_IS_OBSTACLE; 107 | } 108 | 109 | } 110 | 111 | void GridMap::Dilate(shared_ptr &DST, void (*kernel)(MapDataType*,int,int)){ 112 | DST = make_shared(x_size_,y_size_,reslotion_); 113 | for(size_t x=0;xMapData),x,y); 116 | } 117 | } 118 | 119 | 120 | // DistMap::DistMap(shared_ptr _pGridMap, double max_dist){ 121 | // pGridMap = _pGridMap; 122 | // x_size_ = pGridMap->getSizeX(); 123 | // y_size_ = pGridMap->getSizeY(); 124 | // x_upper_ = pGridMap->getMaxX(); 125 | // y_upper_ = pGridMap->getMaxY(); 126 | // reslotion_ = pGridMap->getReslotion(); 127 | // MapData.clear(); 128 | // MapData.resize(x_size_*y_size_, __DBL_MAX__); 129 | 130 | // } 131 | 132 | }// namespace Planning -------------------------------------------------------------------------------- /test/pathsearcher/test_HAS_functions.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A Test For functions in class "HybridAStar"; 6 | * 7 | *********************************************************************/ 8 | 9 | #include 10 | #include 11 | 12 | #define _SEARCHER_HAS_DEBUG_MODE_ 13 | #include 14 | 15 | 16 | #define IMG_FILENAME "./maps/map_basic.png" 17 | #define MAP_UPSIZE_N 10 18 | 19 | #define COLOR_WHITE Vec3b(255,255,255) 20 | #define COLOR_BLACK Vec3b(0,0,0) 21 | #define COLOR_GREEN Vec3b(0,255,0) 22 | #define COLOR_RED Vec3b(0,0,255) 23 | #define COLOR_BLUE Vec3b(255,0,0) 24 | 25 | using Planning::Searcher::HybridAStar; 26 | using Maps::GridMap; 27 | using namespace cv; 28 | using namespace std; 29 | 30 | HybridAStar PlanHandler; 31 | Mat MapImg = cv::imread(IMG_FILENAME,CV_8UC1); 32 | 33 | void ShowGridMap(shared_ptr ptr , Mat & img2show){ 34 | img2show = Mat(ptr->getSizeY(),ptr->getSizeX(),CV_8UC3,Vec3b(255,255,255)); 35 | for(int x=0, maxx=ptr->getSizeX() ;xgetSizeY();yHasObstacle(x,y)) img2show.at(y,x) = COLOR_BLACK; 38 | // else img2show.at(y,x) = COLOR_WHITE; 39 | } 40 | } 41 | 42 | int main(){ 43 | /* Load Data*/ 44 | MapImg = imread(IMG_FILENAME); 45 | Planning::Searcher::HAS_DEBUG_FUNCTION(); 46 | return 0; 47 | } 48 | 49 | Mat MapDataImg; 50 | Mat MapImg2show; 51 | shared_ptr MapData; 52 | 53 | void Planning::Searcher::HAS_DEBUG_FUNCTION(){ 54 | /*Load data*/ 55 | MapDataImg = Mat::zeros(MapImg.rows * MAP_UPSIZE_N,MapImg.cols*MAP_UPSIZE_N, CV_8UC3); 56 | for(size_t i=0;i(ii,jj)==Vec3b(255,255,255)) 59 | MapDataImg.at(i,j) = {255,255,255}; 60 | } 61 | 62 | /* cv Display windows config */ 63 | namedWindow("Map Show",WINDOW_NORMAL); 64 | MapImg2show = MapDataImg.clone(); 65 | // imshow("Map Show",MapImg2show); 66 | // while(1) if(waitKey(1)==27) break; 67 | /* Config Testing*/ 68 | HybridAStar::VehicleShape data(4.7,2.0,1.3,2.7); 69 | // HybridAStar::VehicleShape data(.47,.20,.13,.27); 70 | PlanHandler.SetVehicleShape(data); 71 | 72 | HybridAStar::VehicleDynamicConfig config2; 73 | config2.Vehicle_Max_Steering = 10*M_PI/180; 74 | config2.Vehicle_Steering_disnum = 2; 75 | config2.Vehicle_Move_Stepsize = 0.1; 76 | config2.StateResolution = 1; 77 | config2.AngularGridSize = 72; 78 | config2.RSSteering = 10*M_PI/180; 79 | PlanHandler.SetVehicleDynamic(config2); 80 | 81 | HybridAStar::HybridAStarSearchConfig config1; 82 | config1.Segment_Len = 2.0; 83 | config1.OneShotDist = 5.0; 84 | config1.Penalty_Reversing = 3.0; 85 | config1.Penalty_Steering = 1.05; 86 | config1.Penalty_SteeringChange = 1.5; 87 | PlanHandler.SetSearchConfig(config1); 88 | 89 | int size_x = MapDataImg.cols; 90 | int size_y = MapDataImg.rows; 91 | MapData = make_shared(size_x,size_y,1.0/MAP_UPSIZE_N); 92 | int a = 0; 93 | for(int x=0;x(y,x); 95 | if(color == COLOR_BLACK){ 96 | MapData->SetObstacleByIdx(x,y); 97 | } 98 | } 99 | 100 | PlanHandler.SetMap(MapData,config2.StateResolution); 101 | // ShowGridMap(PlanHandler.DilatedMap,MapImg2show); 102 | // imshow("Map Show",MapImg2show); 103 | // while(1) if(waitKey(1)==27) break; 104 | /* Config Test is Done*/ 105 | 106 | 107 | /* Test Private Functions*/ 108 | // First set search configs 109 | HybridAStar::Pos2d start(57.44673156738281,30-1.5998214721679688,-0.9655056865326423*M_PI); 110 | HybridAStar::Pos2d target(45.91535186767578,30-24.2706241607666,-0.7301509878243283*M_PI); 111 | auto start_grid = PlanHandler.State2StateIndex(start); 112 | auto target_grid = PlanHandler.State2StateIndex(target); 113 | 114 | StateNode::Ptr GoalPtr(new StateNode(target_grid)); 115 | GoalPtr->State = target; 116 | GoalPtr->dir = StateNode::NO; 117 | GoalPtr->Steering = 0; 118 | 119 | StateNode::Ptr FromPtr(new StateNode(start_grid)); 120 | FromPtr->State = start; 121 | FromPtr->Steering = 0; 122 | FromPtr->dir = StateNode::NO; 123 | FromPtr->status = StateNode::IN_OPENSET; 124 | FromPtr->intermediate_states_.clear(); 125 | FromPtr->CostG = 0.0; 126 | FromPtr->CostH = PlanHandler.ComputeH(FromPtr, GoalPtr); 127 | 128 | PlanHandler.From = FromPtr; 129 | PlanHandler.To = GoalPtr; 130 | PlanHandler.StateNodeMap[start_grid.x][start_grid.y][start_grid.phi] = FromPtr; 131 | PlanHandler.StateNodeMap[target_grid.x][target_grid.y][target_grid.phi] = GoalPtr; 132 | 133 | std::vector NeighborIdxs; 134 | 135 | PlanHandler.GetNeighborNodes(FromPtr,NeighborIdxs); 136 | 137 | MapImg2show = Mat(MapData->getSizeY(),MapData->getSizeX(),CV_8UC3,Vec3b(255,255,255)); 138 | ShowGridMap(MapData,MapImg2show); 139 | 140 | for(auto &t :NeighborIdxs){ 141 | auto &points = PlanHandler.StateNodeMap[t.x][t.y][t.phi]->intermediate_states_; 142 | int maxi=points.size(); 143 | for(int i=0;iToIdx(point.x,point.y); 146 | MapImg2show.at(idx[1],idx[0]) = COLOR_GREEN; 147 | } 148 | } 149 | 150 | // imshow("Map Show",MapImg2show); 151 | // while(1) if(waitKey(1)==27) break; 152 | 153 | 154 | PlanHandler.Reset(); 155 | PlanHandler.Search(start, target); 156 | GoalPtr = PlanHandler.StateNodeMap[target_grid.x][target_grid.y][target_grid.phi]; 157 | 158 | while(GoalPtr->parent_ptr!=nullptr){ 159 | auto &points = GoalPtr->intermediate_states_; 160 | int maxi=points.size(); 161 | for(int i=0;iToIdx(point.x,point.y); 164 | MapImg2show.at(idx[1],idx[0]) = COLOR_RED; 165 | } 166 | GoalPtr = GoalPtr->parent_ptr; 167 | } 168 | 169 | // imshow("Map Show",MapImg2show); 170 | // while(waitKey(0)!=27){}; 171 | 172 | 173 | return; 174 | } -------------------------------------------------------------------------------- /app/PlanNode.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A Demo of in class "HybridAStar"; 6 | * 7 | *********************************************************************/ 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | 15 | #define IMG_FILENAME "./maps/map_basic.png" 16 | #define MAP_UPSIZE_N 10 17 | 18 | #define COLOR_WHITE Vec3b(255,255,255) 19 | #define COLOR_BLACK Vec3b(0,0,0) 20 | #define COLOR_GREEN Vec3b(0,255,0) 21 | #define COLOR_RED Vec3b(0,0,255) 22 | #define COLOR_BLUE Vec3b(255,0,0) 23 | 24 | using Planning::Searcher::HybridAStar; 25 | using Maps::GridMap; 26 | using namespace cv; 27 | using namespace std; 28 | 29 | HybridAStar PlanHandler; 30 | Mat MapImg = cv::imread(IMG_FILENAME,CV_8UC1); 31 | 32 | static void __attribute__((unused)) dilate(Vec3b Color,int n,Mat & img) 33 | { 34 | Mat img_backup = img.clone(); 35 | int & rows = img_backup.rows; 36 | int & cols = img_backup.cols; 37 | for(int row = n;row(row+i,col+j) == Color){ 43 | img.at(row,col) = Color; 44 | flag = true; 45 | break; 46 | } 47 | } 48 | if(flag) break; 49 | } 50 | } 51 | } 52 | } 53 | 54 | void ShowGridMap(shared_ptr ptr , Mat & img2show){ 55 | img2show = Mat(ptr->getSizeY(),ptr->getSizeX(),CV_8UC3,Vec3b(255,255,255)); 56 | for(int x=0, maxx=ptr->getSizeX() ;xgetSizeY();yHasObstacle(x,y)) img2show.at(y,x) = COLOR_BLACK; 59 | else img2show.at(y,x) = COLOR_WHITE; 60 | } 61 | } 62 | 63 | void HASWorkFlow(); 64 | 65 | int main(){ 66 | /* Load Data*/ 67 | MapImg = imread(IMG_FILENAME); 68 | HASWorkFlow(); 69 | return 0; 70 | } 71 | 72 | Mat MapDataImg; 73 | Mat Img2show; 74 | shared_ptr MapData; 75 | HybridAStar::Pos2d start; 76 | HybridAStar::Pos2d target; 77 | 78 | int cnt = 0; 79 | void EventMouseClick(int event, int xc, int yc, int flags, void* ustc){ 80 | if(event == EVENT_LBUTTONDOWN){ 81 | double res = MapData->getReslotion(); 82 | double posx = xc*res; 83 | double posy = yc*res; 84 | ShowGridMap(MapData,Img2show); 85 | switch(cnt){ 86 | case 0: 87 | start.x = posx; 88 | start.y = posy; 89 | Img2show.at(start.y/res,start.x/res) = COLOR_GREEN; 90 | dilate(COLOR_GREEN,2,Img2show); 91 | cnt++; 92 | break; 93 | case 1: 94 | start.phi = atan2(posy-start.y,posx-start.x); 95 | Img2show.at(start.y/res,start.x/res) = COLOR_GREEN; 96 | Img2show.at(start.y/res+2/res*sin(start.phi),start.x/res+2/res*cos(start.phi)) = COLOR_GREEN; 97 | dilate(COLOR_GREEN,2,Img2show); 98 | cnt++; 99 | break; 100 | case 2: 101 | target.x = posx; 102 | target.y = posy; 103 | Img2show.at(start.y/res,start.x/res) = COLOR_GREEN; 104 | Img2show.at(start.y/res+2/res*sin(start.phi),start.x/res+2/res*cos(start.phi)) = COLOR_GREEN; 105 | Img2show.at(target.y/res,target.x/res) = COLOR_GREEN; 106 | dilate(COLOR_GREEN,2,Img2show); 107 | cnt++; 108 | break; 109 | case 3: 110 | target.phi = atan2(posy-target.y,posx-target.x); 111 | Img2show.at(yc,xc) = COLOR_GREEN; 112 | Img2show.at(start.y/res,start.x/res) = COLOR_GREEN; 113 | Img2show.at(start.y/res+2/res*sin(start.phi),start.x/res+2/res*cos(start.phi)) = COLOR_GREEN; 114 | Img2show.at(target.y/res,target.x/res) = COLOR_GREEN; 115 | dilate(COLOR_GREEN,2,Img2show); 116 | cnt++; 117 | break; 118 | default: 119 | break; 120 | } 121 | imshow("Map Show",Img2show); 122 | } else if(event == EVENT_RBUTTONDOWN){ 123 | ShowGridMap(MapData,Img2show); 124 | cnt = 0; 125 | imshow("Map Show",Img2show); 126 | } 127 | 128 | } 129 | 130 | void HASWorkFlow(){ 131 | /*Load data*/ 132 | MapDataImg = Mat::zeros(MapImg.rows * MAP_UPSIZE_N,MapImg.cols*MAP_UPSIZE_N, CV_8UC3); 133 | for(int i=0;i(ii,jj)==Vec3b(255,255,255)) 136 | MapDataImg.at(i,j) = {255,255,255}; 137 | } 138 | 139 | /* cv Display windows config */ 140 | namedWindow("Map Show",WINDOW_NORMAL); 141 | Img2show = MapDataImg.clone(); 142 | HybridAStar::VehicleShape data(4.7,2.0,1.3,2.7); 143 | // HybridAStar::VehicleShape data(.47,.20,.13,.27); 144 | PlanHandler.SetVehicleShape(data); 145 | 146 | HybridAStar::VehicleDynamicConfig config2; 147 | config2.Vehicle_Max_Steering = 15*M_PI/180; 148 | config2.Vehicle_Steering_disnum = 4; 149 | config2.Vehicle_Move_Stepsize = 0.1; 150 | config2.StateResolution = 1.5; 151 | config2.AngularGridSize = 72; 152 | config2.RSSteering = 10*M_PI/180; 153 | PlanHandler.SetVehicleDynamic(config2); 154 | 155 | HybridAStar::HybridAStarSearchConfig config1; 156 | config1.Segment_Len = 2.0; 157 | config1.OneShotDist = 5.0; 158 | config1.Penalty_Reversing = 3.0; 159 | config1.Penalty_Steering = 1.05; 160 | config1.Penalty_SteeringChange = 1.5; 161 | PlanHandler.SetSearchConfig(config1); 162 | 163 | int size_x = MapDataImg.cols; 164 | int size_y = MapDataImg.rows; 165 | MapData = make_shared(size_x,size_y,1.0/MAP_UPSIZE_N); 166 | for(int x=0;x(y,x); 168 | if(color == COLOR_BLACK){ 169 | MapData->SetObstacleByIdx(x,y); 170 | } 171 | } 172 | PlanHandler.SetMap(MapData,config2.StateResolution); 173 | 174 | ShowGridMap(MapData,Img2show); 175 | imshow("Map Show",Img2show); 176 | setMouseCallback("Map Show",EventMouseClick); 177 | while(waitKey(2)!=27){ 178 | if(cnt == 4){ 179 | PlanHandler.Reset(); 180 | if(!(PlanHandler.Search(start, target))){ 181 | cout<<"Plan Failed"<ToIdx(point.x,point.y); 187 | Img2show.at(idx[1],idx[0]) = COLOR_RED; 188 | } 189 | 190 | imshow("Map Show",Img2show); 191 | } 192 | cnt++; 193 | } 194 | } 195 | 196 | 197 | return; 198 | } -------------------------------------------------------------------------------- /test/pathsearcher/test_HAS_search.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A Test For functions in class "HybridAStar"; 6 | * 7 | *********************************************************************/ 8 | 9 | #include 10 | #include 11 | 12 | #define _SEARCHER_HAS_DEBUG_MODE_ 13 | #include 14 | 15 | 16 | #define IMG_FILENAME "./maps/map_basic.png" 17 | #define MAP_UPSIZE_N 10 18 | 19 | #define COLOR_WHITE Vec3b(255,255,255) 20 | #define COLOR_BLACK Vec3b(0,0,0) 21 | #define COLOR_GREEN Vec3b(0,255,0) 22 | #define COLOR_RED Vec3b(0,0,255) 23 | #define COLOR_BLUE Vec3b(255,0,0) 24 | 25 | using Planning::Searcher::HybridAStar; 26 | using Maps::GridMap; 27 | using namespace cv; 28 | using namespace std; 29 | 30 | HybridAStar PlanHandler; 31 | Mat MapImg = cv::imread(IMG_FILENAME,CV_8UC1); 32 | 33 | static void __attribute__((unused)) dilate(Vec3b Color,int n,Mat & img) 34 | { 35 | Mat img_backup = img.clone(); 36 | int & rows = img_backup.rows; 37 | int & cols = img_backup.cols; 38 | for(int row = n;row(row+i,col+j) == Color){ 44 | img.at(row,col) = Color; 45 | flag = true; 46 | break; 47 | } 48 | } 49 | if(flag) break; 50 | } 51 | } 52 | } 53 | } 54 | 55 | void ShowGridMap(shared_ptr ptr , Mat & img2show){ 56 | img2show = Mat(ptr->getSizeY(),ptr->getSizeX(),CV_8UC3,Vec3b(255,255,255)); 57 | for(int x=0, maxx=ptr->getSizeX() ;xgetSizeY();yHasObstacle(x,y)) img2show.at(y,x) = COLOR_BLACK; 60 | else img2show.at(y,x) = COLOR_WHITE; 61 | } 62 | } 63 | 64 | int main(){ 65 | /* Load Data*/ 66 | MapImg = imread(IMG_FILENAME); 67 | Planning::Searcher::HAS_DEBUG_FUNCTION(); 68 | return 0; 69 | } 70 | 71 | Mat MapDataImg; 72 | Mat Img2show; 73 | shared_ptr MapData; 74 | HybridAStar::Pos2d start; 75 | HybridAStar::Pos2d target; 76 | 77 | int cnt = 0; 78 | void EventMouseClick(int event, int xc, int yc, int flags, void* ustc){ 79 | if(event == EVENT_LBUTTONDOWN){ 80 | double res = MapData->getReslotion(); 81 | double posx = xc*res; 82 | double posy = yc*res; 83 | ShowGridMap(MapData,Img2show); 84 | switch(cnt){ 85 | case 0: 86 | start.x = posx; 87 | start.y = posy; 88 | Img2show.at(start.y/res,start.x/res) = COLOR_GREEN; 89 | dilate(COLOR_GREEN,2,Img2show); 90 | cnt++; 91 | break; 92 | case 1: 93 | start.phi = atan2(posy-start.y,posx-start.x); 94 | Img2show.at(start.y/res,start.x/res) = COLOR_GREEN; 95 | Img2show.at(start.y/res+2/res*sin(start.phi),start.x/res+2/res*cos(start.phi)) = COLOR_GREEN; 96 | dilate(COLOR_GREEN,2,Img2show); 97 | cnt++; 98 | break; 99 | case 2: 100 | target.x = posx; 101 | target.y = posy; 102 | Img2show.at(start.y/res,start.x/res) = COLOR_GREEN; 103 | Img2show.at(start.y/res+2/res*sin(start.phi),start.x/res+2/res*cos(start.phi)) = COLOR_GREEN; 104 | Img2show.at(target.y/res,target.x/res) = COLOR_GREEN; 105 | dilate(COLOR_GREEN,2,Img2show); 106 | cnt++; 107 | break; 108 | case 3: 109 | target.phi = atan2(posy-target.y,posx-target.x); 110 | Img2show.at(yc,xc) = COLOR_GREEN; 111 | Img2show.at(start.y/res,start.x/res) = COLOR_GREEN; 112 | Img2show.at(start.y/res+2/res*sin(start.phi),start.x/res+2/res*cos(start.phi)) = COLOR_GREEN; 113 | Img2show.at(target.y/res,target.x/res) = COLOR_GREEN; 114 | dilate(COLOR_GREEN,2,Img2show); 115 | cnt++; 116 | break; 117 | default: 118 | break; 119 | } 120 | imshow("Map Show",Img2show); 121 | } else if(event == EVENT_RBUTTONDOWN){ 122 | ShowGridMap(MapData,Img2show); 123 | cnt = 0; 124 | imshow("Map Show",Img2show); 125 | } 126 | 127 | } 128 | 129 | void Planning::Searcher::HAS_DEBUG_FUNCTION(){ 130 | /*Load data*/ 131 | MapDataImg = Mat::zeros(MapImg.rows * MAP_UPSIZE_N,MapImg.cols*MAP_UPSIZE_N, CV_8UC3); 132 | for(int i=0;i(ii,jj)==Vec3b(255,255,255)) 135 | MapDataImg.at(i,j) = {255,255,255}; 136 | } 137 | 138 | /* cv Display windows config */ 139 | namedWindow("Map Show",WINDOW_NORMAL); 140 | Img2show = MapDataImg.clone(); 141 | HybridAStar::VehicleShape data(4.7,2.0,1.3,2.7); 142 | // HybridAStar::VehicleShape data(.47,.20,.13,.27); 143 | PlanHandler.SetVehicleShape(data); 144 | 145 | HybridAStar::VehicleDynamicConfig config2; 146 | config2.Vehicle_Max_Steering = 10*M_PI/180; 147 | config2.Vehicle_Steering_disnum = 2; 148 | config2.Vehicle_Move_Stepsize = 0.1; 149 | config2.StateResolution = 1; 150 | config2.AngularGridSize = 72; 151 | config2.RSSteering = 10*M_PI/180; 152 | PlanHandler.SetVehicleDynamic(config2); 153 | 154 | HybridAStar::HybridAStarSearchConfig config1; 155 | config1.Segment_Len = 2.0; 156 | config1.OneShotDist = 5.0; 157 | config1.Penalty_Reversing = 3.0; 158 | config1.Penalty_Steering = 1.05; 159 | config1.Penalty_SteeringChange = 1.5; 160 | PlanHandler.SetSearchConfig(config1); 161 | 162 | int size_x = MapDataImg.cols; 163 | int size_y = MapDataImg.rows; 164 | MapData = make_shared(size_x,size_y,1.0/MAP_UPSIZE_N); 165 | for(int x=0;x(y,x); 167 | if(color == COLOR_BLACK){ 168 | MapData->SetObstacleByIdx(x,y); 169 | } 170 | } 171 | PlanHandler.SetMap(MapData,config2.StateResolution); 172 | 173 | // MapData = PlanHandler.DilatedMap; 174 | /* Test Private Functions*/ 175 | ShowGridMap(MapData,Img2show); 176 | imshow("Map Show",Img2show); 177 | setMouseCallback("Map Show",EventMouseClick); 178 | while(waitKey(2)!=27){ 179 | if(cnt == 4){ 180 | PlanHandler.Reset(); 181 | if(!(PlanHandler.Search(start, target))){ 182 | cout<<"Plan Failed"<parent_ptr!=nullptr){ 189 | // auto &points = GoalPtr->intermediate_states_; 190 | // int maxi=points.size(); 191 | // for(int i=0;iToIdx(point.x,point.y); 194 | // Img2show.at(idx[1],idx[0]) = COLOR_RED; 195 | // } 196 | // GoalPtr = GoalPtr->parent_ptr; 197 | // } 198 | 199 | auto points = PlanHandler.GetResult(); 200 | for(int i=0;iToIdx(point.x,point.y); 203 | Img2show.at(idx[1],idx[0]) = COLOR_RED; 204 | } 205 | 206 | imshow("Map Show",Img2show); 207 | } 208 | cnt++; 209 | } 210 | } 211 | 212 | 213 | return; 214 | } -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/Interplot/SplineCurve.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A simple cubic spline path library without external 6 | * dependencies. 7 | * 8 | *********************************************************************/ 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #define NEWTOWN_ERROR (1e-5) 15 | #define DIVIDE_ERR (1e-5) 16 | 17 | using namespace std; 18 | 19 | namespace Interplot 20 | { 21 | static inline double distanceSq(SplineCurve::Point p1, SplineCurve::Point p2){ 22 | return (p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y); 23 | } 24 | 25 | static inline double distance(SplineCurve::Point p1, SplineCurve::Point p2){ 26 | return std::sqrt(distanceSq(p1,p2)); 27 | } 28 | 29 | void SplineCurve::setPoints(RefPoints refps){ 30 | Spline::RefPoints points; 31 | vector & s_list = points.x; 32 | vector & x_list = refps.x; 33 | vector & y_list = refps.y; 34 | assert(x_list.size()==y_list.size()); 35 | size_t size = x_list.size(); 36 | assert(size>2); 37 | s_list.resize(x_list.size()); 38 | s_list[0] = 0; 39 | for(int i=1,maxi = s_list.size(); i & s_list = points.x; 59 | vector & x_list = refps.x; 60 | vector & y_list = refps.y; 61 | assert(x_list.size()==y_list.size()); 62 | size_t size = x_list.size(); 63 | assert(size>2); 64 | s_list.resize(x_list.size()); 65 | s_list[0] = 0; 66 | for(int i=1,maxi = s_list.size(); i x; 89 | vector y; 90 | vector s; 91 | int size = max_s / stepsize; 92 | int vecsize = size; 93 | if((max_s - size*stepsize)>(stepsize*0.1)) vecsize++; 94 | x.resize(vecsize);y.resize(vecsize);s.resize(vecsize); 95 | double cur_s = 0; 96 | s[0] = 0; x[0] = xs_(0); y[0] = ys_(0); 97 | for(int i = 1;ioperator()(tmp_s); 138 | double tmp_dissq = distanceSq(target,tmpp); 139 | if(tmp_dissq 14 | 15 | #include 16 | 17 | using namespace std; 18 | 19 | namespace Interplot 20 | { 21 | 22 | namespace Utils 23 | { 24 | band_matrix::band_matrix(int dim, int n_u, int n_l){resize(dim, n_u, n_l);} 25 | 26 | void band_matrix::resize(int dim, int n_u, int n_l){ 27 | assert(dim > 0);assert(n_u >= 0);assert(n_l >= 0); 28 | m_upper.resize(n_u + 1);m_lower.resize(n_l + 1); 29 | for (size_t i = 0; i < m_upper.size(); i++) {m_upper[i].resize(dim);} 30 | for (size_t i = 0; i < m_lower.size(); i++) {m_lower[i].resize(dim);} 31 | } 32 | 33 | int band_matrix::dim() const { return m_upper.size()>0?m_upper[0].size():0;} 34 | double &band_matrix::operator()(int i, int j) { 35 | int k = j-i; 36 | assert((i >= 0) && (i < dim()) && (j >= 0) && (j < dim())); 37 | assert((-num_lower() <= k) && (k <= num_upper())); 38 | return k>=0?m_upper[k][i]:m_lower[-k][i]; 39 | } 40 | 41 | double band_matrix::operator()(int i, int j) const{ 42 | int k =j-i; 43 | assert((i >= 0) && (i < dim()) && (j >= 0) && (j < dim())); 44 | assert((-num_lower() <= k) && (k <= num_upper())); 45 | return k>=0?m_upper[k][i]:m_lower[-k][i]; 46 | } 47 | double &band_matrix::saved_diag(int i){ 48 | assert((i >= 0) && (i < dim())); 49 | return m_lower[0][i]; 50 | } 51 | double band_matrix::saved_diag(int i) const{ 52 | assert((i >= 0) && (i < dim())); 53 | return m_lower[0][i]; 54 | } 55 | 56 | // LR-Decomposition of a band matrix 57 | void band_matrix::lu_decompose() { 58 | int i_max, j_max; 59 | int j_min; 60 | double x; 61 | 62 | // preconditioning 63 | // normalize column i so that a_ii=1 64 | for (int i = 0; i < this->dim(); i++) { 65 | assert(this->operator()(i, i) != 0.0); 66 | this->saved_diag(i) = 1.0 / this->operator()(i, i); 67 | j_min = std::max(0, i - this->num_lower()); 68 | j_max = std::min(this->dim() - 1, i + this->num_upper()); 69 | for (int j = j_min; j <= j_max; j++) { 70 | this->operator()(i, j) *= this->saved_diag(i); 71 | } 72 | this->operator()(i, i) = 1.0; // prevents rounding errors 73 | } 74 | 75 | // Gauss LR-Decomposition 76 | for (int k = 0; k < this->dim(); k++) { 77 | i_max = std::min(this->dim() - 1, k + this->num_lower()); 78 | for (int i = k + 1; i <= i_max; i++) { 79 | assert(this->operator()(k, k) != 0.0); 80 | x = this->operator()(i, k) / this->operator()(k, k); 81 | this->operator()(i, k) = x; // assembly part of L 82 | j_max = std::min(this->dim() - 1, k + this->num_upper()); 83 | for (int j = k + 1; j <= j_max; j++) { 84 | // assembly part of R 85 | this->operator()(i, j) = this->operator()(i, j) - x * this->operator()(k, j); 86 | } 87 | } 88 | } 89 | this->decomposed_flag_ = true; 90 | } 91 | 92 | std::vector band_matrix::r_solve(const std::vector &b) const{ 93 | assert(this->dim() == (int) b.size()); 94 | std::vector x(this->dim()); 95 | int j_stop; 96 | double sum; 97 | for (int i = this->dim() - 1; i >= 0; i--) { 98 | sum = 0; 99 | j_stop = std::min(this->dim() - 1, i + this->num_upper()); 100 | for (int j = i + 1; j <= j_stop; j++) sum += this->operator()(i, j) * x[j]; 101 | x[i] = (b[i] - sum) / this->operator()(i, i); 102 | } 103 | return x; 104 | } 105 | 106 | std::vector band_matrix::l_solve(const std::vector &b) const { 107 | assert(this->dim() == (int) b.size()); 108 | std::vector x(this->dim()); 109 | int j_start; 110 | double sum; 111 | for (int i = 0; i < this->dim(); i++) { 112 | sum = 0; 113 | j_start = std::max(0, i - this->num_lower()); 114 | for (int j = j_start; j < i; j++) sum += this->operator()(i, j) * x[j]; 115 | x[i] = (b[i]*saved_diag(i)) - sum; 116 | } 117 | return x; 118 | } 119 | 120 | std::vector band_matrix::lu_solve(const std::vector &b){ 121 | assert(this->dim() == (int) b.size()); 122 | std::vector x, y; 123 | if (this->isDecomposed() == false) { 124 | this->lu_decompose(); 125 | } 126 | y = this->l_solve(b); 127 | x = this->r_solve(y); 128 | return x; 129 | } 130 | 131 | } // namespace Utils 132 | 133 | void Spline::set_boundary(Spline::BoundType left, double left_value, 134 | Spline::BoundType right, double right_value, 135 | bool force_linear_extrapolation) { 136 | assert(PointsData.x.size() == 0); // set_points() must not have happened yet 137 | m_left = left; 138 | m_right = right; 139 | m_left_value = left_value; 140 | m_right_value = right_value; 141 | m_force_linear_extrapolation = force_linear_extrapolation; 142 | } 143 | 144 | void Spline::set_points(const RefPoints &points, bool cubic_spline){ 145 | PointsData = points; 146 | int n = PointsData.size(); 147 | vector &x = PointsData.x; 148 | vector &y = PointsData.y; 149 | if (cubic_spline == true) { // cubic spline interpolation 150 | // setting up the matrix and right hand side of the equation system 151 | // for the parameters b[] 152 | Utils::band_matrix A(n, 1, 1); 153 | std::vector rhs(n); 154 | for (int i = 1; i < n - 1; i++) { 155 | A(i, i - 1) = 1.0 / 3.0 * (x[i] - x[i - 1]); 156 | A(i, i) = 2.0 / 3.0 * (x[i + 1] - x[i - 1]); 157 | A(i, i + 1) = 1.0 / 3.0 * (x[i + 1] - x[i]); 158 | rhs[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) - (y[i] - y[i - 1]) / (x[i] - x[i - 1]); 159 | } 160 | // boundary conditions 161 | if (m_left == SecondOrderDer) { 162 | // 2*b[0] = f'' 163 | A(0, 0) = 2.0; 164 | A(0, 1) = 0.0; 165 | rhs[0] = m_left_value; 166 | } else if (m_left == FirstOrderDer) { 167 | // c[0] = f', needs to be re-expressed in terms of b: 168 | // (2b[0]+b[1])(x[1]-x[0]) = 3 ((y[1]-y[0])/(x[1]-x[0]) - f') 169 | A(0, 0) = 2.0 * (x[1] - x[0]); 170 | A(0, 1) = 1.0 * (x[1] - x[0]); 171 | rhs[0] = 3.0 * ((y[1] - y[0]) / (x[1] - x[0]) - m_left_value); 172 | } else { 173 | assert(false); 174 | } 175 | if (m_right == Spline::SecondOrderDer) { 176 | // 2*b[n-1] = f'' 177 | A(n - 1, n - 1) = 2.0; 178 | A(n - 1, n - 2) = 0.0; 179 | rhs[n - 1] = m_right_value; 180 | } else if (m_right == Spline::FirstOrderDer) { 181 | // c[n-1] = f', needs to be re-expressed in terms of b: 182 | // (b[n-2]+2b[n-1])(x[n-1]-x[n-2]) 183 | // = 3 (f' - (y[n-1]-y[n-2])/(x[n-1]-x[n-2])) 184 | A(n - 1, n - 1) = 2.0 * (x[n - 1] - x[n - 2]); 185 | A(n - 1, n - 2) = 1.0 * (x[n - 1] - x[n - 2]); 186 | rhs[n - 1] = 3.0 * (m_right_value - (y[n - 1] - y[n - 2]) / (x[n - 1] - x[n - 2])); 187 | } else { 188 | assert(false); 189 | } 190 | 191 | // solve the equation system to obtain the parameters b[] 192 | m_b = A.lu_solve(rhs); 193 | 194 | // calculate parameters a[] and c[] based on b[] 195 | m_a.resize(n); 196 | m_c.resize(n); 197 | for (int i = 0; i < n - 1; i++) { 198 | m_a[i] = 1.0 / 3.0 * (m_b[i + 1] - m_b[i]) / (x[i + 1] - x[i]); 199 | m_c[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) 200 | - 1.0 / 3.0 * (2.0 * m_b[i] + m_b[i + 1]) * (x[i + 1] - x[i]); 201 | } 202 | } else { // linear interpolation 203 | m_a.resize(n); 204 | m_b.resize(n); 205 | m_c.resize(n); 206 | for (int i = 0; i < n - 1; i++) { 207 | m_a[i] = 0.0; 208 | m_b[i] = 0.0; 209 | m_c[i] = (PointsData.y[i + 1] - PointsData.y[i]) / (PointsData.x[i + 1] - PointsData.x[i]); 210 | } 211 | } 212 | 213 | // for left extrapolation coefficients 214 | m_b0 = (m_force_linear_extrapolation == false) ? m_b[0] : 0.0; 215 | m_c0 = m_c[0]; 216 | 217 | // for the right extrapolation coefficients 218 | // f_{n-1}(x) = b*(x-x_{n-1})^2 + c*(x-x_{n-1}) + y_{n-1} 219 | double h = x[n - 1] - x[n - 2]; 220 | // m_b[n-1] is determined by the boundary condition 221 | m_a[n - 1] = 0.0; 222 | m_c[n - 1] = 3.0 * m_a[n - 2] * h * h + 2.0 * m_b[n - 2] * h + m_c[n - 2]; // = f'_{n-2}(x_{n-1}) 223 | if (m_force_linear_extrapolation == true) 224 | m_b[n - 1] = 0.0; 225 | } 226 | 227 | double Spline::operator()(double x) const{ 228 | size_t n = PointsData.x.size(); 229 | // find the closest point PointsData.x[idx] < x, idx=0 even if x::const_iterator it; 231 | it = std::lower_bound(PointsData.x.begin(), PointsData.x.end(), x); 232 | int idx = std::max(int(it - PointsData.x.begin()) - 1, 0); 233 | 234 | double h = x - PointsData.x[idx]; 235 | double interpol; 236 | if (x < PointsData.x[0]) { 237 | // extrapolation to the left 238 | interpol = (m_b0 * h + m_c0) * h + PointsData.y[0]; 239 | } else if (x > PointsData.x[n - 1]) { 240 | // extrapolation to the right 241 | interpol = (m_b[n - 1] * h + m_c[n - 1]) * h + PointsData.y[n - 1]; 242 | } else { 243 | // interpolation 244 | interpol = ((m_a[idx] * h + m_b[idx]) * h + m_c[idx]) * h + PointsData.y[idx]; 245 | } 246 | return interpol; 247 | } 248 | 249 | double Spline::getDeriv(int order, double x) const { 250 | assert(order > 0); 251 | 252 | size_t n = PointsData.x.size(); 253 | // find the closest point PointsData.x[idx] < x, idx=0 even if x::const_iterator it; 255 | it = std::lower_bound(PointsData.x.begin(), PointsData.x.end(), x); 256 | int idx = std::max(int(it - PointsData.x.begin()) - 1, 0); 257 | 258 | double h = x - PointsData.x[idx]; 259 | double interpol; 260 | if (x < PointsData.x[0]) { 261 | // extrapolation to the left 262 | switch (order) { 263 | case 1:interpol = 2.0 * m_b0 * h + m_c0; 264 | break; 265 | case 2:interpol = 2.0 * m_b0 * h; 266 | break; 267 | default:interpol = 0.0; 268 | break; 269 | } 270 | } else if (x > PointsData.x[n - 1]) { 271 | // extrapolation to the right 272 | switch (order) { 273 | case 1:interpol = 2.0 * m_b[n - 1] * h + m_c[n - 1]; 274 | break; 275 | case 2:interpol = 2.0 * m_b[n - 1]; 276 | break; 277 | default:interpol = 0.0; 278 | break; 279 | } 280 | } else { 281 | // interpolation 282 | switch (order) { 283 | case 1:interpol = (3.0 * m_a[idx] * h + 2.0 * m_b[idx]) * h + m_c[idx]; 284 | break; 285 | case 2:interpol = 6.0 * m_a[idx] * h + 2.0 * m_b[idx]; 286 | break; 287 | case 3:interpol = 6.0 * m_a[idx]; 288 | break; 289 | default:interpol = 0.0; 290 | break; 291 | } 292 | } 293 | return interpol; 294 | } 295 | 296 | } // namespace Interplot 297 | 298 | 299 | 300 | 301 | -------------------------------------------------------------------------------- /src/pathsearcher/HybridAStar.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Discription ： 5 | * A implement of Hybrid A Star algorithm. 6 | * 7 | * Reference : 8 | * https://github.com/zm0612/Hybrid_A_Star 9 | * 10 | *********************************************************************/ 11 | 12 | #include 13 | #include 14 | // Utils 15 | #include 16 | #include 17 | 18 | // using namespace std; 19 | 20 | 21 | namespace Planning 22 | { 23 | namespace Searcher 24 | { 25 | namespace Utils{ 26 | 27 | inline static double Distance(double x1,double x2,double y1,double y2){ 28 | return sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)); 29 | } 30 | 31 | inline static double Distance(StateNode::Pos2d& start, StateNode::Pos2d& target) { 32 | return Distance(start.x,target.x,start.y,target.y); 33 | } 34 | 35 | inline static double Distance(StateNode& start, StateNode& target){ 36 | return Distance(start.State,target.State); 37 | } 38 | 39 | inline static double Distance(StateNode::Ptr pstart, StateNode::Ptr ptarget){ 40 | return Distance(*pstart,*ptarget); 41 | } 42 | 43 | // TOo [-pi, pi] 44 | inline static double Mod2Pi(const double &x){ 45 | double v = fmod(x, 2 * M_PI); 46 | if (v < -M_PI) { 47 | v += 2.0 * M_PI; 48 | } else if (v >= M_PI) { 49 | v -= 2.0 * M_PI; 50 | } 51 | return v; 52 | } 53 | } 54 | 55 | void HybridAStar::Reset(){ 56 | StateNodeMap.clear(); 57 | StateNodeMap.resize(STATE_GRID_SIZE_X_); 58 | for(auto it = StateNodeMap.begin();it!=StateNodeMap.end();it++){ 59 | it->resize(STATE_GRID_SIZE_Y_); 60 | for(auto it2 = it->begin();it2!=it->end();it2++){ 61 | it2->resize(STATE_GRID_SIZE_PHI_); 62 | for(auto it3 = it2->begin();it3!=it2->end();it3++){ 63 | *it3 = nullptr; 64 | } 65 | } 66 | } 67 | } 68 | 69 | void HybridAStar::SetMap(std::shared_ptr& map_ptr,double state_grid_resolution) 70 | { 71 | MapData = map_ptr; 72 | MAP_GRID_RESOLUTION_ = MapData->getReslotion(); 73 | STATE_GRID_RESOLUTION_ = state_grid_resolution; 74 | MAP_GRID_SIZE_X_ = MapData->getSizeX(); 75 | MAP_GRID_SIZE_Y_ = MapData->getSizeY(); 76 | STATE_GRID_SIZE_X_ = std::floor(MapData->getMaxX()/STATE_GRID_RESOLUTION_); 77 | STATE_GRID_SIZE_Y_ = std::floor(MapData->getMaxY()/STATE_GRID_RESOLUTION_); 78 | Reset(); 79 | HybridAStarGlobal::Radius = VehicleData.Radius/MAP_GRID_RESOLUTION_+1; 80 | MapData->Dilate(DilatedMap,HybridAStarGlobal::DilateKernal); 81 | } 82 | 83 | void HybridAStar::SetSearchConfig(HybridAStarSearchConfig &config){ 84 | OneShotDist = config.OneShotDist; 85 | Segment_Len = config.Segment_Len; 86 | Penalty_Steering = config.Penalty_Steering; 87 | Penalty_Reversing = config.Penalty_Reversing; 88 | } 89 | 90 | HybridAStar::GridPos HybridAStar::State2StateIndex(Pos2d state){ 91 | GridPos ans; 92 | double phi = Utils::Mod2Pi(state.phi); 93 | ans.x = state.x/STATE_GRID_RESOLUTION_; 94 | ans.y = state.y/STATE_GRID_RESOLUTION_; 95 | ans.phi = (phi+M_PI)/ANGULAR_RESOLUTION_; 96 | 97 | ans.x = std::max(0,ans.x ); ans.x = std::min(static_cast(STATE_GRID_SIZE_X_-1 ),ans.x ); 98 | ans.y = std::max(0,ans.y ); ans.y = std::min(static_cast(STATE_GRID_SIZE_Y_-1 ),ans.y ); 99 | // ans.phi = std::max(0,ans.phi); ans.phi = std::min(static_cast(STATE_GRID_SIZE_PHI_),ans.phi); 100 | 101 | return ans; 102 | } 103 | 104 | double HybridAStar::ComputeH(StateNode::Ptr const &pStart){ 105 | return ComputeH(pStart,To); 106 | } 107 | 108 | double HybridAStar::ComputeH(StateNode::Ptr const &pStart, StateNode::Ptr const &pTarget){ 109 | double ans = 0; 110 | // Distance to Target 111 | double distance = Utils::Distance(pStart,pTarget); 112 | auto s = RSCurveSpace->RSCurveCalc(pStart->State,pTarget->State); 113 | for(int i=0;i<5;i++){ 114 | double cost = 0; 115 | if(s.type_[i]==Curves::RSCurveSegmentType::RS_LEFT||s.type_[i]==Curves::RSCurveSegmentType::RS_LEFT){ 116 | cost+=fabs(s.length[i])*Penalty_Steering; 117 | } else if(s.type_[i]==Curves::RSCurveSegmentType::RS_STRAIGHT) { 118 | cost+=fabs(s.length[i]); 119 | } 120 | if(s.length[i]<0) 121 | cost*=Penalty_Reversing; 122 | ans+=cost; 123 | } 124 | return ans; 125 | // pStart->cost_h = ans; 126 | } 127 | 128 | double HybridAStar::ComputeG(StateNode::Ptr const &pStart, StateNode::Ptr const &pTarget){ 129 | double ans = 0; 130 | if (pTarget->dir == StateNode::FORWARD) { 131 | if (pTarget->Steering != pStart->Steering) { 132 | if (pTarget->Steering == 0) { 133 | ans = Segment_Len * Penalty_SteeringChange; 134 | } else { 135 | ans = Segment_Len * Penalty_SteeringChange * Penalty_Steering; 136 | } 137 | } else { 138 | if (pTarget->Steering == 0) { 139 | ans = Segment_Len; 140 | } else { 141 | ans = Segment_Len * Penalty_Steering; 142 | } 143 | } 144 | } else { 145 | if (pTarget->Steering != pStart->Steering) { 146 | if (pTarget->Steering == 0) { 147 | ans = Segment_Len * Penalty_SteeringChange * Penalty_Reversing; 148 | } else { 149 | ans = Segment_Len * Penalty_SteeringChange * Penalty_Steering * Penalty_Reversing; 150 | } 151 | } else { 152 | if (pTarget->Steering == 0) { 153 | ans = Segment_Len * Penalty_Reversing; 154 | } else { 155 | ans = Segment_Len * Penalty_Steering * Penalty_Reversing; 156 | } 157 | } 158 | } 159 | return ans; 160 | } 161 | 162 | namespace HybridAStarGlobal{ 163 | int Radius = 10; // Pix Rad 164 | inline static void Set(Maps::GridMap::MapDataType *pData, int &x, int &y){ 165 | int size_x = pData->size(); 166 | int size_y = pData->back().size(); 167 | if(x>0&&x0&&ysize(); 173 | int size_y = pData->front().size(); 174 | int x_min = x-Radius>0 ? x-Radius : 0; 175 | int x_max = x+Radius0 ? y-Radius : 0; 177 | int y_max = y+RadiusDilate(DilatedMap,HybridAStarGlobal::DilateKernal); 193 | 194 | }; 195 | 196 | void HybridAStar::SetVehicleDynamic(VehicleDynamicConfig const &data){ 197 | Vehicle_Move_Stepsize = data.Vehicle_Move_Stepsize; 198 | Vehicle_Max_Steering = data.Vehicle_Max_Steering; 199 | Vehicle_Steering_disnum = data.Vehicle_Steering_disnum; 200 | STATE_GRID_RESOLUTION_ = data.StateResolution; 201 | STATE_GRID_SIZE_PHI_ = data.AngularGridSize; 202 | ANGULAR_RESOLUTION_ = 2*M_PI/STATE_GRID_SIZE_PHI_; 203 | RSCurveSpace = std::make_shared(ModelData.Steer2Radius(data.RSSteering)); 204 | } 205 | 206 | bool HybridAStar::HasCollision(Pos2d const & State){ 207 | for(int i=0,maxi = VehicleData.CircleCenters.size();iMAP_GRID_SIZE_X_*MAP_GRID_RESOLUTION_-VehicleData.Radius 212 | || y>MAP_GRID_SIZE_Y_*MAP_GRID_RESOLUTION_-VehicleData.Radius) return true; 213 | size_t idx_x = x/MAP_GRID_RESOLUTION_; size_t idx_y = y/MAP_GRID_RESOLUTION_; 214 | if(DilatedMap->HasObstacle(idx_x,idx_y)) return true; 215 | } 216 | return false; 217 | } 218 | 219 | bool HybridAStar::Expand(StateNode::Ptr const &pCurrent, StateNode::Ptr const &pTarget){ 220 | auto s = RSCurveSpace->RSCurveCalc(pCurrent->State,pTarget->State); 221 | Points::Pos2Ds poses; 222 | PointsHandler.GetPoses(poses,s,Vehicle_Move_Stepsize,pCurrent->State); 223 | // Points::Pos2Ds poses; 224 | for(int i=0,maxi = poses.size();iintermediate_states_ = poses; 228 | pTarget->parent_ptr = pCurrent; 229 | 230 | return true; 231 | } 232 | 233 | void HybridAStar::GetNeighborNodes(const StateNode::Ptr &CurPtr, std::vector &Neighbors){ 234 | Neighbors.clear(); 235 | for(int i = -Vehicle_Steering_disnum;i<=Vehicle_Steering_disnum;i++){ 236 | // double x = CurPtr->State.x; 237 | // double y = CurPtr->State.y; 238 | // double phi = CurPtr->State.phi; 239 | 240 | StateNode::Pos2ds intermediate_states; 241 | 242 | const double steer = i * Vehicle_Max_Steering/Vehicle_Steering_disnum; 243 | bool HasObstacle = false; 244 | int maxj = Segment_Len/Vehicle_Move_Stepsize; 245 | // Forward 246 | Pos2d curpos(CurPtr->State); 247 | for (int j = 1; j <= maxj; j++) { 248 | curpos = ModelData.MoveBySteerting(curpos,steer,Vehicle_Move_Stepsize); 249 | intermediate_states.push_back(curpos); 250 | if (HasCollision(curpos)){ 251 | HasObstacle = true; 252 | break; 253 | } 254 | } 255 | 256 | if(!HasObstacle){ 257 | GridPos curidx = State2StateIndex(curpos); 258 | StateNode::Ptr pcur = std::make_shared(curidx); 259 | pcur->dir = StateNode::FORWARD; 260 | pcur->parent_ptr = CurPtr; 261 | pcur->State = curpos; 262 | pcur->intermediate_states_ = intermediate_states; 263 | pcur->CostH = CurPtr->CostG + ComputeH(pcur); 264 | pcur->CostG = CurPtr->CostG + ComputeG(CurPtr,pcur); 265 | if(StateNodeMap[curidx.x][curidx.y][curidx.phi] == nullptr){ 266 | StateNodeMap[curidx.x][curidx.y][curidx.phi] = pcur; 267 | Neighbors.push_back(curidx); 268 | } else if((StateNodeMap[curidx.x][curidx.y][curidx.phi]->CostG)<(pcur->CostG)){ 269 | StateNodeMap[curidx.x][curidx.y][curidx.phi] = pcur; 270 | Neighbors.push_back(curidx); 271 | } else{} 272 | } 273 | 274 | // Backward 275 | curpos = CurPtr->State; 276 | HasObstacle = false; 277 | for (int j = 1; j <= maxj; j++) { 278 | curpos = ModelData.MoveBySteerting(curpos,steer,-Vehicle_Move_Stepsize); 279 | intermediate_states.push_back(curpos); 280 | if (HasCollision(curpos)){ 281 | HasObstacle = true; 282 | break; 283 | } 284 | } 285 | 286 | if(!HasObstacle){ 287 | GridPos curidx = State2StateIndex(curpos); 288 | StateNode::Ptr pcur = std::make_shared(curidx); 289 | pcur->dir = StateNode::BACKWARD; 290 | pcur->parent_ptr = CurPtr; 291 | pcur->State = curpos; 292 | pcur->intermediate_states_ = intermediate_states; 293 | pcur->CostH = CurPtr->CostG + ComputeH(pcur); 294 | pcur->CostG = CurPtr->CostG + ComputeG(CurPtr,pcur); 295 | if(StateNodeMap[curidx.x][curidx.y][curidx.phi] == nullptr){ 296 | StateNodeMap[curidx.x][curidx.y][curidx.phi] = pcur; 297 | Neighbors.push_back(curidx); 298 | } else if(StateNodeMap[curidx.x][curidx.y][curidx.phi]->status!=StateNode::IN_CLOSESET 299 | &&(StateNodeMap[curidx.x][curidx.y][curidx.phi]->CostG)>(pcur->CostG)){ 300 | StateNodeMap[curidx.x][curidx.y][curidx.phi] = pcur; 301 | Neighbors.push_back(curidx); 302 | } else{} 303 | } 304 | } 305 | } 306 | 307 | 308 | bool HybridAStar::Search( const Pos2d & start, const Pos2d & target){ 309 | GridPos start_grid = State2StateIndex(start); 310 | GridPos target_grid = State2StateIndex(target); 311 | 312 | StateNode::Ptr GoalPtr(new StateNode(target_grid)); 313 | GoalPtr->State = target; 314 | GoalPtr->dir = StateNode::NO; 315 | GoalPtr->Steering = 0; 316 | 317 | StateNode::Ptr FromPtr(new StateNode(start_grid)); 318 | FromPtr->State = start; 319 | FromPtr->Steering = 0; 320 | FromPtr->dir = StateNode::NO; 321 | FromPtr->status = StateNode::IN_OPENSET; 322 | FromPtr->intermediate_states_.clear(); 323 | FromPtr->CostG = 0.0; 324 | FromPtr->CostH = ComputeH(FromPtr, GoalPtr); 325 | 326 | From = FromPtr; 327 | To = GoalPtr; 328 | 329 | StateNodeMap[start_grid.x][start_grid.y][start_grid.phi] = FromPtr; 330 | StateNodeMap[target_grid.x][target_grid.y][target_grid.phi] = GoalPtr; 331 | 332 | struct CompareStruct{ 333 | int idx1,idx2,idx3; 334 | double val; 335 | CompareStruct(int x, int y,int phi,double _val = 0):idx1(x),idx2(y),idx3(phi),val(_val){} 336 | CompareStruct(GridPos p,double _val = 0):idx1(p.x),idx2(p.y),idx3(p.phi),val(_val){} 337 | }; 338 | 339 | class greater{ 340 | public: 341 | bool operator()(const CompareStruct &n1, const CompareStruct & n2){ 342 | if(n1.val>n2.val) return true; 343 | return false; 344 | } 345 | }; 346 | 347 | std::priority_queue, greater> openset_; 348 | openset_.push(CompareStruct(start_grid,0)); 349 | 350 | std::vector NeighborIdxs; 351 | StateNode::Ptr CurrentPtr; 352 | StateNode::Ptr Neighbors; 353 | int count = 0; 354 | while (!openset_.empty()) { 355 | auto &tmp = openset_.top(); 356 | CurrentPtr = StateNodeMap[tmp.idx1][tmp.idx2][tmp.idx3]; 357 | openset_.pop(); 358 | 359 | if(Utils::Distance(CurrentPtr,GoalPtr)status == StateNode::IN_CLOSESET) continue; 366 | CurrentPtr->status = StateNode::IN_CLOSESET; 367 | GetNeighborNodes(CurrentPtr, NeighborIdxs); 368 | for(auto & ptmp : NeighborIdxs){ 369 | CompareStruct tmpdata(ptmp); 370 | auto & ptr = StateNodeMap[tmpdata.idx1][tmpdata.idx2][tmpdata.idx3]; 371 | tmpdata.val = ptr->CostG + ptr->CostH; 372 | ptr ->status = StateNode::IN_OPENSET; 373 | openset_.push(tmpdata); 374 | } 375 | 376 | count++; 377 | if (count > 50000) { 378 | return false; 379 | } 380 | } 381 | 382 | return false; 383 | 384 | } 385 | 386 | HybridAStar::Pos2ds HybridAStar::GetResult(){ 387 | auto GoalPtr = To; 388 | Pos2ds ans; 389 | while(GoalPtr->parent_ptr!=nullptr){ 390 | auto &points = GoalPtr->intermediate_states_; 391 | int maxi=points.size(); 392 | for(int i=0;iparent_ptr; 396 | } 397 | return ans; 398 | } 399 | 400 | void __attribute__((weak)) HAS_DEBUG_FUNCTION(){}; 401 | 402 | 403 | } // namespace Searcher 404 | } // namespace Planning 405 | -------------------------------------------------------------------------------- /SecondParty/BorisMathLib/Curves/ReedsShepp.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Author : BorisVandermeer 3 | * 4 | * Reference : 5 | * ompl/src/base/spaces 6 | * 7 | *********************************************************************/ 8 | 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | namespace Curves 17 | { 18 | const RSCurveSegmentType RSCurveStateSpace::RSCurveStateSpace::RSCurveType[18][5] = { 19 | {RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP}, // 0 20 | {RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP}, // 1 21 | {RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP}, // 2 22 | {RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP}, // 3 23 | {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP}, // 4 24 | {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP}, // 5 25 | {RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP}, // 6 26 | {RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP}, // 7 27 | {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP}, // 8 28 | {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP}, // 9 29 | {RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP}, // 10 30 | {RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP}, // 11 31 | {RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP}, // 12 32 | {RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP}, // 13 33 | {RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP}, // 14 34 | {RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP}, // 15 35 | {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT}, // 16 36 | {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT} // 17 37 | }; 38 | } // namespace Curves 39 | 40 | 41 | namespace 42 | { 43 | // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper. 44 | const double pi = M_PI; 45 | const double twopi = 2. * pi; 46 | #ifndef NDEBUG 47 | const double RS_EPS = 1e-6; 48 | #endif 49 | const double ZERO = 10 * std::numeric_limits::epsilon(); 50 | 51 | inline static double mod2pi(double x) 52 | { 53 | double v = fmod(x, twopi); 54 | if (v < -pi) 55 | v += twopi; 56 | else if (v > pi) 57 | v -= twopi; 58 | return v; 59 | } 60 | inline void polar(double x, double y, double &r, double &theta) 61 | { 62 | r = sqrt(x * x + y * y); 63 | theta = atan2(y, x); 64 | } 65 | inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega) 66 | { 67 | double delta = mod2pi(u - v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.; 68 | double t1 = atan2(eta * A - xi * B, xi * A + eta * B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3; 69 | tau = (t2 < 0) ? mod2pi(t1 + pi) : mod2pi(t1); 70 | omega = mod2pi(tau - u + v - phi); 71 | } 72 | 73 | // formula 8.1 in Reeds-Shepp paper 74 | inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v) 75 | { 76 | polar(x - sin(phi), y - 1. + cos(phi), u, t); 77 | if (t >= -ZERO) 78 | { 79 | v = mod2pi(phi - t); 80 | if (v >= -ZERO) 81 | { 82 | assert(fabs(u * cos(t) + sin(phi) - x) < RS_EPS); 83 | assert(fabs(u * sin(t) - cos(phi) + 1 - y) < RS_EPS); 84 | assert(fabs(mod2pi(t + v - phi)) < RS_EPS); 85 | return true; 86 | } 87 | } 88 | return false; 89 | } 90 | // formula 8.2 91 | inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v) 92 | { 93 | double t1, u1; 94 | polar(x + sin(phi), y - 1. - cos(phi), u1, t1); 95 | u1 = u1 * u1; 96 | if (u1 >= 4.) 97 | { 98 | double theta; 99 | u = sqrt(u1 - 4.); 100 | theta = atan2(2., u); 101 | t = mod2pi(t1 + theta); 102 | v = mod2pi(t - phi); 103 | assert(fabs(2 * sin(t) + u * cos(t) - sin(phi) - x) < RS_EPS); 104 | assert(fabs(-2 * cos(t) + u * sin(t) + cos(phi) + 1 - y) < RS_EPS); 105 | assert(fabs(mod2pi(t - v - phi)) < RS_EPS); 106 | return t >= -ZERO && v >= -ZERO; 107 | } 108 | return false; 109 | } 110 | void CSC(double x, double y, double phi, Curves::RSCurve &path) 111 | { 112 | double t, u, v, Lmin = path.TotalLength(), L; 113 | if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) 114 | { 115 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[14], t, u, v); 116 | Lmin = L; 117 | } 118 | if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip 119 | { 120 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[14], -t, -u, -v); 121 | Lmin = L; 122 | } 123 | if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect 124 | { 125 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[15], t, u, v); 126 | Lmin = L; 127 | } 128 | if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect 129 | { 130 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[15], -t, -u, -v); 131 | Lmin = L; 132 | } 133 | if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) 134 | { 135 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[12], t, u, v); 136 | Lmin = L; 137 | } 138 | if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip 139 | { 140 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[12], -t, -u, -v); 141 | Lmin = L; 142 | } 143 | if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect 144 | { 145 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[13], t, u, v); 146 | Lmin = L; 147 | } 148 | if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect 149 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[13], -t, -u, -v); 150 | } 151 | // formula 8.3 / 8.4 *** TYPO IN PAPER *** 152 | inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v) 153 | { 154 | double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta; 155 | polar(xi, eta, u1, theta); 156 | if (u1 <= 4.) 157 | { 158 | u = -2. * asin(.25 * u1); 159 | t = mod2pi(theta + .5 * u + pi); 160 | v = mod2pi(phi - t + u); 161 | assert(fabs(2 * (sin(t) - sin(t - u)) + sin(phi) - x) < RS_EPS); 162 | assert(fabs(2 * (-cos(t) + cos(t - u)) - cos(phi) + 1 - y) < RS_EPS); 163 | assert(fabs(mod2pi(t - u + v - phi)) < RS_EPS); 164 | return t >= -ZERO && u <= ZERO; 165 | } 166 | return false; 167 | } 168 | void CCC(double x, double y, double phi, Curves::RSCurve &path) 169 | { 170 | double t, u, v, Lmin = path.TotalLength(), L; 171 | if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) 172 | { 173 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[0], t, u, v); 174 | Lmin = L; 175 | } 176 | if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip 177 | { 178 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[0], -t, -u, -v); 179 | Lmin = L; 180 | } 181 | if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect 182 | { 183 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[1], t, u, v); 184 | Lmin = L; 185 | } 186 | if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect 187 | { 188 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[1], -t, -u, -v); 189 | Lmin = L; 190 | } 191 | 192 | // backwards 193 | double xb = x * cos(phi) + y * sin(phi), yb = x * sin(phi) - y * cos(phi); 194 | if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) 195 | { 196 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[0], v, u, t); 197 | Lmin = L; 198 | } 199 | if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip 200 | { 201 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[0], -v, -u, -t); 202 | Lmin = L; 203 | } 204 | if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect 205 | { 206 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[1], v, u, t); 207 | Lmin = L; 208 | } 209 | if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect 210 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[1], -v, -u, -t); 211 | } 212 | // formula 8.7 213 | inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v) 214 | { 215 | double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi * xi + eta * eta)); 216 | if (rho <= 1.) 217 | { 218 | u = acos(rho); 219 | tauOmega(u, -u, xi, eta, phi, t, v); 220 | assert(fabs(2 * (sin(t) - sin(t - u) + sin(t - 2 * u)) - sin(phi) - x) < RS_EPS); 221 | assert(fabs(2 * (-cos(t) + cos(t - u) - cos(t - 2 * u)) + cos(phi) + 1 - y) < RS_EPS); 222 | assert(fabs(mod2pi(t - 2 * u - v - phi)) < RS_EPS); 223 | return t >= -ZERO && v <= ZERO; 224 | } 225 | return false; 226 | } 227 | // formula 8.8 228 | inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v) 229 | { 230 | double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi * xi - eta * eta) / 16.; 231 | if (rho >= 0 && rho <= 1) 232 | { 233 | u = -acos(rho); 234 | if (u >= -.5 * pi) 235 | { 236 | tauOmega(u, u, xi, eta, phi, t, v); 237 | assert(fabs(4 * sin(t) - 2 * sin(t - u) - sin(phi) - x) < RS_EPS); 238 | assert(fabs(-4 * cos(t) + 2 * cos(t - u) + cos(phi) + 1 - y) < RS_EPS); 239 | assert(fabs(mod2pi(t - v - phi)) < RS_EPS); 240 | return t >= -ZERO && v >= -ZERO; 241 | } 242 | } 243 | return false; 244 | } 245 | void CCCC(double x, double y, double phi, Curves::RSCurve &path) 246 | { 247 | double t, u, v, Lmin = path.TotalLength(), L; 248 | if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) 249 | { 250 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[2], t, u, -u, v); 251 | Lmin = L; 252 | } 253 | if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip 254 | { 255 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[2], -t, -u, u, -v); 256 | Lmin = L; 257 | } 258 | if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // reflect 259 | { 260 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[3], t, u, -u, v); 261 | Lmin = L; 262 | } 263 | if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip + reflect 264 | { 265 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[3], -t, -u, u, -v); 266 | Lmin = L; 267 | } 268 | 269 | if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) 270 | { 271 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[2], t, u, u, v); 272 | Lmin = L; 273 | } 274 | if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip 275 | { 276 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[2], -t, -u, -u, -v); 277 | Lmin = L; 278 | } 279 | if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // reflect 280 | { 281 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[3], t, u, u, v); 282 | Lmin = L; 283 | } 284 | if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip + reflect 285 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[3], -t, -u, -u, -v); 286 | } 287 | // formula 8.9 288 | inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v) 289 | { 290 | double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta; 291 | polar(xi, eta, rho, theta); 292 | if (rho >= 2.) 293 | { 294 | double r = sqrt(rho * rho - 4.); 295 | u = 2. - r; 296 | t = mod2pi(theta + atan2(r, -2.)); 297 | v = mod2pi(phi - .5 * pi - t); 298 | assert(fabs(2 * (sin(t) - cos(t)) - u * sin(t) + sin(phi) - x) < RS_EPS); 299 | assert(fabs(-2 * (sin(t) + cos(t)) + u * cos(t) - cos(phi) + 1 - y) < RS_EPS); 300 | assert(fabs(mod2pi(t + pi / 2 + v - phi)) < RS_EPS); 301 | return t >= -ZERO && u <= ZERO && v <= ZERO; 302 | } 303 | return false; 304 | } 305 | // formula 8.10 306 | inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v) 307 | { 308 | double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta; 309 | polar(-eta, xi, rho, theta); 310 | if (rho >= 2.) 311 | { 312 | t = theta; 313 | u = 2. - rho; 314 | v = mod2pi(t + .5 * pi - phi); 315 | assert(fabs(2 * sin(t) - cos(t - v) - u * sin(t) - x) < RS_EPS); 316 | assert(fabs(-2 * cos(t) - sin(t - v) + u * cos(t) + 1 - y) < RS_EPS); 317 | assert(fabs(mod2pi(t + pi / 2 - v - phi)) < RS_EPS); 318 | return t >= -ZERO && u <= ZERO && v <= ZERO; 319 | } 320 | return false; 321 | } 322 | void CCSC(double x, double y, double phi, Curves::RSCurve &path) 323 | { 324 | double t, u, v, Lmin = path.TotalLength() - .5 * pi, L; 325 | if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) 326 | { 327 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[4], t, -.5 * pi, u, v); 328 | Lmin = L; 329 | } 330 | if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip 331 | { 332 | path = 333 | Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[4], -t, .5 * pi, -u, -v); 334 | Lmin = L; 335 | } 336 | if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect 337 | { 338 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[5], t, -.5 * pi, u, v); 339 | Lmin = L; 340 | } 341 | if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect 342 | { 343 | path = 344 | Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[5], -t, .5 * pi, -u, -v); 345 | Lmin = L; 346 | } 347 | 348 | if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) 349 | { 350 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[8], t, -.5 * pi, u, v); 351 | Lmin = L; 352 | } 353 | if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip 354 | { 355 | path = 356 | Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[8], -t, .5 * pi, -u, -v); 357 | Lmin = L; 358 | } 359 | if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect 360 | { 361 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[9], t, -.5 * pi, u, v); 362 | Lmin = L; 363 | } 364 | if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect 365 | { 366 | path = 367 | Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[9], -t, .5 * pi, -u, -v); 368 | Lmin = L; 369 | } 370 | 371 | // backwards 372 | double xb = x * cos(phi) + y * sin(phi), yb = x * sin(phi) - y * cos(phi); 373 | if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) 374 | { 375 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[6], v, u, -.5 * pi, t); 376 | Lmin = L; 377 | } 378 | if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip 379 | { 380 | path = 381 | Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[6], -v, -u, .5 * pi, -t); 382 | Lmin = L; 383 | } 384 | if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect 385 | { 386 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[7], v, u, -.5 * pi, t); 387 | Lmin = L; 388 | } 389 | if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect 390 | { 391 | path = 392 | Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[7], -v, -u, .5 * pi, -t); 393 | Lmin = L; 394 | } 395 | 396 | if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) 397 | { 398 | path = 399 | Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[10], v, u, -.5 * pi, t); 400 | Lmin = L; 401 | } 402 | if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip 403 | { 404 | path = 405 | Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[10], -v, -u, .5 * pi, -t); 406 | Lmin = L; 407 | } 408 | if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect 409 | { 410 | path = 411 | Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[11], v, u, -.5 * pi, t); 412 | Lmin = L; 413 | } 414 | if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect 415 | path = 416 | Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[11], -v, -u, .5 * pi, -t); 417 | } 418 | // formula 8.11 *** TYPO IN PAPER *** 419 | inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v) 420 | { 421 | double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta; 422 | polar(xi, eta, rho, theta); 423 | if (rho >= 2.) 424 | { 425 | u = 4. - sqrt(rho * rho - 4.); 426 | if (u <= ZERO) 427 | { 428 | t = mod2pi(atan2((4 - u) * xi - 2 * eta, -2 * xi + (u - 4) * eta)); 429 | v = mod2pi(t - phi); 430 | assert(fabs(4 * sin(t) - 2 * cos(t) - u * sin(t) - sin(phi) - x) < RS_EPS); 431 | assert(fabs(-4 * cos(t) - 2 * sin(t) + u * cos(t) + cos(phi) + 1 - y) < RS_EPS); 432 | assert(fabs(mod2pi(t - v - phi)) < RS_EPS); 433 | return t >= -ZERO && v >= -ZERO; 434 | } 435 | } 436 | return false; 437 | } 438 | void CCSCC(double x, double y, double phi, Curves::RSCurve &path) 439 | { 440 | double t, u, v, Lmin = path.TotalLength() - pi, L; 441 | if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) 442 | { 443 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[16], t, -.5 * pi, u, 444 | -.5 * pi, v); 445 | Lmin = L; 446 | } 447 | if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip 448 | { 449 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[16], -t, .5 * pi, -u, 450 | .5 * pi, -v); 451 | Lmin = L; 452 | } 453 | if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect 454 | { 455 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[17], t, -.5 * pi, u, 456 | -.5 * pi, v); 457 | Lmin = L; 458 | } 459 | if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect 460 | path = Curves::RSCurve(Curves::RSCurveStateSpace::RSCurveType[17], -t, .5 * pi, -u, 461 | .5 * pi, -v); 462 | } 463 | 464 | Curves::RSCurve reedsShepp(double x, double y, double phi) 465 | { 466 | Curves::RSCurve path; 467 | CSC(x, y, phi, path); 468 | CCC(x, y, phi, path); 469 | CCCC(x, y, phi, path); 470 | CCSC(x, y, phi, path); 471 | CCSCC(x, y, phi, path); 472 | return path; 473 | } 474 | } 475 | 476 | namespace Curves 477 | { 478 | RSCurve::RSCurve(const RSCurveSegmentType *type, double t, double u, double v, double w, double x, double _radius): type_(type){ 479 | length[0] = t; 480 | length[1] = u; 481 | length[2] = v; 482 | length[3] = w; 483 | length[4] = x; 484 | totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x); 485 | radius = _radius; 486 | } 487 | 488 | RSCurve RSCurveStateSpace::RSCurveCalc(double x,double y,double phi) const{ 489 | RSCurve ans = ::reedsShepp(x / radius_, y / radius_, phi); 490 | ans.radius = radius_; 491 | return ans; 492 | } 493 | 494 | RSCurve RSCurveStateSpace::RSCurveCalc(State &from, State &to) const{ 495 | State *s1 = &from, *s2 = &to; 496 | double x1 = s1->x, y1 = s1->y, th1 = s1->phi; 497 | double x2 = s2->x, y2 = s2->y, th2 = s2->phi; 498 | double dx = x2 - x1, dy = y2 - y1, c = cos(th1), s = sin(th1); 499 | double x = c * dx + s * dy, y = -s * dx + c * dy, phi = th2 - th1; 500 | return RSCurveCalc(x,y,phi); 501 | } 502 | 503 | 504 | } // namespace Curves 505 | --------------------------------------------------------------------------------