#8 update purpursuit with hybrid a*

algorithm/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.5)
 
-project(Control VERSION 1.0 LANGUAGES CXX)
+project(control VERSION 1.0 LANGUAGES CXX)
 
 # Set the C++ standard to C++20
 set(CMAKE_CXX_STANDARD 20)
@@ -10,32 +10,27 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g")
 
 
 # Find Python 3 and optionally NumPy
-find_package(Python3 COMPONENTS Interpreter Development NumPy QUIET)
-if(NOT Python3_FOUND)
-    # Fallback to older PythonLibs find module if Python3 components are not found
-    find_package(PythonLibs REQUIRED)
-    set(Python3_INCLUDE_DIRS ${PYTHON_INCLUDE_DIRS})
-    set(Python3_LIBRARIES ${PYTHON_LIBRARIES})
-    # Note: This does not handle NumPy includes for the fallback case
-endif()
-
-# Include directories for header files
+find_package(PythonLibs REQUIRED)
+find_package(Eigen3 3.3 REQUIRED NO_MODULE)
+find_package(Ceres REQUIRED)
+
+
 include_directories(
-    ${Python3_INCLUDE_DIRS}
-    ${Python3_NumPy_INCLUDE_DIRS}  # This may not be set if PythonLibs was used
-    ${CMAKE_SOURCE_DIR}
-    planner
-    utils
-    visual
-    controller
+    ${PYTHON_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
+    ${CERES_INCLUDE_DIRS}
+    lib
+    lib/hybrid-a-star
 )
 
-# Add executable with all relevant source files
-add_executable(${PROJECT_NAME} 
-    main.cpp
-)
+FILE(GLOB NavObjects lib/hybrid-a-star/*.cpp)
+FILE(GLOB Control src/*/*.cpp)
+
+add_executable(${PROJECT_NAME} ${NavObjects} ${Control} "src/main.cpp")
 
-# Link with Python libraries
 target_link_libraries(${PROJECT_NAME}
-    ${Python3_LIBRARIES}
-)
+    ${CERES_LIBRARIES}
+    ompl
+    Eigen3::Eigen
+    ${PYTHON_LIBRARIES}
+)

algorithm/globalmap/parsinginfo.txt

@@ -1,28 +1,31 @@
-1, 140, 64, 0
-2, 360, 64, 0
-3, 480, 110, 60
-4, 515, 180, 90
-5, 520, 200, 90
-6, 630, 335, 0
-7, 800, 335, 0
-8, 864, 394, 60
-9, 915, 480, 90
-10, 915, 540, 90
-11, 786, 658, 180
-12, 642, 658, 180
-13, 544, 658, 180
-14, 447, 658, 180
-15, 339, 658, 180
-16, 265, 658, 180
-17, 465, 705, 180
-18, 355, 705, 180
-19, 75, 220, -90
-20, 340, 705, 180
-21, 70, 64, 0
-22, 610, 64, 0
-23, 915, 250, 90
-24, 915, 540, 90
-25, 265, 658, 180
-26, 78, 483, -90
-27, 78, 244, -90
-28, 120.839, 100.497, -46
+1, 80, 65, 0
+2, 225, 65, 0
+3, 370, 65, 0
+4, 515, 170, 90
+5, 620, 338, 0
+6, 800, 338, 0
+7, 913, 475, 90
+9, 750, 658, 180
+11, 545, 658, 180
+12, 450, 658, 180
+13, 348, 658, 180
+14, 270, 658, 180
+15, 160, 648, -160
+16, 130, 630, -140
+17, 101, 598, -120
+18, 89, 571, -100
+19, 81, 480, -90
+20, 81, 240, -90
+21, 610, 65, 0
+22, 750, 65, 0
+24, 913, 230, 90
+25, 350, 385, 180
+26, 125, 490, 90
+27, 350, 612, 0
+28, 610, 612, 0
+29, 868, 500, -90
+30, 868, 230, -90
+31, 620, 113, 180
+32, 545, 707, 180
+33, 450, 707, 180
+34, 348, 707, 180

algorithm/globalmap/waypoints-set.txt

@@ -0,0 +1,31 @@
+1
+2
+3
+4
+5
+6
+7
+9
+11
+12
+13
+14
+15
+16
+17
+18
+19
+20
+21
+22
+24
+25
+26
+27
+28
+29
+30
+31
+32
+33
+34

algorithm/globalmap/waypoints.txt

@@ -1,7 +1,11 @@
 1
 2
+3
 4
+5
 6
 7
 9
-16
+14
+19
+20

algorithm/lib/controller/pure_pursuit.hpp

