diff --git a/WORKSPACE b/WORKSPACE index bb6e8f7f..3ba69c91 100644 --- a/WORKSPACE +++ b/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", 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/BUILD b/experimental/learn_descriptors/BUILD index cc3983df..1455db51 100644 --- a/experimental/learn_descriptors/BUILD +++ b/experimental/learn_descriptors/BUILD @@ -23,6 +23,17 @@ cc_library( srcs = ["visual_odometry.cc"], deps = [ "@opencv//:opencv" +) + +cc_library( + name = "symphony_lake_parser", + hdrs = ["symphony_lake_parser.hh"], + copts = ["-Wno-unused-parameter"], + visibility = ["//visibility:public"], + srcs = ["symphony_lake_parser.cc"], + deps = [ + "@symphony_lake_parser", + "//common:check" ] ) @@ -35,6 +46,18 @@ cc_test( ] ) +cc_test( + name = "symphony_lake_parser_test", + srcs = ["symphony_lake_parser_test.cc"], + copts = ["-Wno-unused-parameter"], + data = ["@symphony_lake_snippet"], + deps = [ + "@com_google_googletest//:gtest_main", + ":symphony_lake_parser", + "//common:check" + ] +) + cc_test( name = "gtsam_test", srcs = ["gtsam_test.cc"], diff --git a/experimental/learn_descriptors/symphony_lake_parser.cc b/experimental/learn_descriptors/symphony_lake_parser.cc new file mode 100644 index 00000000..5f93ab17 --- /dev/null +++ b/experimental/learn_descriptors/symphony_lake_parser.cc @@ -0,0 +1,15 @@ +#include "experimental/learn_descriptors/symphony_lake_parser.hh" + +#include + +#include "common/check.hh" + +namespace robot::experimental::learn_descriptors { +DataParser::DataParser(const std::filesystem::path &image_root_dir, + const std::vector &survey_list) { + 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() {} +} // namespace robot::experimental::learn_descriptors \ No newline at end of file diff --git a/experimental/learn_descriptors/symphony_lake_parser.hh b/experimental/learn_descriptors/symphony_lake_parser.hh new file mode 100644 index 00000000..021ebd3f --- /dev/null +++ b/experimental/learn_descriptors/symphony_lake_parser.hh @@ -0,0 +1,21 @@ +#pragma once +#pragma once +#include + +#include + +#include "symphony_lake_dataset/SurveyVector.h" + +namespace robot::experimental::learn_descriptors { +class DataParser { + public: + DataParser(const std::filesystem::path &image_root_dir, + const std::vector &survey_list); + ~DataParser(); + + const symphony_lake_dataset::SurveyVector &get_surveys() const { return surveys_; }; + + private: + symphony_lake_dataset::SurveyVector surveys_; +}; +} // namespace robot::experimental::learn_descriptors \ No newline at end of file diff --git a/experimental/learn_descriptors/symphony_lake_parser_test.cc b/experimental/learn_descriptors/symphony_lake_parser_test.cc new file mode 100644 index 00000000..e17ff802 --- /dev/null +++ b/experimental/learn_descriptors/symphony_lake_parser_test.cc @@ -0,0 +1,67 @@ +#include "experimental/learn_descriptors/symphony_lake_parser.hh" + +#include +#include +#include +#include +#include + +#include "common/check.hh" +#include "gtest/gtest.h" +#include "opencv2/opencv.hpp" + +namespace robot::experimental::learn_descriptors { +namespace { +bool is_test() { return std::getenv("BAZEL_TEST") != nullptr; } +} // namespace + +class SymphonyLakeDatasetTestHelper { + public: + static bool images_equal(cv::Mat img1, cv::Mat img2) { + if (img1.size() != img2.size() || img1.type() != img2.type()) { + return false; + } + cv::Mat diff; + cv::absdiff(img1, img2, diff); + diff = diff.reshape(1); + return cv::countNonZero(diff) == 0; + } +}; +TEST(SymphonyLakeParserTest, snippet_140106) { + const std::filesystem::path image_root_dir = "external/symphony_lake_snippet/symphony_lake"; + const std::vector survey_list{"140106_snippet"}; + + DataParser data_parser = DataParser(image_root_dir, survey_list); + + const symphony_lake_dataset::SurveyVector &survey_vector = data_parser.get_surveys(); + + cv::Mat image; + cv::Mat target_img; + if (!is_test()) { + cv::namedWindow("Symphony Dataset Image", cv::WINDOW_AUTOSIZE); + } + printf("Press 'q' in graphic window to quit\n"); + for (int i = 0; i < static_cast(survey_vector.getNumSurveys()); i++) { + const symphony_lake_dataset::Survey &survey = survey_vector.get(i); + for (int j = 0; j < static_cast(survey.getNumImages()); j++) { + image = survey.loadImageByImageIndex(j); + + // get the target image + std::stringstream target_img_name; + target_img_name << "0000" << j << ".jpg"; + const size_t target_img_name_length = 8; + std::string target_img_name_str = target_img_name.str(); + target_img_name_str.replace(0, target_img_name_str.size() - target_img_name_length, ""); + std::filesystem::path target_img_dir = + image_root_dir / survey_list[i] / "0027" / target_img_name_str; + target_img = cv::imread(target_img_dir.string()); + + EXPECT_TRUE(SymphonyLakeDatasetTestHelper::images_equal(image, target_img)); + if (!is_test()) { + cv::imshow("Symphony Dataset Image", image); + cv::waitKey(2); + } + } + } +} +} // namespace robot::experimental::learn_descriptors diff --git a/experimental/overhead_matching/BUILD b/experimental/overhead_matching/BUILD index 9b5dac35..ca15a04d 100644 --- a/experimental/overhead_matching/BUILD +++ b/experimental/overhead_matching/BUILD @@ -1,4 +1,11 @@ + +package(features=["warning_compile_flags"]) + +load("@pybind11_bazel//:build_defs.bzl", "pybind_extension") +load("@rules_python//python:defs.bzl", "py_binary") +load("@pip//:requirements.bzl", "requirement") + cc_library( name = "spectacular_log", hdrs = ["spectacular_log.hh"], @@ -25,3 +32,20 @@ cc_test( "@fmt", ] ) + +pybind_extension( + name = "spectacular_log_python", + srcs = ["spectacular_log_python.cc"], + data = ["//common/time:robot_time_python.so"], + deps = [":spectacular_log"], +) + +py_binary( + name = "spectacular_log_to_rosbag", + srcs = ["spectacular_log_to_rosbag.py"], + data = [":spectacular_log_python.so"], + deps = [ + requirement("rosbags"), + requirement("numpy"), + ], +) diff --git a/experimental/overhead_matching/spectacular_log.cc b/experimental/overhead_matching/spectacular_log.cc index 52a4f823..206c8678 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()}; @@ -154,7 +154,7 @@ std::optional SpectacularLog::get_imu_sample(const time::RobotTimesta } std::optional SpectacularLog::get_frame(const int frame_id) const { - if (frame_id < 0 || frame_id >= frame_info_.size()) { + if (frame_id < 0 || frame_id >= static_cast(frame_info_.size())) { return std::nullopt; } @@ -162,12 +162,12 @@ std::optional SpectacularLog::get_frame(const int frame_id) const { // Read the desired rgb frame if (video_ == nullptr) { - const fs::path rgb_path = log_path_ / "data.mov"; - video_ = std::make_unique(rgb_path.string(), cv::CAP_FFMPEG); + const fs::path color_path = log_path_ / "data.mov"; + video_ = std::make_unique(color_path.string(), cv::CAP_FFMPEG); } - cv::Mat rgb_frame; + cv::Mat bgr_frame; seek_to_frame(frame_info.frame_number, make_in_out(*video_)); - video_->read(rgb_frame); + video_->read(bgr_frame); // Read the desired depth frame const fs::path depth_frame_path = @@ -177,9 +177,9 @@ std::optional SpectacularLog::get_frame(const int frame_id) const { return {{ .time_of_validity = time::as_duration(frame_info.time_of_validity_s) + time::RobotTimestamp(), - .rgb_frame = std::move(rgb_frame), + .bgr_frame = std::move(bgr_frame), .depth_frame = std::move(depth_frame), - .rgb_calibration = frame_info.calibration.at(0), + .color_calibration = frame_info.calibration.at(0), .depth_calibration = frame_info.calibration.at(1), }}; } diff --git a/experimental/overhead_matching/spectacular_log.hh b/experimental/overhead_matching/spectacular_log.hh index bc8f39f4..bcf23a96 100644 --- a/experimental/overhead_matching/spectacular_log.hh +++ b/experimental/overhead_matching/spectacular_log.hh @@ -22,10 +22,10 @@ struct FrameCalibration { struct FrameGroup { time::RobotTimestamp time_of_validity; - cv::Mat rgb_frame; + cv::Mat bgr_frame; cv::Mat depth_frame; - FrameCalibration rgb_calibration; + FrameCalibration color_calibration; FrameCalibration depth_calibration; }; diff --git a/experimental/overhead_matching/spectacular_log_python.cc b/experimental/overhead_matching/spectacular_log_python.cc new file mode 100644 index 00000000..71f82b87 --- /dev/null +++ b/experimental/overhead_matching/spectacular_log_python.cc @@ -0,0 +1,62 @@ + +#include + +#include "experimental/overhead_matching/spectacular_log.hh" +#include "opencv2/core/eigen.hpp" +#include "pybind11/eigen.h" +#include "pybind11/eigen/tensor.h" +#include "pybind11/pybind11.h" +#include "pybind11/stl.h" +#include "pybind11/stl/filesystem.h" +#include "unsupported/Eigen/CXX11/Tensor" + +namespace py = pybind11; +using namespace pybind11::literals; + +namespace robot::experimental::overhead_matching { +namespace { +Eigen::Tensor tensor_map_from_cv_mat(const cv::Mat &mat) { + Eigen::Tensor tensor; + cv::cv2eigen(mat, tensor); + return tensor; +} +} // namespace +PYBIND11_MODULE(spectacular_log_python, m) { + m.doc() = "spectacular log"; + + py::module_::import("common.time.robot_time_python"); + + py::class_(m, "FrameCalibration") + .def(py::init<>()) + .def_readwrite("focal_length", &FrameCalibration::focal_length) + .def_readwrite("principal_point", &FrameCalibration::principal_point) + .def_readwrite("exposure_time", &FrameCalibration::exposure_time_s) + .def_readwrite("depth_scale", &FrameCalibration::depth_scale); + + py::class_(m, "FrameGroup") + .def(py::init<>()) + .def_readwrite("time_of_validity", &FrameGroup::time_of_validity) + .def("bgr_frame", + [](const FrameGroup &self) { return tensor_map_from_cv_mat(self.bgr_frame); }) + .def("depth_frame", + [](const FrameGroup &self) { return tensor_map_from_cv_mat(self.depth_frame); }) + .def_readwrite("color_calibration", &FrameGroup::color_calibration) + .def_readwrite("depth_calibration", &FrameGroup::depth_calibration); + + py::class_(m, "ImuSample") + .def(py::init<>()) + .def_readwrite("time_of_validity", &ImuSample::time_of_validity) + .def_readwrite("accel_mpss", &ImuSample::accel_mpss) + .def_readwrite("gyro_radps", &ImuSample::gyro_radps); + + py::class_(m, "SpectacularLog") + .def(py::init()) + .def("get_imu_sample", &SpectacularLog::get_imu_sample) + .def("get_frame", &SpectacularLog::get_frame) + .def("min_imu_time", &SpectacularLog::min_imu_time) + .def("max_imu_time", &SpectacularLog::max_imu_time) + .def("min_frame_time", &SpectacularLog::min_frame_time) + .def("max_frame_time", &SpectacularLog::max_frame_time) + .def("num_frames", &SpectacularLog::num_frames); +} +} // namespace robot::experimental::overhead_matching diff --git a/experimental/overhead_matching/spectacular_log_test.cc b/experimental/overhead_matching/spectacular_log_test.cc index 4fd26f0a..a1f91317 100644 --- a/experimental/overhead_matching/spectacular_log_test.cc +++ b/experimental/overhead_matching/spectacular_log_test.cc @@ -63,7 +63,7 @@ TEST(SpectacularLogTest, happy_case) { fmt::format("frames2/{:08d}.png", frame_id)); const cv::Mat depth_frame = cv::imread(depth_path, cv::IMREAD_GRAYSCALE); - EXPECT_TRUE(images_equal(expected_frame, frame.rgb_frame)); + EXPECT_TRUE(images_equal(expected_frame, frame.bgr_frame)); EXPECT_TRUE(images_equal(depth_frame, frame.depth_frame)); for (int i = 0; i < FRAME_SKIP - 1; i++) { diff --git a/experimental/overhead_matching/spectacular_log_to_rosbag.py b/experimental/overhead_matching/spectacular_log_to_rosbag.py new file mode 100644 index 00000000..076c3499 --- /dev/null +++ b/experimental/overhead_matching/spectacular_log_to_rosbag.py @@ -0,0 +1,125 @@ + +from rosbags.rosbag1 import Writer +from rosbags.typesys import Stores, get_typestore +import argparse +import numpy as np + +from experimental.overhead_matching import spectacular_log_python as slp +import common.time.robot_time_python as rtp + +from pathlib import Path + + +def main(spectacular_log_path: Path, rosbag_out: Path): + log = slp.SpectacularLog(spectacular_log_path) + + typestore = get_typestore(Stores.ROS1_NOETIC) + Header = typestore.types["std_msgs/msg/Header"] + Imu = typestore.types["sensor_msgs/msg/Imu"] + Image = typestore.types["sensor_msgs/msg/Image"] + Quaternion = typestore.types["geometry_msgs/msg/Quaternion"] + Vector3 = typestore.types["geometry_msgs/msg/Vector3"] + Time = typestore.types["builtin_interfaces/msg/Time"] + + with Writer(rosbag_out) as writer: + imu_conn = writer.add_connection('/imu0', Imu.__msgtype__, typestore=typestore) + bgr_conn = writer.add_connection( + '/cam0/image_raw', Image.__msgtype__, typestore=typestore) + depth_conn = writer.add_connection( + '/cam0/depth_raw', Image.__msgtype__, typestore=typestore) + + # Write out the IMU Data and Camera Data + t = log.min_imu_time() + i = 0 + + curr_frame_id = 0 + frame = log.get_frame(curr_frame_id) + + while t < log.max_imu_time(): + + sample = log.get_imu_sample(t) + imu_time_since_epoch = t.time_since_epoch() + + while (frame is not None) and (frame.time_of_validity < t): + frame_time_since_epoch = frame.time_of_validity.time_since_epoch() + header = Header( + seq=curr_frame_id, + stamp=Time( + sec=int(frame_time_since_epoch.total_seconds()), + nanosec=1000 * frame_time_since_epoch.microseconds), + frame_id='cam0' + ) + + bgr_frame = frame.bgr_frame() + # Foxglove expects mono images to have 16 bit depth, we we convert + # to np.uint16 here. Note that this doubles the size of the images + depth_frame = frame.depth_frame().astype(np.uint16) + + bgr_ros = Image( + header=header, + height=bgr_frame.shape[0], + width=bgr_frame.shape[1], + encoding='bgr8', + is_bigendian=False, + # This is the size of each row in bytes + step=bgr_frame.shape[1] * 3, + data=bgr_frame.flatten() + ) + + depth_ros = Image( + header=header, + height=depth_frame.shape[0], + width=depth_frame.shape[1], + encoding='mono16', + is_bigendian=False, + # This is the size of each row in bytes + step=depth_frame.shape[1] * 2, + # the rosbags library wants a flat buffer, so we view the 16 bit pixels as uint8 + data=depth_frame.flatten().view(np.uint8) + ) + + timestamp_ns = (int(frame_time_since_epoch.total_seconds()) * 1_000_000_000 + + frame_time_since_epoch.microseconds * 1000) + + writer.write( + bgr_conn, timestamp_ns, typestore.serialize_ros1(bgr_ros, Image.__msgtype__)) + writer.write( + depth_conn, timestamp_ns, typestore.serialize_ros1(depth_ros, Image.__msgtype__)) + + curr_frame_id += 1 + frame = log.get_frame(curr_frame_id) + + imu_ros = Imu( + header=Header( + seq=i, + stamp=Time( + sec=int(imu_time_since_epoch.total_seconds()), + nanosec=1000 * imu_time_since_epoch.microseconds), + frame_id='imu0'), + orientation=Quaternion(x=0, y=0, z=0, w=1), + orientation_covariance=-np.ones((3, 3)).flatten(), + angular_velocity=Vector3( + x=sample.gyro_radps[0], y=sample.gyro_radps[1], z=sample.gyro_radps[2]), + angular_velocity_covariance=np.eye(3).flatten(), + linear_acceleration=Vector3( + x=sample.accel_mpss[0], y=sample.accel_mpss[1], z=sample.accel_mpss[2]), + linear_acceleration_covariance=np.eye(3).flatten() + ) + + timestamp_ns = (int(imu_time_since_epoch.total_seconds()) * 1_000_000_000 + + imu_time_since_epoch.microseconds * 1000) + + writer.write(imu_conn, timestamp_ns, typestore.serialize_ros1(imu_ros, Imu.__msgtype__)) + + t += rtp.as_duration(0.01) + i += 1 + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--input", required=True, help="path to unzipped spectacular_log") + parser.add_argument("--output", required=True, help="path to output rosbag") + + args = parser.parse_args() + + main(Path(args.input), Path(args.output)) 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++) { diff --git a/setup.sh b/setup.sh index 104dbf18..4403abee 100755 --- a/setup.sh +++ b/setup.sh @@ -15,5 +15,5 @@ sudo apt-get update sudo DEBIAN_FRONTEND=noninteractive apt-get -y install clang clang-15 clang-format-15 libxcursor-dev \ libxrandr-dev libxinerama-dev libxi-dev freeglut3-dev libstdc++-12-dev gcc-11 g++-11 libtbb-dev \ libfmt-dev libspdlog-dev libvtk9-dev coinor-libipopt-dev coinor-libclp-dev \ - libgirepository1.0-dev libcairo2-dev libcanberra-gtk-module + libgirepository1.0-dev libcairo2-dev libgtk2.0-dev libcanberra-gtk-module diff --git a/third_party/BUILD.symphony_lake_parser b/third_party/BUILD.symphony_lake_parser new file mode 100644 index 00000000..894c9902 --- /dev/null +++ b/third_party/BUILD.symphony_lake_parser @@ -0,0 +1,26 @@ +package(features=["-warning_compile_flags"]) + +cc_library( + name = "symphony_lake_parser", + hdrs = [ + "include/symphony_lake_dataset/SurveyVector.h", + "include/symphony_lake_dataset/csv_functions.h", + "include/symphony_lake_dataset/Function.h", + "include/symphony_lake_dataset/ImagePoint.h", + "include/symphony_lake_dataset/ParseSurvey.h", + "include/symphony_lake_dataset/Pose.h", + "include/symphony_lake_dataset/Survey.h", + ], + copts = ["-Wno-unused-parameter"], + visibility = ["//visibility:public"], + srcs = [ + "src/Function.cpp", + "src/ParseSurvey.cpp", + "src/Survey.cpp", + "src/SurveyVector.cpp" + ], + strip_include_prefix="include/", + deps = [ + "@opencv//:opencv", + ], +) diff --git a/third_party/BUILD.symphony_lake_snippet b/third_party/BUILD.symphony_lake_snippet new file mode 100644 index 00000000..871d4aa1 --- /dev/null +++ b/third_party/BUILD.symphony_lake_snippet @@ -0,0 +1,6 @@ + +filegroup( + name = "symphony_lake_snippet", + srcs = glob(["**/*"]), + visibility=["//visibility:public"], +) diff --git a/third_party/python/requirements.in b/third_party/python/requirements.in index 17a33350..5dd39113 100644 --- a/third_party/python/requirements.in +++ b/third_party/python/requirements.in @@ -21,3 +21,4 @@ spatialmath-python==1.1.8 reportlab==4.1.0 seaborn==0.13.2 depthai==2.28.0.0 +rosbags==0.9.23 diff --git a/third_party/python/requirements_3_10.txt b/third_party/python/requirements_3_10.txt index 7993557c..5c068d45 100644 --- a/third_party/python/requirements_3_10.txt +++ b/third_party/python/requirements_3_10.txt @@ -714,6 +714,44 @@ line-profiler==4.0.3 \ --hash=sha256:fbe0036a306835978270a66c460c7b57869fe985ca620613321971d396de295f \ --hash=sha256:fdaac2e769e7d64cd3d19c4df5d26287e1cd362f47a1d3b42edd7c8420f40101 # via -r third_party/python/requirements.in +lz4==4.3.3 \ + --hash=sha256:01fe674ef2889dbb9899d8a67361e0c4a2c833af5aeb37dd505727cf5d2a131e \ + --hash=sha256:054b4631a355606e99a42396f5db4d22046a3397ffc3269a348ec41eaebd69d2 \ + --hash=sha256:0a136e44a16fc98b1abc404fbabf7f1fada2bdab6a7e970974fb81cf55b636d0 \ + --hash=sha256:0e9c410b11a31dbdc94c05ac3c480cb4b222460faf9231f12538d0074e56c563 \ + --hash=sha256:222a7e35137d7539c9c33bb53fcbb26510c5748779364014235afc62b0ec797f \ + --hash=sha256:24b3206de56b7a537eda3a8123c644a2b7bf111f0af53bc14bed90ce5562d1aa \ + --hash=sha256:2b901c7784caac9a1ded4555258207d9e9697e746cc8532129f150ffe1f6ba0d \ + --hash=sha256:2f7b1839f795315e480fb87d9bc60b186a98e3e5d17203c6e757611ef7dcef61 \ + --hash=sha256:30e8c20b8857adef7be045c65f47ab1e2c4fabba86a9fa9a997d7674a31ea6b6 \ + --hash=sha256:31ea4be9d0059c00b2572d700bf2c1bc82f241f2c3282034a759c9a4d6ca4dc2 \ + --hash=sha256:337cb94488a1b060ef1685187d6ad4ba8bc61d26d631d7ba909ee984ea736be1 \ + --hash=sha256:33c9a6fd20767ccaf70649982f8f3eeb0884035c150c0b818ea660152cf3c809 \ + --hash=sha256:363ab65bf31338eb364062a15f302fc0fab0a49426051429866d71c793c23394 \ + --hash=sha256:43cf03059c0f941b772c8aeb42a0813d68d7081c009542301637e5782f8a33e2 \ + --hash=sha256:56f4fe9c6327adb97406f27a66420b22ce02d71a5c365c48d6b656b4aaeb7775 \ + --hash=sha256:5d35533bf2cee56f38ced91f766cd0038b6abf46f438a80d50c52750088be93f \ + --hash=sha256:6756212507405f270b66b3ff7f564618de0606395c0fe10a7ae2ffcbbe0b1fba \ + --hash=sha256:6cdc60e21ec70266947a48839b437d46025076eb4b12c76bd47f8e5eb8a75dcc \ + --hash=sha256:abc197e4aca8b63f5ae200af03eb95fb4b5055a8f990079b5bdf042f568469dd \ + --hash=sha256:b14d948e6dce389f9a7afc666d60dd1e35fa2138a8ec5306d30cd2e30d36b40c \ + --hash=sha256:b47839b53956e2737229d70714f1d75f33e8ac26e52c267f0197b3189ca6de24 \ + --hash=sha256:b6d9ec061b9eca86e4dcc003d93334b95d53909afd5a32c6e4f222157b50c071 \ + --hash=sha256:b891880c187e96339474af2a3b2bfb11a8e4732ff5034be919aa9029484cd201 \ + --hash=sha256:bca8fccc15e3add173da91be8f34121578dc777711ffd98d399be35487c934bf \ + --hash=sha256:c81703b12475da73a5d66618856d04b1307e43428a7e59d98cfe5a5d608a74c6 \ + --hash=sha256:d2507ee9c99dbddd191c86f0e0c8b724c76d26b0602db9ea23232304382e1f21 \ + --hash=sha256:e36cd7b9d4d920d3bfc2369840da506fa68258f7bb176b8743189793c055e43d \ + --hash=sha256:e7d84b479ddf39fe3ea05387f10b779155fc0990125f4fb35d636114e1c63a2e \ + --hash=sha256:eac9af361e0d98335a02ff12fb56caeb7ea1196cf1a49dbf6f17828a131da807 \ + --hash=sha256:edfd858985c23523f4e5a7526ca6ee65ff930207a7ec8a8f57a01eae506aaee7 \ + --hash=sha256:ee9ff50557a942d187ec85462bb0960207e7ec5b19b3b48949263993771c6205 \ + --hash=sha256:f0e822cd7644995d9ba248cb4b67859701748a93e2ab7fc9bc18c599a52e4604 \ + --hash=sha256:f180904f33bdd1e92967923a43c22899e303906d19b2cf8bb547db6653ea6e7d \ + --hash=sha256:f1d18718f9d78182c6b60f568c9a9cec8a7204d7cb6fad4e511a2ef279e4cb05 \ + --hash=sha256:f4c7bf687303ca47d69f9f0133274958fd672efaa33fb5bcde467862d6c621f0 \ + --hash=sha256:f76176492ff082657ada0d0f10c794b6da5800249ef1692b35cf49b1e93e8ef7 + # via rosbags markupsafe==2.1.5 \ --hash=sha256:00e046b6dd71aa03a41079792f8473dc494d564611a8f89bbbd7cb93295ebdcf \ --hash=sha256:075202fa5b72c86ad32dc7d0b56024ebdbcf2048c0ba09f1cde31bfdd57bcfff \ @@ -908,6 +946,7 @@ numpy==1.23.4 \ # matplotlib # opencv-python # pandas + # rosbags # scikit-learn # scipy # seaborn @@ -1357,6 +1396,10 @@ rfc3986-validator==0.1.1 \ # via # jsonschema # jupyter-events +rosbags==0.9.23 \ + --hash=sha256:0fd14b10ee5b2422a08b3a09d2171dc347925307716779ad6f21e398b280c56a \ + --hash=sha256:89229ca8fbb6d5e1cc452045229e6e47028dc10a8724fbe86c93eda477296dc2 + # via -r third_party/python/requirements.in rpds-py==0.18.0 \ --hash=sha256:01e36a39af54a30f28b73096dd39b6802eddd04c90dbe161c1b8dbe22353189f \ --hash=sha256:044a3e61a7c2dafacae99d1e722cc2d4c05280790ec5a05031b3876809d89a5c \ @@ -1460,6 +1503,53 @@ rpds-py==0.18.0 \ # via # jsonschema # referencing +ruamel-yaml==0.18.6 \ + --hash=sha256:57b53ba33def16c4f3d807c0ccbc00f8a6081827e81ba2491691b76882d0c636 \ + --hash=sha256:8b27e6a217e786c6fbe5634d8f3f11bc63e0f80f6a5890f28863d9c45aac311b + # via rosbags +ruamel-yaml-clib==0.2.12 \ + --hash=sha256:040ae85536960525ea62868b642bdb0c2cc6021c9f9d507810c0c604e66f5a7b \ + --hash=sha256:0467c5965282c62203273b838ae77c0d29d7638c8a4e3a1c8bdd3602c10904e4 \ + --hash=sha256:0b7e75b4965e1d4690e93021adfcecccbca7d61c7bddd8e22406ef2ff20d74ef \ + --hash=sha256:11f891336688faf5156a36293a9c362bdc7c88f03a8a027c2c1d8e0bcde998e5 \ + --hash=sha256:20b0f8dc160ba83b6dcc0e256846e1a02d044e13f7ea74a3d1d56ede4e48c632 \ + --hash=sha256:22353049ba4181685023b25b5b51a574bce33e7f51c759371a7422dcae5402a6 \ + --hash=sha256:32621c177bbf782ca5a18ba4d7af0f1082a3f6e517ac2a18b3974d4edf349680 \ + --hash=sha256:3bc2a80e6420ca8b7d3590791e2dfc709c88ab9152c00eeb511c9875ce5778bf \ + --hash=sha256:3eac5a91891ceb88138c113f9db04f3cebdae277f5d44eaa3651a4f573e6a5da \ + --hash=sha256:4a6679521a58256a90b0d89e03992c15144c5f3858f40d7c18886023d7943db6 \ + --hash=sha256:4c8c5d82f50bb53986a5e02d1b3092b03622c02c2eb78e29bec33fd9593bae1a \ + --hash=sha256:5a0e060aace4c24dcaf71023bbd7d42674e3b230f7e7b97317baf1e953e5b519 \ + --hash=sha256:6442cb36270b3afb1b4951f060eccca1ce49f3d087ca1ca4563a6eb479cb3de6 \ + --hash=sha256:6c8fbb13ec503f99a91901ab46e0b07ae7941cd527393187039aec586fdfd36f \ + --hash=sha256:749c16fcc4a2b09f28843cda5a193e0283e47454b63ec4b81eaa2242f50e4ccd \ + --hash=sha256:7dd5adc8b930b12c8fc5b99e2d535a09889941aa0d0bd06f4749e9a9397c71d2 \ + --hash=sha256:811ea1594b8a0fb466172c384267a4e5e367298af6b228931f273b111f17ef52 \ + --hash=sha256:932205970b9f9991b34f55136be327501903f7c66830e9760a8ffb15b07f05cd \ + --hash=sha256:943f32bc9dedb3abff9879edc134901df92cfce2c3d5c9348f172f62eb2d771d \ + --hash=sha256:95c3829bb364fdb8e0332c9931ecf57d9be3519241323c5274bd82f709cebc0c \ + --hash=sha256:96777d473c05ee3e5e3c3e999f5d23c6f4ec5b0c38c098b3a5229085f74236c6 \ + --hash=sha256:a274fb2cb086c7a3dea4322ec27f4cb5cc4b6298adb583ab0e211a4682f241eb \ + --hash=sha256:a606ef75a60ecf3d924613892cc603b154178ee25abb3055db5062da811fd969 \ + --hash=sha256:ab007f2f5a87bd08ab1499bdf96f3d5c6ad4dcfa364884cb4549aa0154b13a28 \ + --hash=sha256:bb43a269eb827806502c7c8efb7ae7e9e9d0573257a46e8e952f4d4caba4f31e \ + --hash=sha256:bc5f1e1c28e966d61d2519f2a3d451ba989f9ea0f2307de7bc45baa526de9e45 \ + --hash=sha256:bd0a08f0bab19093c54e18a14a10b4322e1eacc5217056f3c063bd2f59853ce4 \ + --hash=sha256:beffaed67936fbbeffd10966a4eb53c402fafd3d6833770516bf7314bc6ffa12 \ + --hash=sha256:bf165fef1f223beae7333275156ab2022cffe255dcc51c27f066b4370da81e31 \ + --hash=sha256:cf12567a7b565cbf65d438dec6cfbe2917d3c1bdddfce84a9930b7d35ea59642 \ + --hash=sha256:d84318609196d6bd6da0edfa25cedfbabd8dbde5140a0a23af29ad4b8f91fb1e \ + --hash=sha256:d85252669dc32f98ebcd5d36768f5d4faeaeaa2d655ac0473be490ecdae3c285 \ + --hash=sha256:e143ada795c341b56de9418c58d028989093ee611aa27ffb9b7f609c00d813ed \ + --hash=sha256:e188d2699864c11c36cdfdada94d781fd5d6b0071cd9c427bceb08ad3d7c70e1 \ + --hash=sha256:e2f1c3765db32be59d18ab3953f43ab62a761327aafc1594a2a1fbe038b8b8a7 \ + --hash=sha256:e5b8daf27af0b90da7bb903a876477a9e6d7270be6146906b276605997c7e9a3 \ + --hash=sha256:e7e3736715fbf53e9be2a79eb4db68e4ed857017344d697e8b9749444ae57475 \ + --hash=sha256:e8c4ebfcfd57177b572e2040777b8abc537cdef58a2120e830124946aa9b42c5 \ + --hash=sha256:f66efbc1caa63c088dead1c4170d148eabc9b80d95fb75b6c92ac0aad2437d76 \ + --hash=sha256:fc4b630cd3fa2cf7fce38afa91d7cfe844a9f75d7f0f36393fa98815e911d987 \ + --hash=sha256:fd5415dded15c3822597455bc02bcd66e81ef8b7a48cb71a33628fc9fdde39df + # via ruamel-yaml scikit-learn==1.2.0 \ --hash=sha256:0834e4cec2a2e0d8978f39cb8fe1cad3be6c27a47927e1774bf5737ea65ec228 \ --hash=sha256:184a42842a4e698ffa4d849b6019de50a77a0aa24d26afa28fa49c9190bb144b \ @@ -1688,6 +1778,105 @@ widgetsnbextension==4.0.10 \ --hash=sha256:64196c5ff3b9a9183a8e699a4227fb0b7002f252c814098e66c4d1cd0644688f \ --hash=sha256:d37c3724ec32d8c48400a435ecfa7d3e259995201fbefa37163124a9fcb393cc # via ipywidgets +zstandard==0.23.0 \ + --hash=sha256:034b88913ecc1b097f528e42b539453fa82c3557e414b3de9d5632c80439a473 \ + --hash=sha256:0a7f0804bb3799414af278e9ad51be25edf67f78f916e08afdb983e74161b916 \ + --hash=sha256:11e3bf3c924853a2d5835b24f03eeba7fc9b07d8ca499e247e06ff5676461a15 \ + --hash=sha256:12a289832e520c6bd4dcaad68e944b86da3bad0d339ef7989fb7e88f92e96072 \ + --hash=sha256:1516c8c37d3a053b01c1c15b182f3b5f5eef19ced9b930b684a73bad121addf4 \ + --hash=sha256:157e89ceb4054029a289fb504c98c6a9fe8010f1680de0201b3eb5dc20aa6d9e \ + --hash=sha256:1bfe8de1da6d104f15a60d4a8a768288f66aa953bbe00d027398b93fb9680b26 \ + --hash=sha256:1e172f57cd78c20f13a3415cc8dfe24bf388614324d25539146594c16d78fcc8 \ + --hash=sha256:1fd7e0f1cfb70eb2f95a19b472ee7ad6d9a0a992ec0ae53286870c104ca939e5 \ + --hash=sha256:203d236f4c94cd8379d1ea61db2fce20730b4c38d7f1c34506a31b34edc87bdd \ + --hash=sha256:27d3ef2252d2e62476389ca8f9b0cf2bbafb082a3b6bfe9d90cbcbb5529ecf7c \ + --hash=sha256:29a2bc7c1b09b0af938b7a8343174b987ae021705acabcbae560166567f5a8db \ + --hash=sha256:2ef230a8fd217a2015bc91b74f6b3b7d6522ba48be29ad4ea0ca3a3775bf7dd5 \ + --hash=sha256:2ef3775758346d9ac6214123887d25c7061c92afe1f2b354f9388e9e4d48acfc \ + --hash=sha256:2f146f50723defec2975fb7e388ae3a024eb7151542d1599527ec2aa9cacb152 \ + --hash=sha256:2fb4535137de7e244c230e24f9d1ec194f61721c86ebea04e1581d9d06ea1269 \ + --hash=sha256:32ba3b5ccde2d581b1e6aa952c836a6291e8435d788f656fe5976445865ae045 \ + --hash=sha256:34895a41273ad33347b2fc70e1bff4240556de3c46c6ea430a7ed91f9042aa4e \ + --hash=sha256:379b378ae694ba78cef921581ebd420c938936a153ded602c4fea612b7eaa90d \ + --hash=sha256:38302b78a850ff82656beaddeb0bb989a0322a8bbb1bf1ab10c17506681d772a \ + --hash=sha256:3aa014d55c3af933c1315eb4bb06dd0459661cc0b15cd61077afa6489bec63bb \ + --hash=sha256:4051e406288b8cdbb993798b9a45c59a4896b6ecee2f875424ec10276a895740 \ + --hash=sha256:40b33d93c6eddf02d2c19f5773196068d875c41ca25730e8288e9b672897c105 \ + --hash=sha256:43da0f0092281bf501f9c5f6f3b4c975a8a0ea82de49ba3f7100e64d422a1274 \ + --hash=sha256:445e4cb5048b04e90ce96a79b4b63140e3f4ab5f662321975679b5f6360b90e2 \ + --hash=sha256:48ef6a43b1846f6025dde6ed9fee0c24e1149c1c25f7fb0a0585572b2f3adc58 \ + --hash=sha256:50a80baba0285386f97ea36239855f6020ce452456605f262b2d33ac35c7770b \ + --hash=sha256:519fbf169dfac1222a76ba8861ef4ac7f0530c35dd79ba5727014613f91613d4 \ + --hash=sha256:53dd9d5e3d29f95acd5de6802e909ada8d8d8cfa37a3ac64836f3bc4bc5512db \ + --hash=sha256:53ea7cdc96c6eb56e76bb06894bcfb5dfa93b7adcf59d61c6b92674e24e2dd5e \ + --hash=sha256:576856e8594e6649aee06ddbfc738fec6a834f7c85bf7cadd1c53d4a58186ef9 \ + --hash=sha256:59556bf80a7094d0cfb9f5e50bb2db27fefb75d5138bb16fb052b61b0e0eeeb0 \ + --hash=sha256:5d41d5e025f1e0bccae4928981e71b2334c60f580bdc8345f824e7c0a4c2a813 \ + --hash=sha256:61062387ad820c654b6a6b5f0b94484fa19515e0c5116faf29f41a6bc91ded6e \ + --hash=sha256:61f89436cbfede4bc4e91b4397eaa3e2108ebe96d05e93d6ccc95ab5714be512 \ + --hash=sha256:62136da96a973bd2557f06ddd4e8e807f9e13cbb0bfb9cc06cfe6d98ea90dfe0 \ + --hash=sha256:64585e1dba664dc67c7cdabd56c1e5685233fbb1fc1966cfba2a340ec0dfff7b \ + --hash=sha256:65308f4b4890aa12d9b6ad9f2844b7ee42c7f7a4fd3390425b242ffc57498f48 \ + --hash=sha256:66b689c107857eceabf2cf3d3fc699c3c0fe8ccd18df2219d978c0283e4c508a \ + --hash=sha256:6a41c120c3dbc0d81a8e8adc73312d668cd34acd7725f036992b1b72d22c1772 \ + --hash=sha256:6f77fa49079891a4aab203d0b1744acc85577ed16d767b52fc089d83faf8d8ed \ + --hash=sha256:72c68dda124a1a138340fb62fa21b9bf4848437d9ca60bd35db36f2d3345f373 \ + --hash=sha256:752bf8a74412b9892f4e5b58f2f890a039f57037f52c89a740757ebd807f33ea \ + --hash=sha256:76e79bc28a65f467e0409098fa2c4376931fd3207fbeb6b956c7c476d53746dd \ + --hash=sha256:774d45b1fac1461f48698a9d4b5fa19a69d47ece02fa469825b442263f04021f \ + --hash=sha256:77da4c6bfa20dd5ea25cbf12c76f181a8e8cd7ea231c673828d0386b1740b8dc \ + --hash=sha256:77ea385f7dd5b5676d7fd943292ffa18fbf5c72ba98f7d09fc1fb9e819b34c23 \ + --hash=sha256:80080816b4f52a9d886e67f1f96912891074903238fe54f2de8b786f86baded2 \ + --hash=sha256:80a539906390591dd39ebb8d773771dc4db82ace6372c4d41e2d293f8e32b8db \ + --hash=sha256:82d17e94d735c99621bf8ebf9995f870a6b3e6d14543b99e201ae046dfe7de70 \ + --hash=sha256:837bb6764be6919963ef41235fd56a6486b132ea64afe5fafb4cb279ac44f259 \ + --hash=sha256:84433dddea68571a6d6bd4fbf8ff398236031149116a7fff6f777ff95cad3df9 \ + --hash=sha256:8c24f21fa2af4bb9f2c492a86fe0c34e6d2c63812a839590edaf177b7398f700 \ + --hash=sha256:8ed7d27cb56b3e058d3cf684d7200703bcae623e1dcc06ed1e18ecda39fee003 \ + --hash=sha256:9206649ec587e6b02bd124fb7799b86cddec350f6f6c14bc82a2b70183e708ba \ + --hash=sha256:983b6efd649723474f29ed42e1467f90a35a74793437d0bc64a5bf482bedfa0a \ + --hash=sha256:98da17ce9cbf3bfe4617e836d561e433f871129e3a7ac16d6ef4c680f13a839c \ + --hash=sha256:9c236e635582742fee16603042553d276cca506e824fa2e6489db04039521e90 \ + --hash=sha256:9da6bc32faac9a293ddfdcb9108d4b20416219461e4ec64dfea8383cac186690 \ + --hash=sha256:a05e6d6218461eb1b4771d973728f0133b2a4613a6779995df557f70794fd60f \ + --hash=sha256:a0817825b900fcd43ac5d05b8b3079937073d2b1ff9cf89427590718b70dd840 \ + --hash=sha256:a4ae99c57668ca1e78597d8b06d5af837f377f340f4cce993b551b2d7731778d \ + --hash=sha256:a8c86881813a78a6f4508ef9daf9d4995b8ac2d147dcb1a450448941398091c9 \ + --hash=sha256:a8fffdbd9d1408006baaf02f1068d7dd1f016c6bcb7538682622c556e7b68e35 \ + --hash=sha256:a9b07268d0c3ca5c170a385a0ab9fb7fdd9f5fd866be004c4ea39e44edce47dd \ + --hash=sha256:ab19a2d91963ed9e42b4e8d77cd847ae8381576585bad79dbd0a8837a9f6620a \ + --hash=sha256:ac184f87ff521f4840e6ea0b10c0ec90c6b1dcd0bad2f1e4a9a1b4fa177982ea \ + --hash=sha256:b0e166f698c5a3e914947388c162be2583e0c638a4703fc6a543e23a88dea3c1 \ + --hash=sha256:b2170c7e0367dde86a2647ed5b6f57394ea7f53545746104c6b09fc1f4223573 \ + --hash=sha256:b2d8c62d08e7255f68f7a740bae85b3c9b8e5466baa9cbf7f57f1cde0ac6bc09 \ + --hash=sha256:b4567955a6bc1b20e9c31612e615af6b53733491aeaa19a6b3b37f3b65477094 \ + --hash=sha256:b69bb4f51daf461b15e7b3db033160937d3ff88303a7bc808c67bbc1eaf98c78 \ + --hash=sha256:b8c0bd73aeac689beacd4e7667d48c299f61b959475cdbb91e7d3d88d27c56b9 \ + --hash=sha256:be9b5b8659dff1f913039c2feee1aca499cfbc19e98fa12bc85e037c17ec6ca5 \ + --hash=sha256:bf0a05b6059c0528477fba9054d09179beb63744355cab9f38059548fedd46a9 \ + --hash=sha256:c16842b846a8d2a145223f520b7e18b57c8f476924bda92aeee3a88d11cfc391 \ + --hash=sha256:c363b53e257246a954ebc7c488304b5592b9c53fbe74d03bc1c64dda153fb847 \ + --hash=sha256:c7c517d74bea1a6afd39aa612fa025e6b8011982a0897768a2f7c8ab4ebb78a2 \ + --hash=sha256:d20fd853fbb5807c8e84c136c278827b6167ded66c72ec6f9a14b863d809211c \ + --hash=sha256:d2240ddc86b74966c34554c49d00eaafa8200a18d3a5b6ffbf7da63b11d74ee2 \ + --hash=sha256:d477ed829077cd945b01fc3115edd132c47e6540ddcd96ca169facff28173057 \ + --hash=sha256:d50d31bfedd53a928fed6707b15a8dbeef011bb6366297cc435accc888b27c20 \ + --hash=sha256:dc1d33abb8a0d754ea4763bad944fd965d3d95b5baef6b121c0c9013eaf1907d \ + --hash=sha256:dc5d1a49d3f8262be192589a4b72f0d03b72dcf46c51ad5852a4fdc67be7b9e4 \ + --hash=sha256:e2d1a054f8f0a191004675755448d12be47fa9bebbcffa3cdf01db19f2d30a54 \ + --hash=sha256:e7792606d606c8df5277c32ccb58f29b9b8603bf83b48639b7aedf6df4fe8171 \ + --hash=sha256:ed1708dbf4d2e3a1c5c69110ba2b4eb6678262028afd6c6fbcc5a8dac9cda68e \ + --hash=sha256:f2d4380bf5f62daabd7b751ea2339c1a21d1c9463f1feb7fc2bdcea2c29c3160 \ + --hash=sha256:f3513916e8c645d0610815c257cbfd3242adfd5c4cfa78be514e5a3ebb42a41b \ + --hash=sha256:f8346bfa098532bc1fb6c7ef06783e969d87a99dd1d2a5a18a892c1d7a643c58 \ + --hash=sha256:f83fa6cae3fff8e98691248c9320356971b59678a17f20656a9e59cd32cee6d8 \ + --hash=sha256:fa6ce8b52c5987b3e34d5674b0ab529a4602b632ebab0a93b07bfb4dfc8f8a33 \ + --hash=sha256:fb2b1ecfef1e67897d336de3a0e3f52478182d6a47eda86cbd42504c5cbd009a \ + --hash=sha256:fc9ca1c9718cb3b06634c7c8dec57d24e9438b2aa9a0f02b8bb36bf478538880 \ + --hash=sha256:fd30d9c67d13d891f2360b2a120186729c111238ac63b43dbd37a5a40670b8ca \ + --hash=sha256:fd7699e8fd9969f455ef2926221e0233f81a2542921471382e77a9e2f2b57f4b \ + --hash=sha256:fe3b385d996ee0822fd46528d9f0443b880d4d05528fd26a9119a54ec3f91c69 + # via rosbags # The following packages are considered to be unsafe in a requirements file: setuptools==67.6.1 \ diff --git a/third_party/python/requirements_3_8.txt b/third_party/python/requirements_3_8.txt index 4214ca1a..9d158db0 100644 --- a/third_party/python/requirements_3_8.txt +++ b/third_party/python/requirements_3_8.txt @@ -741,6 +741,44 @@ line-profiler==4.0.3 \ --hash=sha256:fbe0036a306835978270a66c460c7b57869fe985ca620613321971d396de295f \ --hash=sha256:fdaac2e769e7d64cd3d19c4df5d26287e1cd362f47a1d3b42edd7c8420f40101 # via -r third_party/python/requirements.in +lz4==4.3.3 \ + --hash=sha256:01fe674ef2889dbb9899d8a67361e0c4a2c833af5aeb37dd505727cf5d2a131e \ + --hash=sha256:054b4631a355606e99a42396f5db4d22046a3397ffc3269a348ec41eaebd69d2 \ + --hash=sha256:0a136e44a16fc98b1abc404fbabf7f1fada2bdab6a7e970974fb81cf55b636d0 \ + --hash=sha256:0e9c410b11a31dbdc94c05ac3c480cb4b222460faf9231f12538d0074e56c563 \ + --hash=sha256:222a7e35137d7539c9c33bb53fcbb26510c5748779364014235afc62b0ec797f \ + --hash=sha256:24b3206de56b7a537eda3a8123c644a2b7bf111f0af53bc14bed90ce5562d1aa \ + --hash=sha256:2b901c7784caac9a1ded4555258207d9e9697e746cc8532129f150ffe1f6ba0d \ + --hash=sha256:2f7b1839f795315e480fb87d9bc60b186a98e3e5d17203c6e757611ef7dcef61 \ + --hash=sha256:30e8c20b8857adef7be045c65f47ab1e2c4fabba86a9fa9a997d7674a31ea6b6 \ + --hash=sha256:31ea4be9d0059c00b2572d700bf2c1bc82f241f2c3282034a759c9a4d6ca4dc2 \ + --hash=sha256:337cb94488a1b060ef1685187d6ad4ba8bc61d26d631d7ba909ee984ea736be1 \ + --hash=sha256:33c9a6fd20767ccaf70649982f8f3eeb0884035c150c0b818ea660152cf3c809 \ + --hash=sha256:363ab65bf31338eb364062a15f302fc0fab0a49426051429866d71c793c23394 \ + --hash=sha256:43cf03059c0f941b772c8aeb42a0813d68d7081c009542301637e5782f8a33e2 \ + --hash=sha256:56f4fe9c6327adb97406f27a66420b22ce02d71a5c365c48d6b656b4aaeb7775 \ + --hash=sha256:5d35533bf2cee56f38ced91f766cd0038b6abf46f438a80d50c52750088be93f \ + --hash=sha256:6756212507405f270b66b3ff7f564618de0606395c0fe10a7ae2ffcbbe0b1fba \ + --hash=sha256:6cdc60e21ec70266947a48839b437d46025076eb4b12c76bd47f8e5eb8a75dcc \ + --hash=sha256:abc197e4aca8b63f5ae200af03eb95fb4b5055a8f990079b5bdf042f568469dd \ + --hash=sha256:b14d948e6dce389f9a7afc666d60dd1e35fa2138a8ec5306d30cd2e30d36b40c \ + --hash=sha256:b47839b53956e2737229d70714f1d75f33e8ac26e52c267f0197b3189ca6de24 \ + --hash=sha256:b6d9ec061b9eca86e4dcc003d93334b95d53909afd5a32c6e4f222157b50c071 \ + --hash=sha256:b891880c187e96339474af2a3b2bfb11a8e4732ff5034be919aa9029484cd201 \ + --hash=sha256:bca8fccc15e3add173da91be8f34121578dc777711ffd98d399be35487c934bf \ + --hash=sha256:c81703b12475da73a5d66618856d04b1307e43428a7e59d98cfe5a5d608a74c6 \ + --hash=sha256:d2507ee9c99dbddd191c86f0e0c8b724c76d26b0602db9ea23232304382e1f21 \ + --hash=sha256:e36cd7b9d4d920d3bfc2369840da506fa68258f7bb176b8743189793c055e43d \ + --hash=sha256:e7d84b479ddf39fe3ea05387f10b779155fc0990125f4fb35d636114e1c63a2e \ + --hash=sha256:eac9af361e0d98335a02ff12fb56caeb7ea1196cf1a49dbf6f17828a131da807 \ + --hash=sha256:edfd858985c23523f4e5a7526ca6ee65ff930207a7ec8a8f57a01eae506aaee7 \ + --hash=sha256:ee9ff50557a942d187ec85462bb0960207e7ec5b19b3b48949263993771c6205 \ + --hash=sha256:f0e822cd7644995d9ba248cb4b67859701748a93e2ab7fc9bc18c599a52e4604 \ + --hash=sha256:f180904f33bdd1e92967923a43c22899e303906d19b2cf8bb547db6653ea6e7d \ + --hash=sha256:f1d18718f9d78182c6b60f568c9a9cec8a7204d7cb6fad4e511a2ef279e4cb05 \ + --hash=sha256:f4c7bf687303ca47d69f9f0133274958fd672efaa33fb5bcde467862d6c621f0 \ + --hash=sha256:f76176492ff082657ada0d0f10c794b6da5800249ef1692b35cf49b1e93e8ef7 + # via rosbags markupsafe==2.1.5 \ --hash=sha256:00e046b6dd71aa03a41079792f8473dc494d564611a8f89bbbd7cb93295ebdcf \ --hash=sha256:075202fa5b72c86ad32dc7d0b56024ebdbcf2048c0ba09f1cde31bfdd57bcfff \ @@ -935,6 +973,7 @@ numpy==1.23.4 \ # matplotlib # opencv-python # pandas + # rosbags # scikit-learn # scipy # seaborn @@ -1394,6 +1433,10 @@ rfc3986-validator==0.1.1 \ # via # jsonschema # jupyter-events +rosbags==0.9.23 \ + --hash=sha256:0fd14b10ee5b2422a08b3a09d2171dc347925307716779ad6f21e398b280c56a \ + --hash=sha256:89229ca8fbb6d5e1cc452045229e6e47028dc10a8724fbe86c93eda477296dc2 + # via -r third_party/python/requirements.in rpds-py==0.18.0 \ --hash=sha256:01e36a39af54a30f28b73096dd39b6802eddd04c90dbe161c1b8dbe22353189f \ --hash=sha256:044a3e61a7c2dafacae99d1e722cc2d4c05280790ec5a05031b3876809d89a5c \ @@ -1497,6 +1540,62 @@ rpds-py==0.18.0 \ # via # jsonschema # referencing +ruamel-yaml==0.18.6 \ + --hash=sha256:57b53ba33def16c4f3d807c0ccbc00f8a6081827e81ba2491691b76882d0c636 \ + --hash=sha256:8b27e6a217e786c6fbe5634d8f3f11bc63e0f80f6a5890f28863d9c45aac311b + # via rosbags +ruamel-yaml-clib==0.2.8 \ + --hash=sha256:024cfe1fc7c7f4e1aff4a81e718109e13409767e4f871443cbff3dba3578203d \ + --hash=sha256:03d1162b6d1df1caa3a4bd27aa51ce17c9afc2046c31b0ad60a0a96ec22f8001 \ + --hash=sha256:07238db9cbdf8fc1e9de2489a4f68474e70dffcb32232db7c08fa61ca0c7c462 \ + --hash=sha256:09b055c05697b38ecacb7ac50bdab2240bfca1a0c4872b0fd309bb07dc9aa3a9 \ + --hash=sha256:1707814f0d9791df063f8c19bb51b0d1278b8e9a2353abbb676c2f685dee6afe \ + --hash=sha256:1758ce7d8e1a29d23de54a16ae867abd370f01b5a69e1a3ba75223eaa3ca1a1b \ + --hash=sha256:184565012b60405d93838167f425713180b949e9d8dd0bbc7b49f074407c5a8b \ + --hash=sha256:1b617618914cb00bf5c34d4357c37aa15183fa229b24767259657746c9077615 \ + --hash=sha256:1dc67314e7e1086c9fdf2680b7b6c2be1c0d8e3a8279f2e993ca2a7545fecf62 \ + --hash=sha256:25ac8c08322002b06fa1d49d1646181f0b2c72f5cbc15a85e80b4c30a544bb15 \ + --hash=sha256:25c515e350e5b739842fc3228d662413ef28f295791af5e5110b543cf0b57d9b \ + --hash=sha256:305889baa4043a09e5b76f8e2a51d4ffba44259f6b4c72dec8ca56207d9c6fe1 \ + --hash=sha256:3213ece08ea033eb159ac52ae052a4899b56ecc124bb80020d9bbceeb50258e9 \ + --hash=sha256:3f215c5daf6a9d7bbed4a0a4f760f3113b10e82ff4c5c44bec20a68c8014f675 \ + --hash=sha256:46d378daaac94f454b3a0e3d8d78cafd78a026b1d71443f4966c696b48a6d899 \ + --hash=sha256:4ecbf9c3e19f9562c7fdd462e8d18dd902a47ca046a2e64dba80699f0b6c09b7 \ + --hash=sha256:53a300ed9cea38cf5a2a9b069058137c2ca1ce658a874b79baceb8f892f915a7 \ + --hash=sha256:56f4252222c067b4ce51ae12cbac231bce32aee1d33fbfc9d17e5b8d6966c312 \ + --hash=sha256:5c365d91c88390c8d0a8545df0b5857172824b1c604e867161e6b3d59a827eaa \ + --hash=sha256:700e4ebb569e59e16a976857c8798aee258dceac7c7d6b50cab63e080058df91 \ + --hash=sha256:75e1ed13e1f9de23c5607fe6bd1aeaae21e523b32d83bb33918245361e9cc51b \ + --hash=sha256:77159f5d5b5c14f7c34073862a6b7d34944075d9f93e681638f6d753606c6ce6 \ + --hash=sha256:7f67a1ee819dc4562d444bbafb135832b0b909f81cc90f7aa00260968c9ca1b3 \ + --hash=sha256:840f0c7f194986a63d2c2465ca63af8ccbbc90ab1c6001b1978f05119b5e7334 \ + --hash=sha256:84b554931e932c46f94ab306913ad7e11bba988104c5cff26d90d03f68258cd5 \ + --hash=sha256:87ea5ff66d8064301a154b3933ae406b0863402a799b16e4a1d24d9fbbcbe0d3 \ + --hash=sha256:955eae71ac26c1ab35924203fda6220f84dce57d6d7884f189743e2abe3a9fbe \ + --hash=sha256:a1a45e0bb052edf6a1d3a93baef85319733a888363938e1fc9924cb00c8df24c \ + --hash=sha256:a5aa27bad2bb83670b71683aae140a1f52b0857a2deff56ad3f6c13a017a26ed \ + --hash=sha256:a6a9ffd280b71ad062eae53ac1659ad86a17f59a0fdc7699fd9be40525153337 \ + --hash=sha256:a75879bacf2c987c003368cf14bed0ffe99e8e85acfa6c0bfffc21a090f16880 \ + --hash=sha256:aa2267c6a303eb483de8d02db2871afb5c5fc15618d894300b88958f729ad74f \ + --hash=sha256:aab7fd643f71d7946f2ee58cc88c9b7bfc97debd71dcc93e03e2d174628e7e2d \ + --hash=sha256:b16420e621d26fdfa949a8b4b47ade8810c56002f5389970db4ddda51dbff248 \ + --hash=sha256:b42169467c42b692c19cf539c38d4602069d8c1505e97b86387fcf7afb766e1d \ + --hash=sha256:bba64af9fa9cebe325a62fa398760f5c7206b215201b0ec825005f1b18b9bccf \ + --hash=sha256:beb2e0404003de9a4cab9753a8805a8fe9320ee6673136ed7f04255fe60bb512 \ + --hash=sha256:bef08cd86169d9eafb3ccb0a39edb11d8e25f3dae2b28f5c52fd997521133069 \ + --hash=sha256:c2a72e9109ea74e511e29032f3b670835f8a59bbdc9ce692c5b4ed91ccf1eedb \ + --hash=sha256:c58ecd827313af6864893e7af0a3bb85fd529f862b6adbefe14643947cfe2942 \ + --hash=sha256:c69212f63169ec1cfc9bb44723bf2917cbbd8f6191a00ef3410f5a7fe300722d \ + --hash=sha256:cabddb8d8ead485e255fe80429f833172b4cadf99274db39abc080e068cbcc31 \ + --hash=sha256:d176b57452ab5b7028ac47e7b3cf644bcfdc8cacfecf7e71759f7f51a59e5c92 \ + --hash=sha256:da09ad1c359a728e112d60116f626cc9f29730ff3e0e7db72b9a2dbc2e4beed5 \ + --hash=sha256:e2b4c44b60eadec492926a7270abb100ef9f72798e18743939bdbf037aab8c28 \ + --hash=sha256:e79e5db08739731b0ce4850bed599235d601701d5694c36570a99a0c5ca41a9d \ + --hash=sha256:ebc06178e8821efc9692ea7544aa5644217358490145629914d8020042c24aa1 \ + --hash=sha256:edaef1c1200c4b4cb914583150dcaa3bc30e592e907c01117c08b13a07255ec2 \ + --hash=sha256:f481f16baec5290e45aebdc2a5168ebc6d35189ae6fea7a58787613a25f6e875 \ + --hash=sha256:fff3573c2db359f091e1589c3d7c5fc2f86f5bdb6f24252c2d8e539d4e45f412 + # via ruamel-yaml scikit-learn==1.2.0 \ --hash=sha256:0834e4cec2a2e0d8978f39cb8fe1cad3be6c27a47927e1774bf5737ea65ec228 \ --hash=sha256:184a42842a4e698ffa4d849b6019de50a77a0aa24d26afa28fa49c9190bb144b \ @@ -1728,6 +1827,105 @@ zipp==3.17.0 \ # via # importlib-metadata # importlib-resources +zstandard==0.23.0 \ + --hash=sha256:034b88913ecc1b097f528e42b539453fa82c3557e414b3de9d5632c80439a473 \ + --hash=sha256:0a7f0804bb3799414af278e9ad51be25edf67f78f916e08afdb983e74161b916 \ + --hash=sha256:11e3bf3c924853a2d5835b24f03eeba7fc9b07d8ca499e247e06ff5676461a15 \ + --hash=sha256:12a289832e520c6bd4dcaad68e944b86da3bad0d339ef7989fb7e88f92e96072 \ + --hash=sha256:1516c8c37d3a053b01c1c15b182f3b5f5eef19ced9b930b684a73bad121addf4 \ + --hash=sha256:157e89ceb4054029a289fb504c98c6a9fe8010f1680de0201b3eb5dc20aa6d9e \ + --hash=sha256:1bfe8de1da6d104f15a60d4a8a768288f66aa953bbe00d027398b93fb9680b26 \ + --hash=sha256:1e172f57cd78c20f13a3415cc8dfe24bf388614324d25539146594c16d78fcc8 \ + --hash=sha256:1fd7e0f1cfb70eb2f95a19b472ee7ad6d9a0a992ec0ae53286870c104ca939e5 \ + --hash=sha256:203d236f4c94cd8379d1ea61db2fce20730b4c38d7f1c34506a31b34edc87bdd \ + --hash=sha256:27d3ef2252d2e62476389ca8f9b0cf2bbafb082a3b6bfe9d90cbcbb5529ecf7c \ + --hash=sha256:29a2bc7c1b09b0af938b7a8343174b987ae021705acabcbae560166567f5a8db \ + --hash=sha256:2ef230a8fd217a2015bc91b74f6b3b7d6522ba48be29ad4ea0ca3a3775bf7dd5 \ + --hash=sha256:2ef3775758346d9ac6214123887d25c7061c92afe1f2b354f9388e9e4d48acfc \ + --hash=sha256:2f146f50723defec2975fb7e388ae3a024eb7151542d1599527ec2aa9cacb152 \ + --hash=sha256:2fb4535137de7e244c230e24f9d1ec194f61721c86ebea04e1581d9d06ea1269 \ + --hash=sha256:32ba3b5ccde2d581b1e6aa952c836a6291e8435d788f656fe5976445865ae045 \ + --hash=sha256:34895a41273ad33347b2fc70e1bff4240556de3c46c6ea430a7ed91f9042aa4e \ + --hash=sha256:379b378ae694ba78cef921581ebd420c938936a153ded602c4fea612b7eaa90d \ + --hash=sha256:38302b78a850ff82656beaddeb0bb989a0322a8bbb1bf1ab10c17506681d772a \ + --hash=sha256:3aa014d55c3af933c1315eb4bb06dd0459661cc0b15cd61077afa6489bec63bb \ + --hash=sha256:4051e406288b8cdbb993798b9a45c59a4896b6ecee2f875424ec10276a895740 \ + --hash=sha256:40b33d93c6eddf02d2c19f5773196068d875c41ca25730e8288e9b672897c105 \ + --hash=sha256:43da0f0092281bf501f9c5f6f3b4c975a8a0ea82de49ba3f7100e64d422a1274 \ + --hash=sha256:445e4cb5048b04e90ce96a79b4b63140e3f4ab5f662321975679b5f6360b90e2 \ + --hash=sha256:48ef6a43b1846f6025dde6ed9fee0c24e1149c1c25f7fb0a0585572b2f3adc58 \ + --hash=sha256:50a80baba0285386f97ea36239855f6020ce452456605f262b2d33ac35c7770b \ + --hash=sha256:519fbf169dfac1222a76ba8861ef4ac7f0530c35dd79ba5727014613f91613d4 \ + --hash=sha256:53dd9d5e3d29f95acd5de6802e909ada8d8d8cfa37a3ac64836f3bc4bc5512db \ + --hash=sha256:53ea7cdc96c6eb56e76bb06894bcfb5dfa93b7adcf59d61c6b92674e24e2dd5e \ + --hash=sha256:576856e8594e6649aee06ddbfc738fec6a834f7c85bf7cadd1c53d4a58186ef9 \ + --hash=sha256:59556bf80a7094d0cfb9f5e50bb2db27fefb75d5138bb16fb052b61b0e0eeeb0 \ + --hash=sha256:5d41d5e025f1e0bccae4928981e71b2334c60f580bdc8345f824e7c0a4c2a813 \ + --hash=sha256:61062387ad820c654b6a6b5f0b94484fa19515e0c5116faf29f41a6bc91ded6e \ + --hash=sha256:61f89436cbfede4bc4e91b4397eaa3e2108ebe96d05e93d6ccc95ab5714be512 \ + --hash=sha256:62136da96a973bd2557f06ddd4e8e807f9e13cbb0bfb9cc06cfe6d98ea90dfe0 \ + --hash=sha256:64585e1dba664dc67c7cdabd56c1e5685233fbb1fc1966cfba2a340ec0dfff7b \ + --hash=sha256:65308f4b4890aa12d9b6ad9f2844b7ee42c7f7a4fd3390425b242ffc57498f48 \ + --hash=sha256:66b689c107857eceabf2cf3d3fc699c3c0fe8ccd18df2219d978c0283e4c508a \ + --hash=sha256:6a41c120c3dbc0d81a8e8adc73312d668cd34acd7725f036992b1b72d22c1772 \ + --hash=sha256:6f77fa49079891a4aab203d0b1744acc85577ed16d767b52fc089d83faf8d8ed \ + --hash=sha256:72c68dda124a1a138340fb62fa21b9bf4848437d9ca60bd35db36f2d3345f373 \ + --hash=sha256:752bf8a74412b9892f4e5b58f2f890a039f57037f52c89a740757ebd807f33ea \ + --hash=sha256:76e79bc28a65f467e0409098fa2c4376931fd3207fbeb6b956c7c476d53746dd \ + --hash=sha256:774d45b1fac1461f48698a9d4b5fa19a69d47ece02fa469825b442263f04021f \ + --hash=sha256:77da4c6bfa20dd5ea25cbf12c76f181a8e8cd7ea231c673828d0386b1740b8dc \ + --hash=sha256:77ea385f7dd5b5676d7fd943292ffa18fbf5c72ba98f7d09fc1fb9e819b34c23 \ + --hash=sha256:80080816b4f52a9d886e67f1f96912891074903238fe54f2de8b786f86baded2 \ + --hash=sha256:80a539906390591dd39ebb8d773771dc4db82ace6372c4d41e2d293f8e32b8db \ + --hash=sha256:82d17e94d735c99621bf8ebf9995f870a6b3e6d14543b99e201ae046dfe7de70 \ + --hash=sha256:837bb6764be6919963ef41235fd56a6486b132ea64afe5fafb4cb279ac44f259 \ + --hash=sha256:84433dddea68571a6d6bd4fbf8ff398236031149116a7fff6f777ff95cad3df9 \ + --hash=sha256:8c24f21fa2af4bb9f2c492a86fe0c34e6d2c63812a839590edaf177b7398f700 \ + --hash=sha256:8ed7d27cb56b3e058d3cf684d7200703bcae623e1dcc06ed1e18ecda39fee003 \ + --hash=sha256:9206649ec587e6b02bd124fb7799b86cddec350f6f6c14bc82a2b70183e708ba \ + --hash=sha256:983b6efd649723474f29ed42e1467f90a35a74793437d0bc64a5bf482bedfa0a \ + --hash=sha256:98da17ce9cbf3bfe4617e836d561e433f871129e3a7ac16d6ef4c680f13a839c \ + --hash=sha256:9c236e635582742fee16603042553d276cca506e824fa2e6489db04039521e90 \ + --hash=sha256:9da6bc32faac9a293ddfdcb9108d4b20416219461e4ec64dfea8383cac186690 \ + --hash=sha256:a05e6d6218461eb1b4771d973728f0133b2a4613a6779995df557f70794fd60f \ + --hash=sha256:a0817825b900fcd43ac5d05b8b3079937073d2b1ff9cf89427590718b70dd840 \ + --hash=sha256:a4ae99c57668ca1e78597d8b06d5af837f377f340f4cce993b551b2d7731778d \ + --hash=sha256:a8c86881813a78a6f4508ef9daf9d4995b8ac2d147dcb1a450448941398091c9 \ + --hash=sha256:a8fffdbd9d1408006baaf02f1068d7dd1f016c6bcb7538682622c556e7b68e35 \ + --hash=sha256:a9b07268d0c3ca5c170a385a0ab9fb7fdd9f5fd866be004c4ea39e44edce47dd \ + --hash=sha256:ab19a2d91963ed9e42b4e8d77cd847ae8381576585bad79dbd0a8837a9f6620a \ + --hash=sha256:ac184f87ff521f4840e6ea0b10c0ec90c6b1dcd0bad2f1e4a9a1b4fa177982ea \ + --hash=sha256:b0e166f698c5a3e914947388c162be2583e0c638a4703fc6a543e23a88dea3c1 \ + --hash=sha256:b2170c7e0367dde86a2647ed5b6f57394ea7f53545746104c6b09fc1f4223573 \ + --hash=sha256:b2d8c62d08e7255f68f7a740bae85b3c9b8e5466baa9cbf7f57f1cde0ac6bc09 \ + --hash=sha256:b4567955a6bc1b20e9c31612e615af6b53733491aeaa19a6b3b37f3b65477094 \ + --hash=sha256:b69bb4f51daf461b15e7b3db033160937d3ff88303a7bc808c67bbc1eaf98c78 \ + --hash=sha256:b8c0bd73aeac689beacd4e7667d48c299f61b959475cdbb91e7d3d88d27c56b9 \ + --hash=sha256:be9b5b8659dff1f913039c2feee1aca499cfbc19e98fa12bc85e037c17ec6ca5 \ + --hash=sha256:bf0a05b6059c0528477fba9054d09179beb63744355cab9f38059548fedd46a9 \ + --hash=sha256:c16842b846a8d2a145223f520b7e18b57c8f476924bda92aeee3a88d11cfc391 \ + --hash=sha256:c363b53e257246a954ebc7c488304b5592b9c53fbe74d03bc1c64dda153fb847 \ + --hash=sha256:c7c517d74bea1a6afd39aa612fa025e6b8011982a0897768a2f7c8ab4ebb78a2 \ + --hash=sha256:d20fd853fbb5807c8e84c136c278827b6167ded66c72ec6f9a14b863d809211c \ + --hash=sha256:d2240ddc86b74966c34554c49d00eaafa8200a18d3a5b6ffbf7da63b11d74ee2 \ + --hash=sha256:d477ed829077cd945b01fc3115edd132c47e6540ddcd96ca169facff28173057 \ + --hash=sha256:d50d31bfedd53a928fed6707b15a8dbeef011bb6366297cc435accc888b27c20 \ + --hash=sha256:dc1d33abb8a0d754ea4763bad944fd965d3d95b5baef6b121c0c9013eaf1907d \ + --hash=sha256:dc5d1a49d3f8262be192589a4b72f0d03b72dcf46c51ad5852a4fdc67be7b9e4 \ + --hash=sha256:e2d1a054f8f0a191004675755448d12be47fa9bebbcffa3cdf01db19f2d30a54 \ + --hash=sha256:e7792606d606c8df5277c32ccb58f29b9b8603bf83b48639b7aedf6df4fe8171 \ + --hash=sha256:ed1708dbf4d2e3a1c5c69110ba2b4eb6678262028afd6c6fbcc5a8dac9cda68e \ + --hash=sha256:f2d4380bf5f62daabd7b751ea2339c1a21d1c9463f1feb7fc2bdcea2c29c3160 \ + --hash=sha256:f3513916e8c645d0610815c257cbfd3242adfd5c4cfa78be514e5a3ebb42a41b \ + --hash=sha256:f8346bfa098532bc1fb6c7ef06783e969d87a99dd1d2a5a18a892c1d7a643c58 \ + --hash=sha256:f83fa6cae3fff8e98691248c9320356971b59678a17f20656a9e59cd32cee6d8 \ + --hash=sha256:fa6ce8b52c5987b3e34d5674b0ab529a4602b632ebab0a93b07bfb4dfc8f8a33 \ + --hash=sha256:fb2b1ecfef1e67897d336de3a0e3f52478182d6a47eda86cbd42504c5cbd009a \ + --hash=sha256:fc9ca1c9718cb3b06634c7c8dec57d24e9438b2aa9a0f02b8bb36bf478538880 \ + --hash=sha256:fd30d9c67d13d891f2360b2a120186729c111238ac63b43dbd37a5a40670b8ca \ + --hash=sha256:fd7699e8fd9969f455ef2926221e0233f81a2542921471382e77a9e2f2b57f4b \ + --hash=sha256:fe3b385d996ee0822fd46528d9f0443b880d4d05528fd26a9119a54ec3f91c69 + # via rosbags # The following packages are considered to be unsafe in a requirements file: setuptools==67.6.1 \