From 0f38b38b174a034f518a224397dae8601debdc33 Mon Sep 17 00:00:00 2001 From: efahnestock <13023468+efahnestock@users.noreply.github.com> Date: Mon, 2 Dec 2024 12:34:02 -0500 Subject: [PATCH] Rename CHECK define (#348) --- common/check.hh | 3 ++- common/check_test.cc | 2 +- common/linux/get_memory_usage.cc | 2 +- common/math/cubic_hermite_spline.hh | 6 ++--- common/math/multivariate_normal_cdf.cc | 6 ++--- common/sqlite3/sqlite3.cc | 8 +++---- experimental/beacon_sim/beacon_potential.cc | 4 ++-- experimental/beacon_sim/beacon_potential.hh | 4 ++-- .../beacon_sim/beacon_potential_to_proto.cc | 2 +- experimental/beacon_sim/beacon_sim.cc | 8 +++---- .../beacon_sim/belief_road_map_planner.cc | 15 +++++++------ .../beacon_sim/compute_oracle_results.cc | 18 ++++++++------- .../beacon_sim/correlated_beacon_potential.cc | 10 +++++---- experimental/beacon_sim/ekf_slam.cc | 4 ++-- .../information_lower_bound_search.cc | 2 +- .../beacon_sim/make_belief_updater.cc | 16 +++++++------- .../beacon_sim/precision_matrix_potential.cc | 5 +++-- experimental/beacon_sim/reprocess_result.cc | 12 +++++----- experimental/beacon_sim/run_experiment.cc | 22 +++++++++---------- experimental/beacon_sim/work_server.cc | 10 ++++----- experimental/beacon_sim/world_map.cc | 2 +- .../learn_descriptors/symphony_lake_parser.cc | 4 ++-- .../overhead_matching/spectacular_log.cc | 8 +++---- planning/road_map.cc | 12 +++++----- 24 files changed, 96 insertions(+), 89 deletions(-) diff --git a/common/check.hh b/common/check.hh index bab7ad8d..a6e03163 100644 --- a/common/check.hh +++ b/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; diff --git a/common/check_test.cc b/common/check_test.cc index f8e5c570..64a2c91b 100644 --- a/common/check_test.cc +++ b/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); } diff --git a/common/linux/get_memory_usage.cc b/common/linux/get_memory_usage.cc index b64be09c..9fa1f9d1 100644 --- a/common/linux/get_memory_usage.cc +++ b/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 diff --git a/common/math/cubic_hermite_spline.hh b/common/math/cubic_hermite_spline.hh index 1365f599..94f60867 100644 --- a/common/math/cubic_hermite_spline.hh +++ b/common/math/cubic_hermite_spline.hh @@ -17,12 +17,12 @@ class CubicHermiteSpline { CubicHermiteSpline(std::vector ts, std::vector 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()) { diff --git a/common/math/multivariate_normal_cdf.cc b/common/math/multivariate_normal_cdf.cc index fb38d899..1dde798a 100644 --- a/common/math/multivariate_normal_cdf.cc +++ b/common/math/multivariate_normal_cdf.cc @@ -11,9 +11,9 @@ std::optional 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; diff --git a/common/sqlite3/sqlite3.cc b/common/sqlite3/sqlite3.cc index 8fa2cb71..6b0e4156 100644 --- a/common/sqlite3/sqlite3.cc +++ b/common/sqlite3/sqlite3.cc @@ -20,7 +20,7 @@ struct Database::Impl { template 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 &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 column_names; diff --git a/experimental/beacon_sim/beacon_potential.cc b/experimental/beacon_sim/beacon_potential.cc index 73170550..0315a3c0 100644 --- a/experimental/beacon_sim/beacon_potential.cc +++ b/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}); } diff --git a/experimental/beacon_sim/beacon_potential.hh b/experimental/beacon_sim/beacon_potential.hh index 02a24708..fe306d50 100644 --- a/experimental/beacon_sim/beacon_potential.hh +++ b/experimental/beacon_sim/beacon_potential.hh @@ -63,11 +63,11 @@ class BeaconPotential { double log_prob(const std::unordered_map &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 &present_beacons) const { - CHECK(impl_ != nullptr); + ROBOT_CHECK(impl_ != nullptr); return impl_->log_prob_(present_beacons); } diff --git a/experimental/beacon_sim/beacon_potential_to_proto.cc b/experimental/beacon_sim/beacon_potential_to_proto.cc index ec75198f..9095b660 100644 --- a/experimental/beacon_sim/beacon_potential_to_proto.cc +++ b/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(); } diff --git a/experimental/beacon_sim/beacon_sim.cc b/experimental/beacon_sim/beacon_sim.cc index 82f59c97..a0abaaa3 100644 --- a/experimental/beacon_sim/beacon_sim.cc +++ b/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( 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( 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>(); - 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>(); - 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); diff --git a/experimental/beacon_sim/belief_road_map_planner.cc b/experimental/beacon_sim/belief_road_map_planner.cc index 4e3a3dad..d67ff427 100644 --- a/experimental/beacon_sim/belief_road_map_planner.cc +++ b/experimental/beacon_sim/belief_road_map_planner.cc @@ -187,7 +187,7 @@ template <> std::function make_uncertainty_size( const UncertaintySizeOptions &options) { if (std::holds_alternative(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(options)) { return [opt = std::get(options)](const RobotBelief &belief) -> double { if (opt.position_only) { @@ -267,7 +267,7 @@ std::function 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 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 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(options)) { @@ -406,7 +407,7 @@ std::function 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 make_uncertainty_size( }; } - CHECK(false, "Unknown Uncertainty Size Options", options); + ROBOT_CHECK(false, "Unknown Uncertainty Size Options", options); return [](const auto &) { return 0.0; }; } diff --git a/experimental/beacon_sim/compute_oracle_results.cc b/experimental/beacon_sim/compute_oracle_results.cc index 05a63913..759a7048 100644 --- a/experimental/beacon_sim/compute_oracle_results.cc +++ b/experimental/beacon_sim/compute_oracle_results.cc @@ -87,7 +87,7 @@ std::optional 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 load_environment( // Load Map Config const auto maybe_map_config = robot::proto::load_from_file(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(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( 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 compute_plans( const std::function &progress_function) { const auto maybe_results_file = robot::proto::load_from_file(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 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(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()); } } } diff --git a/experimental/beacon_sim/correlated_beacon_potential.cc b/experimental/beacon_sim/correlated_beacon_potential.cc index 192f6ae6..1abad05e 100644 --- a/experimental/beacon_sim/correlated_beacon_potential.cc +++ b/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; } diff --git a/experimental/beacon_sim/ekf_slam.cc b/experimental/beacon_sim/ekf_slam.cc index 9ecde90f..24d01827 100644 --- a/experimental/beacon_sim/ekf_slam.cc +++ b/experimental/beacon_sim/ekf_slam.cc @@ -27,8 +27,8 @@ std::optional find_beacon_matrix_idx_or_add(const int id, InOutmean.rows() - ROBOT_STATE_DIM) / BEACON_DIM; - CHECK(static_cast(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(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); diff --git a/experimental/beacon_sim/information_lower_bound_search.cc b/experimental/beacon_sim/information_lower_bound_search.cc index 68619c3a..c814e2b3 100644 --- a/experimental/beacon_sim/information_lower_bound_search.cc +++ b/experimental/beacon_sim/information_lower_bound_search.cc @@ -53,7 +53,7 @@ MergeResult should_merge(const std::vector &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> paths_from_node_id; std::priority_queue, diff --git a/experimental/beacon_sim/make_belief_updater.cc b/experimental/beacon_sim/make_belief_updater.cc index b748d6a3..4ec4885f 100644 --- a/experimental/beacon_sim/make_belief_updater.cc +++ b/experimental/beacon_sim/make_belief_updater.cc @@ -76,13 +76,13 @@ std::string configuration_to_key(const std::vector> &beaco std::string merge_configurations(const std::string &a, const std::string &b) { std::string out = a; - CHECK(a.size() == b.size()); + ROBOT_CHECK(a.size() == b.size()); for (int i = 0; i < static_cast(a.size()); i++) { const bool a_is_set = a.at(i) != '?'; const bool b_is_set = b.at(i) != '?'; - CHECK(a_is_set ^ b_is_set || a.at(i) == b.at(i), "a and b are not mergable", i, a, b); + ROBOT_CHECK(a_is_set ^ b_is_set || a.at(i) == b.at(i), "a and b are not mergable", i, a, b); if (b_is_set) { out.at(i) = b.at(i); } @@ -102,8 +102,8 @@ using UnappliedLandmarkBeliefMap = std::unordered_map downsize_and_normalize_belief( const UnappliedLandmarkBeliefMap &belief, const std::optional max_num_components, InOut> gen) { - CHECK(belief.size() > 0); - CHECK(!max_num_components.has_value() || max_num_components.value() > 0); + ROBOT_CHECK(belief.size() > 0); + ROBOT_CHECK(!max_num_components.has_value() || max_num_components.value() > 0); UnappliedLandmarkBeliefMap out; double log_normalizer = 0; @@ -223,7 +223,7 @@ std::vector sample_log_marginals(const std::vector all std::unordered_map assignment_from_config(const std::string &config, const std::vector &members) { - CHECK(config.size() == members.size()); + ROBOT_CHECK(config.size() == members.size()); std::unordered_map out; for (int i = 0; i < static_cast(config.size()); i++) { if (config.at(i) == '?') { @@ -233,7 +233,7 @@ std::unordered_map assignment_from_config(const std::string &config, } else if (config.at(i) == '0') { out[members.at(i)] = false; } else { - CHECK(false, "unable to parse config string", config, i, config.at(i)); + ROBOT_CHECK(false, "unable to parse config string", config, i, config.at(i)); } } return out; @@ -279,7 +279,7 @@ std::tuple sample_beliefs_without_replacemen }; const double belief_sum = math::logsumexp(downselected_unapplied_belief_from_config, log_prob_accessor); - CHECK(std::abs(belief_sum) < 1e-6); + ROBOT_CHECK(std::abs(belief_sum) < 1e-6); } return {downselected_unapplied_belief_from_config, log_probability_mass_tracked}; } @@ -299,7 +299,7 @@ std::tuple sample_beliefs_with_replacement( for (int i = 0; i < max_num_components; i++) { // Sample a component from the belief while (counter > 0.0) { - CHECK(component_iter != initial_belief.belief_from_config.end()); + ROBOT_CHECK(component_iter != initial_belief.belief_from_config.end()); counter -= std::exp(component_iter->second.log_config_prob); if (counter > 0.0) { component_iter++; diff --git a/experimental/beacon_sim/precision_matrix_potential.cc b/experimental/beacon_sim/precision_matrix_potential.cc index 31a45e22..58b48df3 100644 --- a/experimental/beacon_sim/precision_matrix_potential.cc +++ b/experimental/beacon_sim/precision_matrix_potential.cc @@ -59,8 +59,9 @@ double compute_log_prob(const PrecisionMatrixPotential &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 std::vector to_marginalize = missing_keys; diff --git a/experimental/beacon_sim/reprocess_result.cc b/experimental/beacon_sim/reprocess_result.cc index e1c98525..9904a384 100644 --- a/experimental/beacon_sim/reprocess_result.cc +++ b/experimental/beacon_sim/reprocess_result.cc @@ -59,19 +59,19 @@ std::tuple load_environment( // Load Map Config const auto maybe_map_config = robot::proto::load_from_file( config_path.parent_path() / config.map_config_path()); - CHECK(maybe_map_config.has_value()); + ROBOT_CHECK(maybe_map_config.has_value()); const WorldMap world_map(unpack_from(maybe_map_config.value())); // Load EKF State const auto maybe_mapped_landmarks = robot::proto::load_from_file( config_path.parent_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( config_path.parent_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)}; @@ -102,7 +102,7 @@ std::vector> load_oracle_metrics( if (trial_id == 0) { out.push_back({}); } - CHECK(out.size() == eval_trial_id + 1); + ROBOT_CHECK(out.size() == eval_trial_id + 1); out.back().push_back({}); auto &to_fill = out.back().back(); @@ -265,7 +265,7 @@ void reprocess_result(const proto::ExperimentResult &result, std::filesystem::create_directories(output_path.parent_path()); { std::ofstream os(output_path); - CHECK(os.good()); + ROBOT_CHECK(os.good()); out.SerializeToOstream(&os); } } @@ -310,7 +310,7 @@ int main(int argc, const char **argv) { const auto maybe_results_file = robot::proto::load_from_file( results_file_path); - CHECK(maybe_results_file.has_value()); + ROBOT_CHECK(maybe_results_file.has_value()); robot::experimental::beacon_sim::reprocess_result( maybe_results_file.value(), diff --git a/experimental/beacon_sim/run_experiment.cc b/experimental/beacon_sim/run_experiment.cc index c7c1d5ec..ea2a202d 100644 --- a/experimental/beacon_sim/run_experiment.cc +++ b/experimental/beacon_sim/run_experiment.cc @@ -132,13 +132,13 @@ UncertaintySizeOptions from_proto(const proto::UncertaintySize &config) { }; } case proto::UncertaintySize::UNCERTAINTY_SIZE_ONEOF_NOT_SET: { - CHECK(false, "uncertainty size not set"); + ROBOT_CHECK(false, "uncertainty size not set"); return ExpectedDeterminant{ .position_only = false, }; } } - CHECK(false, "uncertainty size not set"); + ROBOT_CHECK(false, "uncertainty size not set"); return ExpectedDeterminant{.position_only = false}; } @@ -318,7 +318,7 @@ proto::ExperimentResult to_proto(const ExperimentConfig &config, for (int planner_idx = 0; planner_idx < static_cast(names.size()); planner_idx++) { std::cout << "Working on planner: " << names.at(planner_idx) << std::endl; - CHECK(result.results.contains(names.at(planner_idx)), "uh oh!", result.results); + ROBOT_CHECK(result.results.contains(names.at(planner_idx)), "uh oh!", result.results); const auto &trial_result = result.results.at(names.at(planner_idx)); proto::PlannerResult &planner_result = *out.add_results(); @@ -351,19 +351,19 @@ void run_experiment(const proto::ExperimentConfig &config, const std::filesystem // Load Map Config const auto maybe_map_config = robot::proto::load_from_file(base_path / config.map_config_path()); - CHECK(maybe_map_config.has_value()); + ROBOT_CHECK(maybe_map_config.has_value()); const WorldMap world_map(unpack_from(maybe_map_config.value())); // Load EKF State const auto maybe_mapped_landmarks = robot::proto::load_from_file(base_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(base_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()); std::mt19937 gen(config.start_goal_seed()); @@ -402,8 +402,8 @@ void run_experiment(const proto::ExperimentConfig &config, const std::filesystem std::unordered_map results; for (const auto &planner_config : config.planner_configs()) { - CHECK(planner_config.planner_config_oneof_case() != - proto::PlannerConfig::PLANNER_CONFIG_ONEOF_NOT_SET); + ROBOT_CHECK(planner_config.planner_config_oneof_case() != + proto::PlannerConfig::PLANNER_CONFIG_ONEOF_NOT_SET); switch (planner_config.planner_config_oneof_case()) { case proto::PlannerConfig::kLandmarkBrmConfig: { results[planner_config.name()] = run_planner( @@ -424,8 +424,8 @@ void run_experiment(const proto::ExperimentConfig &config, const std::filesystem break; } default: { - CHECK(false, "Unhandled Planner Config type", - planner_config.planner_config_oneof_case()); + ROBOT_CHECK(false, "Unhandled Planner Config type", + planner_config.planner_config_oneof_case()); } } } @@ -479,7 +479,7 @@ int main(int argc, const char **argv) { std::filesystem::path config_file_path = args["config_file"].as(); const auto maybe_config_file = robot::proto::load_from_file(config_file_path); - CHECK(maybe_config_file.has_value()); + ROBOT_CHECK(maybe_config_file.has_value()); robot::experimental::beacon_sim::run_experiment( maybe_config_file.value(), config_file_path.remove_filename(), diff --git a/experimental/beacon_sim/work_server.cc b/experimental/beacon_sim/work_server.cc index dab146f7..7ae0c9c2 100644 --- a/experimental/beacon_sim/work_server.cc +++ b/experimental/beacon_sim/work_server.cc @@ -17,7 +17,7 @@ constexpr int CHUNK_SIZE = 200; std::vector create_inputs_from_result( const std::filesystem::path &result_path, const std::filesystem::path &experiment_config_path) { const auto maybe_proto = robot::proto::load_from_file(result_path); - CHECK(maybe_proto.has_value(), "Failed to load proto", result_path); + ROBOT_CHECK(maybe_proto.has_value(), "Failed to load proto", result_path); const int num_eval_trials = maybe_proto.value().experiment_config().num_eval_trials(); std::vector out; @@ -57,7 +57,7 @@ WorkServer::WorkServer(const std::filesystem::path &db_path, // Check if the table exists const auto rows = db_.query("SELECT count(*) FROM sqlite_master WHERE type='table' and name='" + JOB_TABLE_NAME + "'"); - CHECK(rows.size() == 1); + ROBOT_CHECK(rows.size() == 1); const bool does_table_exist = std::get(rows.at(0).value(0)); if (does_table_exist) { // Table exists @@ -183,19 +183,19 @@ grpc::Status WorkServer::get_progress(grpc::ServerContext *, const proto::Progre { const auto rows = db_.query("SELECT count(*) FROM " + JOB_TABLE_NAME + " WHERE job_result IS NOT NULL;"); - CHECK(!rows.empty()); + ROBOT_CHECK(!rows.empty()); jobs_completed = std::get(rows.at(0).value(0)); } { const auto rows = db_.query("SELECT count(*) FROM " + JOB_TABLE_NAME + " WHERE job_status IS NULL;"); - CHECK(!rows.empty()); + ROBOT_CHECK(!rows.empty()); jobs_remaining = std::get(rows.at(0).value(0)); } { const auto rows = db_.query("SELECT count(*) FROM " + JOB_TABLE_NAME + " WHERE job_status IS NOT NULL AND job_result IS NULL;"); - CHECK(!rows.empty()); + ROBOT_CHECK(!rows.empty()); jobs_in_progress = std::get(rows.at(0).value(0)); } response->set_jobs_completed(jobs_completed); diff --git a/experimental/beacon_sim/world_map.cc b/experimental/beacon_sim/world_map.cc index 0e00dea5..790c2ab3 100644 --- a/experimental/beacon_sim/world_map.cc +++ b/experimental/beacon_sim/world_map.cc @@ -25,7 +25,7 @@ std::vector get_beacon_configuration(const CorrelatedBeaconsConfig &config std::vector out(all_members.size(), false); for (const int beacon_id : present_beacon_ids) { const auto iter = std::find(all_members.begin(), all_members.end(), beacon_id); - CHECK(iter != all_members.end()); + ROBOT_CHECK(iter != all_members.end()); const int idx = std::distance(all_members.begin(), iter); out.at(idx) = true; } diff --git a/experimental/learn_descriptors/symphony_lake_parser.cc b/experimental/learn_descriptors/symphony_lake_parser.cc index 4a68951d..5f93ab17 100644 --- a/experimental/learn_descriptors/symphony_lake_parser.cc +++ b/experimental/learn_descriptors/symphony_lake_parser.cc @@ -7,8 +7,8 @@ namespace robot::experimental::learn_descriptors { DataParser::DataParser(const std::filesystem::path &image_root_dir, const std::vector &survey_list) { - CHECK(std::filesystem::exists(image_root_dir), "Image root dir does not exist!", - image_root_dir); + ROBOT_CHECK(std::filesystem::exists(image_root_dir), "Image root dir does not exist!", + image_root_dir); surveys_.load(image_root_dir.string(), survey_list); } DataParser::~DataParser() {} diff --git a/experimental/overhead_matching/spectacular_log.cc b/experimental/overhead_matching/spectacular_log.cc index 52a4f823..5e5b1620 100644 --- a/experimental/overhead_matching/spectacular_log.cc +++ b/experimental/overhead_matching/spectacular_log.cc @@ -42,9 +42,9 @@ struct LogData { }; LogData read_jsonl(const fs::path &file_path) { - CHECK(fs::exists(file_path), "jsonl path does not exist", file_path); + ROBOT_CHECK(fs::exists(file_path), "jsonl path does not exist", file_path); std::ifstream in_file(file_path); - CHECK(in_file.is_open(), "Could not open file", file_path); + ROBOT_CHECK(in_file.is_open(), "Could not open file", file_path); LogData out; for (std::string line; std::getline(in_file, line);) { @@ -73,7 +73,7 @@ LogData read_jsonl(const fs::path &file_path) { { const auto &frame = j["frames"].at(0); const auto &calib_info = frame["calibration"]; - CHECK(frame["colorFormat"] == "rgb"); + ROBOT_CHECK(frame["colorFormat"] == "rgb"); calibration[0].focal_length = {calib_info["focalLengthX"].get(), calib_info["focalLengthY"].get()}; @@ -84,7 +84,7 @@ LogData read_jsonl(const fs::path &file_path) { { const auto &frame = j["frames"].at(1); const auto &calib_info = frame["calibration"]; - CHECK(frame["colorFormat"] == "gray"); + ROBOT_CHECK(frame["colorFormat"] == "gray"); calibration[1].focal_length = {calib_info["focalLengthX"].get(), calib_info["focalLengthY"].get()}; diff --git a/planning/road_map.cc b/planning/road_map.cc index c0798ff1..55b7f3bf 100644 --- a/planning/road_map.cc +++ b/planning/road_map.cc @@ -33,10 +33,10 @@ void RoadMap::add_start_goal(const StartGoalPair &start_goal_pair) { } } - CHECK(!start_neighbors.empty(), "No roadmap points near start", - start_goal_pair.start.transpose(), start_goal_pair.connection_radius_m); - CHECK(!goal_neighbors.empty(), "No roadmap points near goal", start_goal_pair.goal.transpose(), - start_goal_pair.connection_radius_m); + ROBOT_CHECK(!start_neighbors.empty(), "No roadmap points near start", + start_goal_pair.start.transpose(), start_goal_pair.connection_radius_m); + ROBOT_CHECK(!goal_neighbors.empty(), "No roadmap points near goal", + start_goal_pair.goal.transpose(), start_goal_pair.connection_radius_m); start_goal_ = { .start = start_goal_pair.start, @@ -47,7 +47,7 @@ void RoadMap::add_start_goal(const StartGoalPair &start_goal_pair) { } const Eigen::Vector2d &RoadMap::point(const int idx) const { - CHECK(idx >= RoadMap::GOAL_IDX, "Invalid index"); + ROBOT_CHECK(idx >= RoadMap::GOAL_IDX, "Invalid index"); if (idx >= 0) { return points_.at(idx); } else if (idx == RoadMap::START_IDX) { @@ -58,7 +58,7 @@ const Eigen::Vector2d &RoadMap::point(const int idx) const { } std::vector> RoadMap::neighbors(const int idx) const { - CHECK(idx >= RoadMap::GOAL_IDX, "Invalid index"); + ROBOT_CHECK(idx >= RoadMap::GOAL_IDX, "Invalid index"); std::vector> out; if (idx >= 0) { for (int other_idx = 0; other_idx < static_cast(points_.size()); other_idx++) {