@@ -0,0 +1,64 @@
+#pragma once
+
+#include <array>
+#include <vector>
+#include <cmath>
+#include <iostream>
+#include <unistd.h>
+
+class PurePursuit {
+
+public:
+    PurePursuit(std::vector<std::vector<double>> route, double speed, std::vector<double> cpoint, double WB, double Kdd, double Ldc);
+
+    void purepursuit_control();
+
+    std::vector<std::vector<double>> getTrajectory();
+    std::vector<int> getTargetnode();
+
+private:
+    // Vehicle State
+    double x;
+    double y;
+    double yaw;
+    double v;
+    double acceleration;
+
+    double rear_x;
+    double rear_y;
+    double WB;
+
+    // PID 
+    double pid_integral;
+    double previous_error;
+    double kp;
+    // double ki;
+    // double kd;
+
+    double dt;
+
+    // Target Node Searching
+    double Ld;
+    double Kdd;
+    double Ldc;
+
+    // purepursuit
+    double delta;
+
+    // Path
+    std::vector<std::vector<double>> route;
+    int route_size;
+
+    // Target Status
+    double target_speed;
+    int target_node;
+
+    std::vector<std::vector<double>> trajectory;
+    std::vector<int> target_nodes;
+
+    double pid_speed_control();
+    double purepursuit_steer_calc();
+    int update_target_node();
+    void update_state();
+    double calc_distance(double point_x, double point_y);
+};

algorithm/lib/hybrid-a-star/.gitmodules

@@ -0,0 +1,3 @@
+[submodule "footprint-collision-checker"]
+	path = footprint-collision-checker
+	url = https://github.com/bmegli/footprint-collision-checker.git

algorithm/lib/hybrid-a-star/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.1)
+
+set (CMAKE_CXX_STANDARD 14)
+
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release)
+endif()
+
+set(CMAKE_CXX_FLAGS "-Wall -Wextra")
+set(CMAKE_CXX_FLAGS_DEBUG "-g")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3")
+
+# For Ubuntu 20.04 or custom OpenCV build set appropriate path
+# set (OpenCV_DIR /usr/lib/x86_64-linux-gnu/cmake/opencv4/)
+
+find_package( OpenCV REQUIRED )
+find_package (Eigen3 3.3 REQUIRED NO_MODULE)
+
+include_directories( ${OpenCV_INCLUDE_DIRS} )
+
+project(
+    hybrid-a-star
+)
+
+# those are our main targets
+add_executable(simple-example node_se2.cpp examples/simple_example.cpp)
+target_link_libraries(simple-example ompl ${OpenCV_LIBS} Eigen3::Eigen)
+
+add_executable(gui-example node_se2.cpp examples/gui_example.cpp)
+target_link_libraries(gui-example ompl ceres ${OpenCV_LIBS} Eigen3::Eigen)
+

algorithm/lib/hybrid-a-star/README.md

@@ -0,0 +1,79 @@
+# Hybrid A*
+
+C++ Hybrid A*
+- rewritten from [ROS2 navigation2](https://github.com/ros-planning/navigation2/tree/378d53435856f4b3377fc5031b7118e4554410e5) stack
+- with refactored out ROS2 dependencies
+
+Implementation is mostly based on 2020 [nav2_smac_planner](https://github.com/ros-planning/navigation2/tree/main/nav2_smac_planner) by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). This is also the source for high level information about the algorithm.
+
+## State
+
+Functional:
+- Hybrid A* already usable
+- Smoother already usable
+
+Note:
+- interface and implementation subject to change
+- CMake build working but preliminary
+
+As far as I remember implementation (from original) throws exception if there is no possible path.
+This exception is not caught in examples.
+
+The code was refactored out of ROS2 in 2021 and original was probably updated since.
+
+## Dependencies
+
+- ompl
+- eigen
+- ceres
+- opencv (only for GUI example)
+
+Tested with system libraries on Ubuntu 18.04 and 20.04
+
+## Building Instructions
+
+```bash
+sudo apt-get install libompl-dev libeigen3-dev libceres-dev libopencv-dev
+# don't forget recursive, library uses submodules
+git clone --recursive https://github.com/bmegli/hybrid-a-star.git
+cd hybrid-a-star
+mkdir build
+cd build
+cmake ..
+make
+```
+
+For Ubuntu 20.04 or custom OpenCV build set OpenCV path in CMakeLists.txt before `cmake ..`
+
+## Running Examples
+
+```bash
+./simple-example
+```
+
+```bash
+./gui-example
+```
+
+Use:
+- left mouse button -> start position
+- right mouse button -> goal position
+- any key -> plan
+- esc -> quit
+
+
+Optionally enable smoother in `gui_example.cpp` `main` and recompile.
+
+## Using
+
+See examples for now.
+
+## License
+
+A mix of various open sources licenses, mainly:
+- Apache 2.0
+- BSD 2.0
+- BSD
+
+See individual files.
+