Merge "Solve for mounting position as well as angle"
diff --git a/aos/network/log_web_proxy_main.cc b/aos/network/log_web_proxy_main.cc
index 5c099a2..945d034 100644
--- a/aos/network/log_web_proxy_main.cc
+++ b/aos/network/log_web_proxy_main.cc
@@ -42,7 +42,8 @@
event_loop->SkipTimingReport();
- aos::web_proxy::WebProxy web_proxy(event_loop.get(), FLAGS_buffer_size);
+ aos::web_proxy::WebProxy web_proxy(
+ event_loop.get(), aos::web_proxy::StoreHistory::kYes, FLAGS_buffer_size);
web_proxy.SetDataPath(FLAGS_data_dir.c_str());
diff --git a/aos/network/web_proxy.cc b/aos/network/web_proxy.cc
index fdd8f1e..6d4f23f 100644
--- a/aos/network/web_proxy.cc
+++ b/aos/network/web_proxy.cc
@@ -28,7 +28,9 @@
namespace aos {
namespace web_proxy {
WebsocketHandler::WebsocketHandler(::seasocks::Server *server,
- aos::EventLoop *event_loop, int buffer_size)
+ aos::EventLoop *event_loop,
+ StoreHistory store_history,
+ int per_channel_buffer_size_bytes)
: server_(server),
config_(aos::CopyFlatBuffer(event_loop->configuration())),
event_loop_(event_loop) {
@@ -47,7 +49,10 @@
if (aos::configuration::ChannelIsReadableOnNode(channel, self)) {
auto fetcher = event_loop_->MakeRawFetcher(channel);
subscribers_.emplace_back(std::make_unique<aos::web_proxy::Subscriber>(
- std::move(fetcher), i, buffer_size));
+ std::move(fetcher), i, store_history,
+ per_channel_buffer_size_bytes < 0
+ ? -1
+ : per_channel_buffer_size_bytes / channel->max_size()));
} else {
subscribers_.emplace_back(nullptr);
}
@@ -126,19 +131,24 @@
global_epoll->DeleteFd(fd);
}
-WebProxy::WebProxy(aos::EventLoop *event_loop, int buffer_size)
- : WebProxy(event_loop, &internal_epoll_, buffer_size) {}
+WebProxy::WebProxy(aos::EventLoop *event_loop, StoreHistory store_history,
+ int per_channel_buffer_size_bytes)
+ : WebProxy(event_loop, &internal_epoll_, store_history,
+ per_channel_buffer_size_bytes) {}
-WebProxy::WebProxy(aos::ShmEventLoop *event_loop, int buffer_size)
- : WebProxy(event_loop, event_loop->epoll(), buffer_size) {}
+WebProxy::WebProxy(aos::ShmEventLoop *event_loop, StoreHistory store_history,
+ int per_channel_buffer_size_bytes)
+ : WebProxy(event_loop, event_loop->epoll(), store_history,
+ per_channel_buffer_size_bytes) {}
WebProxy::WebProxy(aos::EventLoop *event_loop, aos::internal::EPoll *epoll,
- int buffer_size)
+ StoreHistory store_history,
+ int per_channel_buffer_size_bytes)
: epoll_(epoll),
server_(std::make_shared<aos::seasocks::SeasocksLogger>(
::seasocks::Logger::Level::Info)),
- websocket_handler_(
- new WebsocketHandler(&server_, event_loop, buffer_size)) {
+ websocket_handler_(new WebsocketHandler(
+ &server_, event_loop, store_history, per_channel_buffer_size_bytes)) {
CHECK(!global_epoll);
global_epoll = epoll;
@@ -192,10 +202,13 @@
}
void Subscriber::RunIteration() {
- if (channels_.empty() && buffer_size_ == 0) {
+ if (channels_.empty() && (buffer_size_ == 0 || !store_history_)) {
+ fetcher_->Fetch();
+ message_buffer_.clear();
return;
}
+
while (fetcher_->FetchNext()) {
// If we aren't building up a buffer, short-circuit the FetchNext().
if (buffer_size_ == 0) {
@@ -271,12 +284,6 @@
ChannelInformation info;
info.transfer_method = transfer_method;
- // If we aren't keeping a buffer and there are no existing listeners, call
- // Fetch() to avoid falling behind on future calls to FetchNext().
- if (channels_.empty() && buffer_size_ == 0) {
- fetcher_->Fetch();
- }
-
channels_.emplace_back(std::make_pair(data_channel, info));
data_channel->set_on_message(
diff --git a/aos/network/web_proxy.fbs b/aos/network/web_proxy.fbs
index 6c85acb..f1d6645 100644
--- a/aos/network/web_proxy.fbs
+++ b/aos/network/web_proxy.fbs
@@ -64,7 +64,7 @@
enum TransferMethod : byte {
SUBSAMPLE,
- EVERYTHING_WITH_HISTORY,
+ LOSSLESS,
}
table ChannelRequest {
diff --git a/aos/network/web_proxy.h b/aos/network/web_proxy.h
index 0815ebf..baca26e 100644
--- a/aos/network/web_proxy.h
+++ b/aos/network/web_proxy.h
@@ -24,13 +24,19 @@
class Subscriber;
class ApplicationConnection;
+enum class StoreHistory {
+ kNo,
+ kYes,
+};
+
// Basic class that handles receiving new websocket connections. Creates a new
// Connection to manage the rest of the negotiation and data passing. When the
// websocket closes, it deletes the Connection.
class WebsocketHandler : public ::seasocks::WebSocket::Handler {
public:
WebsocketHandler(::seasocks::Server *server, aos::EventLoop *event_loop,
- int buffer_size);
+ StoreHistory store_history,
+ int per_channel_buffer_size_bytes);
void onConnect(::seasocks::WebSocket *sock) override;
void onData(::seasocks::WebSocket *sock, const uint8_t *data,
size_t size) override;
@@ -50,15 +56,28 @@
// Wrapper class that manages the seasocks server and WebsocketHandler.
class WebProxy {
public:
- WebProxy(aos::EventLoop *event_loop, int buffer_size);
- WebProxy(aos::ShmEventLoop *event_loop, int buffer_size);
+ // Constructs a WebProxy object for interacting with a webpage. store_history
+ // and per_channel_buffer_size_bytes specify how we manage delivering LOSSLESS
+ // messages to the client:
+ // * store_history specifies whether we should always buffer up data for all
+ // channels--even for messages that are played prior to the client
+ // connecting. This is mostly useful for log replay where the client
+ // will typically connect after the logfile has been fully loaded/replayed.
+ // * per_channel_buffer_size_bytes is the maximum amount of data to buffer
+ // up per channel (-1 will indicate infinite data, which is used during log
+ // replay). This is divided by the max_size per channel to determine
+ // how many messages to queue up.
+ WebProxy(aos::EventLoop *event_loop, StoreHistory store_history,
+ int per_channel_buffer_size_bytes);
+ WebProxy(aos::ShmEventLoop *event_loop, StoreHistory store_history,
+ int per_channel_buffer_size_bytes);
~WebProxy();
void SetDataPath(const char *path) { server_.setStaticPath(path); }
private:
WebProxy(aos::EventLoop *event_loop, aos::internal::EPoll *epoll,
- int buffer_size);
+ StoreHistory store_history, int per_channel_buffer_size_bytes);
aos::internal::EPoll internal_epoll_;
aos::internal::EPoll *const epoll_;
@@ -96,9 +115,10 @@
class Subscriber {
public:
Subscriber(std::unique_ptr<RawFetcher> fetcher, int channel_index,
- int buffer_size)
+ StoreHistory store_history, int buffer_size)
: fetcher_(std::move(fetcher)),
channel_index_(channel_index),
+ store_history_(store_history == StoreHistory::kYes),
buffer_size_(buffer_size) {}
void RunIteration();
@@ -133,6 +153,10 @@
std::unique_ptr<RawFetcher> fetcher_;
int channel_index_;
+ // If set, will always build up a buffer of the most recent buffer_size_
+ // messages. If store_history_ is *not* set we will only buffer up messages
+ // while there is an active listener.
+ bool store_history_;
int buffer_size_;
std::deque<Message> message_buffer_;
// The ScopedDataChannel that we use for actually sending data over WebRTC
diff --git a/aos/network/web_proxy_main.cc b/aos/network/web_proxy_main.cc
index 06fe942..f3ad926 100644
--- a/aos/network/web_proxy_main.cc
+++ b/aos/network/web_proxy_main.cc
@@ -6,7 +6,9 @@
DEFINE_string(config, "./config.json", "File path of aos configuration");
DEFINE_string(data_dir, "www", "Directory to serve data files from");
-DEFINE_int32(buffer_size, 0, "-1 if infinite, in # of messages / channel.");
+DEFINE_int32(buffer_size, 1000000,
+ "-1 if infinite, in bytes / channel. If there are no active "
+ "connections, will not store anything.");
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
@@ -16,7 +18,8 @@
aos::ShmEventLoop event_loop(&config.message());
- aos::web_proxy::WebProxy web_proxy(&event_loop, FLAGS_buffer_size);
+ aos::web_proxy::WebProxy web_proxy(
+ &event_loop, aos::web_proxy::StoreHistory::kNo, FLAGS_buffer_size);
web_proxy.SetDataPath(FLAGS_data_dir.c_str());
event_loop.Run();
diff --git a/aos/network/www/proxy.ts b/aos/network/www/proxy.ts
index 5461899..5415400 100644
--- a/aos/network/www/proxy.ts
+++ b/aos/network/www/proxy.ts
@@ -117,7 +117,7 @@
name: string, type: string,
handler: (data: Uint8Array, sentTime: number) => void): void {
this.addHandlerImpl(
- name, type, TransferMethod.EVERYTHING_WITH_HISTORY, handler);
+ name, type, TransferMethod.LOSSLESS, handler);
}
/**
@@ -137,7 +137,7 @@
if (!this.handlerFuncs.has(channel.key())) {
this.handlerFuncs.set(channel.key(), []);
} else {
- if (method == TransferMethod.EVERYTHING_WITH_HISTORY) {
+ if (method == TransferMethod.LOSSLESS) {
console.warn(
'Behavior of multiple reliable handlers is currently poorly ' +
'defined and may not actually deliver all of the messages.');
diff --git a/frc971/analysis/in_process_plotter.cc b/frc971/analysis/in_process_plotter.cc
index 0eaf719..cd52b1c 100644
--- a/frc971/analysis/in_process_plotter.cc
+++ b/frc971/analysis/in_process_plotter.cc
@@ -15,7 +15,7 @@
event_loop_factory_(&config_.message()),
event_loop_(event_loop_factory_.MakeEventLoop("plotter")),
plot_sender_(event_loop_->MakeSender<Plot>("/analysis")),
- web_proxy_(event_loop_.get(), -1),
+ web_proxy_(event_loop_.get(), aos::web_proxy::StoreHistory::kYes, -1),
builder_(plot_sender_.MakeBuilder()) {
web_proxy_.SetDataPath(kDataPath);
event_loop_->SkipTimingReport();
diff --git a/frc971/analysis/live_web_plotter_demo.sh b/frc971/analysis/live_web_plotter_demo.sh
index 4c4c9c4..45f3b92 100755
--- a/frc971/analysis/live_web_plotter_demo.sh
+++ b/frc971/analysis/live_web_plotter_demo.sh
@@ -1 +1 @@
-./aos/network/web_proxy_main --config=aos/network/www/test_config.json --data_dir=frc971/analysis
+./aos/network/web_proxy_main --config=aos/network/www/test_config.json --data_dir=frc971/analysis "$@"
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index cad3221..df6c0f6 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -20,6 +20,7 @@
kalman_q_vel,
kalman_q_voltage,
kalman_r_position,
+ radius = None,
dt=0.00505):
"""Constructs an AngularSystemParams object.
@@ -38,6 +39,7 @@
self.kalman_q_vel = kalman_q_vel
self.kalman_q_voltage = kalman_q_voltage
self.kalman_r_position = kalman_r_position
+ self.radius = radius
self.dt = dt
@@ -80,11 +82,17 @@
glog.debug('Controllability of %d',
numpy.linalg.matrix_rank(controllability))
glog.debug('J: %f', self.J)
- glog.debug('Stall torque: %f', self.motor.stall_torque / self.G)
- glog.debug('Stall acceleration: %f',
+ glog.debug('Stall torque: %f (N m)', self.motor.stall_torque / self.G)
+ if self.params.radius is not None:
+ glog.debug('Stall force: %f (N)',
+ self.motor.stall_torque / self.G / self.params.radius)
+ glog.debug('Stall force: %f (lbf)',
+ self.motor.stall_torque / self.G / self.params.radius * 0.224809)
+
+ glog.debug('Stall acceleration: %f (rad/s^2)',
self.motor.stall_torque / self.G / self.J)
- glog.debug('Free speed is %f',
+ glog.debug('Free speed is %f (rad/s)',
-self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
self.Q = numpy.matrix([[(1.0 / (self.params.q_pos**2.0)), 0.0],
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index 5156b59..75ef7aa 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -106,7 +106,10 @@
"//y2020:config",
],
target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//y2020:__subpackages__"],
+ visibility = [
+ "//y2020:__subpackages__",
+ "//y2022:__subpackages__",
+ ],
deps = [
":charuco_lib",
"//aos:init",
diff --git a/y2022/BUILD b/y2022/BUILD
index 812cac1..76b3903 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -26,6 +26,7 @@
robot_downloader(
name = "pi_download",
binaries = [
+ "//y2020/vision:calibration",
"//y2022/vision:viewer",
],
data = [
@@ -166,6 +167,7 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//y2022/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2022/control_loops/superstructure/intake:intake_plants",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
],
@@ -232,3 +234,10 @@
"//y2022/control_loops/superstructure:superstructure_status_fbs",
],
)
+
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2022/__init__.py b/y2022/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2022/__init__.py
diff --git a/y2022/constants.cc b/y2022/constants.cc
index 98f6511..73e72f3 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -11,6 +11,7 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
#include "glog/logging.h"
+#include "y2022/control_loops/superstructure/intake/integral_intake_plant.h"
namespace y2022 {
namespace constants {
@@ -26,6 +27,37 @@
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
+ // TODO(Yash): Set constants
+ // Intake constants.
+ auto *const intake = &r->intake;
+
+ intake->zeroing_voltage = 3.0;
+ intake->operating_voltage = 12.0;
+ intake->zeroing_profile_params = {0.5, 3.0};
+ intake->default_profile_params = {6.0, 30.0};
+ intake->range = Values::kIntakeRange();
+ intake->make_integral_loop =
+ control_loops::superstructure::intake::MakeIntegralIntakeLoop;
+
+ // The number of samples in the moving average filter.
+ intake->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
+ // The distance that the absolute encoder needs to complete a full rotation.
+ intake->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kIntakeEncoderRatio();
+
+ // Threshold for deciding if we are moving. moving_buffer_size samples need to
+ // be within this distance of each other before we use the middle one to zero.
+ intake->zeroing_constants.zeroing_threshold = 0.0005;
+ // Buffer size for deciding if we are moving.
+ intake->zeroing_constants.moving_buffer_size = 20;
+
+ // Value between 0 and 1 indicating what fraction of one_revolution_distance
+ // it is acceptable for the offset to move.
+ intake->zeroing_constants.allowable_encoder_error = 0.9;
+
+ // Measured absolute position of the encoder when at zero.
+ intake->zeroing_constants.measured_absolute_position = 0.0;
+
switch (team) {
// A set of constants for tests.
case 1:
diff --git a/y2022/constants.h b/y2022/constants.h
index 050a363..050c5c7 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -9,6 +9,7 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2022/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2022/control_loops/superstructure/intake/intake_plant.h"
namespace y2022 {
namespace constants {
@@ -32,6 +33,19 @@
static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
static constexpr double kRollerStatorCurrentLimit() { return 40.0; }
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ intake;
+
+ // TODO (Yash): Constants need to be tuned
+ static constexpr ::frc971::constants::Range kIntakeRange() {
+ return ::frc971::constants::Range{
+ .lower_hard = -0.5, // Back Hard
+ .upper_hard = 2.85 + 0.05, // Front Hard
+ .lower = -0.300, // Back Soft
+ .upper = 2.725 // Front Soft
+ };
+ }
// Climber
static constexpr double kClimberSupplyCurrentLimit() { return 60.0; }
diff --git a/y2022/control_loops/BUILD b/y2022/control_loops/BUILD
new file mode 100644
index 0000000..49bc419
--- /dev/null
+++ b/y2022/control_loops/BUILD
@@ -0,0 +1,7 @@
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = ["//y2022:python_init"],
+)
diff --git a/y2022/control_loops/__init__.py b/y2022/control_loops/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2022/control_loops/__init__.py
diff --git a/y2022/control_loops/python/BUILD b/y2022/control_loops/python/BUILD
index e0b0daa..bc2b624 100644
--- a/y2022/control_loops/python/BUILD
+++ b/y2022/control_loops/python/BUILD
@@ -48,10 +48,26 @@
],
)
+py_binary(
+ name = "intake",
+ srcs = [
+ "intake.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:angular_system",
+ "//frc971/control_loops/python:controls",
+ ],
+)
+
py_library(
name = "python_init",
srcs = ["__init__.py"],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
- deps = ["//y2020/control_loops:python_init"],
+ deps = ["//y2022/control_loops:python_init"],
)
diff --git a/y2022/control_loops/python/intake.py b/y2022/control_loops/python/intake.py
new file mode 100644
index 0000000..e5030e5
--- /dev/null
+++ b/y2022/control_loops/python/intake.py
@@ -0,0 +1,55 @@
+#!/usr/bin/python3
+
+from aos.util.trapezoid_profile import TrapezoidProfile
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import angular_system
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+kIntake = angular_system.AngularSystemParams(
+ name='Intake',
+ motor=control_loop.Falcon(),
+ # TODO(Milo): Change gear ratios when we have all of them
+ G=0.02,
+ J=0.34,
+ q_pos=0.40,
+ q_vel=20.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=4.0,
+ kalman_r_position=0.05,
+ radius=13 * 0.0254)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+ angular_system.PlotKick(kIntake, R)
+ angular_system.PlotMotion(kIntake, R)
+
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the intake and integral intake.'
+ )
+ else:
+ namespaces = ['y2022', 'control_loops', 'superstructure', 'intake']
+ angular_system.WriteAngularSystem(kIntake, argv[1:3], argv[3:5],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2022/control_loops/superstructure/intake/BUILD b/y2022/control_loops/superstructure/intake/BUILD
new file mode 100644
index 0000000..f6e5be0
--- /dev/null
+++ b/y2022/control_loops/superstructure/intake/BUILD
@@ -0,0 +1,34 @@
+package(default_visibility = ["//y2022:__subpackages__"])
+
+genrule(
+ name = "genrule_intake",
+ outs = [
+ "intake_plant.h",
+ "intake_plant.cc",
+ "integral_intake_plant.h",
+ "integral_intake_plant.cc",
+ ],
+ cmd = "$(location //y2022/control_loops/python:intake) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2022/control_loops/python:intake",
+ ],
+)
+
+cc_library(
+ name = "intake_plants",
+ srcs = [
+ "intake_plant.cc",
+ "integral_intake_plant.cc",
+ ],
+ hdrs = [
+ "intake_plant.h",
+ "integral_intake_plant.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2022/vision/blob_detector.cc b/y2022/vision/blob_detector.cc
index aba3550..76b16ca 100644
--- a/y2022/vision/blob_detector.cc
+++ b/y2022/vision/blob_detector.cc
@@ -17,11 +17,11 @@
namespace y2022 {
namespace vision {
-cv::Mat BlobDetector::ThresholdImage(cv::Mat rgb_image) {
- cv::Mat binarized_image(cv::Size(rgb_image.cols, rgb_image.rows), CV_8UC1);
- for (int row = 0; row < rgb_image.rows; row++) {
- for (int col = 0; col < rgb_image.cols; col++) {
- cv::Vec3b pixel = rgb_image.at<cv::Vec3b>(row, col);
+cv::Mat BlobDetector::ThresholdImage(cv::Mat bgr_image) {
+ cv::Mat binarized_image(cv::Size(bgr_image.cols, bgr_image.rows), CV_8UC1);
+ for (int row = 0; row < bgr_image.rows; row++) {
+ for (int col = 0; col < bgr_image.cols; col++) {
+ cv::Vec3b pixel = bgr_image.at<cv::Vec3b>(row, col);
uint8_t blue = pixel.val[0];
uint8_t green = pixel.val[1];
uint8_t red = pixel.val[2];
@@ -143,15 +143,26 @@
}
double DistanceTo(cv::Point2d p) const {
- // Translate the point so that the circle orgin can be (0, 0)
- const auto p_prime = cv::Point2d(p.y - center.y, p.x - center.x);
+ const auto p_prime = TranslateToOrigin(p);
// Now, the distance is simply the difference between distance from the
// origin to p' and the radius.
return std::abs(cv::norm(p_prime) - radius);
}
- // Inverted because y-coordinates go backwards
- bool OnTopHalf(cv::Point2d p) const { return p.y <= center.y; }
+ bool InAngleRange(cv::Point2d p, double theta_min, double theta_max) const {
+ auto p_prime = TranslateToOrigin(p);
+ // Flip the y because y values go downwards.
+ p_prime.y *= -1;
+ const double theta = std::atan2(p_prime.y, p_prime.x);
+ return (theta >= theta_min && theta <= theta_max);
+ }
+
+ private:
+ // Translate the point on the circle
+ // as if the circle's center is the origin (0,0)
+ cv::Point2d TranslateToOrigin(cv::Point2d p) const {
+ return cv::Point2d(p.x - center.x, p.y - center.y);
+ }
};
} // namespace
@@ -176,17 +187,17 @@
// y = -(y_offset - offset_y)
constexpr int kMaxY = 400;
constexpr double kTapeAspectRatio = 5.0 / 2.0;
- constexpr double kAspectRatioThreshold = 1.5;
+ constexpr double kAspectRatioThreshold = 1.6;
constexpr double kMinArea = 10;
- constexpr size_t kMinPoints = 6;
+ constexpr size_t kMinNumPoints = 6;
// Remove all blobs that are at the bottom of the image, have a different
- // aspect ratio than the tape, or have too little area or points
- // TODO(milind): modify to take into account that blobs will be on the side.
+ // aspect ratio than the tape, or have too little area or points.
if ((stats_it->centroid.y <= kMaxY) &&
(std::abs(kTapeAspectRatio - stats_it->aspect_ratio) <
kAspectRatioThreshold) &&
- (stats_it->area >= kMinArea) && (stats_it->num_points >= kMinPoints)) {
+ (stats_it->area >= kMinArea) &&
+ (stats_it->num_points >= kMinNumPoints)) {
filtered_blobs.push_back(*blob_it);
filtered_stats.push_back(*stats_it);
}
@@ -196,6 +207,9 @@
// Threshold for mean distance from a blob centroid to a circle.
constexpr double kCircleDistanceThreshold = 5.0;
+ // We should only expect to see blobs between these angles on a circle.
+ constexpr double kMinBlobAngle = M_PI / 3;
+ constexpr double kMaxBlobAngle = M_PI - kMinBlobAngle;
std::vector<std::vector<cv::Point>> blob_circle;
std::vector<cv::Point2d> centroids;
@@ -230,16 +244,20 @@
continue;
}
- // Only try to fit points to this circle if all of these are on the top
- // half, like how the blobs should be
- if (circle->OnTopHalf(current_centroids[0]) &&
- circle->OnTopHalf(current_centroids[1]) &&
- circle->OnTopHalf(current_centroids[2])) {
+ // Only try to fit points to this circle if all of these are between
+ // certain angles.
+ if (circle->InAngleRange(current_centroids[0], kMinBlobAngle,
+ kMaxBlobAngle) &&
+ circle->InAngleRange(current_centroids[1], kMinBlobAngle,
+ kMaxBlobAngle) &&
+ circle->InAngleRange(current_centroids[2], kMinBlobAngle,
+ kMaxBlobAngle)) {
for (size_t m = 0; m < filtered_blobs.size(); m++) {
// Add this blob to the list if it is close to the circle, is on the
// top half, and isn't one of the other blobs
if ((m != i) && (m != j) && (m != k) &&
- circle->OnTopHalf(filtered_stats[m].centroid) &&
+ circle->InAngleRange(filtered_stats[m].centroid, kMinBlobAngle,
+ kMaxBlobAngle) &&
(circle->DistanceTo(filtered_stats[m].centroid) <
kCircleDistanceThreshold)) {
current_blobs.emplace_back(filtered_blobs[m]);
@@ -293,18 +311,16 @@
cv::circle(view_image, centroid, 3, cv::Scalar(255, 255, 0), cv::FILLED);
}
-void BlobDetector::ExtractBlobs(
- cv::Mat rgb_image, cv::Mat &binarized_image,
- std::vector<std::vector<cv::Point>> &filtered_blobs,
- std::vector<std::vector<cv::Point>> &unfiltered_blobs,
- std::vector<BlobStats> &blob_stats, cv::Point ¢roid) {
+void BlobDetector::ExtractBlobs(cv::Mat bgr_image,
+ BlobDetector::BlobResult *blob_result) {
auto start = aos::monotonic_clock::now();
- binarized_image = ThresholdImage(rgb_image);
- unfiltered_blobs = FindBlobs(binarized_image);
- blob_stats = ComputeStats(unfiltered_blobs);
- auto filtered_pair = FilterBlobs(unfiltered_blobs, blob_stats);
- filtered_blobs = filtered_pair.first;
- centroid = filtered_pair.second;
+ blob_result->binarized_image = ThresholdImage(bgr_image);
+ blob_result->unfiltered_blobs = FindBlobs(blob_result->binarized_image);
+ blob_result->blob_stats = ComputeStats(blob_result->unfiltered_blobs);
+ auto filtered_pair =
+ FilterBlobs(blob_result->unfiltered_blobs, blob_result->blob_stats);
+ blob_result->filtered_blobs = filtered_pair.first;
+ blob_result->centroid = filtered_pair.second;
auto end = aos::monotonic_clock::now();
LOG(INFO) << "Blob detection elapsed time: "
<< std::chrono::duration<double, std::milli>(end - start).count()
diff --git a/y2022/vision/blob_detector.h b/y2022/vision/blob_detector.h
index f8a4ab4..d263d32 100644
--- a/y2022/vision/blob_detector.h
+++ b/y2022/vision/blob_detector.h
@@ -16,11 +16,19 @@
size_t num_points;
};
+ struct BlobResult {
+ cv::Mat binarized_image;
+ std::vector<std::vector<cv::Point>> filtered_blobs, unfiltered_blobs;
+ std::vector<BlobStats> blob_stats;
+ cv::Point centroid;
+ };
+
BlobDetector() {}
+
// Given an image, threshold it to find "green" pixels
// Input: Color image
// Output: Grayscale (binarized) image with green pixels set to 255
- static cv::Mat ThresholdImage(cv::Mat rgb_image);
+ static cv::Mat ThresholdImage(cv::Mat bgr_image);
// Given binary image, extract blobs
static std::vector<std::vector<cv::Point>> FindBlobs(cv::Mat threshold_image);
@@ -44,11 +52,7 @@
const std::vector<std::vector<cv::Point>> &unfiltered_blobs,
const std::vector<BlobStats> &blob_stats, cv::Point centroid);
- static void ExtractBlobs(
- cv::Mat rgb_image, cv::Mat &binarized_image,
- std::vector<std::vector<cv::Point>> &filtered_blobs,
- std::vector<std::vector<cv::Point>> &unfiltered_blobs,
- std::vector<BlobStats> &blob_stats, cv::Point ¢roid);
+ static void ExtractBlobs(cv::Mat bgr_image, BlobResult *blob_result);
};
} // namespace vision
} // namespace y2022
diff --git a/y2022/vision/calib_files/calibration_pi-971-1_2021-09-11_17-49-00.000000000.json b/y2022/vision/calib_files/calibration_pi-971-1_2021-09-11_17-49-00.000000000.json
deleted file mode 100644
index 39c7911..0000000
--- a/y2022/vision/calib_files/calibration_pi-971-1_2021-09-11_17-49-00.000000000.json
+++ /dev/null
@@ -1,23 +0,0 @@
-{
- "node_name": "pi1",
- "team_number": 971,
- "intrinsics": [
- 392.276093,
- 0.0,
- 293.934753,
- 0.0,
- 392.30838,
- 212.287537,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- 0.149561,
- -0.261432,
- -0.000182,
- -0.000697,
- 0.099255
- ],
- "calibration_timestamp": 1597994992500905688
-}
diff --git a/y2022/vision/calib_files/calibration_pi-971-1_2022-02-06_15-19-00.000000000.json b/y2022/vision/calib_files/calibration_pi-971-1_2022-02-06_15-19-00.000000000.json
new file mode 100755
index 0000000..6a4f05c
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-971-1_2022-02-06_15-19-00.000000000.json
@@ -0,0 +1,23 @@
+{
+ "node_name": "pi1",
+ "team_number": 971,
+ "intrinsics": [
+ 398.312439,
+ 0.0,
+ 348.653015,
+ 0.0,
+ 397.627533,
+ 257.368805,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.143741,
+ -0.274336,
+ -0.000311,
+ -0.000171,
+ 0.10252
+ ],
+ "calibration_timestamp": 1635600750700335075
+}
diff --git a/y2022/vision/camera_reader.cc b/y2022/vision/camera_reader.cc
index e1de75a..abce18c 100644
--- a/y2022/vision/camera_reader.cc
+++ b/y2022/vision/camera_reader.cc
@@ -1,17 +1,16 @@
#include "y2022/vision/camera_reader.h"
-#include <cmath>
#include <chrono>
+#include <cmath>
#include <thread>
-#include <opencv2/imgproc.hpp>
-
#include "aos/events/event_loop.h"
#include "aos/events/shm_event_loop.h"
#include "aos/flatbuffer_merge.h"
#include "aos/network/team_number.h"
#include "frc971/vision/v4l2_reader.h"
#include "frc971/vision/vision_generated.h"
+#include "opencv2/imgproc.hpp"
#include "y2022/vision/blob_detector.h"
#include "y2022/vision/calibration_generated.h"
#include "y2022/vision/target_estimator.h"
@@ -82,25 +81,23 @@
} // namespace
void CameraReader::ProcessImage(cv::Mat image_mat) {
- // Remember, we're getting YUYV images, so we start by converting to RGB
-
- std::vector<std::vector<cv::Point>> filtered_blobs, unfiltered_blobs;
- std::vector<BlobDetector::BlobStats> blob_stats;
- cv::Mat binarized_image =
+ BlobDetector::BlobResult blob_result;
+ blob_result.binarized_image =
cv::Mat::zeros(cv::Size(image_mat.cols, image_mat.rows), CV_8UC1);
- cv::Point centroid;
- BlobDetector::ExtractBlobs(image_mat, binarized_image, filtered_blobs,
- unfiltered_blobs, blob_stats, centroid);
+ BlobDetector::ExtractBlobs(image_mat, &blob_result);
auto builder = target_estimate_sender_.MakeBuilder();
- flatbuffers::Offset<BlobResult> blob_result_offset;
+ flatbuffers::Offset<BlobResultFbs> blob_result_offset;
{
- const auto filtered_blobs_offset = CvBlobsToFbs(filtered_blobs, builder);
+ const auto filtered_blobs_offset =
+ CvBlobsToFbs(blob_result.filtered_blobs, builder);
const auto unfiltered_blobs_offset =
- CvBlobsToFbs(unfiltered_blobs, builder);
- const auto blob_stats_offset = BlobStatsToFbs(blob_stats, builder);
- const Point centroid_fbs = Point{centroid.x, centroid.y};
+ CvBlobsToFbs(blob_result.unfiltered_blobs, builder);
+ const auto blob_stats_offset =
+ BlobStatsToFbs(blob_result.blob_stats, builder);
+ const Point centroid_fbs =
+ Point{blob_result.centroid.x, blob_result.centroid.y};
- auto blob_result_builder = builder.MakeBuilder<BlobResult>();
+ auto blob_result_builder = builder.MakeBuilder<BlobResultFbs>();
blob_result_builder.add_filtered_blobs(filtered_blobs_offset);
blob_result_builder.add_unfiltered_blobs(unfiltered_blobs_offset);
blob_result_builder.add_blob_stats(blob_stats_offset);
@@ -109,9 +106,9 @@
}
auto target_estimate_builder = builder.MakeBuilder<TargetEstimate>();
- TargetEstimator::EstimateTargetLocation(centroid, CameraIntrinsics(),
- CameraExtrinsics(),
- &target_estimate_builder);
+ TargetEstimator::EstimateTargetLocation(
+ blob_result.centroid, CameraIntrinsics(), CameraExtrinsics(),
+ &target_estimate_builder);
target_estimate_builder.add_blob_result(blob_result_offset);
builder.CheckOk(builder.Send(target_estimate_builder.Finish()));
@@ -127,8 +124,8 @@
// that can be sent in a second.
std::this_thread::sleep_for(std::chrono::milliseconds(50));
LOG(INFO) << "Reading file " << file;
- cv::Mat rgb_image = cv::imread(file.c_str());
- ProcessImage(rgb_image);
+ cv::Mat bgr_image = cv::imread(file.c_str());
+ ProcessImage(bgr_image);
}
event_loop_->Exit();
return;
diff --git a/y2022/vision/target_estimate.fbs b/y2022/vision/target_estimate.fbs
index 207a37a..707014c 100644
--- a/y2022/vision/target_estimate.fbs
+++ b/y2022/vision/target_estimate.fbs
@@ -18,7 +18,7 @@
}
// Information for debugging blob detection
-table BlobResult {
+table BlobResultFbs {
// Blobs that passed the filtering step
filtered_blobs:[Blob] (id: 0);
// All detected blobs
@@ -37,7 +37,7 @@
// Positive means right of center, negative means left.
angle_to_target:double (id: 1);
- blob_result:BlobResult (id: 2);
+ blob_result:BlobResultFbs (id: 2);
// TODO(milind): add confidence
}
diff --git a/y2022/vision/viewer.cc b/y2022/vision/viewer.cc
index 7c13d1b..c9c2aff 100644
--- a/y2022/vision/viewer.cc
+++ b/y2022/vision/viewer.cc
@@ -68,41 +68,47 @@
// TODO(Milind) Store the target estimates and match them by timestamp to make
// sure we're getting the right one.
- CHECK(target_estimate_fetcher.FetchNext());
- const TargetEstimate *target = target_estimate_fetcher.get();
+ const TargetEstimate *target_est = nullptr;
+ if (target_estimate_fetcher.Fetch()) {
+ target_est = target_estimate_fetcher.get();
+ }
// Create color image:
cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2,
(void *)image->data()->data());
- cv::Mat rgb_image(cv::Size(image->cols(), image->rows()), CV_8UC3);
- cv::cvtColor(image_color_mat, rgb_image, cv::COLOR_YUV2BGR_YUYV);
+ cv::Mat bgr_image(cv::Size(image->cols(), image->rows()), CV_8UC3);
+ cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2RGB_YUYV);
if (!FLAGS_capture.empty()) {
- cv::imwrite(FLAGS_capture, rgb_image);
+ cv::imwrite(FLAGS_capture, bgr_image);
return false;
}
- LOG(INFO) << image->monotonic_timestamp_ns()
- << ": # blobs: " << target->blob_result()->filtered_blobs()->size();
+ LOG(INFO) << image->monotonic_timestamp_ns() << ": # unfiltered blobs: "
+ << target_est->blob_result()->unfiltered_blobs()->size()
+ << "; # filtered blobs: "
+ << target_est->blob_result()->filtered_blobs()->size();
- cv::Mat ret_image;
- BlobDetector::DrawBlobs(
- ret_image, FbsToCvBlobs(*target->blob_result()->filtered_blobs()),
- FbsToCvBlobs(*target->blob_result()->unfiltered_blobs()),
- FbsToBlobStats(*target->blob_result()->blob_stats()),
- cv::Point{target->blob_result()->centroid()->x(),
- target->blob_result()->centroid()->y()});
+ cv::Mat ret_image(cv::Size(image->cols(), image->rows()), CV_8UC3);
+ if (target_est != nullptr) {
+ BlobDetector::DrawBlobs(
+ ret_image, FbsToCvBlobs(*target_est->blob_result()->filtered_blobs()),
+ FbsToCvBlobs(*target_est->blob_result()->unfiltered_blobs()),
+ FbsToBlobStats(*target_est->blob_result()->blob_stats()),
+ cv::Point{target_est->blob_result()->centroid()->x(),
+ target_est->blob_result()->centroid()->y()});
+ cv::imshow("blobs", ret_image);
+ }
- cv::imshow("image", rgb_image);
- cv::imshow("blobs", ret_image);
+ cv::imshow("image", bgr_image);
int keystroke = cv::waitKey(1);
if ((keystroke & 0xFF) == static_cast<int>('c')) {
// Convert again, to get clean image
- cv::cvtColor(image_color_mat, rgb_image, cv::COLOR_YUV2BGR_YUYV);
+ cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
std::stringstream name;
name << "capture-" << aos::realtime_clock::now() << ".png";
- cv::imwrite(name.str(), rgb_image);
+ cv::imwrite(name.str(), bgr_image);
LOG(INFO) << "Saved image file: " << name.str();
} else if ((keystroke & 0xFF) == static_cast<int>('q')) {
return false;
@@ -119,6 +125,9 @@
image_fetcher =
event_loop.MakeFetcher<frc971::vision::CameraImage>(FLAGS_channel);
+ target_estimate_fetcher =
+ event_loop.MakeFetcher<y2022::vision::TargetEstimate>(FLAGS_channel);
+
// Run the display loop
event_loop.AddPhasedLoop(
[&event_loop](int) {