Merge "Switch top and bottom buttons on the pistol grip"
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index 02a9fdd..58c0fc3 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -237,7 +237,8 @@
}
iovec_.clear();
- size_t iovec_size = std::min<size_t>(queue.size(), IOV_MAX);
+ const size_t original_iovec_size = std::min<size_t>(queue.size(), IOV_MAX);
+ size_t iovec_size = original_iovec_size;
iovec_.resize(iovec_size);
size_t counted_size = 0;
@@ -247,7 +248,7 @@
// The file is aligned if it is a multiple of kSector in length. The data is
// aligned if it's memory is kSector aligned, and the length is a multiple of
// kSector in length.
- bool aligned = (total_write_bytes_ % kSector) == 0;
+ bool aligned = (file_written_bytes_ % kSector) == 0;
size_t write_index = 0;
for (size_t i = 0; i < iovec_size; ++i) {
iovec_[write_index].iov_base = const_cast<uint8_t *>(queue[i].data());
@@ -295,11 +296,13 @@
++write_index;
}
- // Either write the aligned data if it is all aligned, or write the rest
- // unaligned if we wrote aligned up above.
- WriteV(iovec_.data(), iovec_.size(), aligned, counted_size);
+ if (counted_size > 0) {
+ // Either write the aligned data if it is all aligned, or write the rest
+ // unaligned if we wrote aligned up above.
+ WriteV(iovec_.data(), iovec_.size(), aligned, counted_size);
- encoder_->Clear(iovec_size);
+ encoder_->Clear(original_iovec_size);
+ }
}
size_t DetachedBufferWriter::WriteV(struct iovec *iovec_data, size_t iovec_size,
@@ -317,21 +320,21 @@
if (written > 0) {
// Flush asynchronously and force the data out of the cache.
- sync_file_range(fd_, total_write_bytes_, written, SYNC_FILE_RANGE_WRITE);
- if (last_synced_bytes_ != 0) {
+ sync_file_range(fd_, file_written_bytes_, written, SYNC_FILE_RANGE_WRITE);
+ if (file_written_bytes_ != 0) {
// Per Linus' recommendation online on how to do fast file IO, do a
// blocking flush of the previous write chunk, and then tell the kernel to
// drop the pages from the cache. This makes sure we can't get too far
// ahead.
sync_file_range(fd_, last_synced_bytes_,
- total_write_bytes_ - last_synced_bytes_,
+ file_written_bytes_ - last_synced_bytes_,
SYNC_FILE_RANGE_WAIT_BEFORE | SYNC_FILE_RANGE_WRITE |
SYNC_FILE_RANGE_WAIT_AFTER);
posix_fadvise(fd_, last_synced_bytes_,
- total_write_bytes_ - last_synced_bytes_,
+ file_written_bytes_ - last_synced_bytes_,
POSIX_FADV_DONTNEED);
- last_synced_bytes_ = total_write_bytes_;
+ last_synced_bytes_ = file_written_bytes_;
}
}
@@ -371,6 +374,7 @@
++total_write_count_;
total_write_messages_ += iovec_size;
total_write_bytes_ += written;
+ file_written_bytes_ += written;
}
void DetachedBufferWriter::FlushAtThreshold(
diff --git a/aos/events/logging/logfile_utils.h b/aos/events/logging/logfile_utils.h
index 50f6b40..0c0ef7f 100644
--- a/aos/events/logging/logfile_utils.h
+++ b/aos/events/logging/logfile_utils.h
@@ -212,6 +212,8 @@
bool supports_odirect_ = true;
int flags_ = 0;
+ size_t file_written_bytes_ = 0;
+
aos::monotonic_clock::time_point last_flush_time_ =
aos::monotonic_clock::min_time;
};
diff --git a/aos/events/logging/lzma_encoder.cc b/aos/events/logging/lzma_encoder.cc
index f354638..d348b70 100644
--- a/aos/events/logging/lzma_encoder.cc
+++ b/aos/events/logging/lzma_encoder.cc
@@ -84,6 +84,13 @@
// efficient to allocate if we go over a threshold to keep the static memory
// in use smaller, or just allocate the worst case like we are doing here?
input_buffer_.resize(max_message_size);
+
+ // Start our queues out with a reasonable space allocation. The cost of this
+ // is negligable compared to the buffer cost, but removes a bunch of
+ // allocations at runtime.
+ queue_.reserve(4);
+ free_queue_.reserve(4);
+ return_queue_.reserve(4);
}
LzmaEncoder::~LzmaEncoder() { lzma_end(&stream_); }
@@ -111,6 +118,9 @@
void LzmaEncoder::Clear(const int n) {
CHECK_GE(n, 0);
CHECK_LE(static_cast<size_t>(n), queue_size());
+ for (int i = 0; i < n; ++i) {
+ free_queue_.emplace_back(std::move(queue_[i]));
+ }
queue_.erase(queue_.begin(), queue_.begin() + n);
if (queue_.empty()) {
stream_.next_out = nullptr;
@@ -155,8 +165,13 @@
// construction or a Reset, or when an input buffer is large enough to fill
// more than one output buffer.
if (stream_.avail_out == 0) {
- queue_.emplace_back();
- queue_.back().resize(kEncodedBufferSizeBytes);
+ if (!free_queue_.empty()) {
+ queue_.emplace_back(std::move(free_queue_.back()));
+ free_queue_.pop_back();
+ } else {
+ queue_.emplace_back();
+ queue_.back().resize(kEncodedBufferSizeBytes);
+ }
stream_.next_out = queue_.back().data();
stream_.avail_out = kEncodedBufferSizeBytes;
// Update the byte count.
diff --git a/aos/events/logging/lzma_encoder.h b/aos/events/logging/lzma_encoder.h
index b4964fb..3136d93 100644
--- a/aos/events/logging/lzma_encoder.h
+++ b/aos/events/logging/lzma_encoder.h
@@ -54,6 +54,11 @@
lzma_stream stream_;
uint32_t compression_preset_;
std::vector<ResizeableBuffer> queue_;
+ // Since we pretty much just allocate a couple of buffers, then allocate and
+ // release them over and over with very similar memory usage and without much
+ // variation in the peak usage, put the allocate chunks in a free queue to
+ // reduce fragmentation.
+ std::vector<ResizeableBuffer> free_queue_;
bool finished_ = false;
// Total bytes that resulted from encoding raw data since the last call to
// Reset.
diff --git a/aos/network/sctp_client.cc b/aos/network/sctp_client.cc
index a2c0439..e3da03a 100644
--- a/aos/network/sctp_client.cc
+++ b/aos/network/sctp_client.cc
@@ -13,6 +13,10 @@
#include "aos/unique_malloc_ptr.h"
#include "glog/logging.h"
+DEFINE_int32(sinit_max_init_timeout, 0,
+ "Timeout in milliseconds for retrying the INIT packet when "
+ "connecting to the message bridge server");
+
namespace aos {
namespace message_bridge {
@@ -29,6 +33,8 @@
memset(&initmsg, 0, sizeof(struct sctp_initmsg));
initmsg.sinit_num_ostreams = streams;
initmsg.sinit_max_instreams = streams;
+ // Max timeout in milliseconds for the INIT packet.
+ initmsg.sinit_max_init_timeo = FLAGS_sinit_max_init_timeout;
PCHECK(setsockopt(fd(), IPPROTO_SCTP, SCTP_INITMSG, &initmsg,
sizeof(struct sctp_initmsg)) == 0);
}
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index f3ebecb..133281b 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -223,6 +223,7 @@
hdrs = [
"visualize_robot.h",
],
+ visibility = ["//visibility:public"],
deps = [
"//aos:init",
"//third_party:opencv",
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index dd332fc..dea883b 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -10,9 +10,11 @@
DEFINE_double(distortion_noise_scalar, 1.0,
"Scale the target pose distortion factor by this when computing "
"the noise.");
+DEFINE_uint64(
+ frozen_target_id, 1,
+ "Freeze the pose of this target so the map can have one fixed point.");
namespace frc971::vision {
-
Eigen::Affine3d PoseUtils::Pose3dToAffine3d(
const ceres::examples::Pose3d &pose3d) {
Eigen::Affine3d H_world_pose =
@@ -160,8 +162,8 @@
{
// Noise for vision-based target localizations. Multiplying this matrix by
- // the distance from camera to target squared results in the uncertainty in
- // that measurement
+ // the distance from camera to target squared results in the uncertainty
+ // in that measurement
TargetMapper::ConfidenceMatrix Q_vision =
TargetMapper::ConfidenceMatrix::Zero();
Q_vision(kX, kX) = std::pow(0.045, 2);
@@ -232,6 +234,15 @@
return std::nullopt;
}
+std::optional<TargetMapper::TargetPose> TargetMapper::GetTargetPoseById(
+ TargetId target_id) {
+ if (target_poses_.count(target_id) > 0) {
+ return TargetMapper::TargetPose{target_id, target_poses_[target_id]};
+ }
+
+ return std::nullopt;
+}
+
// Taken from ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
// Constructs the nonlinear least squares optimization problem from the pose
// graph constraints.
@@ -289,7 +300,10 @@
// algorithm has internal damping which mitigates this issue, but it is
// better to properly constrain the gauge freedom. This can be done by
// setting one of the poses as constant so the optimizer cannot change it.
- ceres::examples::MapOfPoses::iterator pose_start_iter = poses->begin();
+ CHECK_NE(poses->count(FLAGS_frozen_target_id), 0ul)
+ << "Got no poses for frozen target id " << FLAGS_frozen_target_id;
+ ceres::examples::MapOfPoses::iterator pose_start_iter =
+ poses->find(FLAGS_frozen_target_id);
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
@@ -302,6 +316,7 @@
ceres::Solver::Options options;
options.max_num_iterations = FLAGS_max_num_iterations;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
+ options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, problem, &summary);
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index 3724832..35c0977 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -46,6 +46,9 @@
static std::optional<TargetPose> GetTargetPoseById(
std::vector<TargetPose> target_poses, TargetId target_id);
+ // Version that gets based on internal target_poses
+ std::optional<TargetPose> GetTargetPoseById(TargetId target_id);
+
ceres::examples::MapOfPoses target_poses() { return target_poses_; }
private:
diff --git a/frc971/vision/visualize_robot.cc b/frc971/vision/visualize_robot.cc
index 0bbe507..eb2da9a 100644
--- a/frc971/vision/visualize_robot.cc
+++ b/frc971/vision/visualize_robot.cc
@@ -9,6 +9,24 @@
namespace frc971 {
namespace vision {
+void VisualizeRobot::SetDefaultViewpoint(int image_width, double focal_length) {
+ // 10 meters above the origin, rotated so the camera faces straight down
+ Eigen::Translation3d camera_trans(0, 0, 10.0);
+ Eigen::AngleAxisd camera_rot(M_PI, Eigen::Vector3d::UnitX());
+ Eigen::Affine3d camera_viewpoint = camera_trans * camera_rot;
+ SetViewpoint(camera_viewpoint);
+
+ cv::Mat camera_mat;
+ double half_width = static_cast<double>(image_width) / 2.0;
+ double intr[] = {focal_length, 0.0, half_width, 0.0, focal_length,
+ half_width, 0.0, 0.0, 1.0};
+ camera_mat = cv::Mat(3, 3, CV_64FC1, intr);
+ SetCameraParameters(camera_mat);
+
+ cv::Mat dist_coeffs = cv::Mat(1, 5, CV_64F, 0.0);
+ SetDistortionCoefficients(dist_coeffs);
+}
+
cv::Point VisualizeRobot::ProjectPoint(Eigen::Vector3d T_world_point) {
// Map 3D point in world coordinates to camera frame
Eigen::Vector3d T_camera_point = H_world_viewpoint_.inverse() * T_world_point;
@@ -28,7 +46,7 @@
cv::Point start2d = ProjectPoint(start3d);
cv::Point end2d = ProjectPoint(end3d);
- cv::line(image_, start2d, end2d, cv::Scalar(0, 0, 255));
+ cv::line(image_, start2d, end2d, cv::Scalar(0, 200, 0));
}
void VisualizeRobot::DrawFrameAxes(Eigen::Affine3d H_world_target,
@@ -53,21 +71,37 @@
// Grab x axis direction
cv::Vec3d label_offset = r_mat.col(0);
- // Find 3D coordinate of point at the end of the x-axis, and project it
+ // Find 3D coordinate of point at the end of the x-axis, and put label there
+ // Bump it just a few (5) pixels to the right, to make it read easier
cv::Mat label_coord_res =
camera_mat_ * cv::Mat(tvec + label_offset * axis_scale);
cv::Vec3d label_coord = label_coord_res.col(0);
- label_coord[0] = label_coord[0] / label_coord[2];
+ label_coord[0] = label_coord[0] / label_coord[2] + 5;
label_coord[1] = label_coord[1] / label_coord[2];
cv::putText(image_, label, cv::Point(label_coord[0], label_coord[1]),
cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0, 0, 255));
}
}
-void VisualizeRobot::DrawBoardOutline(Eigen::Affine3d H_world_board,
+void VisualizeRobot::DrawRobotOutline(Eigen::Affine3d H_world_robot,
std::string label) {
- LOG(INFO) << "Not yet implemented; drawing axes only";
- DrawFrameAxes(H_world_board, label);
+ DrawFrameAxes(H_world_robot, label);
+ const double kBotHalfWidth = 0.75 / 2.0;
+ const double kBotHalfLength = 1.0 / 2.0;
+ // Compute coordinates for front/rear and right/left corners
+ Eigen::Vector3d fr_corner =
+ H_world_robot * Eigen::Vector3d(kBotHalfLength, kBotHalfWidth, 0);
+ Eigen::Vector3d fl_corner =
+ H_world_robot * Eigen::Vector3d(kBotHalfLength, -kBotHalfWidth, 0);
+ Eigen::Vector3d rl_corner =
+ H_world_robot * Eigen::Vector3d(-kBotHalfLength, -kBotHalfWidth, 0);
+ Eigen::Vector3d rr_corner =
+ H_world_robot * Eigen::Vector3d(-kBotHalfLength, kBotHalfWidth, 0);
+
+ DrawLine(fr_corner, fl_corner);
+ DrawLine(fl_corner, rl_corner);
+ DrawLine(rl_corner, rr_corner);
+ DrawLine(rr_corner, fr_corner);
}
} // namespace vision
diff --git a/frc971/vision/visualize_robot.h b/frc971/vision/visualize_robot.h
index 391a030..f5e75af 100644
--- a/frc971/vision/visualize_robot.h
+++ b/frc971/vision/visualize_robot.h
@@ -31,14 +31,20 @@
// Set camera parameters (for projection into a virtual view)
void SetCameraParameters(cv::Mat camera_intrinsics) {
- camera_mat_ = camera_intrinsics;
+ camera_mat_ = camera_intrinsics.clone();
}
// Set distortion coefficients (defaults to 0)
void SetDistortionCoefficients(cv::Mat dist_coeffs) {
- dist_coeffs_ = dist_coeffs;
+ dist_coeffs_ = dist_coeffs.clone();
}
+ // Sets up a default camera view 10 m above the origin, pointing down
+ // Uses image_width and focal_length to define a default camera projection
+ // matrix
+ void SetDefaultViewpoint(int image_width = 1000,
+ double focal_length = 1000.0);
+
// Helper function to project a point in 3D to the virtual image coordinates
cv::Point ProjectPoint(Eigen::Vector3d point3d);
@@ -50,8 +56,9 @@
void DrawFrameAxes(Eigen::Affine3d H_world_target, std::string label = "",
double axis_scale = 0.25);
- // TODO<Jim>: Need to implement this, and maybe DrawRobotOutline
- void DrawBoardOutline(Eigen::Affine3d H_world_board, std::string label = "");
+ // TODO<Jim>: Also implement DrawBoardOutline? Maybe one function w/
+ // parameters?
+ void DrawRobotOutline(Eigen::Affine3d H_world_robot, std::string label = "");
Eigen::Affine3d H_world_viewpoint_; // Where to view the world from
cv::Mat image_; // Image to draw on
diff --git a/frc971/vision/visualize_robot_sample.cc b/frc971/vision/visualize_robot_sample.cc
index dc38352..59acba0 100644
--- a/frc971/vision/visualize_robot_sample.cc
+++ b/frc971/vision/visualize_robot_sample.cc
@@ -26,25 +26,8 @@
cv::Mat image_mat =
cv::Mat::zeros(cv::Size(image_width, image_width), CV_8UC3);
vis_robot.SetImage(image_mat);
-
- // 10 meters above the origin, rotated so the camera faces straight down
- Eigen::Translation3d camera_trans(0, 0, 10.0);
- Eigen::AngleAxisd camera_rot(M_PI, Eigen::Vector3d::UnitX());
- Eigen::Affine3d camera_viewpoint = camera_trans * camera_rot;
- vis_robot.SetViewpoint(camera_viewpoint);
-
- cv::Mat camera_mat;
double focal_length = 1000.0;
- double intr[] = {focal_length, 0.0, image_width / 2.0,
- 0.0, focal_length, image_width / 2.0,
- 0.0, 0.0, 1.0};
- camera_mat = cv::Mat(3, 3, CV_64FC1, intr);
- vis_robot.SetCameraParameters(camera_mat);
-
- Eigen::Affine3d offset_rotate_origin(Eigen::Affine3d::Identity());
-
- cv::Mat dist_coeffs = cv::Mat(1, 5, CV_64F, 0.0);
- vis_robot.SetDistortionCoefficients(dist_coeffs);
+ vis_robot.SetDefaultViewpoint(image_width, focal_length);
// Go around the clock and plot the coordinate frame at different rotations
for (int i = 0; i < 12; i++) {
@@ -52,8 +35,9 @@
Eigen::Vector3d trans;
trans << 1.0 * cos(angle), 1.0 * sin(angle), 0.0;
- offset_rotate_origin = Eigen::Translation3d(trans) *
- Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX());
+ Eigen::Affine3d offset_rotate_origin =
+ Eigen::Translation3d(trans) *
+ Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX());
vis_robot.DrawFrameAxes(offset_rotate_origin, std::to_string(i));
}
diff --git a/y2023/control_loops/superstructure/led_indicator.h b/y2023/control_loops/superstructure/led_indicator.h
index d88650d..256de30 100644
--- a/y2023/control_loops/superstructure/led_indicator.h
+++ b/y2023/control_loops/superstructure/led_indicator.h
@@ -69,7 +69,7 @@
void DisplayLed(uint8_t r, uint8_t g, uint8_t b);
- ctre::phoenix::led::CANdle candle_{0, ""};
+ ctre::phoenix::led::CANdle candle_{8, "rio"};
aos::EventLoop *event_loop_;
aos::Fetcher<frc971::control_loops::drivetrain::Output>
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 1cdbe36..0f75aba 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -117,6 +117,7 @@
"//frc971/vision:target_map_fbs",
"//frc971/vision:target_mapper",
"//frc971/vision:vision_fbs",
+ "//frc971/vision:visualize_robot",
"//third_party:opencv",
"//third_party/apriltag",
"//y2023/constants:constants_fbs",
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 95ad541..cf2244a 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -2,6 +2,8 @@
#include "y2023/vision/vision_util.h"
+#include <opencv2/highgui.hpp>
+
DEFINE_bool(
debug, false,
"If true, dump a ton of debug and crash on the first valid detection.");
@@ -26,13 +28,12 @@
: calibration_data_(event_loop),
image_size_(0, 0),
ftrace_(),
- image_callback_(
- event_loop, channel_name,
- [&](cv::Mat image_color_mat,
- const aos::monotonic_clock::time_point eof) {
- HandleImage(image_color_mat, eof);
- },
- chrono::milliseconds(5)),
+ image_callback_(event_loop, channel_name,
+ [&](cv::Mat image_color_mat,
+ const aos::monotonic_clock::time_point eof) {
+ HandleImage(image_color_mat, eof);
+ },
+ chrono::milliseconds(5)),
target_map_sender_(
event_loop->MakeSender<frc971::vision::TargetMap>("/camera")),
image_annotations_sender_(
@@ -54,7 +55,6 @@
calibration_data_.constants(), event_loop->node()->name()->string_view());
extrinsics_ = CameraExtrinsics(calibration_);
-
intrinsics_ = CameraIntrinsics(calibration_);
// Create an undistort projection matrix using the intrinsics
projection_matrix_ = cv::Mat::zeros(3, 4, CV_64F);
@@ -181,6 +181,8 @@
AprilRoboticsDetector::DetectionResult AprilRoboticsDetector::DetectTags(
cv::Mat image, aos::monotonic_clock::time_point eof) {
+ cv::Mat color_image;
+ cvtColor(image, color_image, cv::COLOR_GRAY2RGB);
const aos::monotonic_clock::time_point start_time =
aos::monotonic_clock::now();
@@ -275,10 +277,42 @@
.pose = pose,
.pose_error = pose_error,
.distortion_factor = distortion_factor});
+ if (FLAGS_visualize) {
+ // Draw raw (distorted) corner points in green
+ cv::line(color_image, orig_corner_points[0], orig_corner_points[1],
+ cv::Scalar(0, 255, 0), 2);
+ cv::line(color_image, orig_corner_points[1], orig_corner_points[2],
+ cv::Scalar(0, 255, 0), 2);
+ cv::line(color_image, orig_corner_points[2], orig_corner_points[3],
+ cv::Scalar(0, 255, 0), 2);
+ cv::line(color_image, orig_corner_points[3], orig_corner_points[0],
+ cv::Scalar(0, 255, 0), 2);
+
+ // Draw undistorted corner points in red
+ cv::line(color_image, corner_points[0], corner_points[1],
+ cv::Scalar(0, 0, 255), 2);
+ cv::line(color_image, corner_points[2], corner_points[1],
+ cv::Scalar(0, 0, 255), 2);
+ cv::line(color_image, corner_points[2], corner_points[3],
+ cv::Scalar(0, 0, 255), 2);
+ cv::line(color_image, corner_points[0], corner_points[3],
+ cv::Scalar(0, 0, 255), 2);
+ }
+
+ VLOG(1) << "Found tag number " << det->id << " hamming: " << det->hamming
+ << " margin: " << det->decision_margin;
+
} else {
rejections_++;
}
}
+ if (FLAGS_visualize) {
+ // Display the result
+ // Rotate by 180 degrees to make it upright
+ // TDOO<Jim>: Make this a flag, since we don't want it for box of pis
+ cv::rotate(color_image, color_image, 1);
+ cv::imshow("AprilRoboticsDetector Image", color_image);
+ }
const auto corners_offset = builder.fbb()->CreateVector(foxglove_corners);
foxglove::ImageAnnotations::Builder annotation_builder(*builder.fbb());
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index fab2d30..7caa848 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -11,6 +11,7 @@
#include "frc971/vision/target_map_generated.h"
#include "frc971/vision/target_mapper.h"
#include "frc971/vision/vision_generated.h"
+#include "frc971/vision/visualize_robot.h"
#include "opencv2/core/eigen.hpp"
#include "opencv2/imgproc.hpp"
#include "third_party/apriltag/apriltag.h"
@@ -83,8 +84,8 @@
frc971::vision::ImageCallback image_callback_;
aos::Sender<frc971::vision::TargetMap> target_map_sender_;
aos::Sender<foxglove::ImageAnnotations> image_annotations_sender_;
-
size_t rejections_;
+ frc971::vision::VisualizeRobot vis_robot_;
};
} // namespace vision
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 9352bdd..a110794 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -7,6 +7,7 @@
#include "frc971/vision/calibration_generated.h"
#include "frc971/vision/charuco_lib.h"
#include "frc971/vision/target_mapper.h"
+#include "frc971/vision/visualize_robot.h"
#include "opencv2/aruco.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/core/eigen.hpp"
@@ -28,8 +29,8 @@
"Directory to write solved target map to");
DEFINE_string(field_name, "charged_up",
"Field name, for the output json filename and flatbuffer field");
-DEFINE_int32(team_number, 7971,
- "Use the calibration for a node with this team number");
+DEFINE_int32(team_number, 0,
+ "Required: Use the calibration for a node with this team number");
DEFINE_string(mcap_output_path, "/tmp/log.mcap", "Log to output.");
DEFINE_string(pi, "pi1", "Pi name to generate mcap log for; defaults to pi1.");
DEFINE_double(max_pose_error, 1e-6,
@@ -52,16 +53,22 @@
return H_robot_target;
}
+frc971::vision::VisualizeRobot vis_robot_;
+const int kImageWidth = 1000;
+
// Add detected apriltag poses relative to the robot to
// timestamped_target_detections
void HandleAprilTags(const TargetMap &map,
aos::distributed_clock::time_point pi_distributed_time,
std::vector<DataAdapter::TimestampedDetection>
*timestamped_target_detections,
- Eigen::Affine3d extrinsics) {
+ Eigen::Affine3d extrinsics, std::string node_name,
+ frc971::vision::TargetMapper mapper) {
for (const auto *target_pose_fbs : *map.target_poses()) {
// Skip detections with high pose errors
if (target_pose_fbs->pose_error() > FLAGS_max_pose_error) {
+ LOG(INFO) << " Skipping tag " << target_pose_fbs->id()
+ << " due to pose error of " << target_pose_fbs->pose_error();
continue;
}
@@ -88,6 +95,27 @@
.distance_from_camera = distance_from_camera,
.distortion_factor = distortion_factor,
.id = static_cast<TargetMapper::TargetId>(target_pose.id)});
+
+ if (FLAGS_visualize) {
+ Eigen::Affine3d H_world_target = PoseUtils::Pose3dToAffine3d(
+ mapper.GetTargetPoseById(target_pose_fbs->id())->pose);
+ Eigen::Affine3d H_world_robot = H_world_target * H_robot_target.inverse();
+ Eigen::Affine3d H_world_camera =
+ H_world_target * H_camera_target.inverse();
+ LOG(INFO) << node_name << ", " << target_pose_fbs->id()
+ << ", t = " << pi_distributed_time
+ << ", pose_error = " << target_pose_fbs->pose_error()
+ << ", robot_pos (x,y,z) + "
+ << H_world_robot.translation().transpose();
+ vis_robot_.DrawRobotOutline(
+ H_world_robot, node_name + "-" +
+ std::to_string(target_pose_fbs->id()) + " " +
+ std::to_string(target_pose_fbs->pose_error() /
+ FLAGS_max_pose_error));
+ vis_robot_.DrawFrameAxes(H_world_camera, node_name);
+ vis_robot_.DrawFrameAxes(H_world_target,
+ std::to_string(target_pose_fbs->id()));
+ }
}
}
@@ -109,6 +137,10 @@
detectors->emplace_back(std::move(detector_ptr));
+ ceres::examples::VectorOfConstraints target_constraints;
+ frc971::vision::TargetMapper target_loc_mapper(FLAGS_json_path,
+ target_constraints);
+
mapping_event_loop->MakeWatcher("/camera", [=](const TargetMap &map) {
aos::distributed_clock::time_point pi_distributed_time =
reader->event_loop_factory()
@@ -116,8 +148,16 @@
->ToDistributedClock(aos::monotonic_clock::time_point(
aos::monotonic_clock::duration(map.monotonic_timestamp_ns())));
+ std::string node_name = detection_event_loop->node()->name()->str();
HandleAprilTags(map, pi_distributed_time, timestamped_target_detections,
- extrinsics);
+ extrinsics, node_name, target_loc_mapper);
+ if (FLAGS_visualize) {
+ cv::imshow("View", vis_robot_.image_);
+ cv::waitKey();
+ cv::Mat image_mat =
+ cv::Mat::zeros(cv::Size(kImageWidth, kImageWidth), CV_8UC3);
+ vis_robot_.SetImage(image_mat);
+ }
});
}
@@ -197,7 +237,6 @@
std::unique_ptr<aos::McapLogger> relogger;
if (!FLAGS_mcap_output_path.empty()) {
LOG(INFO) << "Writing out mcap file to " << FLAGS_mcap_output_path;
- // TODO: Should make this work for any pi
const aos::Node *node =
aos::configuration::GetNode(reader.configuration(), FLAGS_pi);
reader.event_loop_factory()->GetNodeEventLoopFactory(node)->OnStartup(
@@ -212,6 +251,14 @@
});
}
+ if (FLAGS_visualize) {
+ cv::Mat image_mat =
+ cv::Mat::zeros(cv::Size(kImageWidth, kImageWidth), CV_8UC3);
+ vis_robot_.SetImage(image_mat);
+ const double kFocalLength = 500.0;
+ vis_robot_.SetDefaultViewpoint(kImageWidth, kFocalLength);
+ }
+
reader.event_loop_factory()->Run();
auto target_constraints =
diff --git a/y2023/y2023_imu.json b/y2023/y2023_imu.json
index 416552b..535d97f 100644
--- a/y2023/y2023_imu.json
+++ b/y2023/y2023_imu.json
@@ -452,8 +452,6 @@
"applications": [
{
"name": "message_bridge_client",
- "executable_name": "message_bridge_client",
- "user": "pi",
"nodes": [
"imu"
]
diff --git a/y2023/y2023_logger.json b/y2023/y2023_logger.json
index 55d201d..9b0ba55 100644
--- a/y2023/y2023_logger.json
+++ b/y2023/y2023_logger.json
@@ -417,8 +417,6 @@
"applications": [
{
"name": "message_bridge_client",
- "executable_name": "message_bridge_client",
- "user": "pi",
"nodes": [
"logger"
]
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index 8d9fdaa..31f4045 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -350,7 +350,11 @@
"applications": [
{
"name": "message_bridge_client",
- "args": ["--rt_priority=16"],
+ "executable_name": "message_bridge_client",
+ "args": [
+ "--rt_priority=16",
+ "--sinit_max_init_timeout=5000"
+ ],
"user": "pi",
"nodes": [
"pi{{ NUM }}"
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index 3114ac9..cf3a14e 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -530,7 +530,8 @@
"name": "roborio_message_bridge_client",
"executable_name": "message_bridge_client",
"args": [
- "--rt_priority=16"
+ "--rt_priority=16",
+ "--sinit_max_init_timeout=5000"
],
"nodes": [
"roborio"