diff --git a/ASearch.pro b/ASearch.pro deleted file mode 100644 index da136e2..0000000 --- a/ASearch.pro +++ /dev/null @@ -1,48 +0,0 @@ -#------------------------------------------------- -# -# Project created by QtCreator 2011-02-26T12:08:02 -# -#------------------------------------------------- - -TARGET = ASearch -CONFIG += console -CONFIG -= app_bundle -TEMPLATE = app -QMAKE_CXXFLAGS += -std=c++11 -O2 -Wall -Wextra - -win32 { -QMAKE_LFLAGS += -static -static-libgcc -static-libstdc++ -} - -SOURCES += \ - tinyxml2.cpp \ - xmllogger.cpp \ - isearch.cpp \ - mission.cpp \ - map.cpp \ - dijkstra.cpp \ - config.cpp \ - bfs.cpp \ - astar.cpp \ - asearch.cpp \ - jp_search.cpp \ - theta.cpp \ - environmentoptions.cpp - -HEADERS += \ - tinyxml2.h \ - node.h \ - gl_const.h \ - xmllogger.h \ - isearch.h \ - mission.h \ - map.h \ - ilogger.h \ - dijkstra.h \ - config.h \ - bfs.h \ - astar.h \ - searchresult.h \ - jp_search.h \ - theta.h \ - environmentoptions.h diff --git a/CMakeLists-StaticBuild.txt b/CMakeLists-StaticBuild.txt deleted file mode 100644 index 3684e13..0000000 --- a/CMakeLists-StaticBuild.txt +++ /dev/null @@ -1,35 +0,0 @@ -cmake_minimum_required(VERSION 2.8) -project(AStar-JPS-ThetaStar) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O2 -Wall -Wextra -static -static-libgcc -static-libstdc++") -set(SOURCE_FILES - asearch.cpp - bfs.cpp - config.h - environmentoptions.cpp - ilogger.h - jp_search.cpp - mission.cpp - searchresult.h - xmllogger.cpp - astar.cpp - bfs.h - dijkstra.cpp - environmentoptions.h - isearch.cpp - jp_search.h - map.cpp - mission.h - theta.cpp - xmllogger.h - astar.h - config.cpp - dijkstra.h - gl_const.h - isearch.h - map.h - node.h - theta.h - tinyxml2.cpp - tinyxml2.h - ) -add_executable(AStar-JPS-ThetaStar ${SOURCE_FILES}) \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index f9148e1..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,37 +0,0 @@ -cmake_minimum_required(VERSION 2.8) -project(AStar-JPS-ThetaStar) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O2 -Wall -Wextra") -set(SOURCE_FILES - asearch.cpp - bfs.cpp - config.h - environmentoptions.cpp - ilogger.h - jp_search.cpp - mission.cpp - searchresult.h - xmllogger.cpp - astar.cpp - bfs.h - dijkstra.cpp - environmentoptions.h - isearch.cpp - jp_search.h - map.cpp - mission.h - theta.cpp - xmllogger.h - astar.h - config.cpp - dijkstra.h - gl_const.h - isearch.h - map.h - node.h - theta.h - tinyxml2.cpp - tinyxml2.h - path_smoothing.h - path_smoothing.cpp - ) -add_executable(AStar-JPS-ThetaStar ${SOURCE_FILES}) \ No newline at end of file diff --git a/README.md b/README.md index ae04534..9f64bab 100644 --- a/README.md +++ b/README.md @@ -1,55 +1,123 @@ -# AStar-JPS-ThetaStar +# AStar-ThetaStar Basic algorithms for single-shot grid-based 2D path finding. Single-shot means that the algorithms are tailored to answering a single pathfinding query (as opposed to other pathfinders that are suited to solving sequences of alike queries, e.g. queries on the same map but with different start and goal locations). -Grid-based means that a (regular square) grid is an essential part of the input. We follow center-based assumption which implies that distinct agent's locations are tied to the centers of the traversable cells (not to the corners). +Grid-based means that a (regular square) grid is an essential part of the input. We follow center-based assumption which implies that distinct agent's locations are tied to the centers of the traversable cells (not to the corners). Description ========== This project contains implementations of the following algorithms: -- Breadth First Search -- Dijkstra's algorithm - A* - Theta* -- Jump Point Search (JPS) - -Build and Launch -================ -To build the project you can use QMake (part of QT toolchain) or CMake. Both .pro and CMakeLists files are available in the repo. -Please note that the code relies on C++11 standart. Make sure that your compiler supports it. -The project does not use any external libraries (except tinyXML which is linked at the source level) and is meant to be cross-platform. - -To launch the compiled file and get a result you need to pass a correct input XML-file (see below) as the first (command line) argument. This file encodes all the information about path finding instance (map, start, goal, algorithm's options etc.). - -If the input is correct you'll see the result in the console. Detailed XML log-file will also be created by default. If you choose not to create output XML you can alter the input XML following the instructions given below. - -Input and Output files -====================== -Both files are XML files of a specific structure. -Input file should contain: ->- Mandatory tag \. It describes the environment. - * **\** and **\** - mandatory tags that define size of the map. Origin is in the upper left corner. (0,0) - is upper left, (*width*-1, *height*-1) is lower right. - * **\** and **\** - mandatory tags that define horizontal (X) and vertical (Y) offset of the start location from the upper left corner. Legal values for *startx* are [0, .., *width*-1], for *starty* - [0, .., *height*-1]. - * **\** and **\** - mandatory tags that define horizontal (X) and vertical (Y) offset of the goal location. - * **\** - mandatory tag that describes the square grid constituting the map. It consists of **\** tags. Each **\** contains a sequence of "0" and "1" separated by blanks. "0" stands for traversable cell, "1" - for untraversable (actually any other figure but "0" can be used instead of "1"). - * **\** - optional tag that defines the size of one cell. One might add it to calculate scaled length of the path. - * **\**, **\<URL>**, **\<coordinates>**, etc - optional tags containing additional information on the map. ->- Mandatory tag <b>\<algorithm></b>. It describes the parameters of the algorithm. - * **\<searchtype>** - mandatory tag that defines which algorithm will be used for pathfinding. Possible values - "astar", "theta", "jp_search", "bfs", "dijkstra". - * **\<metrictype>** - defines the type of metric for heuristic function. Possible values - "euclid", "diagonal", "manhattan", "chebyshev". Default value is "euclid". Please note that the lenghs of the actual moves (not the heuristic to goal) are always counted with Euclid metrics, e.g. the length of the move between two cardinally adjacent cells is 1, the length of the move between two diagonally adjacent cells is sqrt(2) etc. - * **\<hweight>** - defines the weight of the heuristic function. Should be real number greater or equal 1. Default value is "1". - * **\<breakingties>** - defines the priority in OPEN list for nodes with equal f-values. Possible values - "g-min" (break ties in favor of the node with smaller g-value), "g-max" (break ties in favor of the node with greater g-value). Default value is "g-max". - * **\<allowdiagonal>** - boolean tag that allows/prohibits diagonal moves. Default value is "true". Setting it to "false" restricts the agent to make cardinal (horizonal, vertical) moves only. If Theta* algorithm is used only cardinal successors will be generated during expansion of the current node, but after resetting parent the resultant move will probably violate this restriction. - * **\<cutcorners>** - boolean tag that defines the possibilty to make diagonal moves when one adjacent cell is untraversable. The tag is ignored if diagonal moves are not allowed. Default value is "false". - * **\<allowsqueeze>** - boolean tag that defines the possibility to make diagonal moves when both adjacent cells are untraversable. The tag is ignored if cutting corners is not allowed. Default value is "false". - * **\<postsmoothing>** - boolean tag that allows to start the path smoothing procedure after pathfinding was ended. The tag is ignored if Theta* algorithm was chosen. Default value is "false". ->- Optional tag <b>\<options></b>. Options that are not related to search. - * **\<loglevel>** - defines the level of detalization of log-file. Default value is "1". Possible values: - * "0" or "none" - log-file is not created. - * "0.5" or "tiny" - All the input data is copied to the log-file plus short **\<summary>** is appended. **\<summary>** contains info of the path length, number of steps, elapsed time, etc. - * "1" or "short" - *0.5*-log plus **\<path>** is appended. It looks like **\<grid>** but cells forming the path are marked by "\*" instead of "0". The following tags are also appended: **\<hplevel>** and **\<lplevel>**. **\<lplevel>** is the sequence of coordinates of cells forming the path (in case Theta* planner is used, this sequence is formed at post-processing step by invoking sequantually line-of-sight procedure on path's segments). **\<hplevel>** is the sequence of sections forming the path (in case planner other from Theta* is used, sections are formed at post-processing step using naive procedure). - * "1.5" or "medium" - *1*-log plus the information (explicit enumeration) on last iteration's OPEN and CLOSE lists. - * "2" or "full" - *1*-log plus OPEN and CLOSE lists are written into the log-file after each step of the algorithm. Can make log-files really huge. - * **\<logpath>** - defines the directory where the log-file should be written. If not specified directory of the input file is used. - * **\<logname>** - defines the name of log-file. If not specified the name of the log file is: "input file name"+"_log"+input file extension. + +Python Wrapper +============== +This project also includes a Python wrapper allowing you to use the A* and Theta* path planning functionalities from Python. + +Prerequisites +------------- +- **CMake** (version 3.1 or higher recommended) +- A C++ compiler that supports C++11 (e.g., GCC, Clang, MSVC) +- **Python** (version 3.6 or higher recommended) +- **pybind11**: This will be used for creating the Python bindings. You can install it via pip: + ```bash + pip install pybind11 + ``` +- **setuptools** (usually comes with Python/pip): + ```bash + pip install setuptools + ``` + +Building the Python Module +-------------------------- +The Python module `ThetaStarPlanner` is built using CMake. + +1. **Navigate to the project root directory** (the one containing this README). +2. **Create a build directory and navigate into it:** + ```bash + mkdir build + cd build + ``` +3. **Run CMake to configure the project:** + * On Linux/macOS: + ```bash + cmake ../src + ``` + * On Windows (if using Visual Studio, you might need to specify a generator, e.g., `cmake ../src -G "Visual Studio 16 2019" -A x64`). Simpler for command line (like MinGW): + ```bash + cmake ../src + ``` + This command tells CMake to look for the `CMakeLists.txt` in the `../src` directory. +4. **Compile the project:** + * On Linux/macOS: + ```bash + make + ``` + * On Windows (with MSVC): + ```bash + cmake --build . --config Release + ``` + (Or open the generated solution file in Visual Studio and build). + If using MinGW makefiles: + ```bash + mingw32-make + ``` + + After a successful build, you should find the Python module file (e.g., `ThetaStarPlanner.cpython-38-x86_64-linux-gnu.so` on Linux, `ThetaStarPlanner.cp38-win_amd64.pyd` on Windows) inside the `build` directory (or a subdirectory like `build/Release` depending on your CMake generator and build type). + +Running the Example +------------------- +An example Python script `example.py` is provided in the root directory. + +1. **Ensure the Python module is built** as described above. +2. **Run the example script from the project root directory:** + ```bash + python example.py + ``` + The `example.py` script attempts to locate the compiled module in common build directory locations (e.g., `build/`, `build/Release/`). + + If you encounter an `ImportError`, ensure that: + * The module (e.g., `planner_cpp...so` or `planner_cpp...pyd`) exists in your build directory. + * The build directory is in your `PYTHONPATH` environment variable, or you are running the script in a way that Python can find the module (the script tries to add the build directory to `sys.path` automatically, but this might not cover all build configurations). For example, you can copy the `.so`/`.pyd` file from the build directory to the project root directory next to `example.py`. + +Using the Wrapper in Your Own Python Code +----------------------------------------- +1. Make sure the compiled `planner_cpp` module is in your Python path. +2. Import the module and use the `plan_2d` function: + +```python +import ThetaStarPlanner # Or ensure the .so/.pyd file is in your PYTHONPATH + +origin = [0.0, 0.0] +dim = [100, 100] # height, width +# Map data: flat list, row-major, 0=free, 1=obstacle +map_data = [0] * (dim[0] * dim[1]) +# Populate map_data with obstacles as needed, e.g.: +# map_data[row * dim[1] + col] = 1 + +start_coords = [0.5, 0.5] # meters +goal_coords = [9.5, 9.5] # meters +map_resolution = 0.1 # meters/cell +use_theta_star = False # False for A*, True for Theta* + +status, path, time_ms = planner_cpp.plan_2d( + origin, + dim, + map_data, + start_coords, + goal_coords, + map_resolution, + use_theta_star +) + +if status == 0: + print(f"Path found: {path}") + print(f"Time taken: {time_ms} ms") +else: + print("Failed to find path.") + +``` +The `map_data` list should contain integers that can be safely converted to `signed char` by the C++ backend (typically values like 0 for free, 1 for obstacle). +The path is returned as a list of `[x, y]` coordinate pairs in meters. +Time spent is returned in milliseconds. diff --git a/asearch.cpp b/asearch.cpp deleted file mode 100644 index 8b6490a..0000000 --- a/asearch.cpp +++ /dev/null @@ -1,45 +0,0 @@ -#include "mission.h" - -int main(int argc, char* argv[]) -{ - if(argc < 2) { - std::cout<<"Error! Pathfinding task file (XML) is not specified!"<<std::endl; - return 0; - } - - Mission mission(argv[1]); - - std::cout<<argv[1]<<std::endl; - std::cout<<"Parsing the map from XML:"<<std::endl; - - if(!mission.getMap()) { - std::cout<<"Incorrect map! Program halted!"<<std::endl; - } - else { - std::cout<<"Map OK!"<<std::endl<<"Parsing configurations (algorithm, log) from XML:"<<std::endl; - if(!mission.getConfig()) - std::cout<<"Incorrect configurations! Program halted!"<<std::endl; - else { - std::cout<<"Configurations OK!"<<std::endl<<"Creating log channel:"<<std::endl; - - if(!mission.createLog()) - std::cout<<"Log chanel has not been created! Program halted!"<<std::endl; - else { - std::cout<<"Log OK!"<<std::endl<<"Start searching the path:"<<std::endl; - - mission.createEnvironmentOptions(); - mission.createSearch(); - mission.startSearch(); - - std::cout<<"Search is finished!"<<std::endl; - - mission.printSearchResultsToConsole(); - mission.saveSearchResultsToLog(); - - std::cout<<"Results are saved (if chosen) via created log channel."<<std::endl; - } - } - } - return 0; -} - diff --git a/bfs.cpp b/bfs.cpp deleted file mode 100644 index 0ae8174..0000000 --- a/bfs.cpp +++ /dev/null @@ -1,31 +0,0 @@ -#include "bfs.h" - -BFS::BFS() -{ - breakingties = CN_SP_BT_GMIN; -} - -double BFS::computeHFromCellToCell(int start_i, int start_j, int fin_i, int fin_j, const EnvironmentOptions &options) -{ - return 0; -} - -void BFS::addOpen(Node newNode) -{ - std::list<Node>::iterator iter=open[newNode.i].begin(); - - while(iter != open[newNode.i].end() && newNode.j != iter->j) - ++iter; - - if(iter != open[newNode.i].end()) { - if(iter->g > newNode.g) { - open[newNode.i].erase(iter); - openSize--; - } - else - return; - } - openSize++; - open[newNode.i].push_back(newNode); - return; -} diff --git a/bfs.h b/bfs.h deleted file mode 100644 index 6a5643b..0000000 --- a/bfs.h +++ /dev/null @@ -1,14 +0,0 @@ -#ifndef BFS_H -#define BFS_H -#include "isearch.h" - -class BFS : public ISearch -{ - public: - BFS(); - - void addOpen(Node newNode); - double computeHFromCellToCell(int start_i, int start_j, int fin_i, int fin_j, const EnvironmentOptions &options); -}; - -#endif diff --git a/config.h b/config.h deleted file mode 100644 index 5f2b75d..0000000 --- a/config.h +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef CONFIG_H -#define CONFIG_H -#include <string> - -class Config -{ - public: - Config(); - Config(const Config& orig); - ~Config(); - bool getConfig(const char *FileName); - - public: - double* SearchParams; - std::string* LogParams; - unsigned int N; - -}; - -#endif - diff --git a/dijkstra.cpp b/dijkstra.cpp deleted file mode 100644 index 62d329e..0000000 --- a/dijkstra.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include "dijkstra.h" - -Dijkstra::Dijkstra() : Astar(0, CN_SP_BT_GMAX) {} - -double Dijkstra::computeHFromCellToCell(int start_i, int start_j, int fin_i, int fin_j, const EnvironmentOptions &options) -{ - return 0; -} - diff --git a/dijkstra.h b/dijkstra.h deleted file mode 100644 index 0e04d44..0000000 --- a/dijkstra.h +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef DIJKSTRA_H -#define DIJKSTRA_H -#include "astar.h" - -class Dijkstra : public Astar -{ - public: - Dijkstra(); - - double computeHFromCellToCell(int start_i, int start_j, int fin_i, int fin_j, const EnvironmentOptions &options); -}; -#endif diff --git a/example.py b/example.py new file mode 100644 index 0000000..03c1c8b --- /dev/null +++ b/example.py @@ -0,0 +1,118 @@ +import sys +import os + +# Add the build directory to the Python path so it can find planner_cpp. +# This assumes the script is run from the root of the project and the build +# directory is 'build' or 'build/Debug' or 'build/Release' etc. +# A more robust solution for distribution would be a proper setup.py install. + +# Try to find the module in common build directory locations +module_found = False +build_dir_candidates = [ + 'build', + 'build/Debug', + 'build/Release', + 'build/RelWithDebInfo', + 'build/MinSizeRel', + # For multi-config generators like Visual Studio, pybind11 often places + # the .pyd file in a subdirectory matching the configuration within the + # main Python extension directory (e.g., build/lib.win-amd64-3.8/Debug) + # This simple path addition might not be enough for all CMake generators/platforms. + # Users might need to set PYTHONPATH or copy the module manually. +] + +# Add current directory in case the module is placed there by some build systems +# or for manual placement. +sys.path.insert(0, os.path.abspath(".")) + + +for candidate_dir in build_dir_candidates: + # Construct path relative to this script's location (project root) + abs_candidate_dir = os.path.abspath(candidate_dir) + if os.path.isdir(abs_candidate_dir): + sys.path.insert(0, abs_candidate_dir) + # Also check common pybind11 output subdirectories (e.g., for Windows) + # Example: build/Debug/planner_cpp.pyd or build/planner_cpp.pyd + # pybind11 might also place it in a subfolder like 'Release' inside the build dir. + # This is a simplified search. + +try: + import ThetaStarPlanner + module_found = True +except ImportError as e: + print(f"Error importing ThetaStarPlanner: {e}") + print("Please ensure that ThetaStarPlanner module is built and either:") + print("1. The script is run from a directory where Python can find it (e.g., build directory after CMake).") + print("2. The build directory (e.g., 'build' or 'build/Release') is in your PYTHONPATH.") + print(f"Current sys.path: {sys.path}") + exit(1) + +def main(): + print("Successfully imported ThetaStarPlanner module.") + + # Example Usage + origin = [0.0, 0.0] # Map origin (meters) + dim = [10, 10] # Map dimensions [height, width] (cells) + resolution = 0.1 # Map resolution (meters/cell) + + # Create a simple map: 10x10 grid + # 0 = free, 1 = obstacle + # Flattened row-major order + map_data = [0] * (dim[0] * dim[1]) + + # Add some obstacles + # map_data[index] where index = row * width + col + # Example: place an obstacle at (row=1, col=2) -> 1 * 10 + 2 = 12 + if dim[0] > 1 and dim[1] > 2: map_data[1 * dim[1] + 2] = 100 + if dim[0] > 2 and dim[1] > 2: map_data[2 * dim[1] + 2] = 100 + if dim[0] > 3 and dim[1] > 2: map_data[3 * dim[1] + 2] = 100 + if dim[0] > 4 and dim[1] > 2: map_data[4 * dim[1] + 2] = 100 + if dim[0] > 5 and dim[1] > 2: map_data[5 * dim[1] + 2] = 100 # Wall + + # Start and Goal in meters + start_m = [0.05, 0.05] # Cell (0,0) + goal_m = [0.85, 0.85] # Cell (8,8) + + print(f"Map Origin: {origin}") + print(f"Map Dimensions (cells): {dim}") + print(f"Map Resolution: {resolution} m/cell") + print(f"Start (meters): {start_m}") + print(f"Goal (meters): {goal_m}") + # print(f"Map Data (first 20 elements): {map_data[:20]}...") # Uncomment to see part of the map + + # Plan using A* + print("\nPlanning with A*...") + use_theta_star_false = False + status_astar, path_astar, time_astar = ThetaStarPlanner.plan_2d( + origin, dim, map_data, start_m, goal_m, resolution, use_theta_star_false + ) + + if status_astar == 0: + print("A* Path Found Successfully!") + print(f" Path: {path_astar}") + print(f" Time spent: {time_astar:.4f} ms") + else: + print("A* Failed to find a path.") + print(f" Status code: {status_astar}") + print(f" Time spent: {time_astar:.4f} ms") + + + # Plan using Theta* + print("\nPlanning with Theta*...") + use_theta_star_true = True + status_theta, path_theta, time_theta = ThetaStarPlanner.plan_2d( + origin, dim, map_data, start_m, goal_m, resolution, use_theta_star_true + ) + + if status_theta == 0: + print("Theta* Path Found Successfully!") + print(f" Path: {path_theta}") + print(f" Time spent: {time_theta:.4f} ms") + else: + print("Theta* Failed to find a path.") + print(f" Status code: {status_theta}") + print(f" Time spent: {time_theta:.4f} ms") + + +if __name__ == "__main__": + main() diff --git a/example2.py b/example2.py new file mode 100644 index 0000000..3751047 --- /dev/null +++ b/example2.py @@ -0,0 +1,184 @@ +import sys +import os + +# Add the build directory to the Python path so it can find planner_cpp. +# This assumes the script is run from the root of the project and the build +# directory is 'build' or 'build/Debug' or 'build/Release' etc. +# A more robust solution for distribution would be a proper setup.py install. + +# Try to find the module in common build directory locations +module_found = False +build_dir_candidates = [ + 'build', + 'build/Debug', + 'build/Release', + 'build/RelWithDebInfo', + 'build/MinSizeRel', + # For multi-config generators like Visual Studio, pybind11 often places + # the .pyd file in a subdirectory matching the configuration within the + # main Python extension directory (e.g., build/lib.win-amd64-3.8/Debug) + # This simple path addition might not be enough for all CMake generators/platforms. + # Users might need to set PYTHONPATH or copy the module manually. +] + +# Add current directory in case the module is placed there by some build systems +# or for manual placement. +sys.path.insert(0, os.path.abspath(".")) + + +for candidate_dir in build_dir_candidates: + # Construct path relative to this script's location (project root) + abs_candidate_dir = os.path.abspath(candidate_dir) + if os.path.isdir(abs_candidate_dir): + sys.path.insert(0, abs_candidate_dir) + # Also check common pybind11 output subdirectories (e.g., for Windows) + # Example: build/Debug/planner_cpp.pyd or build/planner_cpp.pyd + # pybind11 might also place it in a subfolder like 'Release' inside the build dir. + # This is a simplified search. + +try: + import ThetaStarPlanner + module_found = True +except ImportError as e: + print(f"Error importing ThetaStarPlanner: {e}") + print("Please ensure that ThetaStarPlanner module is built and either:") + print("1. The script is run from a directory where Python can find it (e.g., build directory after CMake).") + print("2. The build directory (e.g., 'build' or 'build/Release') is in your PYTHONPATH.") + print(f"Current sys.path: {sys.path}") + exit(1) + +def main(): + import cv2 + import numpy as np + filename = "./data/image.png" + print(f"Loading image from {filename}...") + img = cv2.imread(filename, cv2.IMREAD_GRAYSCALE) # Load as grayscale + + # img = cv2.resize(img, (400, 300)) # Resize to 400x300 pixels for testing + + origin = [0,0] # x, y of the map origin + dim = [img.shape[1], img.shape[0]] # width, height in pixels + resolution = 1 # meters per pixel (assumed) + print(dim) + + map_data = [] + # transform image to map data, for each pixel, if the value is over 200, it is marked as 100 occipied, otherwise 0 free + for r in range(dim[1]): # rows (y) + row_data = [] + for c in range(dim[0]): # cols (x) + pixel_value = img[r, c] + if isinstance(pixel_value, np.ndarray): + # If pixel is multi-channel (e.g., RGB), take the first channel + pixel_value = pixel_value[0] + if pixel_value > 200: # Assuming a threshold for occupied + row_data.append(1) # Occupied + else: + row_data.append(0) + map_data.extend(row_data) + + # random pick start and goal points that are free and at least 1000 pixels apart + free_indices = [i for i, v in enumerate(map_data) if v == 0] + print(f"Number of free cells: {len(free_indices)}") + if len(free_indices) < 2: + print("Not enough free cells to pick start and goal points.") + return + import random + start_index = random.choice(free_indices) + goal_index = random.choice(free_indices) + while abs(start_index - goal_index) < 1000: # Ensure at least 1000 pixels apart + goal_index = random.choice(free_indices) + print(f"Start index: {start_index}, Goal index: {goal_index}") + print(f"Start cell value: {map_data[start_index]}, Goal cell value: {map_data[goal_index]}") + + # Swap start_w and goal_w to (row, col) if needed + start_w = [start_index % dim[0] * resolution, start_index // dim[0] * resolution] # [row, col] + goal_w = [goal_index % dim[0] * resolution, goal_index // dim[0] * resolution] # [row, col] + + print(f"Map Origin: {origin}") + print(f"Map Dimensions (cells): {dim}") + print(f"Map Resolution: {resolution}") + print(f"Start (world): {start_w}") + print(f"Goal (world): {goal_w}") + + print("Successfully imported ThetaStarPlanner module.") + + # Plan using A* + print("\nPlanning with A*...") + use_theta_star_false = False + print("Calling ThetaStarPlanner.plan_2d with:") + print(f" origin={origin}") + print(f" dim={dim}") + print(f" start_w={start_w}") + print(f" goal_w={goal_w}") + print(f" resolution={resolution}") + print(f" use_theta_star={use_theta_star_false}") + status_astar, path_astar, time_astar = ThetaStarPlanner.plan_2d( + origin, dim, map_data, start_w, goal_w, resolution, use_theta_star_false + ) + + if status_astar == 0: + print("A* Path Found Successfully!") + # print(f" Path: {path_astar}") + print(f" Time spent: {time_astar:.4f} ms") + else: + print("A* Failed to find a path.") + print(f" Status code: {status_astar}") + print(f" Time spent: {time_astar:.4f} ms") + + # Plan using Theta* + print("\nPlanning with Theta*...") + use_theta_star_true = True + print("Calling ThetaStarPlanner.plan_2d with:") + print(f" origin={origin}") + print(f" dim={dim}") + print(f" start_w={start_w}") + print(f" goal_w={goal_w}") + print(f" map value of start and goal: {map_data[start_w[1] * dim[0] + start_w[0]]}, {map_data[goal_w[1] * dim[0] + goal_w[0]]}") + print(f" resolution={resolution}") + print(f" use_theta_star={use_theta_star_true}") + status_theta, path_theta, time_theta = ThetaStarPlanner.plan_2d( + origin, dim, map_data, start_w, goal_w, resolution, use_theta_star_true + ) + + if status_theta == 0: + print("Theta* Path Found Successfully!") + # print(f" Path: {path_theta}") + print(f" Time spent: {time_theta:.4f} ms") + else: + print("Theta* Failed to find a path.") + print(f" Status code: {status_theta}") + print(f" Time spent: {time_theta:.4f} ms") + + # Visualization using matplotlib + import matplotlib.pyplot as plt + + img_color = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB) + fig, ax = plt.subplots(figsize=(10, 10)) + ax.imshow(img_color) + + # Draw start and goal + ax.scatter([start_w[0]], [start_w[1]], c='lime', s=80, marker='o', label='Start') + ax.scatter([goal_w[0]], [goal_w[1]], c='red', s=80, marker='o', label='Goal') + + # Draw A* path (blue) + if status_astar == 0 and path_astar: + path_astar_x = [pt[0] for pt in path_astar] + path_astar_y = [pt[1] for pt in path_astar] + ax.plot(path_astar_x, path_astar_y, color='blue', linewidth=2, label="A* Path") + + # Draw Theta* path (magenta) + if status_theta == 0 and path_theta: + path_theta_x = [pt[0] for pt in path_theta] + path_theta_y = [pt[1] for pt in path_theta] + ax.plot(path_theta_x, path_theta_y, color='magenta', linewidth=2, label="Theta* Path") + + ax.legend() + ax.set_title("Planned Path") + plt.axis('off') + plt.savefig("./data/planned_path.png", bbox_inches='tight', pad_inches=0.1) + print("Saved planned path visualization to ./data/planned_path.png") + plt.show() + + +if __name__ == "__main__": + main() diff --git a/examples/config.xml b/examples/config.xml new file mode 100644 index 0000000..021df8b --- /dev/null +++ b/examples/config.xml @@ -0,0 +1,13 @@ +<?xml version="1.0" encoding="UTF-8" ?> +<root> + <algorithm> + <searchtype>theta</searchtype> + <metrictype>euclid</metrictype> + <breakingties>g-max</breakingties> + <hweight>1</hweight> + <allowdiagonal>true</allowdiagonal> + <cutcorners>false</cutcorners> + <allowsqueeze>false</allowsqueeze> + <postsmoothing>true</postsmoothing> + </algorithm> +</root> diff --git a/examples/example_log.xml b/examples/example_log.xml index 813db7e..78d915c 100644 --- a/examples/example_log.xml +++ b/examples/example_log.xml @@ -38,21 +38,22 @@ </map> <algorithm> <searchtype>astar</searchtype> - <metrictype>diagonal</metrictype> + <metrictype>euclid</metrictype> <breakingties>g-max</breakingties> <hweight>1</hweight> <allowdiagonal>true</allowdiagonal> <cutcorners>false</cutcorners> <allowsqueeze>false</allowsqueeze> + <postsmoothing>true</postsmoothing> </algorithm> <options> <loglevel>1</loglevel> - <logpath /> - <logfilename /> + <logpath/> + <logfilename/> </options> <log> - <mapfilename>D:\GitHub\build-ASearch-Desktop_Qt_5_3_MinGW_32bit-Release\release\example.xml</mapfilename> - <summary numberofsteps="373" nodescreated="415" length="30.5563" length_scaled="733.352" time="0.000993" /> + <mapfilename>../../examples/example.xml</mapfilename> + <summary numberofsteps="383" nodescreated="425" length="29.81822" length_scaled="715.63728332519531" time="0.000296"/> <path> <row number="0">0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 </row> <row number="1">0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 </row> @@ -60,20 +61,20 @@ <row number="3">0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 </row> <row number="4">0 0 0 * * * * * * * * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 </row> <row number="5">0 0 0 * 1 1 1 1 1 1 0 * 0 0 0 0 1 1 1 1 1 1 1 0 0 </row> - <row number="6">0 0 0 * 1 1 1 1 1 1 0 0 * 0 0 0 1 1 1 1 1 1 1 0 0 </row> - <row number="7">0 0 0 * 1 1 1 1 1 1 0 0 * 0 0 0 1 1 1 1 1 1 1 0 0 </row> + <row number="6">0 0 0 * 1 1 1 1 1 1 0 * 0 0 0 0 1 1 1 1 1 1 1 0 0 </row> + <row number="7">0 0 0 * 1 1 1 1 1 1 0 0 0 0 0 0 1 1 1 1 1 1 1 0 0 </row> <row number="8">0 0 0 * 1 1 1 1 1 1 0 0 0 0 0 0 1 1 1 1 1 1 1 0 0 </row> <row number="9">0 0 0 * 0 0 0 0 0 0 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 </row> <row number="10">0 0 0 0 * 0 0 0 0 0 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 </row> <row number="11">0 0 0 0 0 * 0 0 0 0 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 </row> - <row number="12">0 0 0 0 0 0 * 0 0 0 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 </row> - <row number="13">0 0 0 0 0 0 0 * 0 0 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 </row> - <row number="14">0 0 0 0 0 0 0 0 * 0 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 </row> - <row number="15">0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 </row> - <row number="16">0 0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 </row> - <row number="17">0 0 0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 </row> - <row number="18">0 0 0 0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 </row> - <row number="19">0 0 0 0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 </row> + <row number="12">0 0 0 0 0 * 0 0 0 0 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 </row> + <row number="13">0 0 0 0 0 0 * 0 0 0 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 </row> + <row number="14">0 0 0 0 0 0 0 * 0 0 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 </row> + <row number="15">0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 </row> + <row number="16">0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 </row> + <row number="17">0 0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 </row> + <row number="18">0 0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 </row> + <row number="19">0 0 0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 </row> <row number="20">0 0 0 0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 </row> <row number="21">0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 </row> <row number="22">0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 </row> @@ -81,41 +82,39 @@ <row number="24">0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 </row> </path> <lplevel> - <node x="12" y="20" number="0" /> - <node x="12" y="19" number="1" /> - <node x="12" y="18" number="2" /> - <node x="11" y="17" number="3" /> - <node x="10" y="16" number="4" /> - <node x="9" y="15" number="5" /> - <node x="8" y="14" number="6" /> - <node x="7" y="13" number="7" /> - <node x="6" y="12" number="8" /> - <node x="5" y="11" number="9" /> - <node x="4" y="10" number="10" /> - <node x="3" y="9" number="11" /> - <node x="3" y="8" number="12" /> - <node x="3" y="7" number="13" /> - <node x="3" y="6" number="14" /> - <node x="3" y="5" number="15" /> - <node x="3" y="4" number="16" /> - <node x="4" y="4" number="17" /> - <node x="5" y="4" number="18" /> - <node x="6" y="4" number="19" /> - <node x="7" y="4" number="20" /> - <node x="8" y="4" number="21" /> - <node x="9" y="4" number="22" /> - <node x="10" y="4" number="23" /> - <node x="11" y="5" number="24" /> - <node x="12" y="6" number="25" /> - <node x="12" y="7" number="26" /> + <node x="12" y="20" number="0"/> + <node x="12" y="20" number="1"/> + <node x="11" y="19" number="2"/> + <node x="10" y="18" number="3"/> + <node x="10" y="17" number="4"/> + <node x="9" y="16" number="5"/> + <node x="8" y="15" number="6"/> + <node x="7" y="14" number="7"/> + <node x="6" y="13" number="8"/> + <node x="5" y="12" number="9"/> + <node x="5" y="11" number="10"/> + <node x="4" y="10" number="11"/> + <node x="3" y="9" number="12"/> + <node x="3" y="8" number="13"/> + <node x="3" y="7" number="14"/> + <node x="3" y="6" number="15"/> + <node x="3" y="5" number="16"/> + <node x="3" y="4" number="17"/> + <node x="4" y="4" number="18"/> + <node x="5" y="4" number="19"/> + <node x="6" y="4" number="20"/> + <node x="7" y="4" number="21"/> + <node x="8" y="4" number="22"/> + <node x="9" y="4" number="23"/> + <node x="10" y="4" number="24"/> + <node x="11" y="5" number="25"/> + <node x="11" y="6" number="26"/> </lplevel> <hplevel> - <section number="0" start.x="12" start.y="20" finish.x="12" finish.y="18" length="2" /> - <section number="1" start.x="12" start.y="18" finish.x="3" finish.y="9" length="12.7279" /> - <section number="2" start.x="3" start.y="9" finish.x="3" finish.y="4" length="5" /> - <section number="3" start.x="3" start.y="4" finish.x="10" finish.y="4" length="7" /> - <section number="4" start.x="10" start.y="4" finish.x="12" finish.y="6" length="2.82843" /> - <section number="5" start.x="12" start.y="6" finish.x="12" finish.y="7" length="1" /> + <section number="0" start.x="12" start.y="20" finish.x="3" finish.y="9" length="14.727922061357859"/> + <section number="1" start.x="3" start.y="9" finish.x="3" finish.y="4" length="5"/> + <section number="2" start.x="3" start.y="4" finish.x="10" finish.y="4" length="7"/> + <section number="3" start.x="10" start.y="4" finish.x="12" finish.y="7" length="3.8284271247461916"/> </hplevel> </log> </root> diff --git a/gl_const.h b/gl_const.h deleted file mode 100644 index 1e06293..0000000 --- a/gl_const.h +++ /dev/null @@ -1,140 +0,0 @@ -#ifndef GL_CONST_H -#define GL_CONST_H - -#define CN_PI_CONSTANT 3.14159265359 -#define CN_SQRT_TWO 1.41421356237 - -//XML tags -#define CNS_TAG_ROOT "root" - - #define CNS_TAG_MAP "map" - #define CNS_TAG_CELLSIZE "cellsize" - #define CNS_TAG_WIDTH "width" - #define CNS_TAG_HEIGHT "height" - #define CNS_TAG_STX "startx" - #define CNS_TAG_STY "starty" - #define CNS_TAG_FINX "finishx" - #define CNS_TAG_FINY "finishy" - #define CNS_TAG_GRID "grid" - #define CNS_TAG_ROW "row" - - #define CNS_TAG_ALG "algorithm" - #define CNS_TAG_ST "searchtype" - #define CNS_TAG_HW "hweight" - #define CNS_TAG_MT "metrictype" - #define CNS_TAG_BT "breakingties" - #define CNS_TAG_AS "allowsqueeze" - #define CNS_TAG_AD "allowdiagonal" - #define CNS_TAG_CC "cutcorners" - #define CNS_TAG_PS "postsmoothing" - - #define CNS_TAG_OPT "options" - #define CNS_TAG_LOGLVL "loglevel" - #define CNS_TAG_LOGPATH "logpath" - #define CNS_TAG_LOGFN "logfilename" - - #define CNS_TAG_LOG "log" - #define CNS_TAG_MAPFN "mapfilename" - #define CNS_TAG_SUM "summary" - #define CNS_TAG_PATH "path" - #define CNS_TAG_LPLEVEL "lplevel" - #define CNS_TAG_HPLEVEL "hplevel" - #define CNS_TAG_SECTION "section" - #define CNS_TAG_LOWLEVEL "lowlevel" - #define CNS_TAG_STEP "step" - #define CNS_TAG_OPEN "open" - #define CNS_TAG_POINT "node" - #define CNS_TAG_CLOSE "close" - -//XML tags' attributes - #define CNS_TAG_ATTR_NUMOFSTEPS "numberofsteps" - #define CNS_TAG_ATTR_NODESCREATED "nodescreated" - #define CNS_TAG_ATTR_LENGTH "length" - #define CNS_TAG_ATTR_LENGTH_SCALED "length_scaled" - #define CNS_TAG_ATTR_TIME "time" - #define CNS_TAG_ATTR_X "x" - #define CNS_TAG_ATTR_Y "y" - #define CNS_TAG_ATTR_NUM "number" - #define CNS_TAG_ATTR_F "F" - #define CNS_TAG_ATTR_G "g" - #define CNS_TAG_ATTR_PARX "parent_x" - #define CNS_TAG_ATTR_PARY "parent_y" - #define CNS_TAG_ATTR_STX "start.x" - #define CNS_TAG_ATTR_STY "start.y" - #define CNS_TAG_ATTR_FINX "finish.x" - #define CNS_TAG_ATTR_FINY "finish.y" - - -//Search Parameters - #define CN_SP_ST 0 - - #define CNS_SP_ST_BFS "bfs" - #define CNS_SP_ST_DIJK "dijkstra" - #define CNS_SP_ST_ASTAR "astar" - #define CNS_SP_ST_JP_SEARCH "jp_search" - #define CNS_SP_ST_TH "theta" - - #define CN_SP_ST_BFS 0 - #define CN_SP_ST_DIJK 1 - #define CN_SP_ST_ASTAR 2 - #define CN_SP_ST_JP_SEARCH 3 - #define CN_SP_ST_TH 4 - - #define CN_SP_AD 1 //AllowDiagonal - - #define CN_SP_CC 2 //CutCorners - - #define CN_SP_AS 3 //AllowSqueeze - - #define CN_SP_MT 4 //MetricType - - #define CNS_SP_MT_DIAG "diagonal" - #define CNS_SP_MT_MANH "manhattan" - #define CNS_SP_MT_EUCL "euclid" - #define CNS_SP_MT_CHEB "chebyshev" - - #define CN_SP_MT_DIAG 0 - #define CN_SP_MT_MANH 1 - #define CN_SP_MT_EUCL 2 - #define CN_SP_MT_CHEB 3 - - #define CN_SP_HW 5 //HeuristicWeight - - #define CN_SP_BT 6 //BreakingTies - - #define CNS_SP_BT_GMIN "g-min" - #define CNS_SP_BT_GMAX "g-max" - - #define CN_SP_BT_GMIN 0 - #define CN_SP_BT_GMAX 1 - - #define CN_SP_PS 7 //PostSmoothing - - //Log Configuration - #define CN_LP_LEVEL 0 - - #define CN_LP_LEVEL_NOPE_VALUE "0" - #define CN_LP_LEVEL_NOPE_WORD "nope" - #define CN_LP_LEVEL_TINY_VALUE "0.5" - #define CN_LP_LEVEL_TINY_WORD "tiny" - #define CN_LP_LEVEL_SHORT_VALUE "1" - #define CN_LP_LEVEL_SHORT_WORD "short" - #define CN_LP_LEVEL_MEDIUM_VALUE "1.5" - #define CN_LP_LEVEL_MEDIUM_WORD "medium" - #define CN_LP_LEVEL_FULL_VALUE "2" - #define CN_LP_LEVEL_FULL_WORD "full" - - #define CN_LP_PATH 1 - #define CN_LP_NAME 2 - - -//Grid Cell - #define CN_GC_NOOBS 0 - #define CN_GC_OBS 1 - -//Other - #define CNS_OTHER_PATHSELECTION "*" - #define CNS_OTHER_MATRIXSEPARATOR ' ' - -#endif - diff --git a/ilogger.h b/ilogger.h deleted file mode 100644 index a9f58d7..0000000 --- a/ilogger.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef ILOGGER_H -#define ILOGGER_H -#include "map.h" -#include "node.h" -#include <unordered_map> -#include <list> - -class ILogger -{ - public: - ILogger(std::string loglevel) {this->loglevel = loglevel;} - virtual bool getLog(const char* FileName, const std::string* LogParams) = 0; - virtual void saveLog() = 0; - virtual void writeToLogMap(const Map& map, const std::list<Node>& path) = 0; - virtual void writeToLogOpenClose(const std::vector<std::list<Node>>& open, const std::unordered_map<int,Node>& close, bool last) = 0; - virtual void writeToLogPath(const std::list<Node>& path) = 0; - virtual void writeToLogHPpath(const std::list<Node>& path) = 0; - virtual void writeToLogNotFound() = 0; - virtual void writeToLogSummary(unsigned int numberofsteps, unsigned int nodescreated, float length, double time, double cellSize) = 0; - virtual ~ILogger() {}; - protected: - std::string loglevel; -}; - -#endif - diff --git a/astar.h b/include/astar.h similarity index 100% rename from astar.h rename to include/astar.h diff --git a/include/config.h b/include/config.h new file mode 100644 index 0000000..5d873c1 --- /dev/null +++ b/include/config.h @@ -0,0 +1,41 @@ +#ifndef CONFIG_H +#define CONFIG_H +#include <string> + + +class Config +{ + public: + Config(); + Config(const Config& orig); + ~Config(); + bool getConfig(const char *FileName); + + // Set all search parameters directly (see gl_const.h for details) + // st: search type (CN_SP_ST_ASTAR or CN_SP_ST_TH) + // hw: heuristic weight (>=1) + // mt: metric type (CN_SP_MT_DIAG, CN_SP_MT_MANH, CN_SP_MT_EUCL, CN_SP_MT_CHEB) + // bt: breaking ties (CN_SP_BT_GMIN or CN_SP_BT_GMAX) + // ad: allow diagonal (0 or 1) + // cc: cut corners (0 or 1) + // asq: allow squeeze (0 or 1) + // ps: post smoothing (0 or 1) + void setConfig(int st, double hw, int mt, int bt, int ad, int cc, int asq, int ps); + + // Set parameters by tag name (as in gl_const.h) and value (string or double) + // Returns true if the tag is recognized and value is set, false otherwise + bool setParamByTag(const std::string& tag, const std::string& value); + bool setParamByTag(const std::string& tag, double value); + + // Reset all parameters to default values (A* with Euclidean, all options off) + void setDefaultConfigAstar(); + void setDefaultConfigTheta(); + + + public: + double* SearchParams; + unsigned int N; + +}; + +#endif diff --git a/environmentoptions.h b/include/environmentoptions.h similarity index 100% rename from environmentoptions.h rename to include/environmentoptions.h diff --git a/include/gl_const.h b/include/gl_const.h new file mode 100644 index 0000000..ea883b4 --- /dev/null +++ b/include/gl_const.h @@ -0,0 +1,65 @@ +#ifndef GL_CONST_H +#define GL_CONST_H + +#define CN_PI_CONSTANT 3.14159265359 +#define CN_SQRT_TWO 1.41421356237 + +//XML tags +#define CNS_TAG_ROOT "root" + + #define CNS_TAG_ALG "algorithm" + #define CNS_TAG_ST "searchtype" + #define CNS_TAG_HW "hweight" + #define CNS_TAG_MT "metrictype" + #define CNS_TAG_BT "breakingties" + #define CNS_TAG_AS "allowsqueeze" + #define CNS_TAG_AD "allowdiagonal" + #define CNS_TAG_CC "cutcorners" + #define CNS_TAG_PS "postsmoothing" + + +//Search Parameters + #define CN_SP_ST 0 + #define CNS_SP_ST_ASTAR "astar" + #define CNS_SP_ST_TH "theta" + + #define CN_SP_ST_ASTAR 2 + #define CN_SP_ST_TH 4 + + #define CN_SP_AD 1 //AllowDiagonal + + #define CN_SP_CC 2 //CutCorners + + #define CN_SP_AS 3 //AllowSqueeze + + #define CN_SP_MT 4 //MetricType + + #define CNS_SP_MT_DIAG "diagonal" + #define CNS_SP_MT_MANH "manhattan" + #define CNS_SP_MT_EUCL "euclid" + #define CNS_SP_MT_CHEB "chebyshev" + + #define CN_SP_MT_DIAG 0 + #define CN_SP_MT_MANH 1 + #define CN_SP_MT_EUCL 2 + #define CN_SP_MT_CHEB 3 + + #define CN_SP_HW 5 //HeuristicWeight + + #define CN_SP_BT 6 //BreakingTies + + #define CNS_SP_BT_GMIN "g-min" + #define CNS_SP_BT_GMAX "g-max" + + #define CN_SP_BT_GMIN 0 + #define CN_SP_BT_GMAX 1 + + #define CN_SP_PS 7 //PostSmoothing + + + +//Grid Cell + #define CN_GC_NOOBS 0 + #define CN_GC_OBS 1 + +#endif diff --git a/isearch.h b/include/isearch.h similarity index 91% rename from isearch.h rename to include/isearch.h index b342704..27c08e5 100644 --- a/isearch.h +++ b/include/isearch.h @@ -1,11 +1,12 @@ #ifndef ISEARCH_H #define ISEARCH_H -#include "ilogger.h" + #include "searchresult.h" #include "environmentoptions.h" #include <list> #include <unordered_map> #include <cmath> +#include "map.h" class ISearch { @@ -13,7 +14,7 @@ class ISearch ISearch(); virtual ~ISearch(void); - SearchResult startSearch(ILogger *Logger, const Map &Map, const EnvironmentOptions &options); + SearchResult startSearch(const Map &Map, const EnvironmentOptions &options); protected: Node findMin(); diff --git a/map.h b/include/map.h similarity index 73% rename from map.h rename to include/map.h index c25c869..d93a20c 100644 --- a/map.h +++ b/include/map.h @@ -5,7 +5,7 @@ #include <sstream> #include <string> #include <algorithm> -#include <vector> +#include <vector> class Map { public: @@ -13,7 +13,7 @@ class Map Map(const Map& orig); ~Map(); - bool getMap(const char *FileName); + bool getMap(const std::vector<std::vector<int>>& map, int startx, int starty, int finishx, int finishy, int cell_size); bool CellIsTraversable (int i, int j) const; bool CellOnGrid (int i, int j) const; bool CellIsObstacle(int i, int j) const; @@ -23,8 +23,7 @@ class Map int start_i, start_j; int goal_i, goal_j; double cellSize; - int** Grid; + const std::vector<std::vector<int>> *Grid; }; #endif - diff --git a/mission.h b/include/mission.h similarity index 75% rename from mission.h rename to include/mission.h index 0f76cca..30ea0f0 100644 --- a/mission.h +++ b/include/mission.h @@ -4,15 +4,10 @@ #include "map.h" #include "config.h" #include "isearch.h" -#include "ilogger.h" #include "searchresult.h" #include "environmentoptions.h" -#include "jp_search.h" #include "astar.h" -#include "bfs.h" -#include "dijkstra.h" #include "theta.h" -#include "xmllogger.h" #include "path_smoothing.h" class Mission @@ -22,14 +17,15 @@ class Mission Mission (const char* fileName); ~Mission(); - bool getMap(); + bool getMap(int startX, int startY, int endX, int endY, int cellSize, std::vector<std::vector<int>> &mapData); bool getConfig(); - bool createLog(); void createSearch(); void createEnvironmentOptions(); void startSearch(); void printSearchResultsToConsole(); - void saveSearchResultsToLog(); + bool setDefaultConfig(bool use_theta); + void getPath(std::vector<std::vector<int>> &path); + bool getPathValid(); private: const char* getAlgorithmName(); @@ -38,10 +34,8 @@ class Mission Config config; EnvironmentOptions options; ISearch* search; - ILogger* logger; const char* fileName; SearchResult sr; }; #endif - diff --git a/node.h b/include/node.h similarity index 100% rename from node.h rename to include/node.h diff --git a/path_smoothing.h b/include/path_smoothing.h similarity index 100% rename from path_smoothing.h rename to include/path_smoothing.h diff --git a/searchresult.h b/include/searchresult.h similarity index 100% rename from searchresult.h rename to include/searchresult.h diff --git a/theta.h b/include/theta.h similarity index 100% rename from theta.h rename to include/theta.h diff --git a/tinyxml2.h b/include/tinyxml2.h similarity index 100% rename from tinyxml2.h rename to include/tinyxml2.h diff --git a/include/wrapper.h b/include/wrapper.h new file mode 100644 index 0000000..a7af7c2 --- /dev/null +++ b/include/wrapper.h @@ -0,0 +1,4 @@ +#pragma once +#include <vector> + +int plan_2d(std::vector<float> &origin, std::vector<int> &dim, std::vector<signed char> &map, std::vector<float> &start, std::vector<float> &goal, float resolution, std::vector<std::vector<double> > &path, double &time_spent, bool use_theta); diff --git a/jp_search.cpp b/jp_search.cpp deleted file mode 100644 index 8e295f9..0000000 --- a/jp_search.cpp +++ /dev/null @@ -1,290 +0,0 @@ -#include "jp_search.h" - -JP_Search::~JP_Search() -{ -} - -void JP_Search::findJP(int move_i, int move_j, Node curNode, const Map &map, std::list<Node> &successors, const EnvironmentOptions &options) -{ - bool findOK = false; - while(!findOK) { - if(map.CellOnGrid(curNode.i + move_i, curNode.j + move_j)) { - if(!options.cutcorners) { - if(move_i != 0 && move_j != 0) - if(!map.CellIsTraversable(curNode.i, curNode.j + move_j) || !map.CellIsTraversable(curNode.i + move_i, curNode.j)) - return; - } - else if(!options.allowsqueeze) { - if(move_i != 0 && move_j != 0) - if(!map.CellIsTraversable(curNode.i, curNode.j + move_j) && !map.CellIsTraversable(curNode.i + move_i, curNode.j)) - return; - } - if(map.CellIsTraversable(curNode.i + move_i, curNode.j + move_j)) { - curNode.i += move_i; - curNode.j += move_j; - if(move_i == 0 || move_j == 0) - curNode.g += 1; - else - curNode.g += sqrt(2); - } - else - return; - } - else - return; - if(map.goal_i == curNode.i && map.goal_j == curNode.j) - findOK = true; - if(options.allowdiagonal) { //check whether diagonal moves is allowed - if(options.cutcorners) { - if(move_i == 0) { //straight move along j - if(map.CellOnGrid(curNode.i + 1, curNode.j + move_j)) - if(map.CellIsTraversable(curNode.i + 1, curNode.j + move_j) && map.CellIsObstacle(curNode.i + 1, curNode.j)) - findOK = true; - if(map.CellOnGrid(curNode.i - 1,curNode.j + move_j)) - if(map.CellIsTraversable(curNode.i - 1, curNode.j + move_j) && map.CellIsObstacle(curNode.i - 1, curNode.j)) - findOK = true; - } - else if(move_j == 0) { //straight move along i - if(map.CellOnGrid(curNode.i + move_i, curNode.j + 1)) - if(map.CellIsTraversable(curNode.i + move_i, curNode.j + 1) && map.CellIsObstacle(curNode.i, curNode.j + 1)) - findOK = true; - if(map.CellOnGrid(curNode.i + move_i, curNode.j - 1)) - if(map.CellIsTraversable(curNode.i + move_i, curNode.j - 1) && map.CellIsObstacle(curNode.i, curNode.j - 1)) - findOK = true; - } - else { //diagonal move - if(map.CellOnGrid(curNode.i - move_i, curNode.j + move_j)) - if(map.CellIsObstacle(curNode.i - move_i, curNode.j) && map.CellIsTraversable(curNode.i - move_i, curNode.j + move_j)) - findOK = true; - if(!findOK && map.CellOnGrid(curNode.i + move_i, curNode.j - move_j)) - if(map.CellIsObstacle(curNode.i, curNode.j - move_j) && map.CellIsTraversable(curNode.i + move_i, curNode.j - move_j)) - findOK = true; - if(!findOK) - if(findNeighbors(move_i, 0, curNode, map, options)) - findOK = true; - if(!findOK) - if(findNeighbors(0, move_j, curNode, map, options)) - findOK = true; - } - } - else - { - if(move_i == 0) { //straight move along j - if(map.CellOnGrid(curNode.i + 1, curNode.j)) - if(map.CellIsTraversable(curNode.i + 1, curNode.j) && map.CellIsObstacle(curNode.i + 1, curNode.j - move_j))//check forced neighbor - findOK = true; - if(map.CellOnGrid(curNode.i - 1,curNode.j)) - if(map.CellIsTraversable(curNode.i - 1, curNode.j) && map.CellIsObstacle(curNode.i - 1, curNode.j - move_j)) - findOK = true; - } - else if(move_j == 0) { //straight move along i - if(map.CellOnGrid(curNode.i, curNode.j + 1)) - if(map.CellIsTraversable(curNode.i, curNode.j + 1) && map.CellIsObstacle(curNode.i - move_i, curNode.j + 1)) - findOK = true; - if(map.CellOnGrid(curNode.i, curNode.j - 1)) - if(map.CellIsTraversable(curNode.i, curNode.j - 1) && map.CellIsObstacle(curNode.i - move_i, curNode.j - 1)) - findOK = true; - } - else { //diagonal move - if(findNeighbors(move_i, 0, curNode, map, options))//looking for forced neighbor along i - findOK = true; - if(!findOK) - if(findNeighbors(0, move_j, curNode, map, options))//looking for forced neighbor along j - findOK = true; - } - } - } - else { //only straight moves is allowed - if(!findOK) - if(findNeighbors(move_j, move_i, curNode, map, options)) - findOK = true; - if(!findOK) - if(findNeighbors(-move_j, -move_i, curNode, map, options)) - findOK = true; - } - } - if(close.find(curNode.i * map.width + curNode.j) == close.end()) - successors.push_front(curNode); - return; -} - -bool JP_Search::findNeighbors(int move_i, int move_j, Node curNode, const Map &map, const EnvironmentOptions &options) -{ - while(map.CellOnGrid(curNode.i, curNode.j) && map.CellIsTraversable(curNode.i, curNode.j)) { - if(map.goal_i == curNode.i && map.goal_j == curNode.j)//goal location is found - return true; - if(options.cutcorners) { - if(move_i == 0 && map.CellOnGrid(curNode.i, curNode.j+move_j)) { - if(map.CellOnGrid(curNode.i + 1, curNode.j)) - if(map.CellIsTraversable(curNode.i + 1, curNode.j+move_j) && map.CellIsObstacle(curNode.i + 1, curNode.j)) - return true; - if(map.CellOnGrid(curNode.i - 1,curNode.j)) - if(map.CellIsTraversable(curNode.i - 1, curNode.j+move_j) && map.CellIsObstacle(curNode.i - 1, curNode.j)) - return true; - } - if(move_j == 0 && map.CellOnGrid(curNode.i + move_i, curNode.j)) { - if(map.CellOnGrid(curNode.i, curNode.j + 1)) - if(map.CellIsTraversable(curNode.i + move_i, curNode.j + 1) && map.CellIsObstacle(curNode.i, curNode.j + 1)) - return true; - if(map.CellOnGrid(curNode.i, curNode.j - 1)) - if(map.CellIsTraversable(curNode.i + move_i, curNode.j - 1) && map.CellIsObstacle(curNode.i, curNode.j - 1)) - return true; - } - } - else { - if(move_i == 0 && map.CellOnGrid(curNode.i, curNode.j - move_j)) { - if(map.CellOnGrid(curNode.i + 1, curNode.j)) - if(map.CellIsTraversable(curNode.i + 1, curNode.j) && map.CellIsObstacle(curNode.i + 1, curNode.j - move_j)) - return true; - if(map.CellOnGrid(curNode.i - 1,curNode.j)) - if(map.CellIsTraversable(curNode.i - 1, curNode.j) && map.CellIsObstacle(curNode.i - 1, curNode.j - move_j)) - return true; - } - if(move_j == 0 && map.CellOnGrid(curNode.i - move_i, curNode.j)) { - if(map.CellOnGrid(curNode.i, curNode.j + 1)) - if(map.CellIsTraversable(curNode.i, curNode.j + 1) && map.CellIsObstacle(curNode.i - move_i, curNode.j + 1)) - return true; - if(map.CellOnGrid(curNode.i, curNode.j - 1)) - if(map.CellIsTraversable(curNode.i, curNode.j - 1) && map.CellIsObstacle(curNode.i - move_i, curNode.j - 1)) - return true; - } - } - curNode.i += move_i; - curNode.j += move_j; - } - return false; -} - - - -int JP_Search::findDirection(int current_i, int parent_i) -{ - if(current_i < parent_i) - return -1; - else if(current_i > parent_i) - return 1; - else - return 0; -} - -std::list<Node> JP_Search::findSuccessors(Node curNode, const Map &map, const EnvironmentOptions &options) -{ - int move_i = 0, move_j = 0; - std::list<Node> successors; - - if(options.allowdiagonal) { - if(curNode.i == map.start_i && curNode.j == map.start_j)//if curNode is the start location, then look for jump points in all directions - for(int n = -1; n <= 1; n++) - for(int m = -1; m <= 1; m++) - if(n != 0 || m != 0) - findJP(n, m, curNode, map, successors, options); - if(curNode.i != map.start_i || curNode.j != map.start_j) { - move_i = findDirection(curNode.i, curNode.parent->i); - move_j = findDirection(curNode.j, curNode.parent->j); - findJP(move_i, move_j, curNode, map, successors, options);//continue to look for jump points in the same direction - - if(move_i != 0 && move_j != 0) { //if curNoode is a diagonal jump point - if(map.CellIsObstacle(curNode.i - move_i, curNode.j)) - findJP(-move_i, move_j, curNode, map, successors, options); - if(map.CellIsObstacle(curNode.i, curNode.j - move_j)) - findJP(move_i, -move_j, curNode, map, successors, options); - findJP(move_i, 0, curNode, map, successors, options);//look for jump point in straight direction along i - findJP(0, move_j, curNode, map, successors, options);//the same check along j - } - - if(options.cutcorners) { //original JPS, when cutcorners is allowed - if(move_i == 0) { - if(map.CellOnGrid(curNode.i - move_j, curNode.j)) - if(map.CellIsObstacle(curNode.i - move_j, curNode.j)) - findJP(-move_j, move_j, curNode, map, successors, options); - if(map.CellOnGrid(curNode.i+move_j, curNode.j)) - if(map.CellIsObstacle(curNode.i + move_j, curNode.j)) - findJP(move_j, move_j, curNode, map, successors, options); - } - else if(move_j == 0) { - if(map.CellOnGrid(curNode.i, curNode.j - move_i)) - if(map.CellIsObstacle(curNode.i, curNode.j - move_i)) - findJP(move_i, -move_i, curNode, map, successors, options); - if(map.CellOnGrid(curNode.i, curNode.j + move_i)) - if(map.CellIsObstacle(curNode.i, curNode.j + move_i)) - findJP(move_i, move_i, curNode, map, successors, options); - } - } - else { //cutcorners disallowed - if(move_i == 0) { - if(map.CellOnGrid(curNode.i - move_j, curNode.j)) - if(map.CellIsObstacle(curNode.i - move_j, curNode.j - move_j)) { - findJP(-move_j, move_j, curNode, map, successors, options); - findJP(-move_j, 0, curNode, map, successors, options); - } - if(map.CellOnGrid(curNode.i + move_j, curNode.j)) - if(map.CellIsObstacle(curNode.i + move_j, curNode.j - move_j)) { - findJP(move_j, 0, curNode, map, successors, options); - findJP(move_j, move_j, curNode, map, successors, options); - } - } - else if(move_j == 0) { - if(map.CellOnGrid(curNode.i, curNode.j-move_i)) - if(map.CellIsObstacle(curNode.i - move_i, curNode.j - move_i)) { - findJP(0, -move_i, curNode, map, successors, options);//additional check - findJP(move_i, -move_i, curNode, map, successors, options); - } - if(map.CellOnGrid(curNode.i, curNode.j + move_i)) - if(map.CellIsObstacle(curNode.i - move_i, curNode.j + move_i)) { - findJP(0, move_i, curNode, map, successors, options);//additional check - findJP(move_i, move_i, curNode, map, successors, options); - } - } - } - } - } - else { //only straight moves as allowed - if(curNode.i == map.start_i && curNode.j == map.start_j) - for(int n = -1; n <= 1; n++) - for(int m = -1; m <= 1; m++) - if((n != 0 && m == 0) || (n == 0 && m != 0)) - findJP(n, m, curNode, map, successors, options); - if(curNode.i != map.start_i || curNode.j != map.start_j) { - move_i = findDirection(curNode.i, curNode.parent->i); - move_j = findDirection(curNode.j, curNode.parent->j); - - findJP(move_i, move_j, curNode, map, successors, options); - findJP(move_j, move_i, curNode, map, successors, options); - findJP(-move_j, -move_i, curNode, map, successors, options); - } - } - return successors; -} - -void JP_Search::makePrimaryPath(Node curNode) -{ - Node current = curNode; - while(current.parent) { - hppath.push_front(current); - current = *current.parent; - } - hppath.push_front(current); -} - -void JP_Search::makeSecondaryPath() -{ - Node pathNode = *hppath.begin(); - std::list<Node>::iterator iter = hppath.begin(); - iter++; - Node nextNode = *iter; - Node inpath; - lppath.push_back(pathNode); - while(iter != hppath.end()) { - int steps = std::max(std::abs(pathNode.i - nextNode.i), std::abs(pathNode.j - nextNode.j)); - int step_i = (pathNode.i < nextNode.i) ? 1 : ((pathNode.i > nextNode.i) ? -1 : 0); - int step_j = (pathNode.j < nextNode.j) ? 1 : ((pathNode.j > nextNode.j) ? -1 : 0); - for (int k = 0; k <= steps; k++) { - inpath.i = pathNode.i + k*step_i; - inpath.j = pathNode.j + k*step_j; - lppath.push_back(inpath); - } - pathNode = nextNode; - iter++; - nextNode = *iter; - } -} diff --git a/jp_search.h b/jp_search.h deleted file mode 100644 index 6f1f79a..0000000 --- a/jp_search.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef JP_SEARCH_H -#define JP_SEARCH_H -#include "astar.h" - -class JP_Search:public Astar -{ -public: - JP_Search(float hweight, bool breakingties):Astar(hweight, breakingties){} - ~JP_Search(); - -private: - bool findNeighbors(int move_i, int move_j, Node curNode, const Map &map, const EnvironmentOptions &options);//checks forced neighbors - void findJP(int move_i, int move_j, Node curNode, const Map &map, std::list<Node> &successors, const EnvironmentOptions &options);//searches jump points - int findDirection(int current_i, int parent_i);//determines the direction of motion - std::list<Node> findSuccessors(Node curNode, const Map &map, const EnvironmentOptions &options); - void makePrimaryPath(Node curNode); - void makeSecondaryPath(); -}; - -#endif // JP_SEARCH_H diff --git a/map.cpp b/map.cpp deleted file mode 100644 index 634d6a8..0000000 --- a/map.cpp +++ /dev/null @@ -1,321 +0,0 @@ -#include "map.h" -#include "tinyxml2.h" - -Map::Map() -{ - height = -1; - width = -1; - start_i = -1; - start_j = -1; - goal_i = -1; - goal_j = -1; - Grid = nullptr; - cellSize = 1; -} - -Map::~Map() -{ - if (Grid) { - for (int i = 0; i < height; ++i) - delete[] Grid[i]; - delete[] Grid; - } -} - -bool Map::CellIsTraversable(int i, int j) const -{ - return (Grid[i][j] == CN_GC_NOOBS); -} - -bool Map::CellIsObstacle(int i, int j) const -{ - return (Grid[i][j] != CN_GC_NOOBS); -} - -bool Map::CellOnGrid(int i, int j) const -{ - return (i < height && i >= 0 && j < width && j >= 0); -} - -bool Map::getMap(const char *FileName) -{ - int rowiter = 0, grid_i = 0, grid_j = 0; - - tinyxml2::XMLElement *root = 0, *map = 0, *element = 0, *mapnode; - - std::string value; - std::stringstream stream; - - bool hasGridMem = false, hasGrid = false, hasHeight = false, hasWidth = false, hasSTX = false, hasSTY = false, hasFINX = false, hasFINY = false, hasCellSize = false; - - tinyxml2::XMLDocument doc; - - // Load XML File - if (doc.LoadFile(FileName) != tinyxml2::XMLError::XML_SUCCESS) { - std::cout << "Error opening XML file!" << std::endl; - return false; - } - - // Get ROOT element - root = doc.FirstChildElement(CNS_TAG_ROOT); - if (!root) { - std::cout << "Error! No '" << CNS_TAG_ROOT << "' tag found in XML file!" << std::endl; - return false; - } - - // Get MAP element - map = root->FirstChildElement(CNS_TAG_MAP); - if (!map) { - std::cout << "Error! No '" << CNS_TAG_MAP << "' tag found in XML file!" << std::endl; - return false; - } - - for (mapnode = map->FirstChildElement(); mapnode; mapnode = mapnode->NextSiblingElement()) { - element = mapnode->ToElement(); - value = mapnode->Value(); - std::transform(value.begin(), value.end(), value.begin(), ::tolower); - - stream.str(""); - stream.clear(); - - if(value != CNS_TAG_GRID) - stream << element->GetText(); - - if (!hasGridMem && hasHeight && hasWidth) { - Grid = new int *[height]; - for (int i = 0; i < height; ++i) - Grid[i] = new int[width]; - hasGridMem = true; - } - - if (value == CNS_TAG_HEIGHT) { - if (hasHeight) { - std::cout << "Warning! Duplicate '" << CNS_TAG_HEIGHT << "' encountered." << std::endl; - std::cout << "Only first value of '" << CNS_TAG_HEIGHT << "' =" << height << "will be used." - << std::endl; - } - else { - if (!((stream >> height) && (height > 0))) { - std::cout << "Warning! Invalid value of '" << CNS_TAG_HEIGHT - << "' tag encountered (or could not convert to integer)." << std::endl; - std::cout << "Value of '" << CNS_TAG_HEIGHT << "' tag should be an integer >=0" << std::endl; - std::cout << "Continue reading XML and hope correct value of '" << CNS_TAG_HEIGHT - << "' tag will be encountered later..." << std::endl; - } - else - hasHeight = true; - } - } - else if (value == CNS_TAG_WIDTH) { - if (hasWidth) { - std::cout << "Warning! Duplicate '" << CNS_TAG_WIDTH << "' encountered." << std::endl; - std::cout << "Only first value of '" << CNS_TAG_WIDTH << "' =" << width << "will be used." << std::endl; - } - else { - if (!((stream >> width) && (width > 0))) { - std::cout << "Warning! Invalid value of '" << CNS_TAG_WIDTH - << "' tag encountered (or could not convert to integer)." << std::endl; - std::cout << "Value of '" << CNS_TAG_WIDTH << "' tag should be an integer AND >0" << std::endl; - std::cout << "Continue reading XML and hope correct value of '" << CNS_TAG_WIDTH - << "' tag will be encountered later..." << std::endl; - - } - else - hasWidth = true; - } - } - else if (value == CNS_TAG_CELLSIZE) { - if (hasCellSize) { - std::cout << "Warning! Duplicate '" << CNS_TAG_CELLSIZE << "' encountered." << std::endl; - std::cout << "Only first value of '" << CNS_TAG_CELLSIZE << "' =" << cellSize << "will be used." - << std::endl; - } - else { - if (!((stream >> cellSize) && (cellSize > 0))) { - std::cout << "Warning! Invalid value of '" << CNS_TAG_CELLSIZE - << "' tag encountered (or could not convert to double)." << std::endl; - std::cout << "Value of '" << CNS_TAG_CELLSIZE - << "' tag should be double AND >0. By default it is defined to '1'" << std::endl; - std::cout << "Continue reading XML and hope correct value of '" << CNS_TAG_CELLSIZE - << "' tag will be encountered later..." << std::endl; - } - else - hasCellSize = true; - } - } - else if (value == CNS_TAG_STX) { - if (!hasWidth) { - std::cout << "Error! '" << CNS_TAG_STX << "' tag encountered before '" << CNS_TAG_WIDTH << "' tag." - << std::endl; - return false; - } - - if (hasSTX) { - std::cout << "Warning! Duplicate '" << CNS_TAG_STX << "' encountered." << std::endl; - std::cout << "Only first value of '" << CNS_TAG_STX << "' =" << start_j << "will be used." << std::endl; - } - else { - if (!(stream >> start_j && start_j >= 0 && start_j < width)) { - std::cout << "Warning! Invalid value of '" << CNS_TAG_STX - << "' tag encountered (or could not convert to integer)" << std::endl; - std::cout << "Value of '" << CNS_TAG_STX << "' tag should be an integer AND >=0 AND < '" - << CNS_TAG_WIDTH << "' value, which is " << width << std::endl; - std::cout << "Continue reading XML and hope correct value of '" << CNS_TAG_STX - << "' tag will be encountered later..." << std::endl; - } - else - hasSTX = true; - } - } - else if (value == CNS_TAG_STY) { - if (!hasHeight) { - std::cout << "Error! '" << CNS_TAG_STY << "' tag encountered before '" << CNS_TAG_HEIGHT << "' tag." - << std::endl; - return false; - } - - if (hasSTY) { - std::cout << "Warning! Duplicate '" << CNS_TAG_STY << "' encountered." << std::endl; - std::cout << "Only first value of '" << CNS_TAG_STY << "' =" << start_i << "will be used." << std::endl; - } - else { - if (!(stream >> start_i && start_i >= 0 && start_i < height)) { - std::cout << "Warning! Invalid value of '" << CNS_TAG_STY - << "' tag encountered (or could not convert to integer)" << std::endl; - std::cout << "Value of '" << CNS_TAG_STY << "' tag should be an integer AND >=0 AND < '" - << CNS_TAG_HEIGHT << "' value, which is " << height << std::endl; - std::cout << "Continue reading XML and hope correct value of '" << CNS_TAG_STY - << "' tag will be encountered later..." << std::endl; - } - else - hasSTY = true; - } - } - else if (value == CNS_TAG_FINX) { - if (!hasWidth) { - std::cout << "Error! '" << CNS_TAG_FINX << "' tag encountered before '" << CNS_TAG_WIDTH << "' tag." - << std::endl; - return false; - } - - if (hasFINX) { - std::cout << "Warning! Duplicate '" << CNS_TAG_FINX << "' encountered." << std::endl; - std::cout << "Only first value of '" << CNS_TAG_FINX << "' =" << goal_j << "will be used." << std::endl; - } - else { - if (!(stream >> goal_j && goal_j >= 0 && goal_j < width)) { - std::cout << "Warning! Invalid value of '" << CNS_TAG_FINX - << "' tag encountered (or could not convert to integer)" << std::endl; - std::cout << "Value of '" << CNS_TAG_FINX << "' tag should be an integer AND >=0 AND < '" - << CNS_TAG_WIDTH << "' value, which is " << width << std::endl; - std::cout << "Continue reading XML and hope correct value of '" << CNS_TAG_FINX - << "' tag will be encountered later..." << std::endl; - } - else - hasFINX = true; - } - } - else if (value == CNS_TAG_FINY) { - if (!hasHeight) { - std::cout << "Error! '" << CNS_TAG_FINY << "' tag encountered before '" << CNS_TAG_HEIGHT << "' tag." - << std::endl; - return false; - } - - if (hasFINY) { - std::cout << "Warning! Duplicate '" << CNS_TAG_FINY << "' encountered." << std::endl; - std::cout << "Only first value of '" << CNS_TAG_FINY << "' =" << goal_i << "will be used." << std::endl; - } - else { - if (!(stream >> goal_i && goal_i >= 0 && goal_i < height)) { - std::cout << "Warning! Invalid value of '" << CNS_TAG_FINY - << "' tag encountered (or could not convert to integer)" << std::endl; - std::cout << "Value of '" << CNS_TAG_FINY << "' tag should be an integer AND >=0 AND < '" - << CNS_TAG_HEIGHT << "' value, which is " << height << std::endl; - std::cout << "Continue reading XML and hope correct value of '" << CNS_TAG_FINY - << "' tag will be encountered later..." << std::endl; - } - else - hasFINY = true; - } - } - else if (value == CNS_TAG_GRID) { - hasGrid = true; - if (!(hasHeight && hasWidth)) { - std::cout << "Error! No '" << CNS_TAG_WIDTH << "' tag or '" << CNS_TAG_HEIGHT << "' tag before '" - << CNS_TAG_GRID << "'tag encountered!" << std::endl; - return false; - } - element = mapnode->FirstChildElement(); - while (grid_i < height) { - if (!element) { - std::cout << "Error! Not enough '" << CNS_TAG_ROW << "' tags inside '" << CNS_TAG_GRID << "' tag." - << std::endl; - std::cout << "Number of '" << CNS_TAG_ROW - << "' tags should be equal (or greater) than the value of '" << CNS_TAG_HEIGHT - << "' tag which is " << height << std::endl; - return false; - } - std::string str = element->GetText(); - std::vector<std::string> elems; - std::stringstream ss(str); - std::string item; - while (std::getline(ss, item, ' ')) - elems.push_back(item); - rowiter = grid_j = 0; - int val; - if (elems.size() > 0) - for (grid_j = 0; grid_j < width; ++grid_j) { - if (grid_j == elems.size()) - break; - stream.str(""); - stream.clear(); - stream << elems[grid_j]; - stream >> val; - Grid[grid_i][grid_j] = val; - } - - if (grid_j != width) { - std::cout << "Invalid value on " << CNS_TAG_GRID << " in the " << grid_i + 1 << " " << CNS_TAG_ROW - << std::endl; - return false; - } - ++grid_i; - - element = element->NextSiblingElement(); - } - } - } - //some additional checks - if (!hasGrid) { - std::cout << "Error! There is no tag 'grid' in xml-file!\n"; - return false; - } - if (!(hasFINX && hasFINY && hasSTX && hasSTY)) - return false; - - if (Grid[start_i][start_j] != CN_GC_NOOBS) { - std::cout << "Error! Start cell is not traversable (cell's value is" << Grid[start_i][start_j] << ")!" - << std::endl; - return false; - } - - if (Grid[goal_i][goal_j] != CN_GC_NOOBS) { - std::cout << "Error! Goal cell is not traversable (cell's value is" << Grid[goal_i][goal_j] << ")!" - << std::endl; - return false; - } - - return true; -} - -int Map::getValue(int i, int j) const -{ - if (i < 0 || i >= height) - return -1; - - if (j < 0 || j >= width) - return -1; - - return Grid[i][j]; -} diff --git a/mission.cpp b/mission.cpp deleted file mode 100644 index d3e10eb..0000000 --- a/mission.cpp +++ /dev/null @@ -1,125 +0,0 @@ -#include "mission.h" -#include "astar.h" -#include "bfs.h" -#include "dijkstra.h" -#include "theta.h" -#include "xmllogger.h" -#include "gl_const.h" - -Mission::Mission() -{ - logger = nullptr; - search = nullptr; - fileName = nullptr; -} - -Mission::Mission(const char *FileName) -{ - fileName = FileName; - logger = nullptr; - search = nullptr; -} - -Mission::~Mission() -{ - if (logger) - delete logger; - if (search) - delete search; -} - -bool Mission::getMap() -{ - return map.getMap(fileName); -} - -bool Mission::getConfig() -{ - return config.getConfig(fileName); -} - -bool Mission::createLog() -{ - if (logger != NULL) delete logger; - logger = new XmlLogger(config.LogParams[CN_LP_LEVEL]); - return logger->getLog(fileName, config.LogParams); -} - -void Mission::createEnvironmentOptions() -{ - if (config.SearchParams[CN_SP_ST] == CN_SP_ST_BFS || config.SearchParams[CN_SP_ST] == CN_SP_ST_DIJK) - options = EnvironmentOptions(config.SearchParams[CN_SP_AS], config.SearchParams[CN_SP_AD], - config.SearchParams[CN_SP_CC]); - else - options = EnvironmentOptions(config.SearchParams[CN_SP_AS], config.SearchParams[CN_SP_AD], - config.SearchParams[CN_SP_CC], config.SearchParams[CN_SP_MT]); -} - -void Mission::createSearch() -{ - if (search) - delete search; - if (config.SearchParams[CN_SP_ST] == CN_SP_ST_BFS) - search = new BFS(); - else if (config.SearchParams[CN_SP_ST] == CN_SP_ST_DIJK) - search = new Dijkstra(); - else if (config.SearchParams[CN_SP_ST] == CN_SP_ST_ASTAR) - search = new Astar(config.SearchParams[CN_SP_HW], config.SearchParams[CN_SP_BT]); - else if (config.SearchParams[CN_SP_ST] == CN_SP_ST_JP_SEARCH) - search = new JP_Search(config.SearchParams[CN_SP_HW], config.SearchParams[CN_SP_BT]); - else if (config.SearchParams[CN_SP_ST] == CN_SP_ST_TH) - search = new Theta(config.SearchParams[CN_SP_HW], config.SearchParams[CN_SP_BT]); -} - -void Mission::startSearch() -{ - - sr = search->startSearch(logger, map, options); - if (config.SearchParams[CN_SP_PS]) - { - smooth_search_result(sr, map, options.cutcorners); - } -} - -void Mission::printSearchResultsToConsole() -{ - std::cout << "Path "; - if (!sr.pathfound) - std::cout << "NOT "; - std::cout << "found!" << std::endl; - std::cout << "numberofsteps=" << sr.numberofsteps << std::endl; - std::cout << "nodescreated=" << sr.nodescreated << std::endl; - if (sr.pathfound) { - std::cout << "pathlength=" << sr.pathlength << std::endl; - std::cout << "pathlength_scaled=" << sr.pathlength * map.cellSize << std::endl; - } - std::cout << "time=" << sr.time << std::endl; -} - -void Mission::saveSearchResultsToLog() -{ - logger->writeToLogSummary(sr.numberofsteps, sr.nodescreated, sr.pathlength, sr.time, map.cellSize); - if (sr.pathfound) { - logger->writeToLogPath(*sr.lppath); - logger->writeToLogHPpath(*sr.hppath); - logger->writeToLogMap(map, *sr.lppath); - } else - logger->writeToLogNotFound(); - logger->saveLog(); -} - -const char *Mission::getAlgorithmName() -{ - if (config.SearchParams[CN_SP_ST] == CN_SP_ST_ASTAR) - return CNS_SP_ST_ASTAR; - else if (config.SearchParams[CN_SP_ST] == CN_SP_ST_DIJK) - return CNS_SP_ST_DIJK; - else if (config.SearchParams[CN_SP_ST] == CN_SP_ST_BFS) - return CNS_SP_ST_BFS; - else if (config.SearchParams[CN_SP_ST] == CN_SP_ST_JP_SEARCH) - return CNS_SP_ST_JP_SEARCH; - else if (config.SearchParams[CN_SP_ST] == CN_SP_ST_TH) - return CNS_SP_ST_TH; - else - return ""; -} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..d6fc903 --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 3.1) # pybind11 prefers a slightly newer version, 2.8 might be too old. +project(AStar-ThetaStar) +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED True) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O2 -Wall -Wextra") + +# Find pybind11 - more robust method for pip-installed pybind11 +# First, ensure Python is found to get its executable. +find_package(PythonInterp REQUIRED) # Ensures PYTHON_EXECUTABLE is set + +execute_process( + COMMAND "${PYTHON_EXECUTABLE}" -c "import pybind11; print(pybind11.get_cmake_dir())" + OUTPUT_VARIABLE pybind11_cmake_dir + RESULT_VARIABLE pybind11_get_cmake_dir_result + OUTPUT_STRIP_TRAILING_WHITESPACE +) + +if(NOT pybind11_get_cmake_dir_result EQUAL 0) + message(FATAL_ERROR "Failed to execute Python to get pybind11 CMake directory. Ensure pybind11 is installed in the Python environment being used by CMake (PYTHON_EXECUTABLE=${PYTHON_EXECUTABLE}).") +endif() + +if(NOT EXISTS "${pybind11_cmake_dir}/pybind11Config.cmake" AND NOT EXISTS "${pybind11_cmake_dir}/pybind11-config.cmake") + message(WARNING "pybind11Config.cmake or pybind11-config.cmake not found in directory reported by pybind11.get_cmake_dir(): ${pybind11_cmake_dir}. Trying find_package directly, but it might fail.") + find_package(pybind11 REQUIRED) # Fallback to direct find_package +else() + # Add the found directory to CMAKE_PREFIX_PATH so find_package can use it + list(APPEND CMAKE_PREFIX_PATH "${pybind11_cmake_dir}") + message(STATUS "Found pybind11 CMake directory: ${pybind11_cmake_dir}. Added to CMAKE_PREFIX_PATH.") + find_package(pybind11 REQUIRED) +endif() + + +set(SOURCE_FILES + asearch.cpp + # config.h # Headers usually not listed directly + config.cpp # ensure config.cpp is also present + environmentoptions.cpp + mission.cpp # Added mission.cpp + # mission.h # Headers usually not listed directly + # searchresult.h # Headers usually not listed directly + astar.cpp + # environmentoptions.h # Headers usually not listed directly + isearch.cpp + map.cpp + # mission.h # Headers usually not listed directly (duplicate) + theta.cpp + # astar.h # Headers usually not listed directly + config.cpp + # gl_const.h # Headers usually not listed directly + # isearch.h # Headers usually not listed directly + # map.h # Headers usually not listed directly + # node.h # Headers usually not listed directly + # theta.h # Headers usually not listed directly + # path_smoothing.h # Headers usually not listed directly + path_smoothing.cpp + # tinyxml2.h # Headers usually not listed directly + tinyxml2.cpp + wrapper.cpp # Added wrapper.cpp + python_bindings.cpp # Added python_bindings.cpp + ) + +add_library(ThetaStarPlanner SHARED ${SOURCE_FILES}) + +# Link pybind11 +target_link_libraries(ThetaStarPlanner PRIVATE pybind11::module) + +# Ensure the library is placed in a location where Python can find it, +# or provide instructions to the user to add the build directory to PYTHONPATH. +# For simplicity, CMake will build it in the build directory. + +# Optional: Define a module name that Python expects, if different from target name. +# By default, pybind11 creates a module based on the target name. +# On Linux, it will be ThetaStarPlanner.so, on Windows ThetaStarPlanner.pyd. +# pybind11_add_module(ThetaStarPlanner_py ${SOURCE_FILES} src/python_bindings.cpp) # Example if using pybind11_add_module + +# Include directories (important for CMake to find headers) +target_include_directories(ThetaStarPlanner PUBLIC ../include) # Make headers in 'include' available +# pybind11::module should handle its own include directories when linked. + +# Get Python's extension suffix (e.g., .cpython-38-x86_64-linux-gnu.so or .pyd on Windows) +execute_process( + COMMAND "${PYTHON_EXECUTABLE}" -c "import sysconfig; suffix = sysconfig.get_config_var('EXT_SUFFIX'); print(suffix if suffix is not None else sysconfig.get_config_var('SO'))" + OUTPUT_VARIABLE PYTHON_EXT_SUFFIX + OUTPUT_STRIP_TRAILING_WHITESPACE + RESULT_VARIABLE PYTHON_EXT_SUFFIX_RESULT +) + +if(NOT PYTHON_EXT_SUFFIX_RESULT EQUAL 0 OR NOT PYTHON_EXT_SUFFIX) + message(WARNING "Could not determine Python extension suffix automatically.") + if(WIN32) + set(PYTHON_EXT_SUFFIX ".pyd") + message(STATUS "Defaulting Python extension suffix to .pyd for Windows.") + else() + set(PYTHON_EXT_SUFFIX ".so") + message(STATUS "Defaulting Python extension suffix to .so for non-Windows.") + endif() +else() + message(STATUS "Determined Python extension suffix: ${PYTHON_EXT_SUFFIX}") +endif() + +# Set the output name and suffix for the Python module +set_target_properties(ThetaStarPlanner PROPERTIES + PREFIX "" # No 'lib' prefix for Python modules + LIBRARY_OUTPUT_NAME "ThetaStarPlanner" # Output name + SUFFIX "${PYTHON_EXT_SUFFIX}" # Suffix like .cpython-XYZ.so or .pyd +) \ No newline at end of file diff --git a/src/asearch.cpp b/src/asearch.cpp new file mode 100644 index 0000000..41c6966 --- /dev/null +++ b/src/asearch.cpp @@ -0,0 +1,72 @@ +#include "mission.h" + +int main(int argc, char* argv[]) +{ + if(argc < 2) { + std::cout<<"Error! Pathfinding task file (XML) is not specified!"<<std::endl; + return 0; + } + + Mission mission(argv[1]); + + std::cout<<argv[1]<<std::endl; + std::cout<<"Parsing the config from XML:"<<std::endl; + + std::vector<std::vector<int>> mapGrid = { + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0}, + {0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0}, + {0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0}, + {0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} + }; + int cellSize = 24; + int startX = 12; + int startY = 20; + int finishX = 12; + int finishY = 7; + + if(!mission.getMap(startX, startY, finishX, finishY, cellSize, mapGrid)) { + std::cout<<"Incorrect map! Program halted!"<<std::endl; + } + else { + std::cout<<"Map OK!"<<std::endl<<"Parsing configurations (algorithm, log) from XML:"<<std::endl; + if(!mission.getConfig()) + std::cout<<"Incorrect configurations! Program halted!"<<std::endl; + else { + std::cout<<"Configurations OK!"<<std::endl<<"Creating log channel:"<<std::endl; + + std::cout<<"Log OK!"<<std::endl<<"Start searching the path:"<<std::endl; + + mission.createEnvironmentOptions(); + mission.createSearch(); + mission.startSearch(); + + std::cout<<"Search is finished!"<<std::endl; + + mission.printSearchResultsToConsole(); + + std::cout<<"Results are saved (if chosen) via created log channel."<<std::endl; + } + } + return 0; +} diff --git a/astar.cpp b/src/astar.cpp similarity index 100% rename from astar.cpp rename to src/astar.cpp diff --git a/config.cpp b/src/config.cpp similarity index 68% rename from config.cpp rename to src/config.cpp index 10cbd68..744fc56 100644 --- a/config.cpp +++ b/src/config.cpp @@ -8,16 +8,118 @@ Config::Config() { - LogParams = nullptr; SearchParams = nullptr; } + Config::~Config() { if (SearchParams) delete[] SearchParams; - if (LogParams) delete[] LogParams; } +// Set configuration parameters directly, instead of reading from XML +void Config::setConfig(int st, double hw, int mt, int bt, int ad, int cc, int asq, int ps) +{ + if (SearchParams) delete[] SearchParams; + N = 8; + SearchParams = new double[N]; + SearchParams[CN_SP_ST] = st; + SearchParams[CN_SP_HW] = hw; + SearchParams[CN_SP_MT] = mt; + SearchParams[CN_SP_BT] = bt; + SearchParams[CN_SP_AD] = ad; + SearchParams[CN_SP_CC] = cc; + SearchParams[CN_SP_AS] = asq; + SearchParams[CN_SP_PS] = ps; +} + +// Set a parameter by tag name (as in gl_const.h) and string value (for bools and enums) +bool Config::setParamByTag(const std::string& tag, const std::string& value) +{ + if (!SearchParams) setDefaultConfigAstar(); // Initialize with A* defaults if not already initialized + std::string v = value; + std::transform(v.begin(), v.end(), v.begin(), ::tolower); + if (tag == CNS_TAG_ST) { + if (v == CNS_SP_ST_ASTAR) { SearchParams[CN_SP_ST] = CN_SP_ST_ASTAR; return true; } + if (v == CNS_SP_ST_TH) { SearchParams[CN_SP_ST] = CN_SP_ST_TH; return true; } + return false; + } + if (tag == CNS_TAG_MT) { + if (v == CNS_SP_MT_DIAG) { SearchParams[CN_SP_MT] = CN_SP_MT_DIAG; return true; } + if (v == CNS_SP_MT_MANH) { SearchParams[CN_SP_MT] = CN_SP_MT_MANH; return true; } + if (v == CNS_SP_MT_EUCL) { SearchParams[CN_SP_MT] = CN_SP_MT_EUCL; return true; } + if (v == CNS_SP_MT_CHEB) { SearchParams[CN_SP_MT] = CN_SP_MT_CHEB; return true; } + return false; + } + if (tag == CNS_TAG_BT) { + if (v == CNS_SP_BT_GMIN) { SearchParams[CN_SP_BT] = CN_SP_BT_GMIN; return true; } + if (v == CNS_SP_BT_GMAX) { SearchParams[CN_SP_BT] = CN_SP_BT_GMAX; return true; } + return false; + } + if (tag == CNS_TAG_AD || tag == CNS_TAG_CC || tag == CNS_TAG_AS || tag == CNS_TAG_PS) { + if (v == "1" || v == "true") { + if (tag == CNS_TAG_AD) SearchParams[CN_SP_AD] = 1; + if (tag == CNS_TAG_CC) SearchParams[CN_SP_CC] = 1; + if (tag == CNS_TAG_AS) SearchParams[CN_SP_AS] = 1; + if (tag == CNS_TAG_PS) SearchParams[CN_SP_PS] = 1; + return true; + } + if (v == "0" || v == "false") { + if (tag == CNS_TAG_AD) SearchParams[CN_SP_AD] = 0; + if (tag == CNS_TAG_CC) SearchParams[CN_SP_CC] = 0; + if (tag == CNS_TAG_AS) SearchParams[CN_SP_AS] = 0; + if (tag == CNS_TAG_PS) SearchParams[CN_SP_PS] = 0; + return true; + } + return false; + } + return false; +} + +// Set a parameter by tag name and double value (for numeric params) +bool Config::setParamByTag(const std::string& tag, double value) +{ + if (!SearchParams) setDefaultConfigAstar(); // Initialize with A* defaults if not already initialized + if (tag == CNS_TAG_HW) { + if (value < 1) value = 1; + SearchParams[CN_SP_HW] = value; + return true; + } + return false; +} + +// Set all parameters to default values (A* with Euclidean, all options off) +void Config::setDefaultConfigAstar() +{ + if (SearchParams) delete[] SearchParams; + N = 8; + SearchParams = new double[N]; + SearchParams[CN_SP_ST] = CN_SP_ST_ASTAR; + SearchParams[CN_SP_HW] = 1; + SearchParams[CN_SP_MT] = CN_SP_MT_EUCL; + SearchParams[CN_SP_BT] = CN_SP_BT_GMAX; + SearchParams[CN_SP_AD] = 1; + SearchParams[CN_SP_CC] = 0; + SearchParams[CN_SP_AS] = 0; + SearchParams[CN_SP_PS] = 0; +} + +void Config::setDefaultConfigTheta() +{ + if (SearchParams) delete[] SearchParams; + N = 8; + SearchParams = new double[N]; + SearchParams[CN_SP_ST] = CN_SP_ST_TH; + SearchParams[CN_SP_HW] = 1; + SearchParams[CN_SP_MT] = CN_SP_MT_EUCL; // Theta* requires Euclidean metric + SearchParams[CN_SP_BT] = CN_SP_BT_GMAX; + SearchParams[CN_SP_AD] = 1; + SearchParams[CN_SP_CC] = 0; + SearchParams[CN_SP_AS] = 0; + SearchParams[CN_SP_PS] = 0; +} + + bool Config::getConfig(const char *FileName) { std::string value; @@ -51,23 +153,11 @@ bool Config::getConfig(const char *FileName) value = element->GetText(); std::transform(value.begin(), value.end(), value.begin(), ::tolower); - if (value == CNS_SP_ST_BFS) { - N = 8; - SearchParams = new double[N]; - SearchParams[CN_SP_ST] = CN_SP_ST_BFS; - } - else if (value == CNS_SP_ST_DIJK) { - N = 8; - SearchParams = new double[N]; - SearchParams[CN_SP_ST] = CN_SP_ST_DIJK; - } - else if (value == CNS_SP_ST_ASTAR || value == CNS_SP_ST_JP_SEARCH || value == CNS_SP_ST_TH) { + if (value == CNS_SP_ST_ASTAR || value == CNS_SP_ST_TH) { N = 8; SearchParams = new double[N]; SearchParams[CN_SP_ST] = CN_SP_ST_ASTAR; - if (value == CNS_SP_ST_JP_SEARCH) - SearchParams[CN_SP_ST] = CN_SP_ST_JP_SEARCH; - else if (value == CNS_SP_ST_TH) + if (value == CNS_SP_ST_TH) SearchParams[CN_SP_ST] = CN_SP_ST_TH; element = algorithm->FirstChildElement(CNS_TAG_HW); if (!element) { @@ -113,7 +203,7 @@ bool Config::getConfig(const char *FileName) } } - + element = algorithm->FirstChildElement(CNS_TAG_BT); if (!element) { std::cout << "Warning! No '" << CNS_TAG_BT << "' tag found in XML file" << std::endl; @@ -135,8 +225,8 @@ bool Config::getConfig(const char *FileName) else { std::cout << "Error! Value of '" << CNS_TAG_ST << "' tag (algorithm name) is not correctly specified." << std::endl; - std::cout << "Supported algorithm's names are: '" << CNS_SP_ST_BFS << "', '" << CNS_SP_ST_DIJK << "', '" - << CNS_SP_ST_ASTAR << "', '" << CNS_SP_ST_TH << "', '" << CNS_SP_ST_JP_SEARCH << "'." << std::endl; + std::cout << "Supported algorithm's names are: '" << + CNS_SP_ST_ASTAR << "', '" << CNS_SP_ST_TH << "'." << std::endl; return false; } @@ -224,7 +314,7 @@ bool Config::getConfig(const char *FileName) } } } - + element = algorithm->FirstChildElement(CNS_TAG_PS); if (!element) { @@ -242,7 +332,7 @@ bool Config::getConfig(const char *FileName) stream >> check; stream.clear(); stream.str(""); - + if (check != "1" && check != "true" && check != "0" && check != "false") { std::cout << "Warning! Value of '" << CNS_TAG_PS << "' is not correctly specified." << std::endl; std::cout << "Value of '" << CNS_TAG_PS << "' was defined to default - false " << std::endl; @@ -254,79 +344,6 @@ bool Config::getConfig(const char *FileName) SearchParams[CN_SP_PS] = 0; } } - - - options = root->FirstChildElement(CNS_TAG_OPT); - LogParams = new std::string[3]; - LogParams[CN_LP_PATH] = ""; - LogParams[CN_LP_NAME] = ""; - - if (!options) { - std::cout << "Warning! No '" << CNS_TAG_OPT << "' tag found in XML file." << std::endl; - std::cout << "Value of '" << CNS_TAG_LOGLVL << "' tag was defined to 'short log' (1)." << std::endl; - LogParams[CN_LP_LEVEL] = CN_LP_LEVEL_SHORT_WORD; - } - else { - element = options->FirstChildElement(CNS_TAG_LOGLVL); - if (!element) { - std::cout << "Warning! No '" << CNS_TAG_LOGLVL << "' tag found in XML file." << std::endl; - std::cout << "Value of '" << CNS_TAG_LOGLVL << "' tag was defined to 'short log' (1)." << std::endl; - LogParams[CN_LP_LEVEL] = CN_LP_LEVEL_SHORT_WORD; - } - else { - stream << element->GetText(); - stream >> value; - stream.str(""); - stream.clear(); - //std::transform(value.begin(), value.end(), value.begin(), ::tolower); - if (value == CN_LP_LEVEL_NOPE_WORD || value == CN_LP_LEVEL_NOPE_VALUE) - LogParams[CN_LP_LEVEL] = CN_LP_LEVEL_NOPE_WORD; - else if (value == CN_LP_LEVEL_TINY_WORD || value == CN_LP_LEVEL_TINY_VALUE) - LogParams[CN_LP_LEVEL] = CN_LP_LEVEL_TINY_WORD; - else if (value == CN_LP_LEVEL_SHORT_WORD || value == CN_LP_LEVEL_SHORT_VALUE) - LogParams[CN_LP_LEVEL] = CN_LP_LEVEL_SHORT_WORD; - else if (value == CN_LP_LEVEL_MEDIUM_WORD || value == CN_LP_LEVEL_MEDIUM_VALUE) - LogParams[CN_LP_LEVEL] = CN_LP_LEVEL_MEDIUM_WORD; - else if (value == CN_LP_LEVEL_FULL_WORD || value == CN_LP_LEVEL_FULL_VALUE) - LogParams[CN_LP_LEVEL] = CN_LP_LEVEL_FULL_WORD; - else { - std::cout << "'" << CNS_TAG_LOGLVL << "' is not correctly specified" << std::endl; - std::cout << "Value of '" << CNS_TAG_LOGLVL << "' tag was defined to 'short log' (1)." << std::endl; - LogParams[CN_LP_LEVEL] = CN_LP_LEVEL_SHORT_WORD; - } - std::cout << "LogLevel: " << LogParams[CN_LP_LEVEL] << std::endl; - } - - - element = options->FirstChildElement(CNS_TAG_LOGPATH); - if (!element) { - std::cout << "Warning! No '" << CNS_TAG_LOGPATH << "' tag found in XML file." << std::endl; - std::cout << "Value of '" << CNS_TAG_LOGPATH << "' tag was defined to 'current directory'." << std::endl; - } - else if (!element->GetText()) { - std::cout << "Warning! Value of '" << CNS_TAG_LOGPATH << "' tag is missing!" << std::endl; - std::cout << "Value of '" << CNS_TAG_LOGPATH << "' tag was defined to 'current directory'." << std::endl; - } - else { - LogParams[CN_LP_PATH] = element->GetText(); - } - - element = options->FirstChildElement(CNS_TAG_LOGFN); - if (!element) { - std::cout << "Warning! No '" << CNS_TAG_LOGFN << "' tag found in XML file!" << std::endl; - std::cout << "Value of '" << CNS_TAG_LOGFN - << "' tag was defined to default (original filename +'_log' + original file extension." - << std::endl; - } - else if (!element->GetText()) { - std::cout << "Warning! Value of '" << CNS_TAG_LOGFN << "' tag is missing." << std::endl; - std::cout << "Value of '" << CNS_TAG_LOGFN - << "' tag was defined to default (original filename +'_log' + original file extension." - << std::endl; - } - else - LogParams[CN_LP_NAME] = element->GetText(); - } return true; } diff --git a/environmentoptions.cpp b/src/environmentoptions.cpp similarity index 100% rename from environmentoptions.cpp rename to src/environmentoptions.cpp diff --git a/isearch.cpp b/src/isearch.cpp similarity index 96% rename from isearch.cpp rename to src/isearch.cpp index dfcf2bc..4ce290b 100644 --- a/isearch.cpp +++ b/src/isearch.cpp @@ -22,7 +22,7 @@ bool ISearch::stopCriterion() return false; } -SearchResult ISearch::startSearch(ILogger *Logger, const Map &map, const EnvironmentOptions &options) +SearchResult ISearch::startSearch(const Map &map, const EnvironmentOptions &options) { std::chrono::time_point<std::chrono::system_clock> start, end; start = std::chrono::system_clock::now(); @@ -58,9 +58,7 @@ SearchResult ISearch::startSearch(ILogger *Logger, const Map &map, const Environ addOpen(*it); it++; } - Logger->writeToLogOpenClose(open, close, false); } - Logger->writeToLogOpenClose(open, close, true); sresult.pathfound = false; sresult.nodescreated = closeSize + openSize; sresult.numberofsteps = closeSize; @@ -74,7 +72,7 @@ SearchResult ISearch::startSearch(ILogger *Logger, const Map &map, const Environ sresult.time = static_cast<double>(std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count()) / 1000000000; if (pathfound) makeSecondaryPath(); - + sresult.hppath = &hppath; //Here is a constant pointer sresult.lppath = &lppath; return sresult; diff --git a/src/map.cpp b/src/map.cpp new file mode 100644 index 0000000..d961c36 --- /dev/null +++ b/src/map.cpp @@ -0,0 +1,82 @@ +#include "map.h" + +Map::Map() +{ + height = -1; + width = -1; + start_i = -1; + start_j = -1; + goal_i = -1; + goal_j = -1; + Grid = nullptr; + cellSize = 1; +} + +Map::~Map() +{ +} + +bool Map::CellIsTraversable(int i, int j) const +{ + return ((*Grid)[i][j] == CN_GC_NOOBS); +} + +bool Map::CellIsObstacle(int i, int j) const +{ + return ((*Grid)[i][j] != CN_GC_NOOBS); +} + +bool Map::CellOnGrid(int i, int j) const +{ + return (i < height && i >= 0 && j < width && j >= 0); +} + +bool Map::getMap(const std::vector<std::vector<int>>& map, int startx, int starty, int finishx, int finishy, int cell_size) +{ + Grid = ↦ + if (!Grid || Grid->empty() || (*Grid)[0].empty()) { + std::cout << "Error! Empty map provided!" << std::endl; + return false; + } + height = map.size(); + width = map[0].size(); + + // printf("Map size: %d x %d\n", height, width); + start_i = starty; + start_j = startx; + goal_i = finishy; + goal_j = finishx; + cellSize = cell_size; + + // printf("Start cell: (%d, %d)\n", start_i, start_j); + // printf("Goal cell: (%d, %d)\n", goal_i, goal_j); + // printf("Cell size: %f\n", cellSize); + // printf("Start cell value: %d\n", (*Grid)[start_i][start_j]); + // printf("Goal cell value: %d\n", (*Grid)[goal_i][goal_j]); + // printf("CN_GC_NOOBS: %d\n", CN_GC_NOOBS); + + if ((*Grid)[start_i][start_j] != CN_GC_NOOBS) { + std::cout << "Error! Start cell is not traversable (cell's value is" << (*Grid)[start_i][start_j] << ")!" + << std::endl; + return false; + } + + if ((*Grid)[goal_i][goal_j] != CN_GC_NOOBS) { + std::cout << "Error! Goal cell is not traversable (cell's value is" << (*Grid)[goal_i][goal_j] << ")!" + << std::endl; + return false; + } + + return true; +} + +int Map::getValue(int i, int j) const +{ + if (i < 0 || i >= height) + return -1; + + if (j < 0 || j >= width) + return -1; + + return (*Grid)[i][j]; +} diff --git a/src/mission.cpp b/src/mission.cpp new file mode 100644 index 0000000..5ae108a --- /dev/null +++ b/src/mission.cpp @@ -0,0 +1,124 @@ +#include "mission.h" +#include "astar.h" +#include "theta.h" +#include "gl_const.h" + +Mission::Mission() +{ + search = nullptr; + fileName = nullptr; +} + +Mission::Mission(const char *FileName) +{ + fileName = FileName; + search = nullptr; +} + +Mission::~Mission() +{ + if (search) + delete search; +} + +bool Mission::getMap(int startX, int startY, int endX, int endY, int cellSize, std::vector<std::vector<int>> &mapData) +{ + return map.getMap(mapData, startX, startY, endX, endY, cellSize); +} + +bool Mission::getConfig() +{ + return config.getConfig(fileName); +} + +bool Mission::setDefaultConfig(bool use_theta) +{ + if (use_theta) + config.setDefaultConfigTheta(); + else + config.setDefaultConfigAstar(); + return true; +} + +void Mission::createEnvironmentOptions() +{ + options = EnvironmentOptions(config.SearchParams[CN_SP_AS], config.SearchParams[CN_SP_AD], + config.SearchParams[CN_SP_CC], config.SearchParams[CN_SP_MT]); +} + +void Mission::createSearch() +{ + if (search) + delete search; + if (config.SearchParams[CN_SP_ST] == CN_SP_ST_ASTAR) + { + std::cout << "Using A* search algorithm." << std::endl; + search = new Astar(config.SearchParams[CN_SP_HW], config.SearchParams[CN_SP_BT]); + } + else if (config.SearchParams[CN_SP_ST] == CN_SP_ST_TH) + { + std::cout << "Using Theta* search algorithm." << std::endl; + search = new Theta(config.SearchParams[CN_SP_HW], config.SearchParams[CN_SP_BT]); + } +} + +void Mission::startSearch() +{ + sr = search->startSearch(map, options); + if (config.SearchParams[CN_SP_PS]) + { + smooth_search_result(sr, map, options.cutcorners); + } +} + +void Mission::getPath(std::vector<std::vector<int>> &path) +{ + path.clear(); + if (sr.pathfound) { + std::list<Node> &srpath = *sr.lppath; + for (std::list<Node>::const_iterator it = srpath.begin(); it != srpath.end(); it++) { + std::vector<int> point; + point.push_back(it->j); + point.push_back(it->i); + path.push_back(point); + } + // The sr.lppath should already contain all nodes from start to goal. + // The following lines attempting to add sr.end are removed as SearchResult has no 'end' member + // and lppath is expected to be complete. + // std::vector<int> endPoint; + // endPoint.push_back(sr.end.j); + // endPoint.push_back(sr.end.i); + // path.push_back(endPoint); + } +} + +bool Mission::getPathValid() +{ + return sr.pathfound; +} + +void Mission::printSearchResultsToConsole() +{ + std::cout << "Path "; + if (!sr.pathfound) + std::cout << "NOT "; + std::cout << "found!" << std::endl; + std::cout << "numberofsteps=" << sr.numberofsteps << std::endl; + std::cout << "nodescreated=" << sr.nodescreated << std::endl; + if (sr.pathfound) { + std::cout << "pathlength=" << sr.pathlength << std::endl; + std::cout << "pathlength_scaled=" << sr.pathlength * map.cellSize << std::endl; + } + std::cout << "time=" << sr.time << std::endl; +} + + +const char *Mission::getAlgorithmName() +{ + if (config.SearchParams[CN_SP_ST] == CN_SP_ST_ASTAR) + return CNS_SP_ST_ASTAR; + else if (config.SearchParams[CN_SP_ST] == CN_SP_ST_TH) + return CNS_SP_ST_TH; + else + return ""; +} diff --git a/path_smoothing.cpp b/src/path_smoothing.cpp similarity index 100% rename from path_smoothing.cpp rename to src/path_smoothing.cpp diff --git a/src/python_bindings.cpp b/src/python_bindings.cpp new file mode 100644 index 0000000..9737855 --- /dev/null +++ b/src/python_bindings.cpp @@ -0,0 +1,58 @@ +#include <pybind11/pybind11.h> +#include <pybind11/stl.h> // For automatic conversion of std::vector +#include "wrapper.h" // Contains the declaration of plan_2d +#include <tuple> // For std::tuple + +namespace py = pybind11; + +PYBIND11_MODULE(ThetaStarPlanner, m) { + m.doc() = R"pbdoc( + Python module for A* and Theta* path planning + --------------------------------------------- + .. currentmodule:: ThetaStarPlanner + .. autosummary:: + :toctree: _generate + plan_2d + )pbdoc"; + + m.def("plan_2d", + [](std::vector<float> &origin, std::vector<int> &dim, std::vector<signed char> &map_data, std::vector<float> &start, std::vector<float> &goal, float resolution, bool use_theta) { + std::vector<std::vector<double>> path; + double time_spent = 0.0; + int status = ::plan_2d(origin, dim, map_data, start, goal, resolution, path, time_spent, use_theta); + return std::make_tuple(status, path, time_spent); + }, + py::arg("origin"), + py::arg("dim"), + py::arg("map_data"), + py::arg("start"), + py::arg("goal"), + py::arg("resolution"), + py::arg("use_theta"), + R"pbdoc( + Plans a 2D path using A* or Theta* algorithm. + + Args: + origin (list[float]): Origin of the map [x, y] in meters. + dim (list[int]): Dimensions of the map [height, width] in grid cells. + map_data (list[int]): Map data as a flattened list (row-major). 0 for free, 1 for obstacle. + Note: C++ expects signed char, Python will pass integers. + start (list[float]): Start coordinates [x, y] in meters. + goal (list[float]): Goal coordinates [x, y] in meters. + resolution (float): Map resolution in meters/cell. + use_theta (bool): If true, uses Theta*; otherwise, uses A*. + + Returns: + tuple[int, list[list[float]], float]: A tuple containing: + - status (int): 0 if successful, -1 otherwise. + - path (list[list[float]]): The calculated path, a list of [x,y] coordinates. + - time_spent (float): The time spent in planning (in milliseconds). + )pbdoc" + ); + +#ifdef VERSION_INFO + m.attr("__version__") = VERSION_INFO; +#else + m.attr("__version__") = "dev"; +#endif +} diff --git a/theta.cpp b/src/theta.cpp similarity index 100% rename from theta.cpp rename to src/theta.cpp diff --git a/tinyxml2.cpp b/src/tinyxml2.cpp similarity index 100% rename from tinyxml2.cpp rename to src/tinyxml2.cpp diff --git a/src/wrapper.cpp b/src/wrapper.cpp new file mode 100644 index 0000000..86ad493 --- /dev/null +++ b/src/wrapper.cpp @@ -0,0 +1,100 @@ +#include "mission.h" +#include <chrono> +#include "wrapper.h" + +// using namespace JPS; // Removed as JPS namespace is not defined or used by plan_2d related components + +class Timer { + typedef std::chrono::high_resolution_clock high_resolution_clock; + typedef std::chrono::milliseconds milliseconds; + public: + explicit Timer(bool run = false) + { + if (run) + Reset(); + } + void Reset() + { + _start = high_resolution_clock::now(); + } + milliseconds Elapsed() const + { + return std::chrono::duration_cast<milliseconds>(high_resolution_clock::now() - _start); + } + private: + high_resolution_clock::time_point _start; +}; + +int plan_2d(std::vector<float> &origin, std::vector<int> &dim, std::vector<signed char> &map, std::vector<float> &start, std::vector<float> &goal, float resolution, std::vector<std::vector<double> > &path, double &time_spent, bool use_theta) +{ + std::vector<std::vector<int>> map_grid; + int height = dim[1]; + int width = dim[0]; + map_grid.resize(height); + for (int i = 0; i < height; ++i) + { + map_grid[i].resize(width); + for (int j = 0; j < width; ++j) + { + map_grid[i][j] = map[i * width + j]; + } + } + int start_x = static_cast<int>((start[0] - origin[0]) / resolution); + int start_y = static_cast<int>((start[1] - origin[1]) / resolution); + int goal_x = static_cast<int>((goal[0] - origin[0]) / resolution); + int goal_y = static_cast<int>((goal[1] - origin[1]) / resolution); + + if (start_x < 0 || start_x >= width || start_y < 0 || start_y >= height || + goal_x < 0 || goal_x >= width || goal_y < 0 || goal_y >= height) + { + return -1; // Invalid start or goal position + } + + Mission mission; + int cellSize = resolution; + if (!mission.getMap(start_x, start_y, goal_x, goal_y, cellSize, map_grid)) + { + return -1; // Failed to get the map + } + + if (use_theta) + { + Timer time_theta(true); + mission.setDefaultConfig(true); // Set default configuration for Theta* + mission.createEnvironmentOptions(); + mission.createSearch(); + mission.startSearch(); + double dt_theta = time_theta.Elapsed().count(); + std::vector<std::vector<int>> path_int; + mission.getPath(path_int); // Get the path from the mission + path.clear(); + for (const auto &pt : path_int) + { + double x = origin[0] + pt[0] * cellSize; + double y = origin[1] + pt[1] * cellSize; + path.push_back({x, y}); + } + time_spent = dt_theta; + return mission.getPathValid() ? 0 : -1; // Return 0 if the path is valid + } + else{ + Timer time_astar(true); + mission.setDefaultConfig(false); // Set default configuration for A* + mission.createEnvironmentOptions(); + mission.createSearch(); + mission.startSearch(); + double dt_astar = time_astar.Elapsed().count(); + std::vector<std::vector<int>> path_int; + mission.getPath(path_int); // Get the path from the mission + path.clear(); + for (const auto &pt : path_int) + { + double x = origin[0] + pt[0] * cellSize; + double y = origin[1] + pt[1] * cellSize; + path.push_back({x, y}); + } + time_spent = dt_astar; + return mission.getPathValid() ? 0 : -1; // Return 0 if the path is valid + } + return 0; +} \ No newline at end of file diff --git a/xmllogger.cpp b/xmllogger.cpp deleted file mode 100644 index c44bcc7..0000000 --- a/xmllogger.cpp +++ /dev/null @@ -1,277 +0,0 @@ -#include "xmllogger.h" - -using tinyxml2::XMLElement; -using tinyxml2::XMLNode; - -bool XmlLogger::getLog(const char *FileName, const std::string *LogParams) -{ - if (loglevel == CN_LP_LEVEL_NOPE_WORD) return true; - - if (doc.LoadFile(FileName) != tinyxml2::XMLError::XML_SUCCESS) { - std::cout << "Error opening input XML file" << std::endl; - return false; - } - - if (LogParams[CN_LP_PATH] == "" && LogParams[CN_LP_NAME] == "") { - std::string str; - str.append(FileName); - size_t found = str.find_last_of("."); - if (found != std::string::npos) - str.insert(found, "_log"); - else - str.append("_log"); - LogFileName.append(str); - } else if (LogParams[CN_LP_PATH] == "") { - LogFileName.append(FileName); - std::string::iterator it = LogFileName.end(); - while (*it != '\\') - it--; - ++it; - LogFileName.erase(it, LogFileName.end()); - LogFileName.append(LogParams[CN_LP_NAME]); - } else if (LogParams[CN_LP_NAME] == "") { - LogFileName.append(LogParams[CN_LP_PATH]); - if (*(--LogParams[CN_LP_PATH].end()) != '\\') LogFileName.append("\\"); - std::string lfn; - lfn.append(FileName); - size_t found = lfn.find_last_of("\\"); - std::string str = lfn.substr(found); - found = str.find_last_of("."); - if (found != std::string::npos) - str.insert(found, "_log"); - else - str.append("_log"); - LogFileName.append(str); - } else { - LogFileName.append(LogParams[CN_LP_PATH]); - if (*(--LogParams[CN_LP_PATH].end()) != '\\') LogFileName.append("\\"); - LogFileName.append(LogParams[CN_LP_NAME]); - } - - XMLElement *log, *root = doc.FirstChildElement(CNS_TAG_ROOT); - - if (!root) { - std::cout << "No '" << CNS_TAG_ROOT << "' element found in XML file" << std::endl; - std::cout << "Can not create log" << std::endl; - return false; - } - - root->InsertEndChild(doc.NewElement(CNS_TAG_LOG)); - - root = (root->LastChild())->ToElement(); - - if (loglevel != CN_LP_LEVEL_NOPE_WORD) { - log = doc.NewElement(CNS_TAG_MAPFN); - log->LinkEndChild(doc.NewText(FileName)); - root->InsertEndChild(log); - - root->InsertEndChild(doc.NewElement(CNS_TAG_SUM)); - - root->InsertEndChild(doc.NewElement(CNS_TAG_PATH)); - - root->InsertEndChild(doc.NewElement(CNS_TAG_LPLEVEL)); - - root->InsertEndChild(doc.NewElement(CNS_TAG_HPLEVEL)); - } - - if (loglevel == CN_LP_LEVEL_FULL_WORD || loglevel == CN_LP_LEVEL_MEDIUM_WORD) - root->InsertEndChild(doc.NewElement(CNS_TAG_LOWLEVEL)); - - return true; -} - -void XmlLogger::saveLog() -{ - if (loglevel == CN_LP_LEVEL_NOPE_WORD) - return; - doc.SaveFile(LogFileName.c_str()); -} - -void XmlLogger::writeToLogMap(const Map &map, const std::list<Node> &path) -{ - if (loglevel == CN_LP_LEVEL_NOPE_WORD || loglevel == CN_LP_LEVEL_TINY_WORD) - return; - - XMLElement *mapTag = doc.FirstChildElement(CNS_TAG_ROOT); - mapTag = mapTag->FirstChildElement(CNS_TAG_LOG)->FirstChildElement(CNS_TAG_PATH); - - int iterate = 0; - bool inPath; - std::string str; - for (int i = 0; i < map.height; ++i) { - XMLElement *element = doc.NewElement(CNS_TAG_ROW); - element->SetAttribute(CNS_TAG_ATTR_NUM, iterate); - - for (int j = 0; j < map.width; ++j) { - inPath = false; - for(std::list<Node>::const_iterator it = path.begin(); it != path.end(); it++) - if(it->i == i && it->j == j) { - inPath = true; - break; - } - if (!inPath) - str += std::to_string(map.getValue(i,j)); - else - str += CNS_OTHER_PATHSELECTION; - str += CNS_OTHER_MATRIXSEPARATOR; - } - - element->InsertEndChild(doc.NewText(str.c_str())); - mapTag->InsertEndChild(element); - str.clear(); - iterate++; - } -} - -void XmlLogger::writeToLogOpenClose(const std::vector<std::list<Node> > &open, const std::unordered_map<int, Node> &close, bool last) -{ - if (loglevel != CN_LP_LEVEL_FULL_WORD && !(loglevel == CN_LP_LEVEL_MEDIUM_WORD && last)) - return; - - XMLElement *element = doc.NewElement(CNS_TAG_STEP); - XMLElement *child = 0; - XMLElement *lowlevel = doc.FirstChildElement(CNS_TAG_ROOT); - lowlevel = lowlevel->FirstChildElement(CNS_TAG_LOG)->FirstChildElement(CNS_TAG_LOWLEVEL); - - int iterate = 0; - for (child = lowlevel->FirstChildElement(); child != nullptr; child = child->NextSiblingElement()) - iterate++; - - element->SetAttribute(CNS_TAG_ATTR_NUM, iterate); - lowlevel->InsertEndChild(element); - lowlevel = lowlevel->LastChildElement(); - - lowlevel->InsertEndChild(doc.NewElement(CNS_TAG_OPEN)); - child = lowlevel->LastChildElement(); - - Node min; - min.F = -1; - int exc = 0; - for (int i = 0; i < open.size(); ++i) { - if (open[i].size() > 0) { - if (open[i].begin()->F <= min.F || min.F == -1) { - if (open[i].begin()->F == min.F && open[i].begin()->g > min.g) { - min = *open[i].begin(); - exc = i; - } else if (open[i].begin()->F < min.F || min.F == -1) { - min = *open[i].begin(); - exc = i; - } - } - } - } - if (min.F != -1) { - element = doc.NewElement(CNS_TAG_POINT); - element->SetAttribute(CNS_TAG_ATTR_X, min.j); - element->SetAttribute(CNS_TAG_ATTR_Y, min.i); - element->SetAttribute(CNS_TAG_ATTR_F, min.F); - element->SetAttribute(CNS_TAG_ATTR_G, min.g); - if (min.g > 0) { - element->SetAttribute(CNS_TAG_ATTR_PARX, min.parent->j); - element->SetAttribute(CNS_TAG_ATTR_PARY, min.parent->i); - } - child->InsertEndChild(element); - } - for (int i = 0; i < open.size(); ++i) { - if (open[i].size() > 0) { - for (auto it = open[i].begin(); it != open[i].end(); ++it) { - if (it != open[exc].begin()) { - element = doc.NewElement(CNS_TAG_POINT); - element->SetAttribute(CNS_TAG_ATTR_X, it->j); - element->SetAttribute(CNS_TAG_ATTR_Y, it->i); - element->SetAttribute(CNS_TAG_ATTR_F, it->F); - element->SetAttribute(CNS_TAG_ATTR_G, it->g); - if (it->g > 0) { - element->SetAttribute(CNS_TAG_ATTR_PARX, it->parent->j); - element->SetAttribute(CNS_TAG_ATTR_PARY, it->parent->i); - } - child->InsertEndChild(element); - } - child->InsertEndChild(element); - } - } - } - - lowlevel->InsertEndChild(doc.NewElement(CNS_TAG_CLOSE)); - child = lowlevel->LastChildElement(); - - for (auto it = close.begin(); it != close.end(); ++it) { - element = doc.NewElement(CNS_TAG_POINT); - element->SetAttribute(CNS_TAG_ATTR_X, it->second.j); - element->SetAttribute(CNS_TAG_ATTR_Y, it->second.i); - element->SetAttribute(CNS_TAG_ATTR_F, it->second.F); - element->SetAttribute(CNS_TAG_ATTR_G, it->second.g); - if (it->second.g > 0) { - element->SetAttribute(CNS_TAG_ATTR_PARX, it->second.parent->j); - element->SetAttribute(CNS_TAG_ATTR_PARY, it->second.parent->i); - } - child->InsertEndChild(element); - } -} - -void XmlLogger::writeToLogPath(const std::list<Node> &path) -{ - if (loglevel == CN_LP_LEVEL_NOPE_WORD || loglevel == CN_LP_LEVEL_TINY_WORD || path.empty()) - return; - int iterate = 0; - XMLElement *lplevel = doc.FirstChildElement(CNS_TAG_ROOT); - lplevel = lplevel->FirstChildElement(CNS_TAG_LOG)->FirstChildElement(CNS_TAG_LPLEVEL); - - for (std::list<Node>::const_iterator it = path.begin(); it != path.end(); it++) { - XMLElement *element = doc.NewElement(CNS_TAG_POINT); - element->SetAttribute(CNS_TAG_ATTR_X, it->j); - element->SetAttribute(CNS_TAG_ATTR_Y, it->i); - element->SetAttribute(CNS_TAG_ATTR_NUM, iterate); - lplevel->InsertEndChild(element); - iterate++; - } -} - -void XmlLogger::writeToLogHPpath(const std::list<Node> &hppath) -{ - if (loglevel == CN_LP_LEVEL_NOPE_WORD || loglevel == CN_LP_LEVEL_TINY_WORD || hppath.empty()) - return; - int partnumber = 0; - XMLElement *hplevel = doc.FirstChildElement(CNS_TAG_ROOT); - hplevel = hplevel->FirstChildElement(CNS_TAG_LOG)->FirstChildElement(CNS_TAG_HPLEVEL); - std::list<Node>::const_iterator iter = hppath.begin(); - std::list<Node>::const_iterator it = hppath.begin(); - - while (iter != --hppath.end()) { - XMLElement *part = doc.NewElement(CNS_TAG_SECTION); - part->SetAttribute(CNS_TAG_ATTR_NUM, partnumber); - part->SetAttribute(CNS_TAG_ATTR_STX, it->j); - part->SetAttribute(CNS_TAG_ATTR_STY, it->i); - ++iter; - part->SetAttribute(CNS_TAG_ATTR_FINX, iter->j); - part->SetAttribute(CNS_TAG_ATTR_FINY, iter->i); - part->SetAttribute(CNS_TAG_ATTR_LENGTH, iter->g - it->g); - hplevel->LinkEndChild(part); - ++it; - ++partnumber; - } -} - -void XmlLogger::writeToLogSummary(unsigned int numberofsteps, unsigned int nodescreated, float length, double time, double cellSize) -{ - if (loglevel == CN_LP_LEVEL_NOPE_WORD) - return; - - XMLElement *summary = doc.FirstChildElement(CNS_TAG_ROOT); - summary = summary->FirstChildElement(CNS_TAG_LOG)->FirstChildElement(CNS_TAG_SUM); - XMLElement *element = summary->ToElement(); - element->SetAttribute(CNS_TAG_ATTR_NUMOFSTEPS, numberofsteps); - element->SetAttribute(CNS_TAG_ATTR_NODESCREATED, nodescreated); - element->SetAttribute(CNS_TAG_ATTR_LENGTH, length); - element->SetAttribute(CNS_TAG_ATTR_LENGTH_SCALED, length*cellSize); - element->SetAttribute(CNS_TAG_ATTR_TIME, std::to_string(time).c_str()); -} - -void XmlLogger::writeToLogNotFound() -{ - if (loglevel == CN_LP_LEVEL_NOPE_WORD) - return; - - XMLElement *node = doc.FirstChildElement(CNS_TAG_ROOT)->FirstChildElement(CNS_TAG_LOG)->FirstChildElement(CNS_TAG_PATH); - node->InsertEndChild(doc.NewText("Path NOT found!")); -} diff --git a/xmllogger.h b/xmllogger.h deleted file mode 100644 index e0f758b..0000000 --- a/xmllogger.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef XMLLOGGER_H -#define XMLLOGGER_H -#include "tinyxml2.h" -#include "ilogger.h" - -class XmlLogger : public ILogger { - -public: - XmlLogger(std::string loglevel):ILogger(loglevel){} - - virtual ~XmlLogger() {}; - - bool getLog(const char *FileName, const std::string *LogParams); - - void saveLog(); - - void writeToLogMap(const Map &Map, const std::list<Node> &path); - - void writeToLogOpenClose(const std::vector<std::list<Node>> &open, const std::unordered_map<int, Node> &close, bool last); - - void writeToLogPath(const std::list<Node> &path); - - void writeToLogHPpath(const std::list<Node> &hppath); - - void writeToLogNotFound(); - - void writeToLogSummary(unsigned int numberofsteps, unsigned int nodescreated, float length, double time, double cellSize); - -private: - std::string LogFileName; - tinyxml2::XMLDocument doc; -}; - -#endif -