diff --git a/.bazelversion b/.bazelversion new file mode 100644 index 0000000..0e79152 --- /dev/null +++ b/.bazelversion @@ -0,0 +1 @@ +8.1.1 diff --git a/.gitignore b/.gitignore index 3fc50cc..10a9a2c 100644 --- a/.gitignore +++ b/.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 \ No newline at end of file +/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 \ No newline at end of file diff --git a/Dockerfile b/Dockerfile index 4c055ec..8005ec8 100644 --- a/Dockerfile +++ b/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"] \ No newline at end of file +CMD ["trap : TERM INT; sleep infinity & wait"] diff --git a/solver/BUILD.bazel b/solver/BUILD.bazel index 9db8b4f..1be0e00 100644 --- a/solver/BUILD.bazel +++ b/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", diff --git a/solver/factory/src/solver_factory.cpp b/solver/factory/src/solver_factory.cpp index c0dfc98..6c73d6d 100644 --- a/solver/factory/src/solver_factory.cpp +++ b/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 @@ -11,6 +12,7 @@ SolverFactory::SolverFactory() { factory_map_[#NAME] = []() { return std::make_shared(__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 \ No newline at end of file +} // namespace mapf::solver diff --git a/solver/main/src/main.cpp b/solver/main/src/main.cpp index 9f98761..b61ed2f 100644 --- a/solver/main/src/main.cpp +++ b/solver/main/src/main.cpp @@ -1,8 +1,9 @@ -#include "models/models.h" +об#include "models/models.h" #include "factory/solver_factory.h" #include +#include #include #include #include diff --git a/solver/solvers/include/solvers/astar_solver.h b/solver/solvers/include/solvers/astar_solver.h new file mode 100644 index 0000000..897467c --- /dev/null +++ b/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 + diff --git a/solver/solvers/include/solvers/bfs_solver.h b/solver/solvers/include/solvers/bfs_solver.h index fa885fd..8b39ccd 100644 --- a/solver/solvers/include/solvers/bfs_solver.h +++ b/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 diff --git a/solver/solvers/src/astar_solver.cpp b/solver/solvers/src/astar_solver.cpp new file mode 100644 index 0000000..4713873 --- /dev/null +++ b/solver/solvers/src/astar_solver.cpp @@ -0,0 +1,213 @@ +#include "solvers/astar_solver.h" + +#include "solvers/bfs_solver.h" + +#include +#include +#include +#include +#include +#include + +namespace mapf::solver { + +namespace { + +using AgentGoalDistances = std::unordered_map>; + +AgentGoalDistances BuildGoalDistances(const models::MAPFProblem& mapf_problem) { + std::unordered_map> 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 distances; + distances.reserve(mapf_problem.graph.nodes.size()); + + std::queue 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::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::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& parent) { + std::unordered_map 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, decltype(cmp)> open_set(cmp); + + std::unordered_map cost_to_come; + std::unordered_map parent; + + const uint64_t start_h = Heuristic(start_state, goal_distances); + if (start_h == std::numeric_limits::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 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::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 + diff --git a/solver/solvers/src/bfs_solver.cpp b/solver/solvers/src/bfs_solver.cpp index b8b891b..956cd01 100644 --- a/solver/solvers/src/bfs_solver.cpp +++ b/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 paths; diff --git a/solver/solvers/tests/test_solvers.cpp b/solver/solvers/tests/test_solvers.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/solver/tests/test_solvers.cpp b/solver/tests/test_solvers.cpp index 534675d..f632a98 100644 --- a/solver/tests/test_solvers.cpp +++ b/solver/tests/test_solvers.cpp @@ -1,3 +1,4 @@ +#include "solvers/astar_solver.h" #include "solvers/bfs_solver.h" #include "graph/graph.h" @@ -7,10 +8,90 @@ #include +#include +#include + using namespace mapf::graph; using namespace mapf::models; using namespace mapf::solver; +// Builds a width x height grid graph with bidirectional edges +Graph BuildGridGraph(uint32_t width, uint32_t height) { + Nodes nodes; + Edges edges; + for (uint32_t y = 0; y < height; ++y) { + for (uint32_t x = 0; x < width; ++x) { + NodeId id = y * width + x; + nodes.emplace(id, Node(id)); + if (x + 1 < width) { + NodeId right = id + 1; + edges.emplace(Edge(id, right)); + edges.emplace(Edge(right, id)); + } + if (y + 1 < height) { + NodeId down = id + width; + edges.emplace(Edge(id, down)); + edges.emplace(Edge(down, id)); + } + } + } + return Graph(std::move(nodes), std::move(edges)); +} + +// Runs both solvers on the same problem and collects metrics +struct BenchmarkResult { + size_t bfs_nodes_expanded; + size_t astar_nodes_expanded; + double bfs_time_ms; + double astar_time_ms; + size_t bfs_makespan; + size_t astar_makespan; +}; + +BenchmarkResult RunBenchmark(const MAPFProblem& problem) { + BenchmarkResult result{}; + + BFSSolver bfs; + AStarSolver astar; + + auto t0 = std::chrono::high_resolution_clock::now(); + auto bfs_solution = bfs.FindSolution(problem); + auto t1 = std::chrono::high_resolution_clock::now(); + auto astar_solution = astar.FindSolution(problem); + auto t2 = std::chrono::high_resolution_clock::now(); + + result.bfs_time_ms = + std::chrono::duration(t1 - t0).count(); + result.astar_time_ms = + std::chrono::duration(t2 - t1).count(); + + result.bfs_nodes_expanded = bfs.GetNodesExpanded(); + result.astar_nodes_expanded = astar.GetNodesExpanded(); + + if (!bfs_solution.agent_paths.empty()) { + result.bfs_makespan = bfs_solution.agent_paths.begin()->second.path.size() - 1; + } + if (!astar_solution.agent_paths.empty()) { + result.astar_makespan = astar_solution.agent_paths.begin()->second.path.size() - 1; + } + + return result; +} + +void PrintBenchmark(const std::string& name, const BenchmarkResult& r) { + std::cout << "\n--- " << name << " ---" << std::endl; + std::cout << " Nodes expanded: BFS=" << r.bfs_nodes_expanded + << " A*=" << r.astar_nodes_expanded + << " (A* is " << (r.bfs_nodes_expanded / std::max(r.astar_nodes_expanded, size_t(1))) + << "x fewer)" << std::endl; + std::cout << " Time (ms): BFS=" << r.bfs_time_ms + << " A*=" << r.astar_time_ms << std::endl; + std::cout << " Makespan: BFS=" << r.bfs_makespan + << " A*=" << r.astar_makespan << std::endl; +} + +// Correctness tests from starter code (4 nodes, 2 agents) + class MAPFProblemTest1 : public testing::Test { protected: MAPFProblemTest1() { @@ -48,7 +129,122 @@ TEST_F(MAPFProblemTest1, BFSSolver) { ASSERT_VALID_SOLUTION(problem, solver.FindSolution(problem)); } +TEST_F(MAPFProblemTest1, AStarSolver) { + auto solver = AStarSolver(); + ASSERT_VALID_SOLUTION(problem, solver.FindSolution(problem)); +} + TEST_F(MAPFProblemTest2, BFSSolver) { auto solver = BFSSolver(); ASSERT_VALID_SOLUTION(problem, solver.FindSolution(problem)); -} \ No newline at end of file +} + +TEST_F(MAPFProblemTest2, AStarSolver) { + auto solver = AStarSolver(); + ASSERT_VALID_SOLUTION(problem, solver.FindSolution(problem)); +} + +// Benchmark: small grid, minimal difference expected +TEST(Benchmark, Grid3x3_2Agents) { + auto graph = BuildGridGraph(3, 3); + AgentTasks tasks{ + {0, {0, {0, 8}}}, + {1, {1, {8, 0}}}, + }; + MAPFProblem problem(std::move(graph), std::move(tasks)); + + BFSSolver bfs; + AStarSolver astar; + ASSERT_VALID_SOLUTION(problem, bfs.FindSolution(problem)); + ASSERT_VALID_SOLUTION(problem, astar.FindSolution(problem)); + + auto r = RunBenchmark(problem); + PrintBenchmark("3x3 grid, 2 agents", r); + + EXPECT_LE(astar.GetNodesExpanded(), bfs.GetNodesExpanded()); + EXPECT_EQ(r.bfs_makespan, r.astar_makespan); +} + +// Benchmark: corridor with a single side pocket (bottleneck) +// 0 — 1 — 2 — 3 — 4 — 5 — 6 — 7 +// | +// 8 +// Agents swap ends: 0->7 and 7->0. Only node 8 allows them to pass. +TEST(Benchmark, CorridorBottleneck_2Agents) { + Nodes nodes; + Edges edges; + for (uint32_t i = 0; i < 9; ++i) { + nodes.emplace(i, Node(i)); + } + // main corridor + for (uint32_t i = 0; i < 7; ++i) { + edges.emplace(Edge(i, i + 1)); + edges.emplace(Edge(i + 1, i)); + } + // side pocket at node 3 + edges.emplace(Edge(3, 8)); + edges.emplace(Edge(8, 3)); + auto graph = Graph(std::move(nodes), std::move(edges)); + + AgentTasks tasks{ + {0, {0, {0, 7}}}, + {1, {1, {7, 0}}}, + }; + MAPFProblem problem(std::move(graph), std::move(tasks)); + + BFSSolver bfs; + AStarSolver astar; + ASSERT_VALID_SOLUTION(problem, bfs.FindSolution(problem)); + ASSERT_VALID_SOLUTION(problem, astar.FindSolution(problem)); + + auto r = RunBenchmark(problem); + PrintBenchmark("corridor with bottleneck, 2 agents", r); + + EXPECT_LE(astar.GetNodesExpanded(), bfs.GetNodesExpanded()); + EXPECT_EQ(r.bfs_makespan, r.astar_makespan); +} + +// Benchmark: 3 agents, larger state space +TEST(Benchmark, Grid4x4_3Agents) { + auto graph = BuildGridGraph(4, 4); + AgentTasks tasks{ + {0, {0, {0, 15}}}, + {1, {1, {3, 12}}}, + {2, {2, {12, 3}}}, + }; + MAPFProblem problem(std::move(graph), std::move(tasks)); + + BFSSolver bfs; + AStarSolver astar; + ASSERT_VALID_SOLUTION(problem, bfs.FindSolution(problem)); + ASSERT_VALID_SOLUTION(problem, astar.FindSolution(problem)); + + auto r = RunBenchmark(problem); + PrintBenchmark("4x4 grid, 3 agents", r); + + EXPECT_LE(astar.GetNodesExpanded(), bfs.GetNodesExpanded()); + EXPECT_EQ(r.bfs_makespan, r.astar_makespan); +} + +// Benchmark: 4 agents, all doing diagonal swaps — hardest case +TEST(Benchmark, Grid4x4_4Agents) { + auto graph = BuildGridGraph(4, 4); + AgentTasks tasks{ + {0, {0, {0, 15}}}, + {1, {1, {15, 0}}}, + {2, {2, {3, 12}}}, + {3, {3, {12, 3}}}, + }; + MAPFProblem problem(std::move(graph), std::move(tasks)); + + BFSSolver bfs; + AStarSolver astar; + ASSERT_VALID_SOLUTION(problem, bfs.FindSolution(problem)); + ASSERT_VALID_SOLUTION(problem, astar.FindSolution(problem)); + + auto r = RunBenchmark(problem); + PrintBenchmark("4x4 grid, 4 agents", r); + + EXPECT_LE(astar.GetNodesExpanded(), bfs.GetNodesExpanded()); + EXPECT_EQ(r.bfs_makespan, r.astar_makespan); +}