Merge "Import foxglove schemas repo"
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/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/target_map.fbs b/frc971/vision/target_map.fbs
index 38bab6d..37260a6 100644
--- a/frc971/vision/target_map.fbs
+++ b/frc971/vision/target_map.fbs
@@ -26,4 +26,6 @@
// Unique name of the field
field_name:string (id: 1);
-}
\ No newline at end of file
+}
+
+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/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 6ef0657..55390e4 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -97,3 +97,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..3bf0adf
--- /dev/null
+++ b/y2023/vision/aprilrobotics.cc
@@ -0,0 +1,169 @@
+#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);
+ }),
+ 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) {
+ 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);
+ 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..6aafd75
--- /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);
+
+ 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/y2023_pi_template.json b/y2023/y2023_pi_template.json
index fd3e97f..4c14031 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": 80,
+ "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",