From 6124f2ce77608fba6d7bd2e1605231017f458a94 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Sat, 14 Feb 2026 00:51:11 -0800 Subject: [PATCH 1/2] init lib --- src/MODULE.bazel | 8 ++++++++ src/extlibs/lapjv_cpp.BUILD | 20 ++++++++++++++++++++ src/software/ai/hl/stp/play/BUILD | 1 + src/software/ai/hl/stp/play/play.cpp | 1 + 4 files changed, 30 insertions(+) create mode 100644 src/extlibs/lapjv_cpp.BUILD diff --git a/src/MODULE.bazel b/src/MODULE.bazel index c7c7167ef2..409c3e2e6f 100644 --- a/src/MODULE.bazel +++ b/src/MODULE.bazel @@ -203,6 +203,14 @@ http_archive( url = "https://github.com/saebyn/munkres-cpp/archive/61086fcf5b3a8ad4bda578cdc2d1dca57b548786.tar.gz", ) +http_archive( + name = "lapjv_cpp", + build_file = "@//extlibs:lapjv_cpp.BUILD", + sha256 = "f4e180eee3eb683e38e4a394060080eabfa49749de7ac5617b55dffb9fee0667", + strip_prefix = "LAPJV-algorithm-c-40b5883b7492107e87ba8fb6abeb2aa05591dd34", + url = "https://github.com/yongyanghz/LAPJV-algorithm-c/archive/40b5883b7492107e87ba8fb6abeb2aa05591dd34.tar.gz", +) + http_archive( name = "LTC4151", build_file = "@//extlibs:LTC4151.BUILD", diff --git a/src/extlibs/lapjv_cpp.BUILD b/src/extlibs/lapjv_cpp.BUILD new file mode 100644 index 0000000000..50fe899747 --- /dev/null +++ b/src/extlibs/lapjv_cpp.BUILD @@ -0,0 +1,20 @@ +# Description: +# This library provides us with an implementation of the LAPJV algorithm +# https://en.wikipedia.org/wiki/Hungarian_algorithm +# https://github.com/yongyanghz/LAPJV-algorithm-c/ + +cc_library( + name = "lapjv_cpp", + hdrs = glob( + [ + "src/**/*.h", + "src/**/*.cpp", + ], + ), + include_prefix = "lapjv/", + includes = [ + "./src", + ], + strip_include_prefix = "src/", + visibility = ["//visibility:public"], +) diff --git a/src/software/ai/hl/stp/play/BUILD b/src/software/ai/hl/stp/play/BUILD index 678a44e215..63c8aff039 100644 --- a/src/software/ai/hl/stp/play/BUILD +++ b/src/software/ai/hl/stp/play/BUILD @@ -98,6 +98,7 @@ cc_library( "//software/ai/passing:pass_with_rating", "//software/util/sml_fsm", "@boost//:coroutine2", + "@lapjv_cpp", "@munkres_cpp", "@tracy", ], diff --git a/src/software/ai/hl/stp/play/play.cpp b/src/software/ai/hl/stp/play/play.cpp index bdeccf8b35..2eee1d0f42 100644 --- a/src/software/ai/hl/stp/play/play.cpp +++ b/src/software/ai/hl/stp/play/play.cpp @@ -1,6 +1,7 @@ #include "software/ai/hl/stp/play/play.h" #include +#include #include From 6b337bb4018ac584b34fdc8f11e40557c464bcc6 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Sat, 14 Feb 2026 01:15:42 -0800 Subject: [PATCH 2/2] proto lapjv --- src/extlibs/lapjv_cpp.BUILD | 4 + src/software/ai/hl/stp/play/BUILD | 1 - src/software/ai/hl/stp/play/play.cpp | 126 ++++++++++++++------------- 3 files changed, 69 insertions(+), 62 deletions(-) diff --git a/src/extlibs/lapjv_cpp.BUILD b/src/extlibs/lapjv_cpp.BUILD index 50fe899747..68501336bf 100644 --- a/src/extlibs/lapjv_cpp.BUILD +++ b/src/extlibs/lapjv_cpp.BUILD @@ -8,6 +8,10 @@ cc_library( hdrs = glob( [ "src/**/*.h", + ], + ), + srcs = glob( + [ "src/**/*.cpp", ], ), diff --git a/src/software/ai/hl/stp/play/BUILD b/src/software/ai/hl/stp/play/BUILD index 63c8aff039..4ffcfba71b 100644 --- a/src/software/ai/hl/stp/play/BUILD +++ b/src/software/ai/hl/stp/play/BUILD @@ -99,7 +99,6 @@ cc_library( "//software/util/sml_fsm", "@boost//:coroutine2", "@lapjv_cpp", - "@munkres_cpp", "@tracy", ], ) diff --git a/src/software/ai/hl/stp/play/play.cpp b/src/software/ai/hl/stp/play/play.cpp index 2eee1d0f42..3459f7e36a 100644 --- a/src/software/ai/hl/stp/play/play.cpp +++ b/src/software/ai/hl/stp/play/play.cpp @@ -1,6 +1,5 @@ #include "software/ai/hl/stp/play/play.h" -#include #include #include @@ -297,7 +296,17 @@ Play::assignTactics(const WorldPtr &world_ptr, TacticVector tactic_vector, // The rows of the matrix are the "workers" (the robots) and the columns are the // "jobs" (the Tactics). - Matrix matrix(num_rows, num_cols); + // LAPJV requires a square matrix. + size_t dim = std::max(num_rows, num_cols); + + // We use a flat vector to store the cost matrix data, and a vector of pointers + // to pass to lapjv as 'double **'. + std::vector cost_matrix_data(dim * dim, 0.0); + std::vector assigncost(dim); + for(size_t i = 0; i < dim; ++i) + { + assigncost[i] = &cost_matrix_data[i * dim]; + } // Initialize the matrix with the cost of assigning each Robot to each Tactic for (size_t row = 0; row < num_rows; row++) @@ -322,85 +331,80 @@ Play::assignTactics(const WorldPtr &world_ptr, TacticVector tactic_vector, robot_capabilities.begin(), robot_capabilities.end(), std::inserter(missing_capabilities, missing_capabilities.begin())); + double cost; if (missing_capabilities.size() > 0) { // We arbitrarily increase the cost, so that robots with missing // capabilities are not assigned - matrix(row, col) = robot_cost_for_tactic * 10.0 + 10.0; + cost = robot_cost_for_tactic * 10.0 + 10.0; } else { // capability requirements are satisfied, use real cost - matrix(row, col) = robot_cost_for_tactic; + cost = robot_cost_for_tactic; } + assigncost[row][col] = cost; } } - // Apply the Munkres/Hungarian algorithm to the matrix. - Munkres m; - m.solve(matrix); + // Apply the LAPJV algorithm to the matrix. + std::vector rowsol(dim); + std::vector colsol(dim); + std::vector u(dim); + std::vector v(dim); - // The Munkres matrix gets solved such that there will be exactly one 0 in every - // row and exactly one 0 in every column. All other values will be -1. The 0's - // indicate the "workers" and "jobs" (robots and tactics for us) that are most - // optimally paired together - // - // Example matrices: - // -1, 0,-1, and 0,-1, - // 0,-1,-1, -1, 0, - // -1,-1, 0, + lap(dim, assigncost.data(), rowsol.data(), colsol.data(), u.data(), v.data()); + + // rowsol[i] is the column assigned to row i. for (size_t row = 0; row < num_rows; row++) { - for (size_t col = 0; col < num_tactics; col++) + int col = rowsol[row]; + // Check if the assigned column is a valid tactic index + // If col >= num_tactics, it was assigned to a dummy tactic + if (col >= 0 && col < static_cast(num_tactics)) { - auto val = matrix(row, col); - if (val == 0) - { - RobotId robot_id = robots_to_assign.at(row).id(); - current_tactic_robot_id_assignment.emplace(tactic_vector.at(col), - robot_id); - tactic_vector.at(col)->setLastExecutionRobot(robot_id); - - auto primitives = primitive_sets.at(col); - CHECK(primitives.contains(robot_id)) - << "Couldn't find a primitive for robot id " << robot_id; - - // Create the list of obstacles - auto motion_constraints = buildMotionConstraintSet( - world_ptr->gameState(), *tactic_vector.at(col)); - - // Only generate primitive proto message for the final primitive to robot - // assignment - auto [traj_path, primitive_proto] = - primitives[robot_id]->generatePrimitiveProtoMessage( - *world_ptr, motion_constraints, robot_trajectories, - obstacle_factory); - - if (traj_path.has_value()) - { - robot_trajectories.insert_or_assign(robot_id, traj_path.value()); - } - else - { - robot_trajectories.erase(robot_id); - } + RobotId robot_id = robots_to_assign.at(row).id(); + current_tactic_robot_id_assignment.emplace(tactic_vector.at(col), + robot_id); + tactic_vector.at(col)->setLastExecutionRobot(robot_id); - primitives_to_run->mutable_robot_primitives()->insert( - {robot_id, *primitive_proto}); - remaining_robots.erase( - std::remove_if(remaining_robots.begin(), remaining_robots.end(), - [robots_to_assign, row](const Robot &robot) { - return robot.id() == robots_to_assign.at(row).id(); - }), - remaining_robots.end()); - - primitives[robot_id]->getVisualizationProtos(obstacle_list, - path_visualization); - break; + auto primitives = primitive_sets.at(col); + CHECK(primitives.contains(robot_id)) + << "Couldn't find a primitive for robot id " << robot_id; + + // Create the list of obstacles + auto motion_constraints = buildMotionConstraintSet( + world_ptr->gameState(), *tactic_vector.at(col)); + + // Only generate primitive proto message for the final primitive to robot + // assignment + auto [traj_path, primitive_proto] = + primitives[robot_id]->generatePrimitiveProtoMessage( + *world_ptr, motion_constraints, robot_trajectories, + obstacle_factory); + + if (traj_path.has_value()) + { + robot_trajectories.insert_or_assign(robot_id, traj_path.value()); } + else + { + robot_trajectories.erase(robot_id); + } + + primitives_to_run->mutable_robot_primitives()->insert( + {robot_id, *primitive_proto}); + remaining_robots.erase( + std::remove_if(remaining_robots.begin(), remaining_robots.end(), + [robots_to_assign, row](const Robot &robot) { + return robot.id() == robots_to_assign.at(row).id(); + }), + remaining_robots.end()); + + primitives[robot_id]->getVisualizationProtos(obstacle_list, + path_visualization); } } - return std::tuple, std::unique_ptr, std::map, RobotId>>{ remaining_robots, std::move(primitives_to_run),