Support box of pis in 2022 mapping code
This is what we'll actually be using for mapping. For now, use the
mapping option that doesn't consider robot position.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I6ed7e7a378da896670aecee3f15acb1541cdec7f
diff --git a/y2022/vision/target_mapping.cc b/y2022/vision/target_mapping.cc
index a7165bb..2b10798 100644
--- a/y2022/vision/target_mapping.cc
+++ b/y2022/vision/target_mapping.cc
@@ -16,9 +16,15 @@
DEFINE_string(json_path, "target_map.json",
"Specify path for json with initial pose guesses.");
-DEFINE_int32(
- team_number, 971,
- "If reading locally, use the calibration for a node with this team number");
+DEFINE_int32(team_number, 971,
+ "Use the calibration for a node with this team number");
+
+DEFINE_bool(
+ use_robot_position, false,
+ "If true, use localizer output messages to get the robot position, and "
+ "superstructure status messages to get the turret angle, at the "
+ "times of target detections. Currently does not work reliably on the box "
+ "of pis.");
namespace y2022 {
namespace vision {
@@ -62,17 +68,23 @@
// Add detected apriltag poses relative to the robot to
// timestamped_target_detections
-void HandleAprilTag(
- aos::distributed_clock::time_point pi_distributed_time,
- std::vector<cv::Vec4i> april_ids, std::vector<Eigen::Vector3d> rvecs_eigen,
- std::vector<Eigen::Vector3d> tvecs_eigen,
- std::vector<DataAdapter::TimestampedDetection>
- *timestamped_target_detections,
- aos::Fetcher<superstructure::Status> *superstructure_status_fetcher,
- Eigen::Affine3d fixed_extrinsics, Eigen::Affine3d turret_extrinsics) {
- CHECK(superstructure_status_fetcher->Fetch());
- double turret_position =
- superstructure_status_fetcher->get()->turret()->position();
+void HandleAprilTag(aos::distributed_clock::time_point pi_distributed_time,
+ std::vector<cv::Vec4i> april_ids,
+ std::vector<Eigen::Vector3d> rvecs_eigen,
+ std::vector<Eigen::Vector3d> tvecs_eigen,
+ std::vector<DataAdapter::TimestampedDetection>
+ *timestamped_target_detections,
+ std::optional<aos::Fetcher<superstructure::Status>>
+ *superstructure_status_fetcher,
+ Eigen::Affine3d fixed_extrinsics,
+ Eigen::Affine3d turret_extrinsics) {
+ double turret_position = 0.0;
+
+ if (superstructure_status_fetcher->has_value()) {
+ CHECK(superstructure_status_fetcher->value().Fetch());
+ turret_position =
+ superstructure_status_fetcher->value().get()->turret()->position();
+ }
for (size_t tag = 0; tag < april_ids.size(); tag++) {
Eigen::Translation3d T_camera_target = Eigen::Translation3d(
@@ -124,7 +136,8 @@
void HandlePiCaptures(
int pi_number, aos::EventLoop *pi_event_loop,
aos::logger::LogReader *reader,
- aos::Fetcher<superstructure::Status> *superstructure_status_fetcher,
+ std::optional<aos::Fetcher<superstructure::Status>>
+ *superstructure_status_fetcher,
std::vector<DataAdapter::TimestampedDetection>
*timestamped_target_detections,
std::vector<std::unique_ptr<CharucoExtractor>> *charuco_extractors,
@@ -182,42 +195,47 @@
// open logfiles
aos::logger::LogReader reader(aos::logger::SortParts(unsorted_logfiles));
reader.Register();
- const aos::Node *imu_node =
- aos::configuration::GetNode(reader.configuration(), "imu");
- std::unique_ptr<aos::EventLoop> imu_event_loop =
- reader.event_loop_factory()->MakeEventLoop("imu", imu_node);
+ std::optional<aos::Fetcher<superstructure::Status>>
+ superstructure_status_fetcher;
- const aos::Node *roborio_node =
- aos::configuration::GetNode(reader.configuration(), "roborio");
+ if (FLAGS_use_robot_position) {
+ const aos::Node *imu_node =
+ aos::configuration::GetNode(reader.configuration(), "imu");
+ std::unique_ptr<aos::EventLoop> imu_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("imu", imu_node);
- std::unique_ptr<aos::EventLoop> roborio_event_loop =
- reader.event_loop_factory()->MakeEventLoop("roborio", roborio_node);
+ imu_event_loop->MakeWatcher(
+ "/localizer", [&](const frc971::controls::LocalizerOutput &localizer) {
+ aos::monotonic_clock::time_point imu_monotonic_time =
+ aos::monotonic_clock::time_point(aos::monotonic_clock::duration(
+ localizer.monotonic_timestamp_ns()));
+ aos::distributed_clock::time_point imu_distributed_time =
+ reader.event_loop_factory()
+ ->GetNodeEventLoopFactory(imu_node)
+ ->ToDistributedClock(imu_monotonic_time);
- imu_event_loop->MakeWatcher(
- "/localizer", [&](const frc971::controls::LocalizerOutput &localizer) {
- aos::monotonic_clock::time_point imu_monotonic_time =
- aos::monotonic_clock::time_point(aos::monotonic_clock::duration(
- localizer.monotonic_timestamp_ns()));
- aos::distributed_clock::time_point imu_distributed_time =
- reader.event_loop_factory()
- ->GetNodeEventLoopFactory(imu_node)
- ->ToDistributedClock(imu_monotonic_time);
+ timestamped_robot_poses.emplace_back(DataAdapter::TimestampedPose{
+ .time = imu_distributed_time,
+ .pose =
+ ceres::examples::Pose2d{.x = localizer.x(),
+ .y = localizer.y(),
+ .yaw_radians = localizer.theta()}});
+ });
- timestamped_robot_poses.emplace_back(DataAdapter::TimestampedPose{
- .time = imu_distributed_time,
- .pose = ceres::examples::Pose2d{.x = localizer.x(),
- .y = localizer.y(),
- .yaw_radians = localizer.theta()}});
- });
+ const aos::Node *roborio_node =
+ aos::configuration::GetNode(reader.configuration(), "roborio");
+ std::unique_ptr<aos::EventLoop> roborio_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("roborio", roborio_node);
+
+ superstructure_status_fetcher =
+ roborio_event_loop->MakeFetcher<superstructure::Status>(
+ "/superstructure");
+ }
// Override target_type to AprilTag, since that's what we're using here
FLAGS_target_type = "april_tag";
- auto superstructure_status_fetcher =
- roborio_event_loop->MakeFetcher<superstructure::Status>(
- "/superstructure");
-
std::vector<std::unique_ptr<CharucoExtractor>> charuco_extractors;
std::vector<std::unique_ptr<ImageCallback>> image_callbacks;
@@ -256,9 +274,11 @@
reader.event_loop_factory()->Run();
auto target_constraints =
- DataAdapter::MatchTargetDetections(timestamped_robot_poses,
- timestamped_target_detections)
- .first;
+ (FLAGS_use_robot_position
+ ? DataAdapter::MatchTargetDetections(timestamped_robot_poses,
+ timestamped_target_detections)
+ .first
+ : DataAdapter::MatchTargetDetections(timestamped_target_detections));
frc971::vision::TargetMapper mapper(FLAGS_json_path, target_constraints);
mapper.Solve("rapid_react");
@@ -279,4 +299,4 @@
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
y2022::vision::MappingMain(argc, argv);
-}
\ No newline at end of file
+}