Add A* algorithm tests and update CMake configuration for navigation and listeners

Author: riccardodebenedictis

CMakeLists.txt

@@ -23,7 +23,8 @@ if(${LOGGING_LEVEL_INDEX} EQUAL -1)
     message(FATAL_ERROR "Invalid logging level: ${LOGGING_LEVEL}")
 endif()
 
-option(BUILD_LISTENERS "Build listners")
+option(BUILD_LISTENERS "Build listners" OFF)
+option(BUILD_NAVIGATION "Build navigation" OFF)
 option(BUILD_CRYPTO "Build crypto" OFF)
 
 if(LOGGING_LEVEL STREQUAL "TRACE")
@@ -54,6 +55,11 @@ if(BUILD_LISTENERS)
     target_compile_definitions(utils PUBLIC BUILD_LISTENERS)
 endif()
 
+message(STATUS "Build navigation: ${BUILD_NAVIGATION}")
+if(BUILD_NAVIGATION)
+    target_compile_definitions(utils PUBLIC BUILD_NAVIGATION)
+endif()
+
 message(STATUS "Build crypto: ${BUILD_CRYPTO}")
 if(BUILD_CRYPTO)
     find_package(OpenSSL REQUIRED)

include/a_star.hpp

@@ -4,6 +4,7 @@
 #include <memory>
 #include <queue>
 #include <unordered_set>
+#include <stdexcept>
 
 namespace utils
 {
@@ -116,12 +117,23 @@ namespace utils
     {
       while (!open_list.empty())
       {
-        auto c_node = open_list.top();
+        auto next = open_list.top();
+        open_list.pop();
+#ifdef BUILD_NAVIGATION
+        backtrack_to(find_common_ancestor(*c_node, *next));
+        if (!go_to(*next))
+        { // Conflict detected, backtrack..
+#ifdef BUILD_LISTENERS
+          inconsistent_node(*next);
+#endif
+          continue;
+        }
+#else
+        c_node = next;
 #ifdef BUILD_LISTENERS
         current_node(*c_node);
 #endif
-        open_list.pop();
-
+#endif
         if (c_node->is_goal())
           return c_node;
 
@@ -138,11 +150,12 @@ namespace utils
     }
 
 #ifdef BUILD_LISTENERS
-    virtual void current_node(const node<Tp> &c_node) {}
+    virtual void current_node(const node<Tp> &n) {}
+    virtual void inconsistent_node(const node<Tp> &n) {}
 #endif
 
   protected:
-    const node<Tp> &find_common_ancestor(const node &a, const node &b) const
+    const node<Tp> &find_common_ancestor(const node<Tp> &a, const node<Tp> &b) const
     {
       std::unordered_set<const node<Tp> *> ancestors;
       auto c_a = &a;
@@ -161,7 +174,47 @@ namespace utils
       throw std::runtime_error("No common ancestor found");
     }
 
+    void backtrack_to(const node<Tp> &n) noexcept
+    {
+      while (&c_node->get() != &n)
+      {
+        retract(*c_node);
+        c_node = c_node->get_parent();
+#ifdef BUILD_LISTENERS
+        current_node(*c_node);
+#endif
+      }
+    }
+
+    virtual void retract(const node<Tp> &n) noexcept {}
+
+    bool go_to(const node<Tp> &target) noexcept
+    {
+      std::vector<const node<Tp> *> path;
+      auto temp_node = &target;
+      while (temp_node != &c_node->get())
+      {
+        path.push_back(temp_node);
+        temp_node = temp_node->get_parent().get();
+      }
+      for (auto it = path.rbegin(); it != path.rend(); ++it)
+      {
+        if (!apply_transition(**it))
+          return false;
+        c_node = (*it)->get_parent();
+#ifdef BUILD_LISTENERS
+        current_node(*c_node);
+#endif
+      }
+      return true;
+    }
+
+    virtual bool apply_transition(const node<Tp> &n) noexcept { return true; }
+
+    [[nodiscard]] node<Tp> &current_node() noexcept { return *c_node; }
+
   private:
+    std::shared_ptr<node<Tp>> c_node;
     std::priority_queue<std::shared_ptr<node<Tp>>, std::vector<std::shared_ptr<node<Tp>>>, node_cmp> open_list;
     std::unordered_set<std::shared_ptr<const node<Tp>>> closed_list;
   };

tests/CMakeLists.txt

@@ -18,7 +18,13 @@ add_dependencies(floyd_warshall_tests utils)
 target_link_libraries(floyd_warshall_tests PRIVATE utils)
 setup_sanitizers(floyd_warshall_tests)
 
+add_executable(a_star_tests test_a_star.cpp)
+add_dependencies(a_star_tests utils)
+target_link_libraries(a_star_tests PRIVATE utils)
+setup_sanitizers(a_star_tests)
+
 add_test(NAME UTILS_LibTest COMMAND utils_lib_tests WORKING_DIRECTORY ${CMAKE_BINARY_DIR})
 add_test(NAME CryptoTest COMMAND crypto_tests WORKING_DIRECTORY ${CMAKE_BINARY_DIR})
 add_test(NAME PointersTest COMMAND pointers_tests WORKING_DIRECTORY ${CMAKE_BINARY_DIR})
-add_test(NAME FloydWarshallTest COMMAND floyd_warshall_tests WORKING_DIRECTORY ${CMAKE_BINARY_DIR})
+add_test(NAME FloydWarshallTest COMMAND floyd_warshall_tests WORKING_DIRECTORY ${CMAKE_BINARY_DIR})
+add_test(NAME AStarTest COMMAND a_star_tests WORKING_DIRECTORY ${CMAKE_BINARY_DIR})

tests/test_a_star.cpp

@@ -0,0 +1,35 @@
+#include "a_star.hpp"
+#include <iostream>
+#include <cassert>
+
+class my_node : public utils::node<int>, public std::enable_shared_from_this<my_node>
+{
+public:
+    my_node(std::weak_ptr<my_node> parent, const int &g_cost) noexcept : utils::node<int>(std::move(parent), g_cost) {}
+    ~my_node() override = default;
+
+    [[nodiscard]] int h_cost() const noexcept override { return 0; }
+
+    [[nodiscard]] std::vector<std::shared_ptr<utils::node<int>>> generate_successors() const override
+    {
+        std::vector<std::shared_ptr<utils::node<int>>> successors;
+        if (g_cost() < 10)
+        {
+            auto succ = std::make_shared<my_node>(shared_from_this(), g_cost() + 1);
+            successors.push_back(succ);
+        }
+        return successors;
+    }
+
+    [[nodiscard]] bool is_goal() const noexcept override { return g_cost() >= 10; }
+};
+
+int main()
+{
+    auto root = std::make_shared<my_node>(std::weak_ptr<my_node>(), 0);
+    utils::a_star<int> astar(root);
+    auto goal_node = astar.search();
+    assert(goal_node);
+    assert(goal_node->g_cost() == 10);
+    return 0;
+}