#3 Make path for control

.gitignore

@@ -0,0 +1,5 @@
+build
+.vscode
+.DStore
+log
+

algorithm/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(PathPlanning VERSION 1.0 LANGUAGES CXX)
+
+# Set the C++ standard to C++20
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g")
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address")
+# set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} -fsanitize=address")
+
+
+# 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
+include_directories(
+    ${Python3_INCLUDE_DIRS}
+    ${Python3_NumPy_INCLUDE_DIRS}  # This may not be set if PythonLibs was used
+    ${CMAKE_SOURCE_DIR}
+    planner
+    utils
+    visual
+)
+
+# Add executable with all relevant source files
+add_executable(${PROJECT_NAME} 
+    main.cpp
+)
+
+# Link with Python libraries
+target_link_libraries(${PROJECT_NAME}
+    ${Python3_LIBRARIES}
+)

algorithm/README.md

@@ -0,0 +1 @@
+HELLOWORLD

algorithm/globalmap/drawmap.py

@@ -0,0 +1,33 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+# color mapping for each element
+number_to_color = {
+    0: (1.0, 1.0, 1.0),  # Driveway
+    1: (37/255, 37/255, 0),  # Solid Line
+    2: (255/255, 110/255, 255/255),  # Parking Spot
+    3: (218/255, 218/255, 255/255),  # Dotted Line
+    4: (0, 145/255, 0),  # Stop Line
+    5: (0, 109/255, 109/255),  # Crosswalk
+    6: (255/255, 146/255, 146/255),  # Roundabout
+}
+
+def plot_map_from_file(file_path):
+    with open(file_path, 'r') as file:
+        map_data = [[int(num) for num in line.split()] for line in file]
+
+    map_array = np.array(map_data)
+
+    color_array = np.zeros((map_array.shape[0], map_array.shape[1], 3))
+
+    for number, color in number_to_color.items():
+        color_array[map_array == number] = color
+
+    plt.imshow(color_array, interpolation='nearest')
+    plt.axis('off')
+    plt.show()
+
+
+# File Path
+file_path = "flipped-track.txt"
+plot_map_from_file(file_path)

algorithm/globalmap/index-map.py

@@ -0,0 +1,26 @@
+import numpy as np
+from PIL import Image
+
+color_mapping = {
+    (0, 0, 0, 255): 0
+}
+
+def map_colors_to_numbers(image_path, output_file):
+    img = Image.open(image_path)
+    img_data = np.array(img)
+    print(img_data.shape)
+
+    result = np.zeros(img_data.shape[:2], dtype=int)
+
+    for y in range(img_data.shape[0]):
+        for x in range(img_data.shape[1]):
+            pixel = tuple(img_data[y, x])
+            result[y, x] = color_mapping.get(pixel, 1)
+
+    with open(output_file, 'w') as file:
+        for row in result:
+            file.write(' '.join(map(str, row)) + '\n')
+
+image_path = "test-track.bmp"
+output_file = "test-track.txt"
+map_colors_to_numbers(image_path, output_file)

algorithm/globalmap/parsinginfo.txt

@@ -0,0 +1,28 @@
+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

algorithm/globalmap/test-parsinginfo.txt

@@ -0,0 +1,2 @@
+1, 10, 10
+2, 99, 49

algorithm/globalmap/test-waypoints.txt

@@ -0,0 +1,2 @@
+1
+2

algorithm/globalmap/waypoints.txt

@@ -0,0 +1,7 @@
+1
+2
+4
+6
+7
+9
+16

algorithm/main.cpp

@@ -0,0 +1,83 @@
+#include <utils/map.h>
+#include <utils/loader.h>
+#include <visual/matplotlibcpp.h>
+
+#include <planner/bfs.h>
+#include <planner/a_star.h>
+
+#include <iostream>
+#include <cmath>
+#include <fstream>
+#include <sstream>
+
+namespace plt = matplotlibcpp;
+
+int main() {
+    // Load Waypoints Info
+    std::string nodes_file_path = "../globalmap/parsinginfo.txt";
+    std::string waypoints_file_path = "../globalmap/waypoints.txt";
+    std::vector<std::vector<double>> waypoints = load_waypoints(nodes_file_path, waypoints_file_path);
+
+    // Make Map Instruction
+    std::string map_file_path = "../globalmap/flipped-track.txt";
+    Map map = Map(map_file_path);
+
+    // Path Planner (Using same map)
+    // auto planner = BFS(map);
+    auto planner = A_Star(map);
+    planner.plan_with_waypoints(waypoints);
+    std::vector<std::vector<int>> route = planner.get_waypoints_path();
+
+    // Only for drawing
+    std::cout << "Draw Map" << std::endl;
+    std::map<int, float> structure_color_map = {
+        {0, 1.0}, // Driveway
+        {1, 0.5}, // Solid Line
+        {2, 0.3}, // Parking Spot
+        {3, 0.4}, // Dotted Line
+        {4, 0.5}, // Stop Line
+        {5, 0.6}, // Crosswalk
+        {6, 0.7}, // Roundabout
+        {10, 0.9}, // Extra
+    };
+
+    // Draw map info
+    std::vector<std::vector<int>> draw_grid_map = map.get_grid_map();
+    std::vector<int> draw_map_info = map.get_map_info();
+
+    int map_width = draw_map_info[0];
+    int map_height = draw_map_info[1];
+
+    std::vector<float> image_data(map_width * map_height);
+
+    for(int x=0; x<map_width; ++x) {
+        for(int y=0; y<map_height; ++y) {
+            int structure = draw_grid_map[x][y];
+            int index = y * map_width + x;
+            image_data[index] = structure_color_map[structure];
+        }
+    }
+
+    plt::imshow(&image_data[0], map_height, map_width, 1);
+
+    // Draw global path : reverse mode
+    for (size_t step = 0; step < route.size(); step += 5) {
+        std::vector<int> path_x;
+        std::vector<int> path_y;
+
+        for (size_t i = 0; i <= step; ++i) {
+            path_x.push_back(route[i][0]);
+            path_y.push_back(route[i][1]);
+        }
+
+        plt::plot(path_x, path_y, "r-");
+        plt::pause(0.01);
+        plt::clf();
+
+        plt::imshow(&image_data[0], map_height, map_width, 1);
+    }
+
+    plt::show();
+
+    return 0;
+}

