Add A* solver and benchmarks

.bazelversion

@@ -0,0 +1 @@
+8.1.1

.gitignore

@@ -1,4 +1,4 @@
-# Ignore BAZEL files
+yj# Ignore BAZEL files
 MODULE.bazel.lock
 /bazel-*
 
@@ -11,4 +11,22 @@ MODULE.bazel.lock
 /third_party/*/lib
 
 # Distable toolchain temporaly
-/toolchain
+/toolchain
+
+# macOS
+.DS_Store
+
+# LaTeX build artifacts
+*.aux
+*.bbl
+*.blg
+*.fdb_latexmk
+*.fls
+*.log
+*.run.xml
+*.synctex.gz
+*.toc
+*-blx.bib
+
+# MCAP recordings (heavy files)
+data/*.mcap

Dockerfile

@@ -1,4 +1,4 @@
-FROM amd64/ubuntu:24.10 AS mapf-base
+FROM ubuntu:24.04 AS mapf-base
 
 WORKDIR /tmp
 
@@ -85,4 +85,4 @@ RUN wget https://github.com/foxglove/mcap/releases/download/releases%2Fmcap-cli%
 
 WORKDIR /mapf
 ENTRYPOINT ["/bin/zsh", "-lc"]
-CMD ["trap : TERM INT; sleep infinity & wait"]
+CMD ["trap : TERM INT; sleep infinity & wait"]

solver/BUILD.bazel

@@ -1,4 +1,4 @@
-load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library")
+load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library", "cc_test")
 
 package(default_visibility = ["//visibility:public"])
 
@@ -35,7 +35,7 @@ cc_binary(
 
 cc_test(
     name = "solvers_tests",
-    size = "small",
+    size = "large",
     srcs = glob(["tests/*.cpp"]),
     deps = [
         ":solvers",

solver/factory/src/solver_factory.cpp

@@ -1,5 +1,6 @@
 #include "factory/solver_factory.h"
 
+#include "solvers/astar_solver.h"
 #include "solvers/bfs_solver.h"
 
 #include <memory>
@@ -11,6 +12,7 @@ SolverFactory::SolverFactory() {
     factory_map_[#NAME] = []() { return std::make_shared<SOLVER_CLS>(__VA_ARGS__); }
 
     REGISTER(bfs_solver, BFSSolver);
+    REGISTER(astar_solver, AStarSolver);
 
 #undef REGISTER
 }
@@ -39,4 +41,4 @@ SolverFactory::SolverFactoryMap::const_iterator SolverFactory::cend() const {
     return factory_map_.cend();
 }
 
-}  // namespace mapf::solver
+}  // namespace mapf::solver

solver/main/src/main.cpp

@@ -1,8 +1,9 @@
-#include "models/models.h"
+об#include "models/models.h"
 #include "factory/solver_factory.h"
 
 #include <boost/program_options.hpp>
 
+#include <chrono>
 #include <filesystem>
 #include <iostream>
 #include <memory>

solver/solvers/include/solvers/astar_solver.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include "solvers/solver_base.h"
+
+#include "models/models.h"
+
+namespace mapf::solver {
+
+struct AStarSolver : public SolverBase {
+    models::MAPFSolution FindSolution(const models::MAPFProblem& mapf_problem) const final;
+    size_t GetNodesExpanded() const { return nodes_expanded_; }
+
+  private:
+    mutable size_t nodes_expanded_ = 0;
+};
+
+}  // namespace mapf::solver
+

solver/solvers/include/solvers/bfs_solver.h

@@ -17,6 +17,10 @@ bool HasCollision(
 
 struct BFSSolver : public SolverBase {
     models::MAPFSolution FindSolution(const models::MAPFProblem& mapf_problem) const final;
+    size_t GetNodesExpanded() const { return nodes_expanded_; }
+
+  private:
+    mutable size_t nodes_expanded_ = 0;
 };
 
 }  // namespace mapf::solver

solver/solvers/src/astar_solver.cpp

@@ -0,0 +1,213 @@
+#include "solvers/astar_solver.h"
+
+#include "solvers/bfs_solver.h"
+
+#include <algorithm>
+#include <cstdint>
+#include <limits>
+#include <queue>
+#include <unordered_map>
+#include <vector>
+
+namespace mapf::solver {
+
+namespace {
+
+using AgentGoalDistances = std::unordered_map<models::AgentId, std::unordered_map<graph::NodeId, uint64_t>>;
+
+AgentGoalDistances BuildGoalDistances(const models::MAPFProblem& mapf_problem) {
+    std::unordered_map<graph::NodeId, std::vector<graph::NodeId>> reverse_adjacency;
+    reverse_adjacency.reserve(mapf_problem.graph.nodes.size());
+    for (const auto& edge : mapf_problem.graph.edges) {
+        reverse_adjacency[edge.to_node_id].emplace_back(edge.from_node_id);
+    }
+
+    AgentGoalDistances goal_distances;
+    goal_distances.reserve(mapf_problem.agent_tasks.size());
+
+    for (const auto& [agent_id, agent_task] : mapf_problem.agent_tasks) {
+        std::unordered_map<graph::NodeId, uint64_t> distances;
+        distances.reserve(mapf_problem.graph.nodes.size());
+
+        std::queue<graph::NodeId> bfs_queue;
+        const graph::NodeId goal_node = agent_task.endpoints.to_node_id;
+        distances[goal_node] = 0;
+        bfs_queue.push(goal_node);
+
+        while (!bfs_queue.empty()) {
+            const graph::NodeId current_node = bfs_queue.front();
+            bfs_queue.pop();
+            const uint64_t current_dist = distances[current_node];
+
+            auto reverse_it = reverse_adjacency.find(current_node);
+            if (reverse_it == reverse_adjacency.end()) {
+                continue;
+            }
+
+            for (const graph::NodeId prev_node : reverse_it->second) {
+                if (distances.contains(prev_node)) {
+                    continue;
+                }
+                distances[prev_node] = current_dist + 1;
+                bfs_queue.push(prev_node);
+            }
+        }
+
+        goal_distances.emplace(agent_id, std::move(distances));
+    }
+
+    return goal_distances;
+}
+
+uint64_t Heuristic(
+    const models::AgentStates& state, const AgentGoalDistances& goal_distances) {
+    uint64_t lower_bound = 0;
+
+    for (const auto& [agent_id, agent_state] : state) {
+        auto distances_it = goal_distances.find(agent_id);
+        if (distances_it == goal_distances.end()) {
+            return std::numeric_limits<uint64_t>::max();
+        }
+
+        auto agent_dist_it = distances_it->second.find(agent_state.node_id);
+        if (agent_dist_it == distances_it->second.end()) {
+            return std::numeric_limits<uint64_t>::max();
+        }
+
+        lower_bound = std::max(lower_bound, agent_dist_it->second);
+    }
+
+    return lower_bound;
+}
+
+models::MAPFSolution BuildSolutionFromParents(
+    const models::MAPFProblem& mapf_problem,
+    models::AgentStates current_state,
+    const std::unordered_map<models::AgentStates, models::AgentStates>& parent) {
+    std::unordered_map<models::AgentId, graph::NodeIdsList> paths;
+    paths.reserve(mapf_problem.agent_tasks.size());
+    for (const auto& [agent_id, _] : mapf_problem.agent_tasks) {
+        paths[agent_id] = {current_state[agent_id].node_id};
+    }
+
+    auto parent_it = parent.find(current_state);
+    while (parent_it != parent.end()) {
+        const auto& prev_state = parent_it->second;
+        for (const auto& [agent_id, _] : mapf_problem.agent_tasks) {
+            paths[agent_id].emplace_back(prev_state.at(agent_id).node_id);
+        }
+        current_state = prev_state;
+        parent_it = parent.find(current_state);
+    }
+
+    models::MAPFSolution solution;
+    solution.agent_paths.reserve(paths.size());
+    for (auto& [agent_id, path] : paths) {
+        std::reverse(path.begin(), path.end());
+        solution.agent_paths.emplace(agent_id, models::AgentPath(agent_id, std::move(path)));
+    }
+    return solution;
+}
+
+}  // namespace
+
+models::MAPFSolution AStarSolver::FindSolution(const models::MAPFProblem& mapf_problem) const {
+    models::AgentStates start_state;
+    start_state.reserve(mapf_problem.agent_tasks.size());
+    for (const auto& [agent_id, agent_task] : mapf_problem.agent_tasks) {
+        start_state.emplace(
+            agent_id, models::AgentState(agent_id, agent_task.endpoints.from_node_id));
+    }
+
+    models::AgentStates finish_state;
+    finish_state.reserve(mapf_problem.agent_tasks.size());
+    for (const auto& [agent_id, agent_task] : mapf_problem.agent_tasks) {
+        finish_state.emplace(
+            agent_id, models::AgentState(agent_id, agent_task.endpoints.to_node_id));
+    }
+
+    const auto goal_distances = BuildGoalDistances(mapf_problem);
+
+    struct OpenNode {
+        uint64_t f_score;
+        uint64_t g_score;
+        models::AgentStates state;
+    };
+
+    auto cmp = [](const OpenNode& lhs, const OpenNode& rhs) {
+        if (lhs.f_score != rhs.f_score) {
+            return lhs.f_score > rhs.f_score;
+        }
+        return lhs.g_score > rhs.g_score;
+    };
+    std::priority_queue<OpenNode, std::vector<OpenNode>, decltype(cmp)> open_set(cmp);
+
+    std::unordered_map<models::AgentStates, uint64_t> cost_to_come;
+    std::unordered_map<models::AgentStates, models::AgentStates> parent;
+
+    const uint64_t start_h = Heuristic(start_state, goal_distances);
+    if (start_h == std::numeric_limits<uint64_t>::max()) {
+        return {};
+    }
+    open_set.push(OpenNode{start_h, 0, start_state});
+    cost_to_come[start_state] = 0;
+
+    nodes_expanded_ = 0;
+
+    while (!open_set.empty()) {
+        auto current = std::move(open_set.top());
+        open_set.pop();
+        ++nodes_expanded_;
+
+        auto current_cost_it = cost_to_come.find(current.state);
+        if (current_cost_it == cost_to_come.end() || current.g_score != current_cost_it->second) {
+            continue;
+        }
+
+        if (current.state == finish_state) {
+            return BuildSolutionFromParents(mapf_problem, std::move(current.state), parent);
+        }
+
+        std::unordered_map<models::AgentId, graph::NodeIdsSet> individual_moves;
+        individual_moves.reserve(mapf_problem.agent_tasks.size());
+        for (const auto& [agent_id, _] : mapf_problem.agent_tasks) {
+            const graph::NodeId current_node = current.state.at(agent_id).node_id;
+            const auto& neighbors = mapf_problem.graph.GetNeighbours(current_node);
+            individual_moves[agent_id] = neighbors;
+            individual_moves[agent_id].emplace(current_node);
+        }
+
+        const auto joint_moves = GenerateJointMoves(individual_moves);
+        for (const auto& joint_move : joint_moves) {
+            models::AgentStates next_state;
+            next_state.reserve(joint_move.size());
+            for (const auto& [agent_id, node_id] : joint_move) {
+                next_state.emplace(agent_id, models::AgentState(agent_id, node_id));
+            }
+
+            if (HasCollision(current.state, next_state)) {
+                continue;
+            }
+
+            const uint64_t tentative_g = current.g_score + 1;
+            auto existing_cost_it = cost_to_come.find(next_state);
+            if (existing_cost_it != cost_to_come.end() && tentative_g >= existing_cost_it->second) {
+                continue;
+            }
+
+            const uint64_t h = Heuristic(next_state, goal_distances);
+            if (h == std::numeric_limits<uint64_t>::max()) {
+                continue;
+            }
+
+            cost_to_come[next_state] = tentative_g;
+            parent[next_state] = current.state;
+            open_set.push(OpenNode{tentative_g + h, tentative_g, std::move(next_state)});
+        }
+    }
+
+    return {};
+}
+
+}  // namespace mapf::solver
+

solver/solvers/src/bfs_solver.cpp

@@ -73,9 +73,12 @@ models::MAPFSolution BFSSolver::FindSolution(const models::MAPFProblem& mapf_pro
     open_set.push(start_state);
     cost_to_come[start_state] = 0;
 
+    nodes_expanded_ = 0;
+
     while (!open_set.empty()) {
         auto current_state = open_set.front();
         open_set.pop();
+        ++nodes_expanded_;
 
         if (current_state == finish_state) {
             std::unordered_map<models::AgentId, graph::NodeIdsList> paths;