Merge "Filling out the status, goal and output messages for the 3rd robot"
diff --git a/aos/events/logging/log_namer.cc b/aos/events/logging/log_namer.cc
index f13b215..dc71348 100644
--- a/aos/events/logging/log_namer.cc
+++ b/aos/events/logging/log_namer.cc
@@ -132,9 +132,9 @@
CHECK_EQ(log_namer_->monotonic_start_time(node_index_, source_node_boot_uuid),
monotonic_start_time_);
CHECK_EQ(state_[node_index_].boot_uuid, source_node_boot_uuid);
+ CHECK(writer);
CHECK(header_written_) << ": Attempting to write message before header to "
<< writer->filename();
- CHECK(writer);
writer->QueueSizedFlatbuffer(fbb, now);
}
diff --git a/aos/events/logging/log_reader.cc b/aos/events/logging/log_reader.cc
index a1269e1..5fe61cd 100644
--- a/aos/events/logging/log_reader.cc
+++ b/aos/events/logging/log_reader.cc
@@ -825,6 +825,9 @@
// all. Read the rest of the messages and drop them on the floor while
// doing some basic validation.
while (state->OldestMessageTime() != BootTimestamp::max_time()) {
+ // TODO(austin): This force queues up the rest of the log file for all
+ // the other nodes. We should do this through the timer instead to
+ // keep memory usage down.
TimestampedMessage next = state->PopOldest();
// Make sure that once we have seen the last message on a channel,
// data doesn't start back up again. If the user wants to play
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index 9522351..6d73d67 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -705,9 +705,13 @@
size_t monotonic_remote_boot = 0xffffff;
if (m.value().message().has_monotonic_remote_time()) {
+ const Node *node = parts().config->nodes()->Get(
+ source_node_index_[m->message().channel_index()]);
+
std::optional<size_t> boot = parts_message_reader_.boot_count(
source_node_index_[m->message().channel_index()]);
- CHECK(boot) << ": Failed to find boot for node "
+ CHECK(boot) << ": Failed to find boot for node " << MaybeNodeName(node)
+ << ", with index "
<< source_node_index_[m->message().channel_index()];
monotonic_remote_boot = *boot;
}
diff --git a/aos/network/web_proxy.cc b/aos/network/web_proxy.cc
index b53feab..972b1d7 100644
--- a/aos/network/web_proxy.cc
+++ b/aos/network/web_proxy.cc
@@ -256,6 +256,12 @@
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(data_channel, info);
}
@@ -542,7 +548,14 @@
configuration::GetChannel(event_loop_->configuration(), channel,
event_loop_->name(), event_loop_->node());
if (comparison_channel == nullptr) {
- LOG(ERROR) << "Channel not available: "
+ LOG(ERROR) << "Channel does not exist: "
+ << configuration::StrippedChannelToString(channel);
+ continue;
+ }
+ if (!configuration::ChannelIsReadableOnNode(comparison_channel,
+ event_loop_->node())) {
+ LOG(ERROR) << "Channel not available on node "
+ << event_loop_->node()->name()->string_view() << ": "
<< configuration::StrippedChannelToString(channel);
continue;
}
diff --git a/third_party/rawrtc/rawrtc/src/diffie_hellman_parameters/parameters.c b/third_party/rawrtc/rawrtc/src/diffie_hellman_parameters/parameters.c
index 7783e59..952511f 100644
--- a/third_party/rawrtc/rawrtc/src/diffie_hellman_parameters/parameters.c
+++ b/third_party/rawrtc/rawrtc/src/diffie_hellman_parameters/parameters.c
@@ -20,6 +20,12 @@
struct ssl_ctx_st* const ssl_context, // not checked
DH const* const dh // not checked
) {
+
+ // Running DH_check on the roborio is obnoxious expensive (~40-50 seconds,
+ // optimized); just YOLO it. Note that this could probably be moved to
+ // somewhere where the cost could be incurred at startup instead of
+ // on connection (or even cached at build-time).
+#ifndef AOS_ARCHITECTURE_arm_frc
int codes;
// Check that the parameters are "likely enough to be valid"
@@ -70,6 +76,9 @@
#endif
return RAWRTC_CODE_INVALID_ARGUMENT;
}
+#else
+ dbg_warning("Skipping DH_check() due to performance concerns.\n");
+#endif
// Apply Diffie-Hellman parameters
if (!SSL_CTX_set_tmp_dh(ssl_context, dh)) {
diff --git a/y2020/BUILD b/y2020/BUILD
index ab03ea3..45164b4 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -7,15 +7,18 @@
robot_downloader(
binaries = [
":setpoint_setter",
+ "//aos/network:web_proxy_main",
],
data = [
":config",
],
dirs = [
"//y2020/actors:splines",
+ "//y2020/www:www_files",
],
start_binaries = [
"//aos/events/logging:logger_main",
+ "//aos/network:web_proxy_main",
":joystick_reader",
":wpilib_interface",
"//aos/network:message_bridge_client",
@@ -251,6 +254,19 @@
)
sh_binary(
+ name = "log_web_proxy",
+ srcs = ["log_web_proxy.sh"],
+ data = [
+ ":config",
+ "//aos/network:log_web_proxy_main",
+ "//y2020/www:camera_main_bundle.min.js",
+ "//y2020/www:field_main_bundle.min.js",
+ "//y2020/www:files",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+sh_binary(
name = "web_proxy",
srcs = ["web_proxy.sh"],
data = [
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index 73be810..5ea681c 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -1,6 +1,6 @@
package(default_visibility = ["//visibility:public"])
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_ts_library")
load("@npm_bazel_typescript//:defs.bzl", "ts_library")
flatbuffer_cc_library(
@@ -25,6 +25,18 @@
target_compatible_with = ["@platforms//os:linux"],
)
+flatbuffer_ts_library(
+ name = "superstructure_status_ts_fbs",
+ srcs = [
+ "superstructure_status.fbs",
+ ],
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
flatbuffer_cc_library(
name = "superstructure_status_fbs",
srcs = [
diff --git a/y2020/log_web_proxy.sh b/y2020/log_web_proxy.sh
new file mode 100755
index 0000000..64d86b6
--- /dev/null
+++ b/y2020/log_web_proxy.sh
@@ -0,0 +1 @@
+./aos/network/log_web_proxy_main --data_dir=y2020/www $@
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index ed2b4b9..d6f3716 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -19,6 +19,8 @@
DEFINE_string(config, "config.json", "Path to the config file to use.");
DEFINE_bool(skip_sift, false,
"If true don't run any feature extraction. Just forward images.");
+DEFINE_bool(ransac_pose, false,
+ "If true, do pose estimate with RANSAC; else, use ITERATIVE mode.");
namespace frc971 {
namespace vision {
@@ -436,9 +438,27 @@
cv::Mat R_field_camera_vec, R_field_camera, T_field_camera;
// Compute the pose of the camera (global origin relative to camera)
- cv::solvePnPRansac(per_image_good_match.training_points_3d,
- per_image_good_match.query_points, CameraIntrinsics(),
- CameraDistCoeffs(), R_camera_field_vec, T_camera_field);
+ if (FLAGS_ransac_pose) {
+ // RANSAC computation is designed to be more robust to outliers.
+ // But, we found it bounces around a lot, even with identical points
+ cv::solvePnPRansac(per_image_good_match.training_points_3d,
+ per_image_good_match.query_points, CameraIntrinsics(),
+ CameraDistCoeffs(), R_camera_field_vec,
+ T_camera_field);
+ } else {
+ // ITERATIVE mode is potentially less robust to outliers, but we
+ // found it to be more stable
+ //
+ // TODO<Jim>: We should explore feeding an initial estimate into
+ // the pose calculations. I found this to help guide the solution
+ // to something hearby, but it also can lock it into an incorrect
+ // solution
+ cv::solvePnP(per_image_good_match.training_points_3d,
+ per_image_good_match.query_points, CameraIntrinsics(),
+ CameraDistCoeffs(), R_camera_field_vec, T_camera_field,
+ false, CV_ITERATIVE);
+ }
+
CHECK_EQ(cv::Size(1, 3), T_camera_field.size());
// Convert to float32's (from float64) to be compatible with the rest
diff --git a/y2020/vision/tools/python_code/target_definition.py b/y2020/vision/tools/python_code/target_definition.py
index be7e223..bb19b9e 100644
--- a/y2020/vision/tools/python_code/target_definition.py
+++ b/y2020/vision/tools/python_code/target_definition.py
@@ -243,6 +243,7 @@
power_port_red_main_panel_polygon_points_2d)
ideal_power_port_red.polygon_list_3d.append(
power_port_red_main_panel_polygon_points_3d)
+ # NOTE: We are currently not using the wing, since our actual targets are all planar
# Define the pose of the target
# Location is on the ground, at the center of the target
@@ -422,28 +423,32 @@
training_target_loading_bay_blue.target_radius = target_radius_default
######################################################################
- # Generate lists of ideal and training targets based on all the
- # definitions above
+ # DEFINE the targets here. Generate lists of ideal and training
+ # targets based on all the definitions above
######################################################################
- ### Taped power port
- ideal_target_list.append(ideal_power_port_taped)
- training_target_list.append(training_target_power_port_taped)
+ ### Taped power port (not currently used)
+ #glog.info("Adding hacked/taped up power port to the model list")
+ #ideal_target_list.append(ideal_power_port_taped)
+ #training_target_list.append(training_target_power_port_taped)
### Red Power Port
- ### NOTE: Temporarily taking this out of the list
- #ideal_target_list.append(ideal_power_port_red)
- #training_target_list.append(training_target_power_port_red)
+ glog.info("Adding red power port to the model list")
+ ideal_target_list.append(ideal_power_port_red)
+ training_target_list.append(training_target_power_port_red)
### Red Loading Bay
+ glog.info("Adding red loading bay to the model list")
ideal_target_list.append(ideal_loading_bay_red)
training_target_list.append(training_target_loading_bay_red)
### Blue Power Port
+ #glog.info("Adding blue power port to the model list")
#ideal_target_list.append(ideal_power_port_blue)
#training_target_list.append(training_target_power_port_blue)
### Blue Loading Bay
+ glog.info("Adding blue loading bay to the model list")
ideal_target_list.append(ideal_loading_bay_blue)
training_target_list.append(training_target_loading_bay_blue)
diff --git a/y2020/www/BUILD b/y2020/www/BUILD
index 2f26122..8760fde 100644
--- a/y2020/www/BUILD
+++ b/y2020/www/BUILD
@@ -35,6 +35,7 @@
"//aos/network:web_proxy_ts_fbs",
"//aos/network/www:proxy",
"//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
+ "//y2020/control_loops/superstructure:superstructure_status_ts_fbs",
"//y2020/vision/sift:sift_ts_fbs",
"@com_github_google_flatbuffers//ts:flatbuffers_ts",
],
diff --git a/y2020/www/field_handler.ts b/y2020/www/field_handler.ts
index b2daea1..4aaa05f 100644
--- a/y2020/www/field_handler.ts
+++ b/y2020/www/field_handler.ts
@@ -5,8 +5,10 @@
import * as drivetrain from 'org_frc971/frc971/control_loops/drivetrain/drivetrain_status_generated';
import * as sift from 'org_frc971/y2020/vision/sift/sift_generated';
import * as web_proxy from 'org_frc971/aos/network/web_proxy_generated';
+import * as ss from 'org_frc971/y2020/control_loops/superstructure/superstructure_status_generated'
import DrivetrainStatus = drivetrain.frc971.control_loops.drivetrain.Status;
+import SuperstructureStatus = ss.y2020.control_loops.superstructure.Status;
import ImageMatchResult = sift.frc971.vision.sift.ImageMatchResult;
import Channel = configuration.aos.Channel;
import SubscriberRequest = web_proxy.aos.web_proxy.SubscriberRequest;
@@ -59,30 +61,44 @@
const ROBOT_WIDTH = 28 * IN_TO_M;
const ROBOT_LENGTH = 30 * IN_TO_M;
+
export class FieldHandler {
private canvas = document.createElement('canvas');
- private imageMatchResult: ImageMatchResult|null = null;
+ private imageMatchResult = new Map<string, ImageMatchResult>();
private drivetrainStatus: DrivetrainStatus|null = null;
+ private superstructureStatus: SuperstructureStatus|null = null;
constructor(private readonly connection: Connection) {
document.body.appendChild(this.canvas);
this.connection.addConfigHandler(() => {
- this.connection.addHandler(
- '/camera', ImageMatchResult.getFullyQualifiedName(), (res) => {
- this.handleImageMatchResult(res);
- });
+ // Go through and register handlers for both all the individual pis as
+ // well as the local pi. Depending on the node that we are running on,
+ // different subsets of these will be available.
+ for (const prefix of ['', '/pi1', '/pi2', '/pi3', '/pi4']) {
+ this.connection.addHandler(
+ prefix + '/camera', ImageMatchResult.getFullyQualifiedName(), (res) => {
+ this.handleImageMatchResult(prefix, res);
+ });
+ }
this.connection.addHandler(
'/drivetrain', DrivetrainStatus.getFullyQualifiedName(), (data) => {
this.handleDrivetrainStatus(data);
});
+ this.connection.addHandler(
+ '/superstructure', SuperstructureStatus.getFullyQualifiedName(),
+ (data) => {
+ this.handleSuperstructureStatus(data);
+ });
});
}
- private handleImageMatchResult(data: Uint8Array): void {
+ private handleImageMatchResult(prefix: string, data: Uint8Array): void {
const fbBuffer = new ByteBuffer(data);
- this.imageMatchResult = ImageMatchResult.getRootAsImageMatchResult(
- fbBuffer as unknown as flatbuffers.ByteBuffer);
+ this.imageMatchResult.set(
+ prefix,
+ ImageMatchResult.getRootAsImageMatchResult(
+ fbBuffer as unknown as flatbuffers.ByteBuffer));
}
private handleDrivetrainStatus(data: Uint8Array): void {
@@ -91,6 +107,12 @@
fbBuffer as unknown as flatbuffers.ByteBuffer);
}
+ private handleSuperstructureStatus(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.superstructureStatus = SuperstructureStatus.getRootAsStatus(
+ fbBuffer as unknown as flatbuffers.ByteBuffer);
+ }
+
drawField(): void {
const MY_COLOR = 'red';
const OTHER_COLOR = 'blue';
@@ -177,13 +199,29 @@
ctx.restore();
}
- drawRobot(x: number, y: number, theta: number): void {
+ drawRobot(x: number, y: number, theta: number, turret: number|null): void {
const ctx = this.canvas.getContext('2d');
ctx.save();
ctx.translate(x, y);
ctx.rotate(theta);
ctx.rect(-ROBOT_LENGTH / 2, -ROBOT_WIDTH / 2, ROBOT_LENGTH, ROBOT_WIDTH);
ctx.stroke();
+ if (turret) {
+ ctx.save();
+ ctx.rotate(turret + Math.PI);
+ const turretRadius = ROBOT_WIDTH / 4.0;
+ ctx.strokeStyle = "red";
+ // Draw circle for turret.
+ ctx.beginPath();
+ ctx.arc(0, 0, turretRadius, 0, 2.0 * Math.PI);
+ ctx.stroke();
+ // Draw line in circle to show forwards.
+ ctx.beginPath();
+ ctx.moveTo(0, 0);
+ ctx.lineTo(turretRadius, 0);
+ ctx.stroke();
+ ctx.restore();
+ }
ctx.beginPath();
ctx.moveTo(0, 0);
ctx.lineTo(ROBOT_LENGTH / 2, 0);
@@ -195,15 +233,19 @@
this.reset();
this.drawField();
// draw cameras
- if (this.imageMatchResult) {
- for (let i = 0; i < this.imageMatchResult.cameraPosesLength(); i++) {
- const pose = this.imageMatchResult.cameraPoses(i);
+ for (const keyPair of this.imageMatchResult) {
+ const value = keyPair[1];
+ for (let i = 0; i < value.cameraPosesLength(); i++) {
+ const pose = value.cameraPoses(i);
const mat = pose.fieldToCamera();
+ // Matrix layout:
+ // [0, 1, 2, 3]
+ // [4, 5, 6, 7]
+ // [8, 9, 10, 11]
+ // [12, 13, 14, 15]
const x = mat.data(3);
const y = mat.data(7);
- const theta = Math.atan2(
- -mat.data(8),
- Math.sqrt(Math.pow(mat.data(9), 2) + Math.pow(mat.data(10), 2)));
+ const theta = Math.atan2(mat.data(6), mat.data(2));
this.drawCamera(x, y, theta);
}
}
@@ -211,7 +253,10 @@
if (this.drivetrainStatus) {
this.drawRobot(
this.drivetrainStatus.x(), this.drivetrainStatus.y(),
- this.drivetrainStatus.theta());
+ this.drivetrainStatus.theta(),
+ this.superstructureStatus ?
+ this.superstructureStatus.turret().position() :
+ null);
}
window.requestAnimationFrame(() => this.draw());
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index 105ffa6..5243d5a 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -224,7 +224,7 @@
"frequency": 4,
"num_senders": 2,
"read_method": "PIN",
- "num_readers": 6
+ "num_readers": 10
},
{
"name": "/drivetrain",
@@ -349,6 +349,13 @@
]
},
{
+ "name": "web_proxy",
+ "executable_name": "web_proxy_main.stripped",
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
"name": "message_bridge_client",
"executable_name": "message_bridge_client.stripped",
"nodes": [