algorithm/planner/a_star.h

@@ -0,0 +1,94 @@
+#pragma once
+
+#include <planner/planner.h>
+
+#include <queue>
+#include <functional>
+#include <cmath>
+
+
+class A_Star : public Planner {
+public:
+    A_Star(Map& map);
+    ~A_Star();
+
+    virtual bool path_planning(std::vector<int> start_point, std::vector<int> end_point);
+    virtual void save_planned_path(std::vector<int> start_point, std::vector<int> end_point);
+
+private:
+    int distance(std::vector<int> point1, std::vector<int> point2);
+    int heuristic(std::vector<int> point, std::vector<int> end_point);
+};
+
+A_Star::A_Star(Map& map) : Planner(map) {}
+
+A_Star::~A_Star() {}
+
+bool A_Star::path_planning(std::vector<int> start_point, std::vector<int> end_point) {
+    int count = 0;
+
+    // Priority Queue for A* (cost & point)
+    std::priority_queue<std::pair<int, std::vector<int>>, std::vector<std::pair<int, std::vector<int>>>, std::greater<std::pair<int, std::vector<int>>>> minimum_queue;
+
+    // Validity Check
+    if (map.is_blocked(start_point)) {return false;}
+    if (map.is_blocked(end_point)) {return false;}
+
+    // Initial Process
+    minimum_queue.push(std::make_pair(this->heuristic(start_point, end_point), start_point));
+    this->set_visit_and_prev(start_point, {0, 0});
+
+    std::pair<int, std::vector<int>> old_cost_point;
+    std::vector<int> old_point;
+    int old_cost;
+    std::pair<int, std::vector<int>> new_cost_point;
+    std::vector<int> new_point;
+    int new_cost;
+
+    // For Test
+    std::vector<std::vector<int>> new_points = {{-1,1},{0,1},{1,1},{-1,0},{1,0},{-1,-1},{0,-1},{1,-1}};
+    while (!minimum_queue.empty()) {
+        count += 1;
+
+        old_cost = minimum_queue.top().first;
+        old_point = minimum_queue.top().second;
+        minimum_queue.pop();
+        // std::cout<<old_point[0]<<"  "<<old_point[1]<<std::endl;
+        for (int i = 0; i < 8; ++i) {
+            new_point = {old_point[0]+new_points[i][0], old_point[1]+new_points[i][1]};
+            if ((map.is_blocked(new_point)) || (this->is_visited(new_point))) {
+                continue;
+            }
+            new_cost = this->distance(new_point, old_point) + this->heuristic(new_point, end_point) + old_cost;
+            minimum_queue.push(std::make_pair(new_cost, new_point));
+            this->set_visit_and_prev(old_point, new_point);
+
+            if (new_point == end_point) {return true;}
+        }
+    }
+    return false;
+}
+
+void A_Star::save_planned_path(std::vector<int> start_point, std::vector<int> end_point) {
+    // std::cout<<"get_route"<<std::endl;
+    std::vector<int> now_point = end_point;
+
+    while (now_point != start_point) {
+        // std::cout<<"x = "<<now_point[0]<<" y = "<<now_point[1]<<std::endl;
+        now_point = this->get_prev_point(now_point);
+        this->iteration_path.push_front(now_point);
+    }
+}
+
+int A_Star::distance(std::vector<int> point1, std::vector<int> point2) {
+    int distance = std::sqrt((point1[0] - point2[0])*(point1[0] - point2[0]) + (point1[1] - point2[1])*(point1[1] - point2[1]));
+    // std::cout<<distance<<std::endl;
+    return distance;
+}
+
+
+int A_Star::heuristic(std::vector<int> point, std::vector<int> end_point) {
+    int heuristic = std::sqrt((end_point[0] - point[0])*(end_point[0] - point[0]) + (end_point[1] - point[1])*(end_point[1] - point[1]));
+    // std::cout<<heuristic<<std::endl;
+    return heuristic;
+}