Merge branch 'main' into nbuono/VO

WORKSPACE

@@ -18,6 +18,13 @@ http_archive(
   integrity = "sha256-CVtgQSRXe8o2wMjNcJrxtlUxiHZY9ZQJ0kde8BkrTPw="
 )
 
+http_archive(
+  name = "symphony_lake_snippet",
+  urls = ["https://www.dropbox.com/scl/fi/gh7855qdos5mspinst77k/symphony_lake_snippet.zip?rlkey=mg7qe0gq51r446dwhr9g2wiut&st=774393fx&dl=1"],
+  build_file = "//third_party:BUILD.symphony_lake_snippet",
+  sha256 = "f16211fb370c9471153c9ed4a345b9fb848d292dbb8b7dc26fea24cb30ba5c15",
+)
+
 http_archive(
     name = "bazel_skylib",
     sha256 = "bc283cdfcd526a52c3201279cda4bc298652efa898b10b4db0837dc51652756f",
@@ -465,6 +472,16 @@ http_archive(
   integrity = "sha256-cSp9CdKiJlL7BqSa9RbgUZeaOYStsGfahnYOYO1Rp/U="
 )
 
+load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive")
+
+http_archive(
+    name = "symphony_lake_parser",
+    urls = ["https://github.com/pizzaroll04/SymphonyLakeDataset/archive/4b38c2519270a43f76858e6c97c1f1de1735e5d0.zip"],
+    strip_prefix = "SymphonyLakeDataset-4b38c2519270a43f76858e6c97c1f1de1735e5d0",
+    build_file = "//third_party:BUILD.symphony_lake_parser",
+    sha256 = "d203e486507c7950ae9a346406fe2c42f8b2d204e8d25ba07a47c834f4ae8ede",
+)
+
 http_archive(
     name = "gtsam",
     build_file = "@rules_gtsam//third_party:gtsam.BUILD",

common/check.hh

@@ -1,7 +1,8 @@
 
 #include "assert/assert.hpp"
 
-#define CHECK(expr, ...) ASSERT_INVOKE(expr, false, true, "CHECK", verification, , __VA_ARGS__)
+#define ROBOT_CHECK(expr, ...) \
+    ASSERT_INVOKE(expr, false, true, "ROBOT_CHECK", verification, , __VA_ARGS__)
 
 namespace robot {
 using check_failure = libassert::verification_failure;

common/check_test.cc

@@ -3,4 +3,4 @@
 
 #include "gtest/gtest.h"
 
-TEST(AssertTest, assert_test) { EXPECT_THROW(CHECK(false), robot::check_failure); }
+TEST(AssertTest, assert_test) { EXPECT_THROW(ROBOT_CHECK(false), robot::check_failure); }

common/linux/get_memory_usage.cc

@@ -15,7 +15,7 @@ int get_memory_usage_kb() {
     std::smatch match;
     std::string contents = stream.str();
     const bool match_found = std::regex_search(contents, match, memory_matcher);
-    CHECK(match_found);
+    ROBOT_CHECK(match_found);
     return std::stoi(match[1]);
 }
 }  // namespace robot::linux

common/math/cubic_hermite_spline.hh

@@ -17,12 +17,12 @@ class CubicHermiteSpline {
 
     CubicHermiteSpline(std::vector<T> ts, std::vector<X> xs)
         : ts_(std::move(ts)), xs_(std::move(xs)) {
-        CHECK(std::is_sorted(ts_.begin(), ts_.end()), "Input times must be sorted!", ts_);
+        ROBOT_CHECK(std::is_sorted(ts_.begin(), ts_.end()), "Input times must be sorted!", ts_);
     }
 
     X operator()(const T &query_time) const {
-        CHECK(query_time >= ts_.front() && query_time <= ts_.back(), "query_time is out of bounds",
-              ts_.front(), ts_.back(), query_time);
+        ROBOT_CHECK(query_time >= ts_.front() && query_time <= ts_.back(),
+                    "query_time is out of bounds", ts_.front(), ts_.back(), query_time);
 
         const auto iter = std::lower_bound(ts_.begin(), ts_.end(), query_time);
         if (iter == ts_.end()) {

common/math/multivariate_normal_cdf.cc

@@ -11,9 +11,9 @@ std::optional<double> multivariate_normal_cdf(const Eigen::VectorXd &mu,
                                               const Eigen::MatrixXd &sigma,
                                               const Eigen::VectorXd &upper_bound,
                                               const bool return_log_p) {
-    CHECK(sigma.rows() == sigma.cols());
-    CHECK(sigma.rows() == mu.rows());
-    CHECK(sigma.rows() == upper_bound.rows());
+    ROBOT_CHECK(sigma.rows() == sigma.cols());
+    ROBOT_CHECK(sigma.rows() == mu.rows());
+    ROBOT_CHECK(sigma.rows() == upper_bound.rows());
 
     const int dim = mu.rows();
     constexpr bool NONSTANDARD = false;

common/sqlite3/sqlite3.cc

@@ -20,7 +20,7 @@ struct Database::Impl {
 
     template <typename... Ts>
     void check_result(const int error_code, Ts... args) {
-        CHECK(error_code == SQLITE_OK, sqlite3_errmsg(db_), args...);
+        ROBOT_CHECK(error_code == SQLITE_OK, sqlite3_errmsg(db_), args...);
     };
 
     Database::Statement prepare(const std::string &statement) {
@@ -37,8 +37,8 @@ struct Database::Impl {
 
     void bind(const Statement &stmt, const std::unordered_map<std::string, Database::Value> &args) {
         sqlite3_stmt *stmt_ptr = stmt.impl_->stmt.get();
-        CHECK(args.size() == sqlite3_bind_parameter_count(stmt_ptr),
-              "insufficient number of arguments", sqlite3_sql(stmt_ptr), args);
+        ROBOT_CHECK(args.size() == sqlite3_bind_parameter_count(stmt_ptr),
+                    "insufficient number of arguments", sqlite3_sql(stmt_ptr), args);
         for (const auto &[key, value] : args) {
             const int param_idx = sqlite3_bind_parameter_index(stmt_ptr, key.c_str());
             std::visit(
@@ -69,7 +69,7 @@ struct Database::Impl {
         if (result == SQLITE_DONE) {
             return std::nullopt;
         }
-        CHECK(result == SQLITE_ROW, sqlite3_errmsg(db_));
+        ROBOT_CHECK(result == SQLITE_ROW, sqlite3_errmsg(db_));
         const int num_columns = sqlite3_column_count(stmt_ptr);
         if (stmt.impl_->column_names == nullptr) {
             std::vector<std::string> column_names;

experimental/beacon_sim/beacon_potential.cc

@@ -122,8 +122,8 @@ BeaconPotential operator*(const BeaconPotential &a, const BeaconPotential &b) {
     std::set_intersection(a_sorted_members.begin(), a_sorted_members.end(),
                           b_sorted_members.begin(), b_sorted_members.end(),
                           std::back_inserter(common_elements));
-    CHECK(common_elements.empty(), "Found overlap in members of potentials", a_sorted_members,
-          b_sorted_members, common_elements);
+    ROBOT_CHECK(common_elements.empty(), "Found overlap in members of potentials", a_sorted_members,
+                b_sorted_members, common_elements);
 
     return CombinedPotential({a, b});
 }

experimental/beacon_sim/beacon_potential.hh

@@ -63,11 +63,11 @@ class BeaconPotential {
 
     double log_prob(const std::unordered_map<int, bool> &assignments,
                     const bool allow_partial_assignment = false) const {
-        CHECK(impl_ != nullptr);
+        ROBOT_CHECK(impl_ != nullptr);
         return impl_->log_prob_(assignments, allow_partial_assignment);
     };
     double log_prob(const std::vector<int> &present_beacons) const {
-        CHECK(impl_ != nullptr);
+        ROBOT_CHECK(impl_ != nullptr);
         return impl_->log_prob_(present_beacons);
     }
 

experimental/beacon_sim/beacon_potential_to_proto.cc

@@ -28,7 +28,7 @@ beacon_sim::BeaconPotential unpack_from(const BeaconPotential &in) {
         case BeaconPotential::kAnticorrelatedPotential:
             return unpack_from(in.anticorrelated_potential());
     }
-    CHECK(false, "Unhandled potential type");
+    ROBOT_CHECK(false, "Unhandled potential type");
     return beacon_sim::BeaconPotential();
 }
 

experimental/beacon_sim/beacon_sim.cc

@@ -363,7 +363,7 @@ void run_simulation(const SimConfig &sim_config) {
             const auto maybe_world_map_config_proto =
                 robot::proto::load_from_file<proto::WorldMapConfig>(
                     sim_config.world_map_config.value());
-            CHECK(maybe_world_map_config_proto.has_value());
+            ROBOT_CHECK(maybe_world_map_config_proto.has_value());
             return WorldMap(unpack_from(maybe_world_map_config_proto.value()));
         } else {
             return WorldMap(world_map_config(sim_config.correlated_beacons_configuration));
@@ -375,7 +375,7 @@ void run_simulation(const SimConfig &sim_config) {
             const auto maybe_road_map_config_proto =
                 robot::proto::load_from_file<planning::proto::RoadMap>(
                     sim_config.road_map_config.value());
-            CHECK(maybe_road_map_config_proto);
+            ROBOT_CHECK(maybe_road_map_config_proto);
             return unpack_from(maybe_road_map_config_proto.value());
         } else {
             return create_road_map(map);
@@ -605,14 +605,14 @@ int main(int argc, char **argv) {
 
     const Eigen::Vector2d goal_in_map = [&]() {
         const auto goal_in_map_vec = args["goal_in_map"].as<std::vector<double>>();
-        CHECK(goal_in_map_vec.size() == 2);
+        ROBOT_CHECK(goal_in_map_vec.size() == 2);
         return Eigen::Vector2d(goal_in_map_vec.data());
     }();
 
     const robot::liegroups::SE2 map_from_initial_robot = [&]() {
         const auto map_from_initial_robot_vec =
             args["map_from_initial_robot"].as<std::vector<double>>();
-        CHECK(map_from_initial_robot_vec.size() == 3);
+        ROBOT_CHECK(map_from_initial_robot_vec.size() == 3);
         const Eigen::Vector2d robot_in_map(map_from_initial_robot_vec.data());
         const double theta_rad = map_from_initial_robot_vec.back();
         return robot::liegroups::SE2(theta_rad, robot_in_map);

experimental/beacon_sim/belief_road_map_planner.cc

@@ -187,7 +187,7 @@ template <>
 std::function<double(const RobotBelief &)> make_uncertainty_size(
     const UncertaintySizeOptions &options) {
     if (std::holds_alternative<ValueAtRiskDeterminant>(options)) {
-        CHECK(false, "ValueAtRiskDeterminant is not supported for unimodal beliefs", options);
+        ROBOT_CHECK(false, "ValueAtRiskDeterminant is not supported for unimodal beliefs", options);
     } else if (std::holds_alternative<ExpectedDeterminant>(options)) {
         return [opt = std::get<ExpectedDeterminant>(options)](const RobotBelief &belief) -> double {
             if (opt.position_only) {
@@ -267,7 +267,7 @@ std::function<double(const RobotBelief &)> make_uncertainty_size(
                 for (const auto &eval_pt : eval_pts) {
                     const auto result =
                         math::multivariate_normal_cdf(Eigen::Vector3d::Zero(), cov, eval_pt);
-                    CHECK(result.has_value(), "Could not compute cdf at eval pt", eval_pt);
+                    ROBOT_CHECK(result.has_value(), "Could not compute cdf at eval pt", eval_pt);
                     probs.push_back(result.value());
                 }
                 return probs.at(0) - probs.at(1) + probs.at(2) - probs.at(3) - probs.at(4) +
@@ -279,7 +279,7 @@ std::function<double(const RobotBelief &)> make_uncertainty_size(
         };
     }
 
-    CHECK(false, "Unknown Uncertainty Size Options", options);
+    ROBOT_CHECK(false, "Unknown Uncertainty Size Options", options);
     return [](const auto &) { return 0.0; };
 }
 
@@ -305,8 +305,9 @@ std::function<double(const LandmarkRobotBelief &)> make_uncertainty_size(
                     return elem.cov_in_robot.determinant();
                 }
             }
-            CHECK(false, "Landmark Belief has insufficient probability mass to get to threshold",
-                  accumulated_prob);
+            ROBOT_CHECK(false,
+                        "Landmark Belief has insufficient probability mass to get to threshold",
+                        accumulated_prob);
             return elements.back().cov_in_robot.determinant();
         };
     } else if (std::holds_alternative<ExpectedDeterminant>(options)) {
@@ -406,7 +407,7 @@ std::function<double(const LandmarkRobotBelief &)> make_uncertainty_size(
                 for (const auto &eval_pt : eval_pts) {
                     const auto result =
                         math::multivariate_normal_cdf(Eigen::Vector3d::Zero(), cov, eval_pt);
-                    CHECK(result.has_value(), "Could not compute cdf at eval pt", eval_pt);
+                    ROBOT_CHECK(result.has_value(), "Could not compute cdf at eval pt", eval_pt);
                     probs.push_back(result.value());
                 }
                 return probs.at(0) - probs.at(1) + probs.at(2) - probs.at(3) - probs.at(4) +
@@ -422,7 +423,7 @@ std::function<double(const LandmarkRobotBelief &)> make_uncertainty_size(
         };
     }
 
-    CHECK(false, "Unknown Uncertainty Size Options", options);
+    ROBOT_CHECK(false, "Unknown Uncertainty Size Options", options);
     return [](const auto &) { return 0.0; };
 }
 

experimental/beacon_sim/compute_oracle_results.cc

@@ -87,7 +87,7 @@ std::optional<proto::GetJobResponse> get_job(const std::string &worker_name,
     proto::GetJobRequest job_request;
     job_request.mutable_worker()->set_name(worker_name);
     proto::GetJobResponse job_response;
-    CHECK(client.get_job(&context, job_request, &job_response).ok());
+    ROBOT_CHECK(client.get_job(&context, job_request, &job_response).ok());
     if (!job_response.has_job_id()) {
         return std::nullopt;
     }
@@ -123,19 +123,19 @@ std::tuple<BeaconPotential, planning::RoadMap, EkfSlam> load_environment(
     // Load Map Config
     const auto maybe_map_config =
         robot::proto::load_from_file<proto::WorldMapConfig>(config_path / config.map_config_path());
-    CHECK(maybe_map_config.has_value(), config_path / config.map_config_path());
+    ROBOT_CHECK(maybe_map_config.has_value(), config_path / config.map_config_path());
     const WorldMap world_map(unpack_from(maybe_map_config.value()));
 
     // Load EKF State
     const auto maybe_mapped_landmarks =
         robot::proto::load_from_file<proto::MappedLandmarks>(config_path / config.ekf_state_path());
-    CHECK(maybe_mapped_landmarks.has_value());
+    ROBOT_CHECK(maybe_mapped_landmarks.has_value());
     const auto mapped_landmarks = unpack_from(maybe_mapped_landmarks.value());
 
     // Create a road map
     const auto maybe_road_map = robot::proto::load_from_file<planning::proto::RoadMap>(
         config_path / config.road_map_path());
-    CHECK(maybe_road_map.has_value());
+    ROBOT_CHECK(maybe_road_map.has_value());
     const planning::RoadMap road_map = unpack_from(maybe_road_map.value());
 
     return {world_map.beacon_potential(), road_map, load_ekf_slam(mapped_landmarks)};
@@ -196,7 +196,7 @@ std::vector<proto::OraclePlan> compute_plans(
     const std::function<void(const int, const int)> &progress_function) {
     const auto maybe_results_file =
         robot::proto::load_from_file<proto::ExperimentResult>(results_file_path);
-    CHECK(maybe_results_file.has_value());
+    ROBOT_CHECK(maybe_results_file.has_value());
     const auto &results_file = maybe_results_file.value();
 
     const auto &experiment_config = results_file.experiment_config();
@@ -269,7 +269,7 @@ std::vector<proto::OraclePlan> compute_plans(
                     }};
                 const auto maybe_plan =
                     compute_belief_road_map_plan(road_map, ekf, conditioned_potential, options);
-                CHECK(maybe_plan.has_value());
+                ROBOT_CHECK(maybe_plan.has_value());
                 proto::OraclePlan plan_out;
                 plan_out.set_trial_id(trial_idx);
                 plan_out.set_eval_trial_id(eval_trial_idx);
@@ -322,7 +322,8 @@ void compute_oracle_results(const std::string server_address, const int server_p
             update.set_progress(static_cast<double>(plans_completed) / total_plans);
             pack_into(job_start_time, update.mutable_start_time());
             pack_into(time::current_robot_time(), update.mutable_current_time());
-            CHECK(client.update_job_status(&client_context, update_request, &update_response).ok());
+            ROBOT_CHECK(
+                client.update_job_status(&client_context, update_request, &update_response).ok());
         };
 
         // Compute the results
@@ -337,7 +338,8 @@ void compute_oracle_results(const std::string server_address, const int server_p
             result_request.mutable_job_result()->mutable_plan()->Add(plans.begin(), plans.end());
             result_request.set_job_id(maybe_job_response->job_id());
             proto::JobResultResponse result_response;
-            CHECK(client.submit_job_result(&client_context, result_request, &result_response).ok());
+            ROBOT_CHECK(
+                client.submit_job_result(&client_context, result_request, &result_response).ok());
         }
     }
 }

experimental/beacon_sim/correlated_beacon_potential.cc

@@ -72,8 +72,9 @@ double compute_log_prob(const CorrelatedBeaconPotential &pot,
     std::set_intersection(sorted_members.begin(), sorted_members.end(), keys.begin(), keys.end(),
                           std::back_inserter(keys_to_keep));
 
-    CHECK(allow_partial_assignment || missing_keys.empty(),
-          "partial assignment specified when not enabled", assignment, missing_keys, pot.members);
+    ROBOT_CHECK(allow_partial_assignment || missing_keys.empty(),
+                "partial assignment specified when not enabled", assignment, missing_keys,
+                pot.members);
 
     const int num_beacons_present =
         std::count_if(keys_to_keep.begin(), keys_to_keep.end(),
@@ -209,8 +210,9 @@ CorrelatedBeaconPotential condition_on(const CorrelatedBeaconPotential &pot,
         new_assignment = pot.conditioning->conditioned_members;
         for (const auto &[id, value] : assignment) {
             const auto &existing_conditioned = pot.conditioning->conditioned_members;
-            CHECK(!existing_conditioned.contains(id) || (existing_conditioned.at(id) == value),
-                  "Inconsistent conditioning", existing_conditioned, assignment);
+            ROBOT_CHECK(
+                !existing_conditioned.contains(id) || (existing_conditioned.at(id) == value),
+                "Inconsistent conditioning", existing_conditioned, assignment);
 
             new_assignment[id] = value;
         }

experimental/beacon_sim/ekf_slam.cc

@@ -27,8 +27,8 @@ std::optional<int> find_beacon_matrix_idx_or_add(const int id, InOut<EkfSlamEsti
     }
 
     const int max_num_landmarks = (est->mean.rows() - ROBOT_STATE_DIM) / BEACON_DIM;
-    CHECK(static_cast<int>(est->beacon_ids.size()) < max_num_landmarks,
-          "Not enough capacity to add beacon to EKF, increase max_num_beacons in ekf_config");
+    ROBOT_CHECK(static_cast<int>(est->beacon_ids.size()) < max_num_landmarks,
+                "Not enough capacity to add beacon to EKF, increase max_num_beacons in ekf_config");
 
     est->beacon_ids.push_back(id);
     return find_beacon_matrix_idx(est->beacon_ids, id);

experimental/beacon_sim/information_lower_bound_search.cc

@@ -53,7 +53,7 @@ MergeResult should_merge(const std::vector<InProgressPath> &existing,
 InformationLowerBoundResult information_lower_bound_search(
     const planning::RoadMap &road_map, const double start_information,
     const double end_information_lower_bound, const LowerBoundReversePropagator &rev_propagator) {
-    CHECK(road_map.has_start_goal());
+    ROBOT_CHECK(road_map.has_start_goal());
 
     std::unordered_map<int, std::vector<detail::InProgressPath>> paths_from_node_id;
     std::priority_queue<detail::InProgressPath, std::vector<detail::InProgressPath>,