Merge "Add yaw corrections to plot, normalize angles in status message"
diff --git a/frc971/orin/BUILD b/frc971/orin/BUILD
index 8c4e9ac..e483d65 100644
--- a/frc971/orin/BUILD
+++ b/frc971/orin/BUILD
@@ -176,3 +176,17 @@
"@com_google_absl//absl/strings",
],
)
+
+cc_binary(
+ name = "argus_monitor",
+ srcs = [
+ "argus_monitor.cc",
+ ],
+ target_compatible_with = ["@platforms//cpu:arm64"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:aos_cli_utils",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
diff --git a/frc971/orin/argus_camera.cc b/frc971/orin/argus_camera.cc
index 811efdd..f886fab 100644
--- a/frc971/orin/argus_camera.cc
+++ b/frc971/orin/argus_camera.cc
@@ -575,13 +575,6 @@
ArgusCamera::MappedBuffer buffer = camera.NextImageBlocking();
if (buffer.nvbuf_surf() == nullptr) {
- // TODO(austin): Control-C isn't working for some reason, debug it...
- // We're restarting nvargus-daemon here because if we exit like this its
- // likely that nvargus-daemon has run into an error that it can't
- // recover from. Which means even if this program restarts it can't get
- // new camera images.
- CHECK_EQ(std::system("sudo systemctl restart nvargus-daemon.service"),
- 0);
event_loop.Exit();
return;
}
diff --git a/frc971/orin/argus_monitor.cc b/frc971/orin/argus_monitor.cc
new file mode 100644
index 0000000..8f129b2
--- /dev/null
+++ b/frc971/orin/argus_monitor.cc
@@ -0,0 +1,107 @@
+#include <unistd.h>
+
+#include <iostream>
+
+#include "gflags/gflags.h"
+
+#include "aos/aos_cli_utils.h"
+#include "aos/configuration.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/realtime.h"
+
+DEFINE_int32(priority, -1, "If set, the RT priority to run at.");
+DEFINE_double(max_jitter, 5.00,
+ "The max time in seconds between messages before considering the "
+ "camera processes dead.");
+DEFINE_double(grace_period, 10.00,
+ "The grace period at startup before enforcing that messages must "
+ "flow from the camera processes.");
+
+namespace aos {
+
+class State {
+ public:
+ State(aos::EventLoop *event_loop, const Channel *channel)
+ : channel_(channel),
+ channel_name_(aos::configuration::StrippedChannelToString(channel_)) {
+ LOG(INFO) << "Watching for healthy message sends on " << channel_name_;
+
+ event_loop->MakeRawNoArgWatcher(
+ channel_,
+ [this](const aos::Context &context) { HandleMessage(context); });
+
+ timer_handle_ = event_loop->AddTimer(
+ [this, event_loop]() { RunHealthCheck(event_loop); });
+ timer_handle_->set_name("jitter");
+ event_loop->OnRun([this, event_loop]() {
+ timer_handle_->Schedule(
+ event_loop->monotonic_now() +
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ std::chrono::duration<double>(FLAGS_grace_period)),
+ std::chrono::milliseconds(1000));
+ });
+ }
+
+ void HandleMessage(const aos::Context &context) {
+ last_time_ = context.monotonic_event_time;
+ }
+
+ void RunHealthCheck(aos::EventLoop *event_loop) {
+ if (last_time_ + std::chrono::duration_cast<std::chrono::nanoseconds>(
+ std::chrono::duration<double>(FLAGS_max_jitter)) <
+ event_loop->monotonic_now()) {
+ // Restart camera services
+ LOG(INFO) << "Restarting camera services";
+ CHECK_EQ(std::system("aos_starter stop argus_camera0"), 0);
+ CHECK_EQ(std::system("aos_starter stop argus_camera1"), 0);
+ CHECK_EQ(std::system("sudo systemctl restart nvargus-daemon.service"), 0);
+ CHECK_EQ(std::system("aos_starter start argus_camera0"), 0);
+ CHECK_EQ(std::system("aos_starter start argus_camera1"), 0);
+
+ std::exit(0);
+ return;
+ }
+ }
+
+ private:
+ const Channel *channel_;
+
+ std::string channel_name_;
+
+ aos::monotonic_clock::time_point last_time_ = aos::monotonic_clock::min_time;
+
+ aos::TimerHandler *timer_handle_;
+};
+
+} // namespace aos
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::CliUtilInfo cli_info;
+ if (cli_info.Initialize(
+ &argc, &argv,
+ [&cli_info](const aos::Channel *channel) {
+ return aos::configuration::ChannelIsReadableOnNode(
+ channel, cli_info.event_loop->node());
+ },
+ "channel is readeable on node", true)) {
+ return 0;
+ }
+
+ std::vector<std::unique_ptr<aos::State>> states;
+
+ for (const aos::Channel *channel : cli_info.found_channels) {
+ states.emplace_back(
+ std::make_unique<aos::State>(&(cli_info.event_loop.value()), channel));
+ }
+
+ if (FLAGS_priority > 0) {
+ cli_info.event_loop->SetRuntimeRealtimePriority(FLAGS_priority);
+ }
+
+ cli_info.event_loop->Run();
+
+ return 0;
+}
diff --git a/y2024/BUILD b/y2024/BUILD
index 6687dac..2727ac7 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -56,6 +56,7 @@
"//aos/util:foxglove_websocket",
"//frc971/image_streamer:image_streamer",
"//frc971/orin:hardware_monitor",
+ "//frc971/orin:argus_monitor",
"//frc971/vision:intrinsics_calibration",
"//aos/util:filesystem_monitor",
"//y2024/vision:viewer",
diff --git a/y2024/autonomous/autonomous_actor.cc b/y2024/autonomous/autonomous_actor.cc
index bde0d4b..0504cf3 100644
--- a/y2024/autonomous/autonomous_actor.cc
+++ b/y2024/autonomous/autonomous_actor.cc
@@ -248,7 +248,6 @@
Aim();
if (!WaitForPreloaded()) return;
- std::this_thread::sleep_for(chrono::milliseconds(500));
Shoot();
AOS_LOG(
@@ -330,13 +329,13 @@
if (!splines[4].WaitForPlan()) return;
splines[4].Start();
- if (!splines[4].WaitForSplineDistanceRemaining(0.05)) return;
+ if (!splines[4].WaitForSplineDistanceRemaining(0.01)) return;
AOS_LOG(
INFO, "Done with spline %lfs\n",
aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
- std::this_thread::sleep_for(chrono::milliseconds(300));
+ std::this_thread::sleep_for(chrono::milliseconds(500));
AOS_LOG(
INFO, "Shooting last note! %lfs\n",
diff --git a/y2024/autonomous/splines/five_note.0.json b/y2024/autonomous/splines/five_note.0.json
index ba8ac2c..e21be61 100644
--- a/y2024/autonomous/splines/five_note.0.json
+++ b/y2024/autonomous/splines/five_note.0.json
@@ -1,20 +1,20 @@
{
"spline_count": 1,
"spline_x": [
- -6.96,
- -6.616444808957066,
- -6.440474125870341,
- -6.410863453750628,
- -6.106827763281826,
- -5.963275576067123
+ -6.837790264225116,
+ -6.431035594540573,
+ -6.2550649114538475,
+ -6.681741927563226,
+ -6.377706237094424,
+ -5.921735095811581
],
"spline_y": [
- 0.474,
- 0.474,
- 0.3795291901917933,
- 0.4482348806874367,
- 0.31444590142149437,
- 0.26227873892724274
+ 1.3535788089450442,
+ 1.352246384830838,
+ 1.2577755750226314,
+ 0.6390756151537094,
+ 0.505286635887767,
+ 0.26109745109761073
],
"constraints": [
{
diff --git a/y2024/autonomous/splines/five_note.1.json b/y2024/autonomous/splines/five_note.1.json
index d65db75..02c7944 100644
--- a/y2024/autonomous/splines/five_note.1.json
+++ b/y2024/autonomous/splines/five_note.1.json
@@ -1,20 +1,20 @@
{
"spline_count": 1,
"spline_x": [
- -5.963275576067123,
- -6.043899374861988,
- -6.14218730470054,
- -6.299979097775238,
- -6.333362095101602,
- -6.385753156016853
+ -5.921735095811581,
+ -5.997356157615366,
+ -6.1394966638452235,
+ -6.356632365068064,
+ -6.390015362394428,
+ -6.442406423309679
],
"spline_y": [
- 0.26227873892724274,
- 0.2915775945855339,
- 0.27680408013743,
- 0.19285548109740952,
- 0.09423634731960748,
- -0.012742650262493882
+ 0.26109745109761073,
+ 0.30159528914692224,
+ 0.3303773156538068,
+ 0.24034081023232307,
+ 0.14172167645452102,
+ 0.03474267887241966
],
"constraints": [
{
diff --git a/y2024/autonomous/splines/five_note.2.json b/y2024/autonomous/splines/five_note.2.json
index e67e8f0..442ce48 100644
--- a/y2024/autonomous/splines/five_note.2.json
+++ b/y2024/autonomous/splines/five_note.2.json
@@ -1,17 +1,17 @@
{
"spline_count": 1,
"spline_x": [
- -6.385753156016853,
- -6.038148386644435,
- -5.498099209905131,
+ -6.442406423309679,
+ -6.09480165393726,
+ -5.554752477197956,
-5.500010615292536,
-5.505673744840998,
-5.450725524128315
],
"spline_y": [
- -0.012742650262493882,
- 0.6970427431345343,
- 1.0584941253106013,
+ 0.03474267887241966,
+ 0.7445280722694474,
+ 1.1059794544455144,
1.0226571920273129,
1.4628922679287215,
2.245294280677646
@@ -31,7 +31,7 @@
},
{
"constraint_type": "VELOCITY",
- "value": 0.4,
+ "value": 0.6,
"start_distance": 1.5,
"end_distance": 10.0
}
diff --git a/y2024/autonomous/splines/five_note.3.json b/y2024/autonomous/splines/five_note.3.json
index 148c79d..f4239d6 100644
--- a/y2024/autonomous/splines/five_note.3.json
+++ b/y2024/autonomous/splines/five_note.3.json
@@ -4,17 +4,17 @@
-5.450725524128315,
-5.4125513915961285,
-3.9062446310670764,
- -2.1352203510523458,
- -1.1691608738981074,
- -0.19389194554058786
+ -2.390277169363972,
+ -1.4242176922097336,
+ -0.44894876385221405
],
"spline_y": [
2.245294280677646,
2.7888517924722676,
3.9025565085593934,
- 2.6507161302136577,
- 2.1538810971324764,
- 1.7882868785936012
+ 2.9014194169393095,
+ 2.4045843838581282,
+ 2.0389901653192535
],
"constraints": [
{
@@ -27,7 +27,7 @@
},
{
"constraint_type": "VOLTAGE",
- "value": 10.0
+ "value": 8.0
},
{
"constraint_type": "VELOCITY",
@@ -36,4 +36,4 @@
"end_distance": 100.0
}
]
-}
+}
\ No newline at end of file
diff --git a/y2024/autonomous/splines/five_note.4.json b/y2024/autonomous/splines/five_note.4.json
index 93421e2..1c210a5 100644
--- a/y2024/autonomous/splines/five_note.4.json
+++ b/y2024/autonomous/splines/five_note.4.json
@@ -1,17 +1,17 @@
{
"spline_count": 1,
"spline_x": [
- -0.19389194554058786,
- -1.169167650726454,
- -2.1352271278806922,
+ -0.44894876385221405,
+ -1.42422446903808,
+ -2.3902839461923184,
-3.6552329072641223,
-4.682377123355169,
-5.357290416833793
],
"spline_y": [
- 1.7882868785936012,
- 2.153883637528455,
- 2.650718670609636,
+ 2.0389901653192535,
+ 2.4045869242541067,
+ 2.901421957335288,
2.3043000750345737,
1.9165787798578524,
1.7991159872216307
@@ -36,4 +36,4 @@
"end_distance": 100.0
}
]
-}
+}
\ No newline at end of file
diff --git a/y2024/y2024_imu.json b/y2024/y2024_imu.json
index 2680856..aa8b042 100644
--- a/y2024/y2024_imu.json
+++ b/y2024/y2024_imu.json
@@ -503,6 +503,20 @@
]
},
{
+ "name": "argus_monitor_imu",
+ "executable_name": "argus_monitor",
+ "args": [
+ "/imu/camera0",
+ "frc971.vision.TargetMap",
+ "/imu/camera1",
+ "frc971.vision.TargetMap",
+ ],
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
"name": "argus_camera0",
"executable_name": "argus_camera",
"args": [
diff --git a/y2024/y2024_orin1.json b/y2024/y2024_orin1.json
index 6072da8..e9c8093 100644
--- a/y2024/y2024_orin1.json
+++ b/y2024/y2024_orin1.json
@@ -27,7 +27,7 @@
"source_node": "orin1",
"frequency": 50,
"num_senders": 20,
- "max_size": 2000
+ "max_size": 3000
},
{
"name": "/orin1/aos",
@@ -371,6 +371,20 @@
]
},
{
+ "name": "argus_monitor_orin1",
+ "executable_name": "argus_monitor",
+ "args": [
+ "/orin1/camera0",
+ "frc971.vision.TargetMap",
+ "/orin1/camera1",
+ "frc971.vision.TargetMap",
+ ],
+ "user": "pi",
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
"name": "argus_camera0",
"executable_name": "argus_camera",
"args": [