Merge "Add more detail to y2023 localizer plots"
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index 58c0fc3..02a9fdd 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -237,8 +237,7 @@
}
iovec_.clear();
- const size_t original_iovec_size = std::min<size_t>(queue.size(), IOV_MAX);
- size_t iovec_size = original_iovec_size;
+ size_t iovec_size = std::min<size_t>(queue.size(), IOV_MAX);
iovec_.resize(iovec_size);
size_t counted_size = 0;
@@ -248,7 +247,7 @@
// The file is aligned if it is a multiple of kSector in length. The data is
// aligned if it's memory is kSector aligned, and the length is a multiple of
// kSector in length.
- bool aligned = (file_written_bytes_ % kSector) == 0;
+ bool aligned = (total_write_bytes_ % kSector) == 0;
size_t write_index = 0;
for (size_t i = 0; i < iovec_size; ++i) {
iovec_[write_index].iov_base = const_cast<uint8_t *>(queue[i].data());
@@ -296,13 +295,11 @@
++write_index;
}
- if (counted_size > 0) {
- // Either write the aligned data if it is all aligned, or write the rest
- // unaligned if we wrote aligned up above.
- WriteV(iovec_.data(), iovec_.size(), aligned, counted_size);
+ // Either write the aligned data if it is all aligned, or write the rest
+ // unaligned if we wrote aligned up above.
+ WriteV(iovec_.data(), iovec_.size(), aligned, counted_size);
- encoder_->Clear(original_iovec_size);
- }
+ encoder_->Clear(iovec_size);
}
size_t DetachedBufferWriter::WriteV(struct iovec *iovec_data, size_t iovec_size,
@@ -320,21 +317,21 @@
if (written > 0) {
// Flush asynchronously and force the data out of the cache.
- sync_file_range(fd_, file_written_bytes_, written, SYNC_FILE_RANGE_WRITE);
- if (file_written_bytes_ != 0) {
+ sync_file_range(fd_, total_write_bytes_, written, SYNC_FILE_RANGE_WRITE);
+ if (last_synced_bytes_ != 0) {
// Per Linus' recommendation online on how to do fast file IO, do a
// blocking flush of the previous write chunk, and then tell the kernel to
// drop the pages from the cache. This makes sure we can't get too far
// ahead.
sync_file_range(fd_, last_synced_bytes_,
- file_written_bytes_ - last_synced_bytes_,
+ total_write_bytes_ - last_synced_bytes_,
SYNC_FILE_RANGE_WAIT_BEFORE | SYNC_FILE_RANGE_WRITE |
SYNC_FILE_RANGE_WAIT_AFTER);
posix_fadvise(fd_, last_synced_bytes_,
- file_written_bytes_ - last_synced_bytes_,
+ total_write_bytes_ - last_synced_bytes_,
POSIX_FADV_DONTNEED);
- last_synced_bytes_ = file_written_bytes_;
+ last_synced_bytes_ = total_write_bytes_;
}
}
@@ -374,7 +371,6 @@
++total_write_count_;
total_write_messages_ += iovec_size;
total_write_bytes_ += written;
- file_written_bytes_ += written;
}
void DetachedBufferWriter::FlushAtThreshold(
diff --git a/aos/events/logging/logfile_utils.h b/aos/events/logging/logfile_utils.h
index 0c0ef7f..50f6b40 100644
--- a/aos/events/logging/logfile_utils.h
+++ b/aos/events/logging/logfile_utils.h
@@ -212,8 +212,6 @@
bool supports_odirect_ = true;
int flags_ = 0;
- size_t file_written_bytes_ = 0;
-
aos::monotonic_clock::time_point last_flush_time_ =
aos::monotonic_clock::min_time;
};
diff --git a/frc971/control_loops/drivetrain/localization/utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
index ff027d0..d9e239b 100644
--- a/frc971/control_loops/drivetrain/localization/utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -30,6 +30,14 @@
: true;
}
+aos::Alliance LocalizationUtils::Alliance() {
+ joystick_state_fetcher_.Fetch();
+ return (joystick_state_fetcher_.get() != nullptr)
+ ? joystick_state_fetcher_->alliance()
+ : aos::Alliance::kInvalid;
+
+}
+
std::optional<aos::monotonic_clock::duration> LocalizationUtils::ClockOffset(
std::string_view node) {
std::optional<aos::monotonic_clock::duration> monotonic_offset;
diff --git a/frc971/control_loops/drivetrain/localization/utils.h b/frc971/control_loops/drivetrain/localization/utils.h
index 26242f9..cfd443a 100644
--- a/frc971/control_loops/drivetrain/localization/utils.h
+++ b/frc971/control_loops/drivetrain/localization/utils.h
@@ -29,6 +29,7 @@
// Returns true if either there is no JoystickState message available or if
// we are currently in autonomous mode.
bool MaybeInAutonomous();
+ aos::Alliance Alliance();
// Returns the offset between our node and the specified node (or nullopt if
// no offset is available). The sign of this will be such that the time on
diff --git a/y2023/constants/common.json b/y2023/constants/common.json
index c559810..d63c518 100644
--- a/y2023/constants/common.json
+++ b/y2023/constants/common.json
@@ -1,2 +1,6 @@
"target_map": {% include 'y2023/vision/maps/target_map.json' %},
- "scoring_map": {% include 'y2023/constants/scoring_map.json' %}
+ "scoring_map": {% include 'y2023/constants/scoring_map.json' %},
+ "ignore_targets": {
+ "red": [4],
+ "blue": [5]
+ }
diff --git a/y2023/constants/constants.fbs b/y2023/constants/constants.fbs
index bd038b8..61c3365 100644
--- a/y2023/constants/constants.fbs
+++ b/y2023/constants/constants.fbs
@@ -29,11 +29,19 @@
tof:TimeOfFlight (id: 0);
}
+// Set of april tag targets, by april tag ID, to ignore when on a
+// given alliance.
+table IgnoreTargets {
+ red:[uint64] (id: 0);
+ blue:[uint64] (id: 1);
+}
+
table Constants {
cameras:[CameraConfiguration] (id: 0);
target_map:frc971.vision.TargetMap (id: 1);
scoring_map:localizer.ScoringMap (id: 2);
robot:RobotConstants (id: 3);
+ ignore_targets:IgnoreTargets (id: 4);
}
root_type Constants;
diff --git a/y2023/constants/test_data/test_team.json b/y2023/constants/test_data/test_team.json
index a1e77af..0a226f0 100644
--- a/y2023/constants/test_data/test_team.json
+++ b/y2023/constants/test_data/test_team.json
@@ -28,5 +28,9 @@
}
]
}
+ },
+ "ignore_targets": {
+ "red": [4],
+ "blue": [5]
}
}
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 77902e7..19e5a34 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -266,7 +266,7 @@
control1=np.array([3.6130298244820453, -0.2781204657180023]),
control2=np.array([3.804763224169111, -0.5179424890517237]),
end=points['ScoreBackMidConeUp'],
- control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
+ control_alpha_rolls=[(.95, np.pi / 2.0)],
))
points['ScoreBackLowConeUp'] = to_theta_with_circular_index_and_roll(
@@ -279,7 +279,7 @@
control1=np.array([3.260123029490386, -0.5296803702636037]),
control2=np.array([3.1249665389044283, -0.7810758529482493]),
end=points['ScoreBackLowConeUp'],
- control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
+ control_alpha_rolls=[(.95, np.pi / 2.0)],
))
named_segments.append(
@@ -335,8 +335,8 @@
ThetaSplineSegment(
name="NeutralToHPPickupBackConeUp",
start=points['Neutral'],
- control1=np.array([2.1007555300246317, -1.0652453944299012]),
- control2=np.array([0.7579462988809116, -1.6518672114346526]),
+ control1=np.array([2.7014360412658567, -0.32490272351246796]),
+ control2=np.array([0.8282769211604871, -1.8026615176254461]),
end=points['HPPickupBackConeUp'],
control_alpha_rolls=[(.9, np.pi / 2.0)],
alpha_unitizer=np.matrix(
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index e5bc830..afca18b 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -166,6 +166,7 @@
],
target_compatible_with = ["//tools/platforms/hardware:roborio"],
deps = [
+ ":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
":superstructure_status_fbs",
diff --git a/y2023/control_loops/superstructure/led_indicator.cc b/y2023/control_loops/superstructure/led_indicator.cc
index afd0201..5cee266 100644
--- a/y2023/control_loops/superstructure/led_indicator.cc
+++ b/y2023/control_loops/superstructure/led_indicator.cc
@@ -1,6 +1,7 @@
#include "y2023/control_loops/superstructure/led_indicator.h"
namespace led = ctre::phoenix::led;
+namespace chrono = std::chrono;
namespace y2023::control_loops::superstructure {
@@ -13,6 +14,8 @@
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_position_fetcher_(
event_loop_->MakeFetcher<Position>("/superstructure")),
+ superstructure_goal_fetcher_(
+ event_loop_->MakeFetcher<Goal>("/superstructure")),
server_statistics_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
"/roborio/aos")),
@@ -35,7 +38,9 @@
candle_.ConfigAllSettings(config, 0);
event_loop_->AddPhasedLoop([this](int) { DecideColor(); },
- std::chrono::milliseconds(20));
+ chrono::milliseconds(20));
+ event_loop_->OnRun(
+ [this]() { startup_time_ = event_loop_->monotonic_now(); });
}
// This method will be called once per scheduler run
@@ -75,6 +80,7 @@
drivetrain_output_fetcher_.Fetch();
drivetrain_status_fetcher_.Fetch();
client_statistics_fetcher_.Fetch();
+ superstructure_goal_fetcher_.Fetch();
gyro_reading_fetcher_.Fetch();
localizer_output_fetcher_.Fetch();
@@ -96,15 +102,18 @@
// Not zeroed
if (superstructure_status_fetcher_.get() &&
!superstructure_status_fetcher_->zeroed()) {
- DisplayLed(255, 0, 255);
+ DisplayLed(255, 255, 0);
return;
}
- // If the imu gyro readings are not being sent/updated recently
- if (!gyro_reading_fetcher_.get() ||
- gyro_reading_fetcher_.context().monotonic_event_time <
- event_loop_->monotonic_now() -
- frc971::controls::kLoopFrequency * 10) {
+ // If the imu gyro readings are not being sent/updated recently. Only do this
+ // after we've been on for a bit.
+ if (event_loop_->context().monotonic_event_time >
+ startup_time_ + chrono::seconds(5) &&
+ (!gyro_reading_fetcher_.get() ||
+ gyro_reading_fetcher_.context().monotonic_event_time +
+ frc971::controls::kLoopFrequency * 10 <
+ event_loop_->monotonic_now())) {
if (flash_counter_.Flash()) {
DisplayLed(255, 0, 0);
} else {
@@ -127,54 +136,44 @@
return;
}
- if (superstructure_status_fetcher_.get()) {
+ if (superstructure_status_fetcher_.get() &&
+ superstructure_goal_fetcher_.get()) {
+ const bool cone = (superstructure_status_fetcher_->game_piece() ==
+ vision::Class::CONE_UP ||
+ superstructure_status_fetcher_->game_piece() ==
+ vision::Class::CONE_DOWN);
+ const bool intaking = (superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_CONE_UP ||
+ superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_CUBE ||
+ superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_LAST ||
+ superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_CONE_DOWN);
// Check if end effector is intaking.
if (superstructure_status_fetcher_->end_effector_state() ==
- EndEffectorState::INTAKING) {
+ EndEffectorState::LOADED &&
+ intaking) {
if (flash_counter_.Flash()) {
- DisplayLed(255, 165, 0);
- } else {
- DisplayLed(0, 0, 0);
+ if (cone) {
+ DisplayLed(255, 165, 0);
+ } else {
+ DisplayLed(138, 43, 226);
+ }
+ return;
}
-
- return;
- }
- // Check if end effector is spitting.
- if (superstructure_status_fetcher_->end_effector_state() ==
- EndEffectorState::SPITTING) {
- if (flash_counter_.Flash()) {
- DisplayLed(0, 255, 0);
- } else {
- DisplayLed(0, 0, 0);
- }
-
- return;
- }
-
- // Check the if there is a cone in the end effector.
- if (superstructure_status_fetcher_->game_piece() ==
- vision::Class::CONE_UP ||
- superstructure_status_fetcher_->game_piece() ==
- vision::Class::CONE_DOWN) {
- DisplayLed(255, 255, 0);
- return;
- }
- // Check if the cube beam break is triggered.
- if (superstructure_status_fetcher_->game_piece() == vision::Class::CUBE) {
- DisplayLed(138, 43, 226);
- return;
}
// Check if there is a target that is in sight
- if (drivetrain_status_fetcher_.get() != nullptr &&
- drivetrain_status_fetcher_->line_follow_logging()->have_target()) {
- DisplayLed(255, 165, 0);
- return;
- }
-
if (event_loop_->monotonic_now() <
- last_accepted_time_ + std::chrono::milliseconds(500)) {
- DisplayLed(0, 0, 255);
+ last_accepted_time_ + chrono::milliseconds(100)) {
+ if (drivetrain_status_fetcher_.get() != nullptr &&
+ drivetrain_status_fetcher_->line_follow_logging()->have_target()) {
+ DisplayLed(0, 255, 0);
+ return;
+ } else {
+ DisplayLed(0, 0, 255);
+ }
return;
}
}
diff --git a/y2023/control_loops/superstructure/led_indicator.h b/y2023/control_loops/superstructure/led_indicator.h
index 256de30..dfdf4b1 100644
--- a/y2023/control_loops/superstructure/led_indicator.h
+++ b/y2023/control_loops/superstructure/led_indicator.h
@@ -12,6 +12,7 @@
#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
#include "frc971/control_loops/profiled_subsystem_generated.h"
#include "frc971/queues/gyro_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023/control_loops/superstructure/superstructure_output_generated.h"
#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
@@ -76,6 +77,7 @@
drivetrain_output_fetcher_;
aos::Fetcher<Status> superstructure_status_fetcher_;
aos::Fetcher<Position> superstructure_position_fetcher_;
+ aos::Fetcher<Goal> superstructure_goal_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics>
server_statistics_fetcher_;
aos::Fetcher<aos::message_bridge::ClientStatistics>
@@ -89,6 +91,9 @@
aos::monotonic_clock::time_point last_accepted_time_ =
aos::monotonic_clock::min_time;
+ aos::monotonic_clock::time_point startup_time_ =
+ aos::monotonic_clock::min_time;
+
FlashCounter flash_counter_{kFlashIterations};
};
diff --git a/y2023/control_loops/superstructure/superstructure_goal.fbs b/y2023/control_loops/superstructure/superstructure_goal.fbs
index 7ac7000..2ee2d2b 100644
--- a/y2023/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023/control_loops/superstructure/superstructure_goal.fbs
@@ -28,5 +28,4 @@
}
-
root_type Goal;
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index efffca3..c663954 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -248,6 +248,29 @@
return builder->Finish();
}
+bool Localizer::UseAprilTag(uint64_t target_id) {
+ if (target_poses_.count(target_id) == 0) {
+ return false;
+ }
+
+ const flatbuffers::Vector<uint64_t> *ignore_tags = nullptr;
+
+ switch (utils_.Alliance()) {
+ case aos::Alliance::kRed:
+ ignore_tags =
+ CHECK_NOTNULL(constants_fetcher_.constants().ignore_targets()->red());
+ break;
+ case aos::Alliance::kBlue:
+ ignore_tags = CHECK_NOTNULL(
+ constants_fetcher_.constants().ignore_targets()->blue());
+ break;
+ case aos::Alliance::kInvalid:
+ return true;
+ }
+ return std::find(ignore_tags->begin(), ignore_tags->end(), target_id) ==
+ ignore_tags->end();
+}
+
flatbuffers::Offset<TargetEstimateDebug> Localizer::HandleTarget(
int camera_index, const aos::monotonic_clock::time_point capture_time,
const frc971::vision::TargetPoseFbs &target,
@@ -267,7 +290,7 @@
const uint64_t target_id = target.id();
builder.add_april_tag(target_id);
VLOG(2) << aos::FlatbufferToJson(&target);
- if (target_poses_.count(target_id) == 0) {
+ if (!UseAprilTag(target_id)) {
VLOG(1) << "Rejecting target due to invalid ID " << target_id;
return RejectImage(camera_index, RejectionReason::NO_SUCH_TARGET, &builder);
}
diff --git a/y2023/localizer/localizer.h b/y2023/localizer/localizer.h
index accb0c1..4b0715b 100644
--- a/y2023/localizer/localizer.h
+++ b/y2023/localizer/localizer.h
@@ -91,6 +91,8 @@
static flatbuffers::Offset<CumulativeStatistics> StatisticsForCamera(
const CameraState &camera, flatbuffers::FlatBufferBuilder *fbb);
+ bool UseAprilTag(uint64_t target_id);
+
aos::EventLoop *const event_loop_;
const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;