Merge "Make viewer capture bfbs files as well"
diff --git a/WORKSPACE b/WORKSPACE
index 5e26090..3e1801c 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -1303,9 +1303,9 @@
http_archive(
name = "com_github_foxglove_ws-protocol",
- build_file = "//third_party/foxglove_ws_protocol:foxglove_ws_protocol.BUILD",
+ build_file = "//third_party/foxglove/ws_protocol:foxglove_ws_protocol.BUILD",
patch_args = ["-p1"],
- patches = ["//third_party/foxglove_ws_protocol:foxglove_ws_protocol.patch"],
+ patches = ["//third_party/foxglove/ws_protocol:foxglove_ws_protocol.patch"],
sha256 = "3256f09a67419f6556778c443d332f1a4bf53ba0e7a464179bf838abffa366ab",
strip_prefix = "ws-protocol-releases-typescript-ws-protocol-examples-v0.0.6",
url = "https://github.com/foxglove/ws-protocol/archive/refs/tags/releases/typescript/ws-protocol-examples/v0.0.6.tar.gz",
@@ -1325,3 +1325,11 @@
strip_prefix = "asio-1.24.0",
url = "https://downloads.sourceforge.net/project/asio/asio/1.24.0%2520%2528Stable%2529/asio-1.24.0.tar.bz2",
)
+
+http_archive(
+ name = "com_github_foxglove_schemas",
+ build_file = "//third_party/foxglove/schemas:schemas.BUILD",
+ sha256 = "c0d08365eb8fba0af7773b5f0095fb53fb53f020bde46edaa308af5bb939fc15",
+ strip_prefix = "schemas-7a3e077b88142ac46bb4e2616f83dc029b45352e/schemas/flatbuffer",
+ url = "https://github.com/foxglove/schemas/archive/7a3e077b88142ac46bb4e2616f83dc029b45352e.tar.gz",
+)
diff --git a/aos/BUILD b/aos/BUILD
index 9768753..b8468bd 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -157,7 +157,10 @@
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
"//aos/time",
+ "//aos/util:top",
"@com_github_google_glog//:glog",
],
)
diff --git a/aos/dump_rtprio.cc b/aos/dump_rtprio.cc
index 8020857..d36a1c0 100644
--- a/aos/dump_rtprio.cc
+++ b/aos/dump_rtprio.cc
@@ -18,9 +18,14 @@
#include <set>
#include <string>
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/util/top.h"
#include "aos/time/time.h"
#include "glog/logging.h"
+DEFINE_string(config, "aos_config.json", "File path of aos configuration");
+
namespace {
const char *policy_string(uint32_t policy) {
@@ -52,17 +57,7 @@
return str.substr(0, str.size() - 1);
}
-int find_pid_max() {
- int r;
- FILE *pid_max_file = fopen("/proc/sys/kernel/pid_max", "r");
- PCHECK(pid_max_file != nullptr)
- << ": Failed to open /proc/sys/kernel/pid_max";
- CHECK_EQ(1, fscanf(pid_max_file, "%d", &r));
- PCHECK(fclose(pid_max_file) == 0);
- return r;
-}
-
-cpu_set_t find_all_cpus() {
+cpu_set_t FindAllCpus() {
long nproc = sysconf(_SC_NPROCESSORS_CONF);
PCHECK(nproc != -1);
cpu_set_t r;
@@ -253,47 +248,63 @@
} // namespace
-int main() {
- const int pid_max = find_pid_max();
- const cpu_set_t all_cpus = find_all_cpus();
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
- std::multiset<Thread> threads;
+ aos::ShmEventLoop event_loop(&config.message());
+ event_loop.SkipTimingReport();
+ event_loop.SkipAosLog();
+ aos::util::Top top(&event_loop);
+ top.set_track_top_processes(true);
- for (int i = 0; i < pid_max; ++i) {
- bool not_there = false;
+ const cpu_set_t all_cpus = FindAllCpus();
- const cpu_set_t cpu_mask = find_cpu_mask(i, ¬_there);
- const sched_param param = find_sched_param(i, ¬_there);
- const int scheduler = find_scheduler(i, ¬_there);
- const ::std::string exe = find_exe(i, ¬_there);
- const int nice_value = find_nice_value(i, ¬_there);
+ top.set_on_reading_update([&]() {
+ std::multiset<Thread> threads;
- int ppid = 0, sid = 0;
- read_stat(i, &ppid, &sid, ¬_there);
+ for (const std::pair<const pid_t, aos::util::Top::ProcessReadings>
+ &reading : top.readings()) {
+ pid_t tid = reading.first;
+ bool not_there = false;
- int pgrp = 0;
- ::std::string name;
- read_status(i, ppid, &pgrp, &name, ¬_there);
+ const cpu_set_t cpu_mask = find_cpu_mask(tid, ¬_there);
+ const sched_param param = find_sched_param(tid, ¬_there);
+ const int scheduler = find_scheduler(tid, ¬_there);
+ const ::std::string exe = find_exe(tid, ¬_there);
+ const int nice_value = find_nice_value(tid, ¬_there);
- if (not_there) continue;
+ int ppid = 0, sid = 0;
+ read_stat(tid, &ppid, &sid, ¬_there);
- const char *cpu_mask_string =
- CPU_EQUAL(&cpu_mask, &all_cpus) ? "all" : "???";
+ int pgrp = 0;
+ ::std::string name;
+ read_status(tid, ppid, &pgrp, &name, ¬_there);
- threads.emplace(Thread{.policy = static_cast<uint32_t>(scheduler),
- .exe = exe,
- .name = name,
- .cpu_mask = cpu_mask_string,
- .nice_value = nice_value,
- .sched_priority = param.sched_priority,
- .tid = i,
- .pid = pgrp,
- .ppid = ppid,
- .sid = sid});
- }
+ if (not_there) continue;
- printf("exe,name,cpumask,policy,nice,priority,tid,pid,ppid,sid\n");
- for (const auto &t : threads) {
- t.Print();
- }
+ const char *cpu_mask_string =
+ CPU_EQUAL(&cpu_mask, &all_cpus) ? "all" : "???";
+
+ threads.emplace(Thread{.policy = static_cast<uint32_t>(scheduler),
+ .exe = exe,
+ .name = name,
+ .cpu_mask = cpu_mask_string,
+ .nice_value = nice_value,
+ .sched_priority = param.sched_priority,
+ .tid = tid,
+ .pid = pgrp,
+ .ppid = ppid,
+ .sid = sid});
+ }
+
+ printf("exe,name,cpumask,policy,nice,priority,tid,pid,ppid,sid\n");
+ for (const auto &t : threads) {
+ t.Print();
+ }
+ event_loop.Exit();
+ });
+
+ event_loop.Run();
}
diff --git a/aos/flatbuffers.bzl b/aos/flatbuffers.bzl
index 3db4ca0..c26906a 100644
--- a/aos/flatbuffers.bzl
+++ b/aos/flatbuffers.bzl
@@ -1,16 +1,18 @@
-def cc_static_flatbuffer(name, target, function, visibility = None):
+def cc_static_flatbuffer(name, target, function, bfbs_name = None, visibility = None):
"""Creates a cc_library which encodes a file as a Span.
args:
target, The file to encode.
function, The inline function, with full namespaces, to create.
+ bfbs_name, For flatbuffer targets that have multiple fbs files, this
+ specifies the basename of the bfbs file to generate a schema for.
"""
native.genrule(
name = name + "_gen",
tools = ["@org_frc971//aos:flatbuffers_static"],
srcs = [target],
outs = [name + ".h"],
- cmd = "$(location @org_frc971//aos:flatbuffers_static) $(SRCS) $(OUTS) '" + function + "'",
+ cmd = "$(location @org_frc971//aos:flatbuffers_static) '$(SRCS)' $(OUTS) '" + function + "' " + (bfbs_name if bfbs_name else "-"),
)
native.cc_library(
diff --git a/aos/flatbuffers_static.py b/aos/flatbuffers_static.py
index 199c5b9..3bd28fe 100644
--- a/aos/flatbuffers_static.py
+++ b/aos/flatbuffers_static.py
@@ -8,12 +8,26 @@
def main(argv):
- if len(argv) != 4:
+ if len(argv) != 5:
+ print(
+ f"Incorrect number of arguments {len(argv)} to flatbuffers_static."
+ )
+ print(argv)
return 1
input_path = sys.argv[1]
output_path = sys.argv[2]
function = sys.argv[3].split("::")
+ bfbs_name = sys.argv[4]
+ if bfbs_name != '-':
+ inputs = input_path.split(' ')
+ valid_paths = [path for path in inputs if path.endswith(bfbs_name)]
+ if len(valid_paths) != 1:
+ print(
+ f"Expected exactly one match for {bfbs_name}; got {valid_paths}."
+ )
+ return 1
+ input_path = valid_paths[0]
include_guard = output_path.replace('/', '_').replace('-', '_').replace(
'.', '_').upper() + '_'
diff --git a/aos/util/log_to_mcap.cc b/aos/util/log_to_mcap.cc
index a94473e..38a3be7 100644
--- a/aos/util/log_to_mcap.cc
+++ b/aos/util/log_to_mcap.cc
@@ -51,7 +51,7 @@
if (FLAGS_include_clocks) {
aos::logger::LogReader config_reader(logfiles);
- const aos::Configuration *raw_config = config_reader.configuration();
+ const aos::Configuration *raw_config = config_reader.logged_configuration();
config = aos::configuration::AddChannelToConfiguration(
raw_config, "/clocks",
aos::FlatbufferSpan<reflection::Schema>(aos::ClockTimepointsSchema()),
@@ -74,26 +74,33 @@
std::unique_ptr<aos::EventLoop> clock_event_loop;
std::unique_ptr<aos::ClockPublisher> clock_publisher;
if (FLAGS_include_clocks) {
- // TODO(james): Currently, because of RegisterWithoutStarting, this ends up
- // running from t=0.0 rather than the start of the logfile. Fix that.
- clock_event_loop =
- reader.event_loop_factory()->MakeEventLoop("clock", node);
- clock_publisher =
- std::make_unique<aos::ClockPublisher>(&factory, clock_event_loop.get());
+ reader.OnStart(node, [&clock_event_loop, &reader, &clock_publisher,
+ &factory, node]() {
+ clock_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("clock", node);
+ clock_publisher = std::make_unique<aos::ClockPublisher>(
+ &factory, clock_event_loop.get());
+ });
}
- std::unique_ptr<aos::EventLoop> mcap_event_loop =
- reader.event_loop_factory()->MakeEventLoop("mcap", node);
+ std::unique_ptr<aos::EventLoop> mcap_event_loop;
CHECK(!FLAGS_output_path.empty());
- aos::McapLogger relogger(
- mcap_event_loop.get(), FLAGS_output_path,
- FLAGS_mode == "flatbuffer" ? aos::McapLogger::Serialization::kFlatbuffer
- : aos::McapLogger::Serialization::kJson,
- FLAGS_canonical_channel_names
- ? aos::McapLogger::CanonicalChannelNames::kCanonical
- : aos::McapLogger::CanonicalChannelNames::kShortened,
- FLAGS_compress ? aos::McapLogger::Compression::kLz4
- : aos::McapLogger::Compression::kNone);
+ std::unique_ptr<aos::McapLogger> relogger;
+ factory.GetNodeEventLoopFactory(node)
+ ->OnStartup([&relogger, &mcap_event_loop, &reader, node]() {
+ mcap_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("mcap", node);
+ relogger = std::make_unique<aos::McapLogger>(
+ mcap_event_loop.get(), FLAGS_output_path,
+ FLAGS_mode == "flatbuffer"
+ ? aos::McapLogger::Serialization::kFlatbuffer
+ : aos::McapLogger::Serialization::kJson,
+ FLAGS_canonical_channel_names
+ ? aos::McapLogger::CanonicalChannelNames::kCanonical
+ : aos::McapLogger::CanonicalChannelNames::kShortened,
+ FLAGS_compress ? aos::McapLogger::Compression::kLz4
+ : aos::McapLogger::Compression::kNone);
+ });
reader.event_loop_factory()->Run();
reader.Deregister();
}
diff --git a/aos/util/mcap_logger.cc b/aos/util/mcap_logger.cc
index 88476ed..96a9b60 100644
--- a/aos/util/mcap_logger.cc
+++ b/aos/util/mcap_logger.cc
@@ -229,17 +229,6 @@
// Manually add in a special /configuration channel.
if (register_handlers == RegisterHandlers::kYes) {
configuration_id_ = ++id;
- event_loop_->OnRun([this]() {
- // TODO(james): Make it so that the timestamp for the configuration
- // message is not 0.0.
- Context config_context;
- config_context.monotonic_event_time = event_loop_->monotonic_now();
- config_context.queue_index = 0;
- config_context.size = configuration_.span().size();
- config_context.data = configuration_.span().data();
- WriteMessage(configuration_id_, &configuration_channel_.message(),
- config_context, ¤t_chunks_[configuration_id_]);
- });
}
std::vector<SummaryOffset> offsets;
@@ -276,6 +265,18 @@
return offsets;
}
+void McapLogger::WriteConfigurationMessage() {
+ Context config_context;
+ config_context.monotonic_event_time = event_loop_->monotonic_now();
+ config_context.queue_index = 0;
+ config_context.size = configuration_.span().size();
+ config_context.data = configuration_.span().data();
+ // Avoid infinite recursion...
+ wrote_configuration_ = true;
+ WriteMessage(configuration_id_, &configuration_channel_.message(),
+ config_context, ¤t_chunks_[configuration_id_]);
+}
+
void McapLogger::WriteMagic() { output_ << "\x89MCAP0\r\n"; }
void McapLogger::WriteHeader() {
@@ -390,6 +391,9 @@
void McapLogger::WriteMessage(uint16_t channel_id, const Channel *channel,
const Context &context, ChunkStatus *chunk) {
+ if (!wrote_configuration_) {
+ WriteConfigurationMessage();
+ }
CHECK_NOTNULL(context.data);
message_counts_[channel_id]++;
diff --git a/aos/util/mcap_logger.h b/aos/util/mcap_logger.h
index 70a1328..c3fdda1 100644
--- a/aos/util/mcap_logger.h
+++ b/aos/util/mcap_logger.h
@@ -143,6 +143,11 @@
const Context &context, ChunkStatus *chunk);
void WriteChunk(ChunkStatus *chunk);
+ // Writes out the special configuration channel. This gets called right before
+ // the first actual message is written so that we can have a reasonable
+ // monotonic clock time.
+ void WriteConfigurationMessage();
+
// The helpers for writing records which appear in the Summary section will
// return SummaryOffset's so that they can be referenced in the SummaryOffset
// section.
@@ -199,6 +204,7 @@
uint16_t configuration_id_ = 0;
FlatbufferDetachedBuffer<Channel> configuration_channel_;
FlatbufferDetachedBuffer<Configuration> configuration_;
+ bool wrote_configuration_ = false;
// Memory buffer to use for compressing data.
std::vector<uint8_t> compression_buffer_;
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 80a4a53..2fed5f0 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -3,6 +3,7 @@
from frc971.control_loops.python import control_loop
from frc971.control_loops.python import controls
import numpy
+import math
import sys
from matplotlib import pylab
import glog
@@ -33,7 +34,8 @@
dt=0.00505,
controller_poles=[0.90, 0.90],
observer_poles=[0.02, 0.02],
- robot_cg_offset=0.0):
+ robot_cg_offset=0.0,
+ coefficient_of_friction=1.0):
"""Defines all constants of a drivetrain.
Args:
@@ -107,6 +109,7 @@
self.num_motors = num_motors
self.controller_poles = controller_poles
self.observer_poles = observer_poles
+ self.coefficient_of_friction = coefficient_of_friction
class Drivetrain(control_loop.ControlLoop):
@@ -533,6 +536,221 @@
kf_loop_writer.Write(kf_drivetrain_files[0], kf_drivetrain_files[1])
+def PlotDrivetrainSprint(drivetrain_params):
+ # Simulate the response of the system to a step input.
+ drivetrain = KFDrivetrain(left_low=False,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
+ simulated_left_position = []
+ simulated_right_position = []
+ simulated_left_velocity = []
+ simulated_right_velocity = []
+
+ simulated_left_motor_currents = []
+ simulated_left_breaker_currents = []
+ simulated_right_motor_currents = []
+ simulated_right_breaker_currents = []
+
+ simulated_battery_heat_wattages = []
+ simulated_wattage = []
+ motor_inverter_voltages = []
+ voltage_left = []
+ voltage_right = []
+ simulated_motor_heat_wattages = []
+ simulated_motor_wattage = []
+
+ max_motor_currents = []
+ overall_currents = []
+ simulated_battery_wattage = []
+
+ # Distance in meters to call 1/2 field.
+ kSprintDistance = 8.0
+
+ vbat = 12.6
+ # Measured resistance of the battery, pd board, and breakers.
+ Rw = 0.023
+ top_speed = drivetrain.free_speed * (drivetrain.Gr +
+ drivetrain.Gl) / 2.0 * drivetrain.r
+
+ passed_distance = False
+ max_breaker_current = 0
+ heat_energy_usage = 0.0
+ for index in range(800):
+ # Current per side
+ left_traction_current = (drivetrain.mass / 2.0 *
+ drivetrain_params.coefficient_of_friction *
+ 9.81 * drivetrain.r * drivetrain.Gl /
+ drivetrain.Kt)
+ right_traction_current = (drivetrain.mass / 2.0 *
+ drivetrain_params.coefficient_of_friction *
+ 9.81 * drivetrain.r * drivetrain.Gr /
+ drivetrain.Kt)
+
+ # Detect if we've traveled over the sprint distance and report stats.
+ if (drivetrain.X[0, 0] + drivetrain.X[2, 0]) / 2.0 > kSprintDistance:
+ if not passed_distance:
+ velocity = (drivetrain.X[1, 0] + drivetrain.X[3, 0]) / 2.0
+ print("Took", index * drivetrain.dt,
+ "to pass 1/2 field, going", velocity, "m/s,",
+ velocity / 0.0254 / 12.0, "Traction limit current",
+ left_traction_current / drivetrain_params.num_motors,
+ "max breaker current", max_breaker_current, "top speed",
+ top_speed, "m/s", top_speed / 0.0254 / 12.0,
+ "fps, gear ratio", drivetrain.Gl, "heat energy",
+ heat_energy_usage)
+ passed_distance = True
+
+ bemf_left = drivetrain.X[
+ 1, 0] / drivetrain.r / drivetrain.Gl / drivetrain.Kv
+ bemf_right = drivetrain.X[
+ 3, 0] / drivetrain.r / drivetrain.Gr / drivetrain.Kv
+
+ # Max current we could push through the motors is what we would get if
+ # we short the battery through the battery resistance into the motor.
+ max_motor_current = (vbat - (bemf_left + bemf_right) / 2.0) / (
+ Rw + drivetrain.resistance / 2.0)
+
+ max_motor_currents.append(max_motor_current /
+ (drivetrain_params.num_motors * 2))
+
+ # From this current, we can compute the voltage we can apply.
+ # This is either the traction limit or the current limit.
+ max_voltage_left = bemf_left + min(
+ max_motor_current / 2,
+ left_traction_current) * drivetrain.resistance
+ max_voltage_right = bemf_right + min(
+ max_motor_current / 2,
+ right_traction_current) * drivetrain.resistance
+
+ simulated_left_position.append(drivetrain.X[0, 0])
+ simulated_left_velocity.append(drivetrain.X[1, 0])
+ simulated_right_position.append(drivetrain.X[2, 0])
+ simulated_right_velocity.append(drivetrain.X[3, 0])
+
+ U = numpy.matrix([[min(max_voltage_left, vbat)],
+ [min(max_voltage_right, vbat)]])
+
+ # Stator current
+ simulated_left_motor_current = (U[0, 0] -
+ bemf_left) / drivetrain.resistance
+ simulated_right_motor_current = (U[1, 0] -
+ bemf_right) / drivetrain.resistance
+
+ # And this gives us the power pushed into the motors.
+ power = (U[0, 0] * simulated_left_motor_current +
+ U[1, 0] * simulated_right_motor_current)
+
+ simulated_wattage.append(power)
+
+ # Solve for the voltage we'd have to supply to the input of the motor
+ # controller to generate the power required.
+ motor_inverter_voltage = (
+ vbat + numpy.sqrt(vbat * vbat - 4.0 * power * Rw)) / 2.0
+
+ overall_current = (vbat - motor_inverter_voltage) / Rw
+ overall_currents.append(overall_current)
+
+ motor_inverter_voltages.append(motor_inverter_voltage)
+
+ # Overall left and right currents at the breaker
+ simulated_left_breaker_current = (
+ simulated_left_motor_current /
+ drivetrain_params.num_motors) * U[0, 0] / motor_inverter_voltage
+ simulated_right_breaker_current = (
+ simulated_right_motor_current /
+ drivetrain_params.num_motors) * U[1, 0] / motor_inverter_voltage
+
+ simulated_left_motor_currents.append(simulated_left_motor_current /
+ drivetrain_params.num_motors)
+ simulated_left_breaker_currents.append(simulated_left_breaker_current)
+ simulated_right_motor_currents.append(simulated_right_motor_current /
+ drivetrain_params.num_motors)
+ simulated_right_breaker_currents.append(
+ simulated_right_breaker_current)
+
+ # Save out the peak battery current observed.
+ max_breaker_current = max(
+ max_breaker_current,
+ max(simulated_left_breaker_current,
+ simulated_right_breaker_current))
+
+ # Compute the heat burned in the battery
+ simulated_battery_heat_wattage = math.pow(
+ vbat - motor_inverter_voltage, 2.0) / Rw
+ simulated_battery_heat_wattages.append(simulated_battery_heat_wattage)
+
+ motor_heat_wattage = (math.pow(simulated_left_motor_current, 2.0) *
+ drivetrain.resistance +
+ math.pow(simulated_right_motor_current, 2.0) *
+ drivetrain.resistance)
+ simulated_motor_heat_wattages.append(motor_heat_wattage)
+
+ simulated_motor_wattage.append(simulated_left_motor_current * U[0, 0] +
+ simulated_right_motor_current * U[1, 0])
+
+ simulated_battery_wattage.append(vbat * overall_current)
+
+ # And then the overall energy outputted by the battery.
+ heat_energy_usage += (motor_heat_wattage +
+ simulated_battery_heat_wattage) * drivetrain.dt
+
+ voltage_left.append(U[0, 0])
+ voltage_right.append(U[1, 0])
+
+ drivetrain.Update(U)
+
+ t = [drivetrain.dt * x for x in range(len(simulated_left_position))]
+ pylab.rc('lines', linewidth=4)
+ pylab.subplot(3, 1, 1)
+ pylab.plot(t, simulated_left_position, label='left position')
+ pylab.plot(t, simulated_right_position, 'r--', label='right position')
+ pylab.plot(t, simulated_left_velocity, label='left velocity')
+ pylab.plot(t, simulated_right_velocity, label='right velocity')
+
+ pylab.suptitle('Acceleration Test\n12 Volt Step Input')
+ pylab.legend(loc='lower right')
+
+ pylab.subplot(3, 1, 2)
+
+ pylab.plot(t, simulated_left_motor_currents, label='left rotor current')
+ pylab.plot(t,
+ simulated_right_motor_currents,
+ 'r--',
+ label='right rotor current')
+ pylab.plot(t,
+ simulated_left_breaker_currents,
+ label='left breaker current')
+ pylab.plot(t,
+ simulated_right_breaker_currents,
+ 'r--',
+ label='right breaker current')
+ pylab.plot(t, motor_inverter_voltages, label='motor inverter voltage')
+ pylab.plot(t, voltage_left, label='left voltage')
+ pylab.plot(t, voltage_right, label='right voltage')
+ pylab.plot(t, max_motor_currents, label='max_currents')
+ pylab.legend(loc='lower right')
+
+ wattage_axis = pylab.subplot(3, 1, 3)
+ wattage_axis.plot(t, simulated_wattage, label='wattage')
+ wattage_axis.plot(t,
+ simulated_battery_heat_wattages,
+ label='battery wattage')
+ wattage_axis.plot(t,
+ simulated_motor_heat_wattages,
+ label='motor heat wattage')
+ wattage_axis.plot(t, simulated_motor_wattage, label='motor wattage')
+ wattage_axis.plot(t, simulated_battery_wattage, label='overall wattage')
+ pylab.legend(loc='upper left')
+ overall_current_axis = wattage_axis.twinx()
+ overall_current_axis.plot(t, overall_currents, 'c--', label='current')
+
+ pylab.legend(loc='lower right')
+
+ pylab.suptitle('Acceleration Test\n12 Volt Step Input\n%f fps' %
+ (top_speed / 0.0254 / 12.0, ))
+ pylab.show()
+
+
def PlotDrivetrainMotions(drivetrain_params):
# Test out the voltage error.
drivetrain = KFDrivetrain(left_low=False,
diff --git a/frc971/rockpi/build_rootfs.sh b/frc971/rockpi/build_rootfs.sh
index 4bffe67..74d4dc1 100755
--- a/frc971/rockpi/build_rootfs.sh
+++ b/frc971/rockpi/build_rootfs.sh
@@ -186,7 +186,7 @@
target "apt-get -y install -t bullseye-backports bpfcc-tools"
-target "apt-get install -y sudo openssh-server python3 bash-completion git v4l-utils cpufrequtils pmount rsync vim-nox chrony libopencv-calib3d4.5 libopencv-contrib4.5 libopencv-core4.5 libopencv-features2d4.5 libopencv-flann4.5 libopencv-highgui4.5 libopencv-imgcodecs4.5 libopencv-imgproc4.5 libopencv-ml4.5 libopencv-objdetect4.5 libopencv-photo4.5 libopencv-shape4.5 libopencv-stitching4.5 libopencv-superres4.5 libopencv-video4.5 libopencv-videoio4.5 libopencv-videostab4.5 libopencv-viz4.5 libnice10 pmount libnice-dev feh libgstreamer1.0-0 libgstreamer-plugins-base1.0-0 libgstreamer-plugins-bad1.0-0 gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly gstreamer1.0-nice usbutils locales trace-cmd clinfo"
+target "apt-get install -y sudo openssh-server python3 bash-completion git v4l-utils cpufrequtils pmount rsync vim-nox chrony libopencv-calib3d4.5 libopencv-contrib4.5 libopencv-core4.5 libopencv-features2d4.5 libopencv-flann4.5 libopencv-highgui4.5 libopencv-imgcodecs4.5 libopencv-imgproc4.5 libopencv-ml4.5 libopencv-objdetect4.5 libopencv-photo4.5 libopencv-shape4.5 libopencv-stitching4.5 libopencv-superres4.5 libopencv-video4.5 libopencv-videoio4.5 libopencv-videostab4.5 libopencv-viz4.5 libnice10 pmount libnice-dev feh libgstreamer1.0-0 libgstreamer-plugins-base1.0-0 libgstreamer-plugins-bad1.0-0 gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly gstreamer1.0-nice usbutils locales trace-cmd clinfo jq"
target "cd /tmp && wget https://software.frc971.org/Build-Dependencies/libmali-midgard-t86x-r14p0-x11_1.9-1_arm64.deb && sudo dpkg -i libmali-midgard-t86x-r14p0-x11_1.9-1_arm64.deb && rm libmali-midgard-t86x-r14p0-x11_1.9-1_arm64.deb"
target "apt-get clean"
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index 80b1fd4..b8d4086 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -230,12 +230,6 @@
marker_length_ = 0.15;
square_length_ = 0.1651;
dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_250);
- } else if (target_type_ == TargetType::kAprilTag) {
- // Tag will be 6 in (15.24 cm) according to
- // https://www.firstinspires.org/robotics/frc/blog/2022-2023-approved-devices-rules-preview-and-vision-target-update
- square_length_ = 0.1524;
- dictionary_ =
- cv::aruco::getPredefinedDictionary(cv::aruco::DICT_APRILTAG_16h5);
} else {
// Bail out if it's not a supported target
LOG(FATAL) << "Target type undefined: "
@@ -435,9 +429,8 @@
<< ", not enough marker IDs for charuco board, got "
<< marker_ids.size() << ", needed " << FLAGS_min_charucos;
}
- } else if (target_type_ == TargetType::kAprilTag ||
- target_type_ == TargetType::kAruco) {
- // estimate pose for april tags doesn't return valid, so marking true
+ } else if (target_type_ == TargetType::kAruco) {
+ // estimate pose for arucos doesn't return valid, so marking true
valid = true;
std::vector<cv::Vec3d> rvecs, tvecs;
cv::aruco::estimatePoseSingleMarkers(marker_corners, square_length_,
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index c7269f9..de6d4fb 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -58,9 +58,7 @@
std::function<void(cv::Mat, aos::monotonic_clock::time_point)>
&&handle_image_fn);
- void set_format(Format format) {
- format_ = format;
- }
+ void set_format(Format format) { format_ = format; }
private:
void DisableTracing();
@@ -80,10 +78,9 @@
// Types of targets that a CharucoExtractor can detect in images
enum class TargetType : uint8_t {
- kAprilTag = 0,
- kAruco = 1,
- kCharuco = 2,
- kCharucoDiamond = 3
+ kAruco = 0,
+ kCharuco = 1,
+ kCharucoDiamond = 2
};
// Class which calls a callback each time an image arrives with the information
diff --git a/frc971/vision/target_map.fbs b/frc971/vision/target_map.fbs
index 38bab6d..57627e8 100644
--- a/frc971/vision/target_map.fbs
+++ b/frc971/vision/target_map.fbs
@@ -5,8 +5,8 @@
// AprilTag ID of this target
id:uint64 (id: 0);
- // Pose of target relative to field origin.
- // To get the pose of the target in the field frame, do:
+ // Pose of target relative to either the field origin or robot.
+ // To get the pose of the target, do:
// Translation3d(x, y, z) *
// (AngleAxisd(yaw) * AngleAxisd(pitch) * AngleAxisd(roll))
x:double (id: 1);
@@ -20,10 +20,18 @@
}
// Map of all target poses on a field.
-// This would be solved for by TargetMapper
+// There are two possible uses for this:
+// 1. Static april tag poses on the field solved for by TargetMapper.
+// 2. List of detected april poses relative to the robot.
table TargetMap {
target_poses:[TargetPoseFbs] (id: 0);
- // Unique name of the field
+ // Unique name of the field (for use case 1.)
field_name:string (id: 1);
-}
\ No newline at end of file
+
+ // End-of-frame timestamp for the frame with tag detections.
+ // (for use case 2.).
+ monotonic_timestamp_ns:int64 (id: 2);
+}
+
+root_type TargetMap;
diff --git a/third_party/apriltag/BUILD b/third_party/apriltag/BUILD
new file mode 100644
index 0000000..b11e34f
--- /dev/null
+++ b/third_party/apriltag/BUILD
@@ -0,0 +1,93 @@
+load("@//tools/build_rules:select.bzl", "compiler_select")
+
+cc_library(
+ name = "apriltag",
+ srcs = [
+ "apriltag.c",
+ "apriltag_pose.c",
+ "apriltag_quad_thresh.c",
+ "common/g2d.c",
+ "common/getopt.c",
+ "common/homography.c",
+ "common/image_u8.c",
+ "common/image_u8x3.c",
+ "common/image_u8x4.c",
+ "common/matd.c",
+ "common/pam.c",
+ "common/pjpeg.c",
+ "common/pjpeg-idct.c",
+ "common/pnm.c",
+ "common/string_util.c",
+ "common/svd22.c",
+ "common/time_util.c",
+ "common/unionfind.c",
+ "common/workerpool.c",
+ "common/zarray.c",
+ "common/zhash.c",
+ "common/zmaxheap.c",
+ "tag16h5.c",
+ "tag25h9.c",
+ "tag36h11.c",
+ "tagCircle21h7.c",
+ "tagCircle49h12.c",
+ "tagCustom48h12.c",
+ "tagStandard41h12.c",
+ "tagStandard52h13.c",
+ ],
+ hdrs = [
+ "apriltag.h",
+ "apriltag_math.h",
+ "apriltag_pose.h",
+ "common/debug_print.h",
+ "common/g2d.h",
+ "common/getopt.h",
+ "common/homography.h",
+ "common/image_types.h",
+ "common/image_u8.h",
+ "common/image_u8x3.h",
+ "common/image_u8x4.h",
+ "common/matd.h",
+ "common/math_util.h",
+ "common/pam.h",
+ "common/pjpeg.h",
+ "common/pnm.h",
+ "common/postscript_utils.h",
+ "common/pthreads_cross.h",
+ "common/string_util.h",
+ "common/svd22.h",
+ "common/time_util.h",
+ "common/timeprofile.h",
+ "common/unionfind.h",
+ "common/workerpool.h",
+ "common/zarray.h",
+ "common/zhash.h",
+ "common/zmaxheap.h",
+ "tag16h5.h",
+ "tag25h9.h",
+ "tag36h11.h",
+ "tagCircle21h7.h",
+ "tagCircle49h12.h",
+ "tagCustom48h12.h",
+ "tagStandard41h12.h",
+ "tagStandard52h13.h",
+ ],
+ copts = compiler_select({
+ "clang": [
+ "-Wno-cast-align",
+ "-Wno-incompatible-pointer-types-discards-qualifiers",
+ ],
+ "gcc": [
+ "-Wno-discarded-qualifiers",
+ "-Wno-shift-negative-value",
+ ],
+ }) + [
+ "-Wno-sign-compare",
+ "-Wno-cast-qual",
+ "-Wno-unused-parameter",
+ "-Wno-unused-variable",
+ "-Wno-format-nonliteral",
+ ],
+ includes = ["."],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
diff --git a/third_party/apriltag/common/workerpool.c b/third_party/apriltag/common/workerpool.c
index a0170ef..29eccfc 100644
--- a/third_party/apriltag/common/workerpool.c
+++ b/third_party/apriltag/common/workerpool.c
@@ -41,20 +41,6 @@
#include "workerpool.h"
#include "debug_print.h"
-struct workerpool {
- int nthreads;
- zarray_t *tasks;
- int taskspos;
-
- pthread_t *threads;
- int *status;
-
- pthread_mutex_t mutex;
- pthread_cond_t startcond; // used to signal the availability of work
- pthread_cond_t endcond; // used to signal completion of all work
-
- int end_count; // how many threads are done?
-};
struct task
{
diff --git a/third_party/apriltag/common/workerpool.h b/third_party/apriltag/common/workerpool.h
index 2c32ab1..a233b5b 100644
--- a/third_party/apriltag/common/workerpool.h
+++ b/third_party/apriltag/common/workerpool.h
@@ -31,6 +31,21 @@
typedef struct workerpool workerpool_t;
+struct workerpool {
+ int nthreads;
+ zarray_t *tasks;
+ int taskspos;
+
+ pthread_t *threads;
+ int *status;
+
+ pthread_mutex_t mutex;
+ pthread_cond_t startcond; // used to signal the availability of work
+ pthread_cond_t endcond; // used to signal completion of all work
+
+ int end_count; // how many threads are done?
+};
+
// as a special case, if nthreads==1, no additional threads are
// created, and workerpool_run will run synchronously.
workerpool_t *workerpool_create(int nthreads);
@@ -41,7 +56,8 @@
// runs all added tasks, waits for them to complete.
void workerpool_run(workerpool_t *wp);
-// same as workerpool_run, except always single threaded. (mostly for debugging).
+// same as workerpool_run, except always single threaded. (mostly for
+// debugging).
void workerpool_run_single(workerpool_t *wp);
int workerpool_get_nthreads(workerpool_t *wp);
diff --git a/third_party/foxglove_ws_protocol/BUILD b/third_party/foxglove/BUILD
similarity index 100%
copy from third_party/foxglove_ws_protocol/BUILD
copy to third_party/foxglove/BUILD
diff --git a/third_party/foxglove_ws_protocol/BUILD b/third_party/foxglove/schemas/BUILD
similarity index 100%
copy from third_party/foxglove_ws_protocol/BUILD
copy to third_party/foxglove/schemas/BUILD
diff --git a/third_party/foxglove/schemas/schemas.BUILD b/third_party/foxglove/schemas/schemas.BUILD
new file mode 100644
index 0000000..a1a5d29
--- /dev/null
+++ b/third_party/foxglove/schemas/schemas.BUILD
@@ -0,0 +1,21 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "DEFAULT_FLATC_ARGS")
+
+FLATC_ARGS = [arg for arg in DEFAULT_FLATC_ARGS if arg != "--require-explicit-ids"]
+
+flatbuffer_cc_library(
+ name = "schemas",
+ srcs = glob(["*.fbs"]),
+ flatc_args = FLATC_ARGS,
+ gen_reflections = True,
+ visibility = ["//visibility:public"],
+)
+
+load("@org_frc971//aos:flatbuffers.bzl", "cc_static_flatbuffer")
+
+[cc_static_flatbuffer(
+ name = filename[:-4] + "_schema",
+ bfbs_name = filename[:-4] + ".bfbs",
+ function = "foxglove::" + filename[:-4] + "Schema",
+ target = ":schemas_reflection_out",
+ visibility = ["//visibility:public"],
+) for filename in glob(["*.fbs"])]
diff --git a/third_party/foxglove_ws_protocol/BUILD b/third_party/foxglove/ws_protocol/BUILD
similarity index 100%
rename from third_party/foxglove_ws_protocol/BUILD
rename to third_party/foxglove/ws_protocol/BUILD
diff --git a/third_party/foxglove_ws_protocol/foxglove_ws_protocol.BUILD b/third_party/foxglove/ws_protocol/foxglove_ws_protocol.BUILD
similarity index 100%
rename from third_party/foxglove_ws_protocol/foxglove_ws_protocol.BUILD
rename to third_party/foxglove/ws_protocol/foxglove_ws_protocol.BUILD
diff --git a/third_party/foxglove_ws_protocol/foxglove_ws_protocol.patch b/third_party/foxglove/ws_protocol/foxglove_ws_protocol.patch
similarity index 100%
rename from third_party/foxglove_ws_protocol/foxglove_ws_protocol.patch
rename to third_party/foxglove/ws_protocol/foxglove_ws_protocol.patch
diff --git a/y2022/vision/calibrate_extrinsics.cc b/y2022/vision/calibrate_extrinsics.cc
index 05d3481..b5005fe 100644
--- a/y2022/vision/calibrate_extrinsics.cc
+++ b/y2022/vision/calibrate_extrinsics.cc
@@ -19,7 +19,7 @@
DEFINE_bool(plot, false, "Whether to plot the resulting data.");
DEFINE_bool(turret, true, "If true, the camera is on the turret");
DEFINE_string(target_type, "charuco",
- "Type of target: april_tag|aruco|charuco|charuco_diamond");
+ "Type of target: aruco|charuco|charuco_diamond");
DEFINE_string(image_channel, "/camera", "Channel to listen for images on");
namespace frc971 {
@@ -68,9 +68,7 @@
factory.MakeEventLoop("calibration", pi_node);
TargetType target_type = TargetType::kCharuco;
- if (FLAGS_target_type == "april_tag") {
- target_type = TargetType::kAprilTag;
- } else if (FLAGS_target_type == "aruco") {
+ if (FLAGS_target_type == "aruco") {
target_type = TargetType::kAruco;
} else if (FLAGS_target_type == "charuco") {
target_type = TargetType::kCharuco;
@@ -78,7 +76,7 @@
target_type = TargetType::kCharucoDiamond;
} else {
LOG(FATAL) << "Unknown target type: " << FLAGS_target_type
- << ", expected: april_tag|aruco|charuco|charuco_diamond";
+ << ", expected: aruco|charuco|charuco_diamond";
}
// Now, hook Calibration up to everything.
diff --git a/y2023/BUILD b/y2023/BUILD
index 0879de7..60d218b 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -7,7 +7,7 @@
binaries = [
"//y2020/vision:calibration",
"//y2023/vision:viewer",
- "//y2022/localizer:imu_main",
+ "//y2023/vision:aprilrobotics",
"//y2022/localizer:localizer_main",
"//aos/network:web_proxy_main",
"//aos/events/logging:log_cat",
@@ -48,6 +48,7 @@
"//aos/network:timestamp_fbs",
"//frc971/input:robot_state_fbs",
"//frc971/vision:vision_fbs",
+ "//frc971/vision:target_map_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
@@ -73,6 +74,7 @@
"//aos/network:remote_message_fbs",
"//y2022/localizer:localizer_output_fbs",
"//frc971/vision:calibration_fbs",
+ "//frc971/vision:target_map_fbs",
"//frc971/vision:vision_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
@@ -102,6 +104,7 @@
"//y2022/localizer:localizer_status_fbs",
"//y2022/localizer:localizer_output_fbs",
"//y2022/localizer:localizer_visualization_fbs",
+ "//frc971/vision:target_map_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
@@ -121,6 +124,7 @@
"//aos/network:remote_message_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:vision_fbs",
+ "//frc971/vision:target_map_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
diff --git a/y2023/rockpi/BUILD b/y2023/rockpi/BUILD
new file mode 100644
index 0000000..91e8729
--- /dev/null
+++ b/y2023/rockpi/BUILD
@@ -0,0 +1,11 @@
+cc_binary(
+ name = "imu_main",
+ srcs = ["imu_main.cc"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/imu_reader:imu",
+ ],
+)
diff --git a/y2023/rockpi/imu_main.cc b/y2023/rockpi/imu_main.cc
new file mode 100644
index 0000000..ac0c141
--- /dev/null
+++ b/y2023/rockpi/imu_main.cc
@@ -0,0 +1,24 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/realtime.h"
+#include "frc971/imu_reader/imu.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+ // TODO(austin): Set the ratio...
+ frc971::imu::Imu imu(&event_loop, 1.0);
+
+ event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({0}));
+ event_loop.SetRuntimeRealtimePriority(55);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index cafdfb7..2ddb735 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -88,6 +88,7 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//y2023:__subpackages__"],
deps = [
+ ":aprilrobotics_lib",
":calibration_data",
"//aos:init",
"//aos/events:simulated_event_loop",
@@ -99,3 +100,39 @@
"//third_party:opencv",
],
)
+
+cc_library(
+ name = "aprilrobotics_lib",
+ srcs = [
+ "aprilrobotics.cc",
+ "aprilrobotics.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2023:__subpackages__"],
+ deps = [
+ ":calibration_data",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:charuco_lib",
+ "//frc971/vision:target_map_fbs",
+ "//frc971/vision:target_mapper",
+ "//frc971/vision:vision_fbs",
+ "//third_party:opencv",
+ "//third_party/apriltag",
+ ],
+)
+
+cc_binary(
+ name = "aprilrobotics",
+ srcs = [
+ "aprilrobotics_main.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2023:__subpackages__"],
+ deps = [
+ ":aprilrobotics_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
new file mode 100644
index 0000000..fc9d799
--- /dev/null
+++ b/y2023/vision/aprilrobotics.cc
@@ -0,0 +1,170 @@
+#include "y2023/vision/aprilrobotics.h"
+
+DEFINE_bool(
+ debug, false,
+ "If true, dump a ton of debug and crash on the first valid detection.");
+
+DEFINE_int32(team_number, 971,
+ "Use the calibration for a node with this team number");
+namespace y2023 {
+namespace vision {
+
+AprilRoboticsDetector::AprilRoboticsDetector(aos::EventLoop *event_loop,
+ std::string_view channel_name)
+ : calibration_data_(CalibrationData()),
+ ftrace_(),
+ image_callback_(event_loop, channel_name,
+ [&](cv::Mat image_color_mat,
+ const aos::monotonic_clock::time_point eof) {
+ HandleImage(image_color_mat, eof);
+ }),
+ target_map_sender_(
+ event_loop->MakeSender<frc971::vision::TargetMap>("/camera")) {
+ tag_family_ = tag16h5_create();
+ tag_detector_ = apriltag_detector_create();
+
+ apriltag_detector_add_family_bits(tag_detector_, tag_family_, 1);
+ tag_detector_->nthreads = 6;
+ tag_detector_->wp = workerpool_create(tag_detector_->nthreads);
+ tag_detector_->qtp.min_white_black_diff = 5;
+ tag_detector_->debug = FLAGS_debug;
+
+ std::string hostname = aos::network::GetHostname();
+
+ // Check team string is valid
+ std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(hostname);
+ std::optional<uint16_t> team_number =
+ aos::network::team_number_internal::ParsePiTeamNumber(hostname);
+ CHECK(pi_number) << "Unable to parse pi number from '" << hostname << "'";
+ CHECK(team_number);
+
+ calibration_ = FindCameraCalibration(&calibration_data_.message(),
+ "pi" + std::to_string(*pi_number));
+ intrinsics_ = CameraIntrinsics(calibration_);
+ camera_distortion_coeffs_ = CameraDistCoeffs(calibration_);
+
+ image_callback_.set_format(frc971::vision::ImageCallback::Format::GRAYSCALE);
+}
+
+AprilRoboticsDetector::~AprilRoboticsDetector() {
+ apriltag_detector_destroy(tag_detector_);
+ free(tag_family_);
+}
+
+void AprilRoboticsDetector::SetWorkerpoolAffinities() {
+ for (int i = 0; i < tag_detector_->wp->nthreads; i++) {
+ cpu_set_t affinity;
+ CPU_ZERO(&affinity);
+ CPU_SET(i, &affinity);
+ pthread_setaffinity_np(tag_detector_->wp->threads[i], sizeof(affinity),
+ &affinity);
+ struct sched_param param;
+ param.sched_priority = 20;
+ int res = pthread_setschedparam(tag_detector_->wp->threads[i], SCHED_FIFO,
+ ¶m);
+ PCHECK(res == 0) << "Failed to set priority of threadpool threads";
+ }
+}
+
+void AprilRoboticsDetector::HandleImage(cv::Mat image_color_mat,
+ aos::monotonic_clock::time_point eof) {
+ std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> detections =
+ DetectTags(image_color_mat);
+
+ auto builder = target_map_sender_.MakeBuilder();
+ std::vector<flatbuffers::Offset<frc971::vision::TargetPoseFbs>> target_poses;
+ for (const auto &[detection, pose] : detections) {
+ target_poses.emplace_back(
+ BuildTargetPose(pose, detection.id, builder.fbb()));
+ }
+ const auto target_poses_offset = builder.fbb()->CreateVector(target_poses);
+ auto target_map_builder = builder.MakeBuilder<frc971::vision::TargetMap>();
+ target_map_builder.add_target_poses(target_poses_offset);
+ target_map_builder.add_monotonic_timestamp_ns(eof.time_since_epoch().count());
+ builder.CheckOk(builder.Send(target_map_builder.Finish()));
+}
+
+flatbuffers::Offset<frc971::vision::TargetPoseFbs>
+AprilRoboticsDetector::BuildTargetPose(
+ const apriltag_pose_t &pose,
+ frc971::vision::TargetMapper::TargetId target_id,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ const auto T =
+ Eigen::Translation3d(pose.t->data[0], pose.t->data[1], pose.t->data[2]);
+ const auto rpy = frc971::vision::PoseUtils::RotationMatrixToEulerAngles(
+ Eigen::Matrix3d(pose.R->data));
+ return frc971::vision::CreateTargetPoseFbs(*fbb, target_id, T.x(), T.y(),
+ T.z(), rpy(0), rpy(1), rpy(2));
+}
+
+std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>>
+AprilRoboticsDetector::DetectTags(cv::Mat image) {
+ const aos::monotonic_clock::time_point start_time =
+ aos::monotonic_clock::now();
+
+ image_u8_t im = {
+ .width = image.cols,
+ .height = image.rows,
+ .stride = image.cols,
+ .buf = image.data,
+ };
+
+ ftrace_.FormatMessage("Starting detect\n");
+ zarray_t *detections = apriltag_detector_detect(tag_detector_, &im);
+ ftrace_.FormatMessage("Done detecting\n");
+
+ std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> results;
+
+ for (int i = 0; i < zarray_size(detections); i++) {
+ apriltag_detection_t *det;
+ zarray_get(detections, i, &det);
+
+ if (det->decision_margin > 30) {
+ VLOG(1) << "Found tag number " << det->id << " hamming: " << det->hamming
+ << " margin: " << det->decision_margin;
+
+ const aos::monotonic_clock::time_point before_pose_estimation =
+ aos::monotonic_clock::now();
+ // First create an apriltag_detection_info_t struct using your known
+ // parameters.
+ apriltag_detection_info_t info;
+ info.det = det;
+ info.tagsize = 0.1524;
+ info.fx = intrinsics_.at<double>(0, 0);
+ info.fy = intrinsics_.at<double>(1, 1);
+ info.cx = intrinsics_.at<double>(0, 2);
+ info.cy = intrinsics_.at<double>(1, 2);
+
+ apriltag_pose_t pose;
+ double err = estimate_tag_pose(&info, &pose);
+
+ VLOG(1) << "err: " << err;
+
+ results.emplace_back(*det, pose);
+
+ const aos::monotonic_clock::time_point after_pose_estimation =
+ aos::monotonic_clock::now();
+
+ VLOG(1) << "Took "
+ << std::chrono::duration<double>(after_pose_estimation -
+ before_pose_estimation)
+ .count()
+ << " seconds for pose estimation";
+ }
+ }
+
+ apriltag_detections_destroy(detections);
+
+ const aos::monotonic_clock::time_point end_time = aos::monotonic_clock::now();
+
+ timeprofile_display(tag_detector_->tp);
+
+ VLOG(1) << "Took "
+ << std::chrono::duration<double>(end_time - start_time).count()
+ << " seconds to detect overall";
+
+ return results;
+}
+
+} // namespace vision
+} // namespace y2023
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
new file mode 100644
index 0000000..a68b1d9
--- /dev/null
+++ b/y2023/vision/aprilrobotics.h
@@ -0,0 +1,101 @@
+
+#include <string>
+
+#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/network/team_number.h"
+#include "aos/realtime.h"
+#include "frc971/vision/calibration_generated.h"
+#include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/target_map_generated.h"
+#include "frc971/vision/target_mapper.h"
+#include "frc971/vision/vision_generated.h"
+#include "opencv2/core/eigen.hpp"
+#include "opencv2/imgproc.hpp"
+#include "third_party/apriltag/apriltag.h"
+#include "third_party/apriltag/apriltag_pose.h"
+#include "third_party/apriltag/tag16h5.h"
+#include "y2023/vision/calibration_data.h"
+
+DECLARE_int32(team_number);
+
+namespace y2023 {
+namespace vision {
+
+class AprilRoboticsDetector {
+ public:
+ AprilRoboticsDetector(aos::EventLoop *event_loop,
+ std::string_view channel_name);
+
+ ~AprilRoboticsDetector();
+
+ void SetWorkerpoolAffinities();
+
+ std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> DetectTags(
+ cv::Mat image);
+
+ private:
+ void HandleImage(cv::Mat image, aos::monotonic_clock::time_point eof);
+
+ flatbuffers::Offset<frc971::vision::TargetPoseFbs> BuildTargetPose(
+ const apriltag_pose_t &pose,
+ frc971::vision::TargetMapper::TargetId target_id,
+ flatbuffers::FlatBufferBuilder *fbb);
+
+ static const frc971::vision::calibration::CameraCalibration *
+ FindCameraCalibration(
+ const frc971::vision::calibration::CalibrationData *calibration_data,
+ std::string_view node_name) {
+ for (const frc971::vision::calibration::CameraCalibration *candidate :
+ *calibration_data->camera_calibrations()) {
+ if (candidate->node_name()->string_view() != node_name) {
+ continue;
+ }
+ if (candidate->team_number() != FLAGS_team_number) {
+ continue;
+ }
+ return candidate;
+ }
+ LOG(FATAL) << ": Failed to find camera calibration for " << node_name
+ << " on " << FLAGS_team_number;
+ }
+
+ static cv::Mat CameraIntrinsics(
+ const frc971::vision::calibration::CameraCalibration
+ *camera_calibration) {
+ cv::Mat result(3, 3, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->intrinsics()->data())));
+ result.convertTo(result, CV_64F);
+ CHECK_EQ(result.total(), camera_calibration->intrinsics()->size());
+
+ return result;
+ }
+
+ static cv::Mat CameraDistCoeffs(
+ const frc971::vision::calibration::CameraCalibration
+ *camera_calibration) {
+ const cv::Mat result(5, 1, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->dist_coeffs()->data())));
+ CHECK_EQ(result.total(), camera_calibration->dist_coeffs()->size());
+ return result;
+ }
+
+ apriltag_family_t *tag_family_;
+ apriltag_detector_t *tag_detector_;
+
+ const aos::FlatbufferSpan<frc971::vision::calibration::CalibrationData>
+ calibration_data_;
+ const frc971::vision::calibration::CameraCalibration *calibration_;
+ cv::Mat intrinsics_;
+ cv::Mat camera_distortion_coeffs_;
+
+ aos::Ftrace ftrace_;
+
+ frc971::vision::ImageCallback image_callback_;
+ aos::Sender<frc971::vision::TargetMap> target_map_sender_;
+};
+
+} // namespace vision
+} // namespace y2023
diff --git a/y2023/vision/aprilrobotics_main.cc b/y2023/vision/aprilrobotics_main.cc
new file mode 100644
index 0000000..67b853f
--- /dev/null
+++ b/y2023/vision/aprilrobotics_main.cc
@@ -0,0 +1,34 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2023/vision/aprilrobotics.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+namespace y2023::vision {
+void AprilViewerMain() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ AprilRoboticsDetector detector(&event_loop, "/camera");
+
+ detector.SetWorkerpoolAffinities();
+
+ event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({5}));
+
+ struct sched_param param;
+ param.sched_priority = 21;
+ PCHECK(sched_setscheduler(0, SCHED_FIFO, ¶m) == 0);
+
+ event_loop.Run();
+}
+
+} // namespace y2023::vision
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ y2023::vision::AprilViewerMain();
+
+ return 0;
+}
diff --git a/y2023/vision/camera_reader.cc b/y2023/vision/camera_reader.cc
index a69caf3..5949048 100644
--- a/y2023/vision/camera_reader.cc
+++ b/y2023/vision/camera_reader.cc
@@ -62,8 +62,8 @@
media_device->FindEntity("rkisp1_resizer_selfpath");
rkisp1_resizer_selfpath->pads(0)->SetSubdevFormat(width, height,
MEDIA_BUS_FMT_YUYV8_2X8);
- rkisp1_resizer_selfpath->pads(1)->SetSubdevFormat(width, height,
- MEDIA_BUS_FMT_YUYV8_2X8);
+ rkisp1_resizer_selfpath->pads(1)->SetSubdevFormat(
+ width * 2 / 3, height * 2 / 3, MEDIA_BUS_FMT_YUYV8_2X8);
rkisp1_resizer_selfpath->pads(0)->SetSubdevCrop(width, height);
Entity *rkisp1_resizer_mainpath =
@@ -79,7 +79,7 @@
rkisp1_mainpath->SetFormat(width / 2, height / 2, V4L2_PIX_FMT_YUV422P);
Entity *rkisp1_selfpath = media_device->FindEntity("rkisp1_selfpath");
- rkisp1_selfpath->SetFormat(width, height, V4L2_PIX_FMT_YUYV);
+ rkisp1_selfpath->SetFormat(width * 2 / 3, height * 2 / 3, V4L2_PIX_FMT_YUYV);
media_device->Enable(
media_device->FindLink(camera_device_string, 0, "rkisp1_csi", 0));
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index d4c8c50..38465c9 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -12,14 +12,12 @@
#include "opencv2/highgui.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc.hpp"
+#include "y2023/vision/aprilrobotics.h"
#include "y2023/vision/calibration_data.h"
DEFINE_string(json_path, "target_map.json",
"Specify path for json with initial pose guesses.");
-DEFINE_int32(team_number, 7971,
- "Use the calibration for a node with this team number");
-
-DECLARE_string(image_channel);
+DECLARE_int32(team_number);
namespace y2023 {
namespace vision {
@@ -27,6 +25,7 @@
using frc971::vision::DataAdapter;
using frc971::vision::ImageCallback;
using frc971::vision::PoseUtils;
+using frc971::vision::TargetMap;
using frc971::vision::TargetMapper;
namespace calibration = frc971::vision::calibration;
@@ -40,21 +39,19 @@
// 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,
+void HandleAprilTag(const TargetMap &map,
+ aos::distributed_clock::time_point pi_distributed_time,
std::vector<DataAdapter::TimestampedDetection>
*timestamped_target_detections,
Eigen::Affine3d extrinsics) {
- for (size_t tag = 0; tag < april_ids.size(); tag++) {
+ for (const auto *target_pose : *map.target_poses()) {
Eigen::Translation3d T_camera_target = Eigen::Translation3d(
- tvecs_eigen[tag][0], tvecs_eigen[tag][1], tvecs_eigen[tag][2]);
- Eigen::AngleAxisd r_angle = Eigen::AngleAxisd(
- rvecs_eigen[tag].norm(), rvecs_eigen[tag] / rvecs_eigen[tag].norm());
- CHECK(rvecs_eigen[tag].norm() != 0) << "rvecs norm = 0; divide by 0";
+ target_pose->x(), target_pose->y(), target_pose->z());
+ Eigen::Quaterniond R_camera_target =
+ PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d(
+ target_pose->roll(), target_pose->pitch(), target_pose->yaw()));
- Eigen::Affine3d H_camcv_target = T_camera_target * r_angle;
+ Eigen::Affine3d H_camcv_target = T_camera_target * R_camera_target;
// With X, Y, Z being robot axes and x, y, z being camera axes,
// x = -Y, y = -Z, z = X
static const Eigen::Affine3d H_camcv_camrob =
@@ -69,12 +66,15 @@
PoseUtils::Affine3dToPose3d(H_camrob_target);
double distance_from_camera = target_pose_camera.p.norm();
+ CHECK(map.has_monotonic_timestamp_ns())
+ << "Need detection timestamps for mapping";
+
timestamped_target_detections->emplace_back(
DataAdapter::TimestampedDetection{
.time = pi_distributed_time,
.H_robot_target = H_robot_target,
.distance_from_camera = distance_from_camera,
- .id = april_ids[tag][0]});
+ .id = static_cast<TargetMapper::TargetId>(target_pose->id())});
}
}
@@ -115,49 +115,33 @@
// Get images from pi and pass apriltag positions to HandleAprilTag()
void HandlePiCaptures(
- int pi_number, aos::EventLoop *pi_event_loop,
- aos::logger::LogReader *reader,
+ aos::EventLoop *pi_event_loop, aos::logger::LogReader *reader,
std::vector<DataAdapter::TimestampedDetection>
*timestamped_target_detections,
- std::vector<std::unique_ptr<CharucoExtractor>> *charuco_extractors,
- std::vector<std::unique_ptr<ImageCallback>> *image_callbacks) {
+ std::vector<std::unique_ptr<AprilRoboticsDetector>> *detectors) {
const aos::FlatbufferSpan<calibration::CalibrationData> calibration_data(
CalibrationData());
- const calibration::CameraCalibration *calibration = FindCameraCalibration(
- &calibration_data.message(), "pi" + std::to_string(pi_number));
+
+ const auto node_name = pi_event_loop->node()->name()->string_view();
+ const calibration::CameraCalibration *calibration =
+ FindCameraCalibration(&calibration_data.message(), node_name);
const auto extrinsics = CameraExtrinsics(calibration);
// TODO(milind): change to /camera once we log at full frequency
static constexpr std::string_view kImageChannel = "/camera/decimated";
- charuco_extractors->emplace_back(std::make_unique<CharucoExtractor>(
- pi_event_loop,
- "pi-" + std::to_string(FLAGS_team_number) + "-" +
- std::to_string(pi_number),
- frc971::vision::TargetType::kAprilTag, kImageChannel,
- [=](cv::Mat /*rgb_image*/, aos::monotonic_clock::time_point eof,
- std::vector<cv::Vec4i> april_ids,
- std::vector<std::vector<cv::Point2f>> /*april_corners*/, bool valid,
- std::vector<Eigen::Vector3d> rvecs_eigen,
- std::vector<Eigen::Vector3d> tvecs_eigen) {
- aos::distributed_clock::time_point pi_distributed_time =
- reader->event_loop_factory()
- ->GetNodeEventLoopFactory(pi_event_loop->node())
- ->ToDistributedClock(eof);
+ detectors->emplace_back(
+ std::make_unique<AprilRoboticsDetector>(pi_event_loop, kImageChannel));
- if (valid) {
- HandleAprilTag(pi_distributed_time, april_ids, rvecs_eigen,
- tvecs_eigen, timestamped_target_detections,
- extrinsics);
- }
- }));
+ pi_event_loop->MakeWatcher("/camera", [=](const TargetMap &map) {
+ aos::distributed_clock::time_point pi_distributed_time =
+ reader->event_loop_factory()
+ ->GetNodeEventLoopFactory(pi_event_loop->node())
+ ->ToDistributedClock(aos::monotonic_clock::time_point(
+ aos::monotonic_clock::duration(map.monotonic_timestamp_ns())));
- image_callbacks->emplace_back(std::make_unique<ImageCallback>(
- pi_event_loop, kImageChannel,
- [&, charuco_extractor =
- charuco_extractors->at(charuco_extractors->size() - 1).get()](
- cv::Mat rgb_image, const aos::monotonic_clock::time_point eof) {
- charuco_extractor->HandleImage(rgb_image, eof);
- }));
+ HandleAprilTag(map, pi_distributed_time, timestamped_target_detections,
+ extrinsics);
+ });
}
void MappingMain(int argc, char *argv[]) {
@@ -170,40 +154,35 @@
aos::logger::LogReader reader(aos::logger::SortParts(unsorted_logfiles));
reader.Register();
- std::vector<std::unique_ptr<CharucoExtractor>> charuco_extractors;
- std::vector<std::unique_ptr<ImageCallback>> image_callbacks;
+ std::vector<std::unique_ptr<AprilRoboticsDetector>> detectors;
const aos::Node *pi1 =
aos::configuration::GetNode(reader.configuration(), "pi1");
std::unique_ptr<aos::EventLoop> pi1_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi1", pi1);
- HandlePiCaptures(1, pi1_event_loop.get(), &reader,
- ×tamped_target_detections, &charuco_extractors,
- &image_callbacks);
+ HandlePiCaptures(pi1_event_loop.get(), &reader,
+ ×tamped_target_detections, &detectors);
const aos::Node *pi2 =
aos::configuration::GetNode(reader.configuration(), "pi2");
std::unique_ptr<aos::EventLoop> pi2_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi2", pi2);
- HandlePiCaptures(2, pi2_event_loop.get(), &reader,
- ×tamped_target_detections, &charuco_extractors,
- &image_callbacks);
+ HandlePiCaptures(pi2_event_loop.get(), &reader,
+ ×tamped_target_detections, &detectors);
const aos::Node *pi3 =
aos::configuration::GetNode(reader.configuration(), "pi3");
std::unique_ptr<aos::EventLoop> pi3_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi3", pi3);
- HandlePiCaptures(3, pi3_event_loop.get(), &reader,
- ×tamped_target_detections, &charuco_extractors,
- &image_callbacks);
+ HandlePiCaptures(pi3_event_loop.get(), &reader,
+ ×tamped_target_detections, &detectors);
const aos::Node *pi4 =
aos::configuration::GetNode(reader.configuration(), "pi4");
std::unique_ptr<aos::EventLoop> pi4_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi4", pi4);
- HandlePiCaptures(4, pi4_event_loop.get(), &reader,
- ×tamped_target_detections, &charuco_extractors,
- &image_callbacks);
+ HandlePiCaptures(pi4_event_loop.get(), &reader,
+ ×tamped_target_detections, &detectors);
reader.event_loop_factory()->Run();
@@ -211,15 +190,11 @@
DataAdapter::MatchTargetDetections(timestamped_target_detections);
frc971::vision::TargetMapper mapper(FLAGS_json_path, target_constraints);
- mapper.Solve("rapid_react");
+ mapper.Solve("charged_up");
- // Pointers need to be deleted to destruct all fetchers
- for (auto &charuco_extractor_ptr : charuco_extractors) {
- charuco_extractor_ptr.reset();
- }
-
- for (auto &image_callback_ptr : image_callbacks) {
- image_callback_ptr.reset();
+ // Clean up all the pointers
+ for (auto &detector_ptr : detectors) {
+ detector_ptr.reset();
}
}
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index fd3e97f..1b31e91 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -167,7 +167,63 @@
"max_size": 4200000,
"num_readers": 4,
"read_method": "PIN",
- "num_senders": 18
+ "num_senders": 18,
+ "logger": "NOT_LOGGED"
+ },
+ {
+ "name": "/pi{{ NUM }}/camera/decimated",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "pi{{ NUM }}",
+ "frequency": 2,
+ "max_size": 4200000,
+ "num_senders": 2
+ },
+ {
+ "name": "/pi{{ NUM }}/camera",
+ "type": "frc971.vision.TargetMap",
+ "source_node": "pi{{ NUM }}",
+ "frequency": 30,
+ "num_senders": 2,
+ "max_size": 40000,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu",
+ "logger"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 4,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "pi{{ NUM }}"
+ ],
+ "time_to_live": 5000000
+ },
+ {
+ "name": "logger",
+ "priority": 4,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "pi{{ NUM }}"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/pi{{ NUM }}/aos/remote_timestamps/imu/pi{{ NUM }}/camera/frc971-vision-TargetMap",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 80,
+ "source_node": "pi{{ NUM }}",
+ "max_size": 208
+ },
+ {
+ "name": "/pi{{ NUM }}/aos/remote_timestamps/logger/pi{{ NUM }}/camera/frc971-vision-TargetMap",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 80,
+ "source_node": "pi{{ NUM }}",
+ "max_size": 208
},
{
"name": "/logger/aos",