cmt-20240921-5: +A*

+ AStar (A*)

Author: nicewang

CMakeLists.txt

@@ -6,6 +6,7 @@ set(CMAKE_CXX_STANDARD 11)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
 # Add subdirectories and compile separately
+add_subdirectory(astar)
 add_subdirectory(monte_carlo_tree_search)
 add_subdirectory(rapidly_exploring_random_tree)
 

README.md

@@ -1,6 +1,7 @@
 ## Robot Path Planning
 * [Monte Carlo Tree Search](monte_carlo_tree_search/README.md)
 * [Rapidly Exploring Random Tree (RRT)](rapidly_exploring_random_tree/README.md)
+* [AStar (A*)](astar/README.md)
 
 ### Appendix: Build & Run
 `CMakeList.txt` mainly for github workflows' building process.
@@ -21,6 +22,8 @@ Run:
 ./monte_carlo_tree_search/mcts.so
 # rrt
 ./rapidly_exploring_random_tree/rrt.so
+# AStar (A*)
+./astar/astar.so
 ```
 
 Clean:

astar/CMakeLists.txt

@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 3.10)
+project(mcts_project)
+
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_FLAGS "-Wall")
+
+# Load configs from config.properties
+file(READ ${CMAKE_CURRENT_LIST_DIR}/config.properties CONFIG_CONTENT)
+
+string(REGEX MATCH "CONFIG_WIDTH=[0-9]+" WIDTH_MATCH ${CONFIG_CONTENT})
+string(REGEX MATCH "CONFIG_HEIGHT=[0-9]+" HEIGHT_MATCH ${CONFIG_CONTENT})
+string(REGEX MATCH "CONFIG_START_X=[0-9]+" START_X_MATCH ${CONFIG_CONTENT})
+string(REGEX MATCH "CONFIG_START_Y=[0-9]+" START_Y_MATCH ${CONFIG_CONTENT})
+string(REGEX MATCH "CONFIG_GOAL_X=[0-9]+" GOAL_X_MATCH ${CONFIG_CONTENT})
+string(REGEX MATCH "CONFIG_GOAL_Y=[0-9]+" GOAL_Y_MATCH ${CONFIG_CONTENT})
+
+# config values
+string(REPLACE "CONFIG_WIDTH=" "" CONFIG_WIDTH ${WIDTH_MATCH})
+string(REPLACE "CONFIG_HEIGHT=" "" CONFIG_HEIGHT ${HEIGHT_MATCH})
+string(REPLACE "CONFIG_START_X=" "" CONFIG_START_X ${START_X_MATCH})
+string(REPLACE "CONFIG_START_Y=" "" CONFIG_START_Y ${START_Y_MATCH})
+string(REPLACE "CONFIG_GOAL_X=" "" CONFIG_GOAL_X ${GOAL_X_MATCH})
+string(REPLACE "CONFIG_GOAL_Y=" "" CONFIG_GOAL_Y ${GOAL_Y_MATCH})
+
+# compile options
+add_compile_definitions(
+    CONFIG_WIDTH=${CONFIG_WIDTH}
+    CONFIG_HEIGHT=${CONFIG_HEIGHT}
+    CONFIG_START_X=${CONFIG_START_X}
+    CONFIG_START_Y=${CONFIG_START_Y}
+    CONFIG_GOAL_X=${CONFIG_GOAL_X}
+    CONFIG_GOAL_Y=${CONFIG_GOAL_Y}
+)
+
+# Set dir of include, src and obj
+set(INCDIR ${CMAKE_SOURCE_DIR}/include)
+set(SRCDIR ${CMAKE_SOURCE_DIR}/src)
+set(OBJDIR ${CMAKE_BINARY_DIR}/obj)
+
+# Set obj files and src files
+set(SRCFILES ${CMAKE_CURRENT_LIST_DIR}/src/astar.cpp ${CMAKE_CURRENT_LIST_DIR}/src/main.cpp)
+set(OBJFILES ${OBJDIR}/astar.o ${OBJDIR}/main.o)
+
+# Include Dir
+include_directories(${INCDIR})
+
+# Add an object library to control the generation of object files
+add_library(astar_objects OBJECT ${SRCFILES})
+
+# Set the output directory to the obj folder
+set_target_properties(astar_objects PROPERTIES
+    ARCHIVE_OUTPUT_DIRECTORY ${OBJDIR}
+    LIBRARY_OUTPUT_DIRECTORY ${OBJDIR}
+    RUNTIME_OUTPUT_DIRECTORY ${OBJDIR}
+)
+
+# executable file
+add_executable(astar.so $<TARGET_OBJECTS:astar_objects>)
+
+# clean
+add_custom_target(clean_astar
+    COMMAND ${CMAKE_COMMAND} -E remove_directory ${OBJDIR}
+    COMMAND ${CMAKE_COMMAND} -E remove ${CMAKE_BINARY_DIR}/astar.so
+)

astar/Makefile

@@ -0,0 +1,38 @@
+CONFIG_FILE = config.properties
+
+CONFIG_WIDTH = $(shell grep ^CONFIG_WIDTH $(CONFIG_FILE) | cut -d '=' -f2)
+CONFIG_HEIGHT = $(shell grep ^CONFIG_HEIGHT $(CONFIG_FILE) | cut -d '=' -f2)
+CONFIG_START_X = $(shell grep ^CONFIG_START_X $(CONFIG_FILE) | cut -d '=' -f2)
+CONFIG_START_Y = $(shell grep ^CONFIG_START_Y $(CONFIG_FILE) | cut -d '=' -f2)
+CONFIG_GOAL_X = $(shell grep ^CONFIG_GOAL_X $(CONFIG_FILE) | cut -d '=' -f2)
+CONFIG_GOAL_Y = $(shell grep ^CONFIG_GOAL_Y $(CONFIG_FILE) | cut -d '=' -f2)
+
+# Compiler
+CXX = g++
+CXXFLAGS = -std=c++11 -Wall -DCONFIG_WIDTH=$(CONFIG_WIDTH) -DCONFIG_HEIGHT=$(CONFIG_HEIGHT) -DCONFIG_START_X=$(CONFIG_START_X) -DCONFIG_START_Y=$(CONFIG_START_Y) -DCONFIG_GOAL_X=$(CONFIG_GOAL_X) -DCONFIG_GOAL_Y=$(CONFIG_GOAL_Y)
+
+# Directories
+INCDIR = include
+SRCDIR = src
+OBJDIR = obj
+
+# Files
+SRCFILES = $(SRCDIR)/astar.cpp $(SRCDIR)/main.cpp
+OBJFILES = $(OBJDIR)/astar.o $(OBJDIR)/main.o
+TARGET = astar.so
+
+# Ensure the object directory exists and compile target
+$(TARGET): $(OBJFILES)
+	$(CXX) $(OBJFILES) -o $(TARGET)
+
+# Create object directory if not exists and compile source files into objects
+$(OBJDIR)/%.o: $(SRCDIR)/%.cpp | $(OBJDIR)
+	$(CXX) $(CXXFLAGS) -I$(INCDIR) -c $< -o $@
+
+# Create object directory
+$(OBJDIR):
+	mkdir -p $(OBJDIR)
+
+# Clean up
+clean:
+	rm -rf $(OBJDIR) $(TARGET)

astar/README.md

@@ -0,0 +1,25 @@
+## AStar (A*)
+[Original Paper](paper/astar.pdf)
+
+### Appendix: Build & Run
+`CMakeList.txt` mainly for github workflows' building process.
+
+If you want to build manually, use `Makefile` there. 
+
+* Build
+
+```shell
+make
+```
+
+* Run:
+
+```shell
+./astar.so
+```
+
+* Clean:
+
+```shell
+make clean
+```

astar/config.properties

@@ -0,0 +1,6 @@
+CONFIG_WIDTH=7
+CONFIG_HEIGHT=7
+CONFIG_START_X=0
+CONFIG_START_Y=1
+CONFIG_GOAL_X=5
+CONFIG_GOAL_Y=6

astar/include/astar.h

@@ -0,0 +1,17 @@
+#ifndef ASTAR_H
+#define ASTAR_H
+
+#include <vector>
+#include <utility>
+
+struct Node {
+    int x, y;
+    double f, g, h;
+    bool operator>(const Node& other) const;
+};
+
+double heuristic(int x1, int y1, int x2, int y2);
+std::vector<std::pair<int, int>> getNeighbors(int x, int y, int width, int height);
+std::vector<std::pair<int, int>> aStar(int startX, int startY, int goalX, int goalY, int width, int height);
+
+#endif // ASTAR_H

astar/paper/astar.pdf

@@ -0,0 +1,80 @@
+#include "../include/astar.h"
+#include <queue>
+#include <unordered_map>
+#include <cmath>
+#include <iostream>
+
+using namespace std;
+
+bool Node::operator>(const Node& other) const {
+    return f > other.f;
+}
+
+double heuristic(int x1, int y1, int x2, int y2) {
+    return abs(x1 - x2) + abs(y1 - y2); // Manhattan distance
+}
+
+vector<pair<int, int>> getNeighbors(int x, int y, int width, int height) {
+    vector<pair<int, int>> neighbors;
+    if (x > 0) neighbors.emplace_back(x - 1, y);
+    if (x < width - 1) neighbors.emplace_back(x + 1, y);
+    if (y > 0) neighbors.emplace_back(x, y - 1);
+    if (y < height - 1) neighbors.emplace_back(x, y + 1);
+    return neighbors;
+}
+
+void printOpenSet(priority_queue<Node, vector<Node>, greater<Node>> openSet) {
+    cout << "Open set (nodes in queue):" << endl;
+    while (!openSet.empty()) {
+        Node node = openSet.top();
+        cout << "Node(" << node.x << ", " << node.y << ") - f: " << node.f << ", g: " << node.g << ", h: " << node.h << endl;
+        openSet.pop();
+    }
+}
+
+vector<pair<int, int>> aStar(int startX, int startY, int goalX, int goalY, int width, int height) {
+    priority_queue<Node, vector<Node>, greater<Node>> openSet;
+    unordered_map<int, Node> allNodes;
+    vector<vector<bool>> closedSet(height, vector<bool>(width, false));
+
+    Node start{startX, startY, 0, 0, heuristic(startX, startY, goalX, goalY)};
+    openSet.push(start);
+    allNodes[startY * width + startX] = start;
+
+    while (!openSet.empty()) {
+        Node current = openSet.top();
+        openSet.pop();
+
+        cout << "Current node: (" << current.x << ", " << current.y << ") - f: " << current.f << ", g: " << current.g << ", h: " << current.h << endl;
+
+        if (current.x == goalX && current.y == goalY) {
+            vector<pair<int, int>> path;
+            path.emplace_back(current.x, current.y);
+            return path; // Return path (only the target node is returned here, and the actual application needs to track the path)
+        }
+
+        closedSet[current.y][current.x] = true;
+
+        for (const auto& neighbor : getNeighbors(current.x, current.y, width, height)) {
+            int neighborX = neighbor.first;
+            int neighborY = neighbor.second;
+
+            if (closedSet[neighborY][neighborX]) continue;
+
+            double tentative_g = current.g + 1; // 假设移动成本为1
+            Node neighborNode{neighborX, neighborY, 0, tentative_g, heuristic(neighborX, neighborY, goalX, goalY)};
+
+            if (allNodes.find(neighborY * width + neighborX) == allNodes.end() || tentative_g < allNodes[neighborY * width + neighborX].g) {
+                neighborNode.f = neighborNode.g + neighborNode.h;
+                allNodes[neighborY * width + neighborX] = neighborNode;
+                openSet.push(neighborNode);
+            }
+        }
+
+        // 打印当前优先队列中的节点
+        printOpenSet(openSet);
+        cout << "-------------------------------" << endl;
+    }
+
+    return {}; // No path found
+}

astar/src/astar.cpp

@@ -0,0 +1,48 @@
+#include <iostream>
+#include "../include/astar.h"
+
+#ifndef CONFIG_WIDTH
+#define CONFIG_WIDTH 5  
+#endif
+constexpr int WIDTH = CONFIG_WIDTH; 
+
+#ifndef CONFIG_HEIGHT
+#define CONFIG_HEIGHT 5  
+#endif
+constexpr int HEIGHT = CONFIG_HEIGHT;
+
+#ifndef CONFIG_START_X
+#define CONFIG_START_X 0
+#endif
+constexpr int START_X = CONFIG_START_X;
+
+#ifndef CONFIG_START_Y
+#define CONFIG_START_Y 0
+#endif
+constexpr int START_Y = CONFIG_START_Y;
+
+#ifndef CONFIG_GOAL_X
+#define CONFIG_GOAL_X 4
+#endif
+constexpr int GOAL_X = CONFIG_GOAL_X;
+
+#ifndef CONFIG_GOAL_Y
+#define CONFIG_GOAL_Y 4
+#endif
+constexpr int GOAL_Y = CONFIG_GOAL_Y;
+
+int main() {
+    int width = WIDTH, height = HEIGHT;
+    std::vector<std::pair<int, int>> path = aStar(START_X, START_Y, GOAL_X, GOAL_Y, width, height);
+
+    if (!path.empty()) {
+        std::cout << "Path found: ";
+        for (const auto& p : path) {
+            std::cout << "(" << p.first << ", " << p.second << ") \n";
+        }
+    } else {
+        std::cout << "No path found!\n";
+    }
+
+    return 0;
+}

astar/src/main.cpp

@@ -36,7 +36,7 @@ set(OBJDIR ${CMAKE_BINARY_DIR}/obj)
 
 # Set obj files and src files
 set(SRCFILES ${CMAKE_CURRENT_LIST_DIR}/src/state.cpp ${CMAKE_CURRENT_LIST_DIR}/src/mcts.cpp ${CMAKE_CURRENT_LIST_DIR}/src/main.cpp)
-set(OBJFILES ${OBJDIR}/mcts.o ${OBJDIR}/main.o)
+set(OBJFILES ${OBJDIR}/state.o ${OBJDIR}/mcts.o ${OBJDIR}/main.o)
 
 # Include Dir
 include_directories(${INCDIR})

monte_carlo_tree_search/CMakeLists.txt

@@ -6,24 +6,32 @@ CONFIG_GOAL_Y = $(shell grep ^CONFIG_GOAL_Y $(CONFIG_FILE) | cut -d '=' -f2)
 CONFIG_MAX_SIM_STEPS = $(shell grep ^CONFIG_MAX_SIM_STEPS $(CONFIG_FILE) | cut -d '=' -f2)
 CONFIG_C = $(shell grep ^CONFIG_C $(CONFIG_FILE) | cut -d '=' -f2)
 
+# Compiler
 CXX = g++
 CXXFLAGS = -std=c++11 -Wall -DCONFIG_GRID_SIZE=$(CONFIG_GRID_SIZE) -DCONFIG_GOAL_X=$(CONFIG_GOAL_X) -DCONFIG_GOAL_Y=$(CONFIG_GOAL_Y) -DCONFIG_MAX_SIM_STEPS=$(CONFIG_MAX_SIM_STEPS) -DCONFIG_C=$(CONFIG_C)
 
-# exe file name
-TARGET = mcts.so
+# Directories
+INCDIR = include
+SRCDIR = src
+OBJDIR = obj
 
-# source file
-SRC = src/main.cpp src/mcts.cpp src/state.cpp
+# Files
+SRCFILES = $(SRCDIR)/state.cpp $(SRCDIR)/mcts.cpp $(SRCDIR)/main.cpp
+OBJFILES = $(OBJDIR)/state.o $(OBJDIR)/mcts.o $(OBJDIR)/main.o
+TARGET = mcts.so
 
-# header file
-HDR = include/mcts.h include/state.h
+# Ensure the object directory exists and compile target
+$(TARGET): $(OBJFILES)
+	$(CXX) $(OBJFILES) -o $(TARGET)
 
-# compile rules
-all: $(TARGET)
+# Create object directory if not exists and compile source files into objects
+$(OBJDIR)/%.o: $(SRCDIR)/%.cpp | $(OBJDIR)
+	$(CXX) $(CXXFLAGS) -I$(INCDIR) -c $< -o $@
 
-$(TARGET): $(SRC) $(HDR)
-	$(CXX) $(CXXFLAGS) -o $(TARGET) $(SRC)
+# Create object directory
+$(OBJDIR):
+	mkdir -p $(OBJDIR)
 
-# clear all out files
+# Clean up
 clean:
-	rm -f $(TARGET)
+	rm -rf $(OBJDIR) $(TARGET)