A few more updates to handle pi/orin naming
Change-Id: I82006584955fe291e8fea43c19b1b39997cec1f2
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/aos/network/team_number.cc b/aos/network/team_number.cc
index da6d0c5..cc81e26 100644
--- a/aos/network/team_number.cc
+++ b/aos/network/team_number.cc
@@ -123,6 +123,15 @@
void OverrideTeamNumber(uint16_t team) { override_team = team; }
+std::optional<std::string_view> ParsePiOrOrin(const std::string_view hostname) {
+ if (hostname.substr(0, 3) == "pi-") {
+ return std::string_view("pi");
+ } else if (hostname.substr(0, 5) == "orin-") {
+ return std::string_view("orin");
+ } else
+ return std::nullopt;
+}
+
std::optional<uint16_t> ParsePiOrOrinNumber(const std::string_view hostname) {
if ((hostname.substr(0, 3) != "pi-") && (hostname.substr(0, 5) != "orin-")) {
return std::nullopt;
diff --git a/aos/network/team_number.h b/aos/network/team_number.h
index 9b9029a..7b9acbb 100644
--- a/aos/network/team_number.h
+++ b/aos/network/team_number.h
@@ -31,6 +31,9 @@
// for pi-971-5 or 2 for orin-9971-2)
std::optional<uint16_t> ParsePiOrOrinNumber(const std::string_view hostname);
+// Returns whether the device is a "pi" or an "orin" based on hostname
+std::optional<std::string_view> ParsePiOrOrin(const std::string_view hostname);
+
namespace team_number_internal {
std::optional<uint16_t> ParseRoborioTeamNumber(const std::string_view hostname);
diff --git a/frc971/orin/contents/etc/systemd/system/jetson-clocks.service b/frc971/orin/contents/etc/systemd/system/jetson-clocks.service
index 76e88f4..bfc68fa 100644
--- a/frc971/orin/contents/etc/systemd/system/jetson-clocks.service
+++ b/frc971/orin/contents/etc/systemd/system/jetson-clocks.service
@@ -3,7 +3,7 @@
[Unit]
Description=Service to pin the device to max clock rate
-Requres=cpufrequtils.service
+Requires=cpufrequtils.service
After=cpufrequtils.service
[Service]
diff --git a/frc971/orin/contents/root/bin/change_hostname.sh b/frc971/orin/contents/root/bin/change_hostname.sh
index 12e1564..cd97907 100755
--- a/frc971/orin/contents/root/bin/change_hostname.sh
+++ b/frc971/orin/contents/root/bin/change_hostname.sh
@@ -22,6 +22,11 @@
echo "${HOSTNAME}" > /etc/hostname
+# If the file doesn't exist, create it and add an localhost entry
+if [[ ! -f /etc/hosts ]]; then
+ echo -e "127.0.0.1\tlocalhost" > /etc/hosts
+fi
+
# Make sure a 127.0.* entry exists to make things looking up localhost happy.
if grep '^127.0.1.1' /etc/hosts > /dev/null;
then
diff --git a/frc971/vision/calibration_accumulator.cc b/frc971/vision/calibration_accumulator.cc
index 2c0e603..9c6222b 100644
--- a/frc971/vision/calibration_accumulator.cc
+++ b/frc971/vision/calibration_accumulator.cc
@@ -143,7 +143,7 @@
Calibration::Calibration(
aos::SimulatedEventLoopFactory *event_loop_factory,
aos::EventLoop *image_event_loop, aos::EventLoop *imu_event_loop,
- std::string_view pi,
+ std::string_view hostname,
const calibration::CameraCalibration *intrinsics_calibration,
TargetType target_type, std::string_view image_channel,
CalibrationData *data)
@@ -166,10 +166,10 @@
// TODO: Need to make this work for pi or orin
image_callback_(
image_event_loop_,
- absl::StrCat(
- "/pi",
- std::to_string(aos::network::ParsePiOrOrinNumber(pi).value()),
- image_channel),
+ absl::StrCat("/", aos::network::ParsePiOrOrin(hostname).value(),
+ std::to_string(
+ aos::network::ParsePiOrOrinNumber(hostname).value()),
+ image_channel),
[this](cv::Mat rgb_image, const monotonic_clock::time_point eof) {
charuco_extractor_.HandleImage(rgb_image, eof);
}),
diff --git a/frc971/vision/calibration_accumulator.h b/frc971/vision/calibration_accumulator.h
index 6451d0c..9a6f483 100644
--- a/frc971/vision/calibration_accumulator.h
+++ b/frc971/vision/calibration_accumulator.h
@@ -107,7 +107,7 @@
public:
Calibration(aos::SimulatedEventLoopFactory *event_loop_factory,
aos::EventLoop *image_event_loop, aos::EventLoop *imu_event_loop,
- std::string_view pi,
+ std::string_view hostname,
const calibration::CameraCalibration *intrinsics_calibration,
TargetType target_type, std::string_view image_channel,
CalibrationData *data);
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index c24d02c..d006e2d 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -316,7 +316,6 @@
LOG(INFO) << "Camera matrix " << calibration_.CameraIntrinsics();
LOG(INFO) << "Distortion Coefficients " << calibration_.CameraDistCoeffs();
-
LOG(INFO) << "Connecting to channel " << image_channel_;
}
diff --git a/frc971/vision/intrinsics_calibration_lib.cc b/frc971/vision/intrinsics_calibration_lib.cc
index 49224f0..a5ffec7 100644
--- a/frc971/vision/intrinsics_calibration_lib.cc
+++ b/frc971/vision/intrinsics_calibration_lib.cc
@@ -3,11 +3,13 @@
namespace frc971::vision {
IntrinsicsCalibration::IntrinsicsCalibration(
- aos::EventLoop *event_loop, std::string_view pi, std::string_view camera_id,
- std::string_view base_intrinsics_file, bool display_undistorted,
- std::string_view calibration_folder, aos::ExitHandle *exit_handle)
- : pi_(pi),
- pi_number_(aos::network::ParsePiOrOrinNumber(pi)),
+ aos::EventLoop *event_loop, std::string_view hostname,
+ std::string_view camera_id, std::string_view base_intrinsics_file,
+ bool display_undistorted, std::string_view calibration_folder,
+ aos::ExitHandle *exit_handle)
+ : hostname_(hostname),
+ cpu_type_(aos::network::ParsePiOrOrin(hostname_)),
+ cpu_number_(aos::network::ParsePiOrOrinNumber(hostname_)),
camera_id_(camera_id),
prev_H_camera_board_(Eigen::Affine3d()),
prev_image_H_camera_board_(Eigen::Affine3d()),
@@ -25,13 +27,11 @@
HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners, valid,
rvecs_eigen, tvecs_eigen);
}),
- // TODO: Need to make this work with /pi or /orin
image_callback_(
event_loop,
- absl::StrCat(
- "/pi",
- std::to_string(aos::network::ParsePiOrOrinNumber(pi).value()),
- "/camera"),
+ absl::StrCat("/", aos::network::ParsePiOrOrin(hostname_).value(),
+ // TODO: Need to make this work with multiple cameras
+ std::to_string(cpu_number_.value()), "/camera"),
[this](cv::Mat rgb_image,
const aos::monotonic_clock::time_point eof) {
charuco_extractor_.HandleImage(rgb_image, eof);
@@ -40,8 +40,9 @@
display_undistorted_(display_undistorted),
calibration_folder_(calibration_folder),
exit_handle_(exit_handle) {
- CHECK(pi_number_) << ": Invalid pi number " << pi
- << ", failed to parse pi number";
+ LOG(INFO) << "Hostname is: " << hostname_;
+ CHECK(cpu_number_) << ": Invalid cpu number " << hostname_
+ << ", failed to parse cpu number";
std::regex re{"^[0-9][0-9]-[0-9][0-9]"};
CHECK(std::regex_match(camera_id_, re))
<< ": Invalid camera_id '" << camera_id_ << "', should be of form YY-NN";
@@ -155,11 +156,11 @@
aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
IntrinsicsCalibration::BuildCalibration(
cv::Mat camera_matrix, cv::Mat dist_coeffs,
- aos::realtime_clock::time_point realtime_now, int pi_number,
- std::string_view camera_id, uint16_t team_number) {
+ aos::realtime_clock::time_point realtime_now, std::string_view cpu_type,
+ int cpu_number, std::string_view camera_id, uint16_t team_number) {
flatbuffers::FlatBufferBuilder fbb;
flatbuffers::Offset<flatbuffers::String> name_offset =
- fbb.CreateString(absl::StrFormat("pi%d", pi_number));
+ fbb.CreateString(absl::StrFormat("%s%d", cpu_type, cpu_number));
flatbuffers::Offset<flatbuffers::String> camera_id_offset =
fbb.CreateString(camera_id);
flatbuffers::Offset<flatbuffers::Vector<float>> intrinsics_offset =
@@ -215,21 +216,21 @@
const aos::realtime_clock::time_point realtime_now =
aos::realtime_clock::now();
std::optional<uint16_t> team_number =
- aos::network::team_number_internal::ParsePiOrOrinTeamNumber(pi_);
- CHECK(team_number) << ": Invalid pi hostname " << pi_
+ aos::network::team_number_internal::ParsePiOrOrinTeamNumber(hostname_);
+ CHECK(team_number) << ": Invalid hostname " << hostname_
<< ", failed to parse team number";
aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
- camera_calibration = BuildCalibration(camera_matrix, dist_coeffs,
- realtime_now, pi_number_.value(),
- camera_id_, team_number.value());
+ camera_calibration = BuildCalibration(
+ camera_matrix, dist_coeffs, realtime_now, cpu_type_.value(),
+ cpu_number_.value(), camera_id_, team_number.value());
std::stringstream time_ss;
time_ss << realtime_now;
const std::string calibration_filename =
calibration_folder_ +
- absl::StrFormat("/calibration_pi-%d-%d_cam-%s_%s.json",
- team_number.value(), pi_number_.value(), camera_id_,
- time_ss.str());
+ absl::StrFormat("/calibration_%s-%d-%d_cam-%s_%s.json",
+ cpu_type_.value(), team_number.value(),
+ cpu_number_.value(), camera_id_, time_ss.str());
LOG(INFO) << calibration_filename << " -> "
<< aos::FlatbufferToJson(camera_calibration,
diff --git a/frc971/vision/intrinsics_calibration_lib.h b/frc971/vision/intrinsics_calibration_lib.h
index 2da33f1..3076ed9 100644
--- a/frc971/vision/intrinsics_calibration_lib.h
+++ b/frc971/vision/intrinsics_calibration_lib.h
@@ -21,7 +21,7 @@
class IntrinsicsCalibration {
public:
- IntrinsicsCalibration(aos::EventLoop *event_loop, std::string_view pi,
+ IntrinsicsCalibration(aos::EventLoop *event_loop, std::string_view hostname,
std::string_view camera_id,
std::string_view base_intrinsics_file,
bool display_undistorted,
@@ -39,7 +39,8 @@
static aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
BuildCalibration(cv::Mat camera_matrix, cv::Mat dist_coeffs,
- aos::realtime_clock::time_point realtime_now, int pi_number,
+ aos::realtime_clock::time_point realtime_now,
+ std::string_view cpu_type, int cpu_number,
std::string_view camera_id, uint16_t team_number);
private:
@@ -49,8 +50,9 @@
static constexpr double kFrameDeltaRLimit = M_PI / 60;
static constexpr double kFrameDeltaTLimit = 0.01;
- std::string pi_;
- const std::optional<uint16_t> pi_number_;
+ std::string hostname_;
+ const std::optional<std::string_view> cpu_type_;
+ const std::optional<uint16_t> cpu_number_;
const std::string camera_id_;
std::vector<std::vector<int>> all_charuco_ids_;
diff --git a/y2024/y2024_orin_template.json b/y2024/y2024_orin_template.json
index 6d378f7..6206cf9 100644
--- a/y2024/y2024_orin_template.json
+++ b/y2024/y2024_orin_template.json
@@ -257,7 +257,7 @@
"executable_name": "argus_camera",
"args": [
"--enable_ftrace",
- "--camera=0",
+ "--camera=1",
],
"user": "pi",
"nodes": [
@@ -267,6 +267,9 @@
{
"name": "apriltag_detector",
"executable_name": "apriltag_detector",
+ "args": [
+ "--channel=/camera",
+ ],
"user": "pi",
"nodes": [
"orin{{ NUM }}"