Merge "Adding button for import and export Spline GUI"
diff --git a/aos/network/web_proxy.cc b/aos/network/web_proxy.cc
index f4da7d9..5b50072 100644
--- a/aos/network/web_proxy.cc
+++ b/aos/network/web_proxy.cc
@@ -67,6 +67,10 @@
rtc::CopyOnWriteBuffer(buffer.data(), buffer.size()),
true /* binary array */);
for (auto conn : channels_) {
+ if (conn->buffered_amount() > 14000000) {
+ VLOG(1) << "skipping a send because buffered amount is too high";
+ continue;
+ }
conn->Send(data_buffer);
}
}
@@ -160,7 +164,7 @@
webrtc::DataBuffer data_buffer(
rtc::CopyOnWriteBuffer(buffer.data(), buffer.size()),
true /* binary array */);
- VLOG(2) << "Sending " << buffer.size() << "bytes to a client";
+ VLOG(1) << "Sending " << buffer.size() << "bytes to a client";
data_channel_->Send(data_buffer);
}
diff --git a/aos/network/web_proxy_main.cc b/aos/network/web_proxy_main.cc
index 0e49760..c674520 100644
--- a/aos/network/web_proxy_main.cc
+++ b/aos/network/web_proxy_main.cc
@@ -24,7 +24,6 @@
LOG(INFO) << "My node is " << aos::FlatbufferToJson(self);
- // TODO(alex): skip fetchers on the wrong node.
for (uint i = 0; i < config.message().channels()->size(); ++i) {
auto channel = config.message().channels()->Get(i);
if (aos::configuration::ChannelIsReadableOnNode(channel, self)) {
diff --git a/aos/network/www/config_handler.ts b/aos/network/www/config_handler.ts
index 3d20f0d..6615f39 100644
--- a/aos/network/www/config_handler.ts
+++ b/aos/network/www/config_handler.ts
@@ -3,13 +3,14 @@
import {Connection} from './proxy';
export class ConfigHandler {
- private readonly root_div = document.getElementById('config');
+ private readonly root_div = document.createElement('div');
private readonly tree_div;
private config: Configuration|null = null
constructor(private readonly connection: Connection) {
this.connection.addConfigHandler((config) => this.handleConfig(config));
+ document.body.appendChild(this.root_div);
const show_button = document.createElement('button');
show_button.addEventListener('click', () => this.toggleConfig());
const show_text = document.createTextNode('Show/Hide Config');
diff --git a/aos/network/www/proxy.ts b/aos/network/www/proxy.ts
index 9eb0bb6..7bcf575 100644
--- a/aos/network/www/proxy.ts
+++ b/aos/network/www/proxy.ts
@@ -66,6 +66,11 @@
this.configHandlers.add(handler);
}
+ /**
+ * Add a handler for a specific message type. Until we need to handle
+ * different channel names with the same type differently, this is good
+ * enough.
+ */
addHandler(id: string, handler: (data: Uint8Array) => void): void {
this.handlerFuncs.set(id, handler);
}
@@ -79,7 +84,7 @@
'message', (e) => this.onWebSocketMessage(e));
}
- get config() {
+ getConfig() {
return this.config_internal;
}
@@ -88,7 +93,7 @@
onDataChannelMessage(e: MessageEvent): void {
const fbBuffer = new flatbuffers.ByteBuffer(new Uint8Array(e.data));
this.configInternal = Configuration.getRootAsConfiguration(fbBuffer);
- for (handler of this.configHandlers) {
+ for (const handler of Array.from(this.configHandlers)) {
handler(this.configInternal);
}
}
@@ -183,11 +188,12 @@
}
/**
- * Subscribes to messages.
+ * Subscribes to messages. Only the most recent connect message is in use. Any
+ * channels not specified in the message are implicitely unsubscribed.
* @param a Finished flatbuffer.Builder containing a Connect message to send.
*/
sendConnectMessage(builder: any) {
- const array = builder.assUint8Array();
+ const array = builder.asUint8Array();
this.dataChannel.send(array.buffer.slice(array.byteOffset));
}
}
diff --git a/frc971/analysis/plot_configs/localizer.pb b/frc971/analysis/plot_configs/localizer.pb
index 644b516..930d63d 100644
--- a/frc971/analysis/plot_configs/localizer.pb
+++ b/frc971/analysis/plot_configs/localizer.pb
@@ -185,6 +185,18 @@
line {
y_signal {
channel: "DrivetrainStatus"
+ field: "trajectory_logging.left_velocity"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "trajectory_logging.right_velocity"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
field: "localizer.left_velocity"
}
}
diff --git a/frc971/constants.h b/frc971/constants.h
index debfb55..7c2121d 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -97,9 +97,9 @@
double lower;
double upper;
- double middle() const { return (lower_hard + upper_hard) / 2.0; }
+ constexpr double middle() const { return (lower_hard + upper_hard) / 2.0; }
- double range() const { return upper_hard - lower_hard; }
+ constexpr double range() const { return upper_hard - lower_hard; }
};
} // namespace constants
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 30abc87..d223d13 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -562,7 +562,7 @@
":trajectory",
"//aos/logging:implementations",
"//aos/network:team_number",
- "//y2019/control_loops/drivetrain:drivetrain_base",
+ "//y2020/control_loops/drivetrain:drivetrain_base",
"@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index d2c0be3..cb50165 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -146,7 +146,7 @@
static constexpr int kNOutputs = 3;
// Time constant to use for estimating how the longitudinal/lateral velocity
// offsets decay, in seconds.
- static constexpr double kVelocityOffsetTimeConstant = 10.0;
+ static constexpr double kVelocityOffsetTimeConstant = 1.0;
static constexpr double kLateralVelocityTimeConstant = 1.0;
// Inputs are [left_volts, right_volts]
typedef Eigen::Matrix<Scalar, kNInputs, 1> Input;
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index 5ad175e..a9820f7 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -8,7 +8,7 @@
#include "frc971/control_loops/drivetrain/distance_spline.h"
#include "frc971/control_loops/drivetrain/spline.h"
#include "frc971/control_loops/drivetrain/trajectory.h"
-#include "y2019/control_loops/drivetrain/drivetrain_base.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
namespace frc971 {
namespace control_loops {
@@ -124,7 +124,7 @@
Trajectory *NewTrajectory(DistanceSpline *spline, double vmax,
int num_distance) {
return new Trajectory(
- spline, ::y2019::control_loops::drivetrain::GetDrivetrainConfig(), vmax,
+ spline, ::y2020::control_loops::drivetrain::GetDrivetrainConfig(), vmax,
num_distance);
}
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index b28334c..051e71a 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -138,7 +138,7 @@
(::Eigen::DiagonalMatrix<double, 5>().diagonal()
<< 1.0 / ::std::pow(0.12, 2),
1.0 / ::std::pow(0.12, 2), 1.0 / ::std::pow(0.1, 2),
- 1.0 / ::std::pow(1.5, 2), 1.0 / ::std::pow(1.5, 2))
+ 1.0 / ::std::pow(1.0, 2), 1.0 / ::std::pow(1.0, 2))
.finished()
.asDiagonal();
const ::Eigen::DiagonalMatrix<double, 2> R =
diff --git a/frc971/control_loops/pose.h b/frc971/control_loops/pose.h
index 20672cf..ab8afd0 100644
--- a/frc971/control_loops/pose.h
+++ b/frc971/control_loops/pose.h
@@ -42,6 +42,8 @@
// position of (10, -5, 0) and a yaw of pi / 2, that suggests the robot is
// facing straight to the left from the driver's perspective and is placed 10m
// from the driver's station wall and 5m to the right of the center of the wall.
+// For 2020, we move the origin to be the center of the field and make positive
+// x always point towards the red alliance driver stations.
//
// Furthermore, Poses can be chained such that a Pose can be placed relative to
// another Pose; the other Pose can dynamically update, thus allowing us to,
@@ -134,7 +136,7 @@
// If new_base == nullptr, provides a Pose referenced to the global frame.
// Note that the lifetime of new_base should be greater than the lifetime of
// the returned object (unless new_base == nullptr).
- TypedPose Rebase(const TypedPose<Scalar> *new_base) const;
+ [[nodiscard]] TypedPose Rebase(const TypedPose<Scalar> *new_base) const;
// Convert this pose to the heading/distance/skew numbers that we
// traditionally use for EKF corrections.
diff --git a/frc971/control_loops/python/lib_spline_test.py b/frc971/control_loops/python/lib_spline_test.py
index d544cbb..b4efe72 100644
--- a/frc971/control_loops/python/lib_spline_test.py
+++ b/frc971/control_loops/python/lib_spline_test.py
@@ -32,7 +32,7 @@
trajectory = Trajectory(dSpline)
trajectory.Plan()
plan = trajectory.GetPlanXVA(5.05*1e-3)
- self.assertEqual(plan.shape, (3, 617))
+ self.assertEqual(plan.shape, (3, 624))
def testLimits(self):
""" A plan with a lower limit should take longer. """
@@ -44,7 +44,7 @@
trajectory.LimitVelocity(0, trajectory.Length(), 3)
trajectory.Plan()
plan = trajectory.GetPlanXVA(5.05*1e-3)
- self.assertEqual(plan.shape, (3, 650))
+ self.assertEqual(plan.shape, (3, 656))
if __name__ == '__main__':
diff --git a/y2020/BUILD b/y2020/BUILD
index 4aa851f..bfa6ce2 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -4,6 +4,9 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
robot_downloader(
+ binaries = [
+ ":setpoint_setter",
+ ],
data = [
":config.json",
],
@@ -114,6 +117,7 @@
":joystick_reader.cc",
],
deps = [
+ ":setpoint_fbs",
"//aos:init",
"//aos/actions:action_lib",
"//aos/input:action_joystick_input",
@@ -134,6 +138,7 @@
name = "config",
src = "y2020.json",
flatbuffers = [
+ ":setpoint_fbs",
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
"//aos/network:timestamp_fbs",
@@ -173,3 +178,23 @@
"//y2020/www:main_bundle",
],
)
+
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+flatbuffer_cc_library(
+ name = "setpoint_fbs",
+ srcs = [
+ "setpoint.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+cc_binary(
+ name = "setpoint_setter",
+ srcs = ["setpoint_setter.cc"],
+ deps = [
+ ":setpoint_fbs",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
diff --git a/y2020/actors/auto_splines.cc b/y2020/actors/auto_splines.cc
index 86eed14..3498a3e 100644
--- a/y2020/actors/auto_splines.cc
+++ b/y2020/actors/auto_splines.cc
@@ -59,16 +59,16 @@
{longitudinal_constraint_offset, lateral_constraint_offset,
voltage_constraint_offset});
- const float startx = 0.4;
- const float starty = 3.4;
+ const float startx = 0.0;
+ const float starty = 0.05;
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
- builder->fbb()->CreateVector<float>({0.0f + startx, 0.6f + startx,
- 0.6f + startx, 0.4f + startx,
- 0.4f + startx, 1.0f + startx});
+ builder->fbb()->CreateVector<float>({0.0f + startx, 0.8f + startx,
+ 0.8f + startx, 1.2f + startx,
+ 1.2f + startx, 2.0f + startx});
flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
builder->fbb()->CreateVector<float>({starty - 0.0f, starty - 0.0f,
- starty - 0.3f, starty - 0.7f,
- starty - 1.0f, starty - 1.0f});
+ starty - 0.1f, starty - 0.2f,
+ starty - 0.3f, starty - 0.3f});
frc971::MultiSpline::Builder multispline_builder =
builder->MakeBuilder<frc971::MultiSpline>();
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index d015c65..e87e064 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -8,6 +8,7 @@
#include "aos/logging/logging.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+#include "y2020/actors/auto_splines.h"
namespace y2020 {
namespace actors {
@@ -19,17 +20,43 @@
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
: frc971::autonomous::BaseAutonomousActor(
- event_loop, control_loops::drivetrain::GetDrivetrainConfig()) {}
+ event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
+ localizer_control_sender_(event_loop->MakeSender<
+ ::frc971::control_loops::drivetrain::LocalizerControl>(
+ "/drivetrain")) {}
void AutonomousActor::Reset() {
InitializeEncoders();
ResetDrivetrain();
+
+ {
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ // TODO(james): Set starting position based on the alliance.
+ localizer_control_builder.add_x(0.0);
+ localizer_control_builder.add_y(0.0);
+ localizer_control_builder.add_theta(0.0);
+ localizer_control_builder.add_theta_uncertainty(0.00001);
+ if (!builder.Send(localizer_control_builder.Finish())) {
+ AOS_LOG(ERROR, "Failed to reset localizer.\n");
+ }
+ }
}
bool AutonomousActor::RunAction(
const ::frc971::autonomous::AutonomousActionParams *params) {
Reset();
+ SplineHandle spline1 =
+ PlanSpline(AutonomousSplines::BasicSSpline, SplineDirection::kForward);
+
+ if (!spline1.WaitForPlan()) return true;
+ spline1.Start();
+
+ if (!spline1.WaitForSplineDistanceRemaining(0.02)) return true;
+
AOS_LOG(INFO, "Params are %d\n", params->mode());
return true;
}
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index 3bfa610..d86feab 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -6,6 +6,7 @@
#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
namespace y2020 {
namespace actors {
@@ -19,6 +20,9 @@
private:
void Reset();
+
+ ::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
+ localizer_control_sender_;
};
} // namespace actors
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay.cc b/y2020/control_loops/drivetrain/drivetrain_replay.cc
index af6a841..faf2cef 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay.cc
@@ -41,15 +41,19 @@
"frc971.control_loops.drivetrain.Output");
reader.Register();
+ const aos::Node *node = nullptr;
+ if (aos::configuration::MultiNode(reader.configuration())) {
+ node = aos::configuration::GetNode(reader.configuration(), "roborio");
+ }
+
aos::logger::DetachedBufferWriter file_writer(FLAGS_output_file);
std::unique_ptr<aos::EventLoop> log_writer_event_loop =
- reader.event_loop_factory()->MakeEventLoop("log_writer");
+ reader.event_loop_factory()->MakeEventLoop("log_writer", node);
log_writer_event_loop->SkipTimingReport();
- CHECK(nullptr == log_writer_event_loop->node());
aos::logger::Logger writer(&file_writer, log_writer_event_loop.get());
std::unique_ptr<aos::EventLoop> drivetrain_event_loop =
- reader.event_loop_factory()->MakeEventLoop("drivetrain");
+ reader.event_loop_factory()->MakeEventLoop("drivetrain", node);
drivetrain_event_loop->SkipTimingReport();
y2020::control_loops::drivetrain::Localizer localizer(
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index d81b6b5..4534404 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -62,8 +62,11 @@
":superstructure_status_fbs",
"//aos/controls:control_loop",
"//aos/events:event_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2020:constants",
"//y2020/control_loops/superstructure/shooter",
+ "//y2020/control_loops/superstructure/turret:aiming",
],
)
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 5dfd2d1..8947f85 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -16,7 +16,11 @@
hood_(constants::GetValues().hood),
intake_joint_(constants::GetValues().intake),
turret_(constants::GetValues().turret.subsystem_params),
- shooter_() {
+ drivetrain_status_fetcher_(
+ event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
+ joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -34,6 +38,22 @@
const aos::monotonic_clock::time_point position_timestamp =
event_loop()->context().monotonic_event_time;
+ if (drivetrain_status_fetcher_.Fetch()) {
+ aos::Alliance alliance = aos::Alliance::kInvalid;
+ if (joystick_state_fetcher_.Fetch()) {
+ alliance = joystick_state_fetcher_->alliance();
+ }
+ const turret::Aimer::WrapMode mode =
+ (unsafe_goal != nullptr && unsafe_goal->shooting())
+ ? turret::Aimer::WrapMode::kAvoidWrapping
+ : turret::Aimer::WrapMode::kAvoidEdges;
+ aimer_.Update(drivetrain_status_fetcher_.get(), alliance, mode,
+ turret::Aimer::ShotMode::kShootOnTheFly);
+ }
+
+ const flatbuffers::Offset<AimerStatus> aimer_status_offset =
+ aimer_.PopulateStatus(status->fbb());
+
OutputT output_struct;
flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> hood_status_offset =
@@ -73,10 +93,14 @@
output != nullptr ? &(output_struct.intake_joint_voltage) : nullptr,
status->fbb());
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *turret_goal = unsafe_goal != nullptr ? (unsafe_goal->turret_tracking()
+ ? aimer_.TurretGoal()
+ : unsafe_goal->turret())
+ : nullptr;
flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
turret_status_offset = turret_.Iterate(
- unsafe_goal != nullptr ? unsafe_goal->turret() : nullptr,
- position->turret(),
+ turret_goal, position->turret(),
output != nullptr ? &(output_struct.turret_voltage) : nullptr,
status->fbb());
@@ -100,6 +124,9 @@
output_struct.turret_voltage +=
std::clamp(turret_velocity_sign, -kTurretFrictionVoltageLimit,
kTurretFrictionVoltageLimit);
+ output_struct.turret_voltage =
+ std::clamp(output_struct.turret_voltage, -turret_.operating_voltage(),
+ turret_.operating_voltage());
// Friction is a pain and putting a really high burden on the integrator.
const double hood_velocity_sign = hood_status->velocity() * kHoodFrictionGain;
@@ -134,6 +161,7 @@
status_builder.add_intake(intake_status_offset);
status_builder.add_turret(turret_status_offset);
status_builder.add_shooter(shooter_status_offset);
+ status_builder.add_aimer(aimer_status_offset);
status->Send(status_builder.Finish());
@@ -144,7 +172,7 @@
if (shooter_.ready() &&
unsafe_goal->shooter()->velocity_accelerator() > 10.0 &&
unsafe_goal->shooter()->velocity_finisher() > 10.0) {
- output_struct.feeder_voltage = 9.0;
+ output_struct.feeder_voltage = 12.0;
} else {
output_struct.feeder_voltage = 0.0;
}
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 1526610..1a9d401 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -3,6 +3,8 @@
#include "aos/controls/control_loop.h"
#include "aos/events/event_loop.h"
+#include "aos/robot_state/joystick_state_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2020/constants.h"
#include "y2020/control_loops/superstructure/shooter/shooter.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
@@ -10,6 +12,7 @@
#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
#include "y2020/control_loops/superstructure/climber.h"
+#include "y2020/control_loops/superstructure/turret/aiming.h"
namespace y2020 {
namespace control_loops {
@@ -53,6 +56,11 @@
AbsoluteEncoderSubsystem intake_joint_;
PotAndAbsoluteEncoderSubsystem turret_;
shooter::Shooter shooter_;
+ turret::Aimer aimer_;
+
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
+ aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
Climber climber_;
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 6e4e08b..e8f1f8d 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -36,6 +36,7 @@
CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
using ::frc971::control_loops::PositionSensorSimulator;
using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+typedef ::frc971::control_loops::drivetrain::Status DrivetrainStatus;
typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
typedef Superstructure::PotAndAbsoluteEncoderSubsystem
PotAndAbsoluteEncoderSubsystem;
@@ -72,7 +73,6 @@
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
event_loop_->MakeFetcher<Output>("/superstructure")),
-
hood_plant_(new CappedTestPlant(hood::MakeHoodPlant())),
hood_encoder_(constants::GetValues()
.hood.zeroing_constants.one_revolution_distance),
@@ -398,6 +398,10 @@
test_event_loop_->MakeFetcher<Output>("/superstructure")),
superstructure_position_fetcher_(
test_event_loop_->MakeFetcher<Position>("/superstructure")),
+ drivetrain_status_sender_(
+ test_event_loop_->MakeSender<DrivetrainStatus>("/drivetrain")),
+ joystick_state_sender_(
+ test_event_loop_->MakeSender<aos::JoystickState>("/aos")),
superstructure_event_loop_(MakeEventLoop("superstructure", roborio_)),
superstructure_(superstructure_event_loop_.get()),
superstructure_plant_event_loop_(MakeEventLoop("plant", roborio_)),
@@ -506,6 +510,8 @@
::aos::Fetcher<Status> superstructure_status_fetcher_;
::aos::Fetcher<Output> superstructure_output_fetcher_;
::aos::Fetcher<Position> superstructure_position_fetcher_;
+ ::aos::Sender<DrivetrainStatus> drivetrain_status_sender_;
+ ::aos::Sender<aos::JoystickState> joystick_state_sender_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -827,6 +833,79 @@
VerifyNearGoal();
}
+class SuperstructureAllianceTest
+ : public SuperstructureTest,
+ public ::testing::WithParamInterface<aos::Alliance> {};
+
+// Tests that the turret switches to auto-aiming when we set turret_tracking to
+// true.
+TEST_P(SuperstructureAllianceTest, TurretAutoAim) {
+ SetEnabled(true);
+ // Set a reasonable goal.
+ const frc971::control_loops::Pose target = turret::OuterPortPose(GetParam());
+
+ WaitUntilZeroed();
+
+ constexpr double kShotAngle = 1.0;
+ {
+ auto builder = joystick_state_sender_.MakeBuilder();
+
+ aos::JoystickState::Builder joystick_builder =
+ builder.MakeBuilder<aos::JoystickState>();
+
+ joystick_builder.add_alliance(GetParam());
+
+ ASSERT_TRUE(builder.Send(joystick_builder.Finish()));
+ }
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_turret_tracking(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+
+ {
+ auto builder = drivetrain_status_sender_.MakeBuilder();
+
+ frc971::control_loops::drivetrain::LocalizerState::Builder
+ localizer_builder = builder.MakeBuilder<
+ frc971::control_loops::drivetrain::LocalizerState>();
+ localizer_builder.add_left_velocity(0.0);
+ localizer_builder.add_right_velocity(0.0);
+ const auto localizer_offset = localizer_builder.Finish();
+
+ DrivetrainStatus::Builder status_builder =
+ builder.MakeBuilder<DrivetrainStatus>();
+
+ // Set the robot up at kShotAngle off from the target, 1m away.
+ status_builder.add_x(target.abs_pos().x() + std::cos(kShotAngle));
+ status_builder.add_y(target.abs_pos().y() + std::sin(kShotAngle));
+ status_builder.add_theta(0.0);
+ status_builder.add_localizer(localizer_offset);
+
+ ASSERT_TRUE(builder.Send(status_builder.Finish()));
+ }
+
+ // Give it time to stabilize.
+ RunFor(chrono::seconds(1));
+
+ superstructure_status_fetcher_.Fetch();
+ EXPECT_NEAR(kShotAngle, superstructure_status_fetcher_->turret()->position(),
+ 1e-4);
+ EXPECT_FLOAT_EQ(kShotAngle,
+ superstructure_status_fetcher_->aimer()->turret_position());
+ EXPECT_FLOAT_EQ(0,
+ superstructure_status_fetcher_->aimer()->turret_velocity());
+}
+
+INSTANTIATE_TEST_CASE_P(ShootAnyAlliance, SuperstructureAllianceTest,
+ ::testing::Values(aos::Alliance::kRed,
+ aos::Alliance::kBlue,
+ aos::Alliance::kInvalid));
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 0b12d33..81293d3 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -29,6 +29,15 @@
accelerator_right:FlywheelControllerStatus;
}
+table AimerStatus {
+ // The current goal angle for the turret auto-tracking, in radians.
+ turret_position:double;
+ // The current goal velocity for the turret, in radians / sec.
+ turret_velocity:double;
+ // Whether we are currently aiming for the inner port.
+ aiming_for_inner_port:bool;
+}
+
table Status {
// All subsystems know their location.
zeroed:bool;
@@ -46,6 +55,9 @@
// Status of the control_panel
control_panel:frc971.control_loops.RelativeEncoderProfiledJointStatus;
+
+ // Status of the vision auto-tracking.
+ aimer:AimerStatus;
}
root_type Status;
diff --git a/y2020/control_loops/superstructure/turret/BUILD b/y2020/control_loops/superstructure/turret/BUILD
index 894d418..010c6fd 100644
--- a/y2020/control_loops/superstructure/turret/BUILD
+++ b/y2020/control_loops/superstructure/turret/BUILD
@@ -30,3 +30,28 @@
"//frc971/control_loops:state_feedback_loop",
],
)
+
+cc_library(
+ name = "aiming",
+ srcs = ["aiming.cc"],
+ hdrs = ["aiming.h"],
+ deps = [
+ "//aos:flatbuffers",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:pose",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//y2020:constants",
+ "//y2020/control_loops/drivetrain:drivetrain_base",
+ "//y2020/control_loops/superstructure:superstructure_status_fbs",
+ ],
+)
+
+cc_test(
+ name = "aiming_test",
+ srcs = ["aiming_test.cc"],
+ deps = [
+ ":aiming",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/turret/aiming.cc b/y2020/control_loops/superstructure/turret/aiming.cc
new file mode 100644
index 0000000..360fd1a
--- /dev/null
+++ b/y2020/control_loops/superstructure/turret/aiming.cc
@@ -0,0 +1,252 @@
+#include "y2020/control_loops/superstructure/turret/aiming.h"
+
+#include "y2020/constants.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+
+using frc971::control_loops::Pose;
+
+// Shooting-on-the-fly concept:
+// The current way that we manage shooting-on-the fly endeavors to be reasonably
+// simple, until we get a chance to see how the actual dynamics play out.
+// Essentially, we assume that the robot's velocity will represent a constant
+// offset to the ball's velocity over the entire trajectory to the goal and
+// then offset the target that we are pointing at based on that.
+// Let us assume that, if the robot shoots while not moving, regardless of shot
+// distance, the ball's average speed-over-ground to the target will be a
+// constant s_shot (this implies that if the robot is driving straight towards
+// the target, the actual ball speed-over-ground will be greater than s_shot).
+// We will define things in the robot's coordinate frame. We will be shooting
+// at a target that is at position (target_x, target_y) in the robot frame. The
+// robot is travelling at (v_robot_x, v_robot_y). In order to shoot the ball,
+// we need to generate some virtual target (virtual_x, virtual_y) that we will
+// shoot at as if we were standing still. The total time-of-flight to that
+// target will be t_shot = norm2(virtual_x, virtual_y) / s_shot.
+// we will have virtual_x + v_robot_x * t_shot = target_x, and the same
+// for y. This gives us three equations and three unknowns (virtual_x,
+// virtual_y, and t_shot), and given appropriate assumptions, can be solved
+// analytically. However, doing so is obnoxious and given appropriate functions
+// for t_shot may not be feasible. As such, instead of actually solving the
+// equation analytically, we will use an iterative solution where we maintain
+// a current virtual target estimate. We start with this estimate as if the
+// robot is stationary. We then use this estimate to calculate t_shot, and
+// calculate the next value for the virtual target.
+
+namespace {
+// The overall length and width of the field, in meters.
+constexpr double kFieldLength = 15.983;
+constexpr double kFieldWidth = 8.212;
+// Height of the center of the port(s) above the ground, in meters.
+constexpr double kPortHeight = 2.494;
+
+// Maximum shot angle at which we will attempt to make the shot into the inner
+// port, in radians. Zero would imply that we could only shoot if we were
+// exactly perpendicular to the target. Larger numbers allow us to aim at the
+// inner port more aggressively, at the risk of being more likely to miss the
+// outer port entirely.
+constexpr double kMaxInnerPortAngle = 20.0 * M_PI / 180.0;
+
+// Distance (in meters) from the edge of the field to the port.
+constexpr double kEdgeOfFieldToPort = 2.404;
+
+// The amount (in meters) that the inner port is set back from the outer port.
+constexpr double kInnerPortBackset = 0.743;
+
+// Average speed-over-ground of the ball on its way to the target. Our current
+// model assumes constant ball velocity regardless of shot distance.
+// TODO(james): Is this an appropriate model? For the outer port it should be
+// good enough that it doesn't really matter, but for the inner port it may be
+// more appropriate to do something more dynamic--however, it is not yet clear
+// how we would best estimate speed-over-ground given a hood angle + shooter
+// speed. Assuming a constant average speed over the course of the trajectory
+// should be reasonable, since all we are trying to do here is calculate an
+// overall time-of-flight (we don't actually care about the ball speed itself).
+constexpr double kBallSpeedOverGround = 15.0; // m/s
+
+// Minimum distance that we must be from the inner port in order to attempt the
+// shot--this is to account for the fact that if we are too close to the target,
+// then we won't have a clear shot on the inner port.
+constexpr double kMinimumInnerPortShotDistance = 4.0;
+
+// Amount of buffer, in radians, to leave to help avoid wrapping. I.e., any time
+// that we are in kAvoidEdges mode, we will keep ourselves at least
+// kAntiWrapBuffer radians away from the hardstops.
+constexpr double kAntiWrapBuffer = 0.2;
+
+// If the turret is at zero, then it will be at this angle relative to pointed
+// straight forwards on the robot.
+constexpr double kTurretZeroOffset = M_PI;
+
+constexpr double kTurretRange = constants::Values::kTurretRange().range();
+static_assert((kTurretRange - 2.0 * kAntiWrapBuffer) > 2.0 * M_PI,
+ "kAntiWrap buffer should be small enough that we still have 360 "
+ "degrees of range.");
+
+Pose ReverseSideOfField(Pose target) {
+ *target.mutable_pos() *= -1;
+ target.set_theta(aos::math::NormalizeAngle(target.rel_theta() + M_PI));
+ return target;
+}
+
+flatbuffers::DetachedBuffer MakePrefilledGoal() {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(true);
+ Aimer::Goal::Builder builder(fbb);
+ builder.add_unsafe_goal(0);
+ builder.add_goal_velocity(0);
+ builder.add_ignore_profile(true);
+ fbb.Finish(builder.Finish());
+ return fbb.Release();
+}
+
+// This implements the iteration in the described shooting-on-the-fly algorithm.
+// robot_pose: Current robot pose.
+// robot_velocity: Current robot velocity, in the absolute field frame.
+// target_pose: Absolute goal Pose.
+// current_virtual_pose: Current estimate of where we want to shoot at.
+Pose IterateVirtualGoal(const Pose &robot_pose,
+ const Eigen::Vector3d &robot_velocity,
+ const Pose &target_pose,
+ const Pose ¤t_virtual_pose) {
+ const double air_time =
+ current_virtual_pose.Rebase(&robot_pose).xy_norm() / kBallSpeedOverGround;
+ const Eigen::Vector3d virtual_target =
+ target_pose.abs_pos() - air_time * robot_velocity;
+ return Pose(virtual_target, target_pose.abs_theta());
+}
+} // namespace
+
+Pose InnerPortPose(aos::Alliance alliance) {
+ const Pose target({kFieldLength / 2 + kInnerPortBackset,
+ -kFieldWidth / 2.0 + kEdgeOfFieldToPort, kPortHeight},
+ 0.0);
+ if (alliance == aos::Alliance::kRed) {
+ return ReverseSideOfField(target);
+ }
+ return target;
+}
+
+Pose OuterPortPose(aos::Alliance alliance) {
+ Pose target(
+ {kFieldLength / 2, -kFieldWidth / 2.0 + kEdgeOfFieldToPort, kPortHeight},
+ 0.0);
+ if (alliance == aos::Alliance::kRed) {
+ return ReverseSideOfField(target);
+ }
+ return target;
+}
+
+Aimer::Aimer() : goal_(MakePrefilledGoal()) {}
+
+void Aimer::Update(const Status *status, aos::Alliance alliance,
+ WrapMode wrap_mode, ShotMode shot_mode) {
+ const Pose robot_pose({status->x(), status->y(), 0}, status->theta());
+ const Pose inner_port = InnerPortPose(alliance);
+ const Pose outer_port = OuterPortPose(alliance);
+ const Pose robot_pose_from_inner_port = robot_pose.Rebase(&inner_port);
+
+ // TODO(james): This code should probably just be in the localizer and have
+ // xdot/ydot get populated in the status message directly... that way we don't
+ // keep duplicating this math.
+ // Also, this doesn't currently take into account the lateral velocity of the
+ // robot. All of this would be helped by just doing this work in the Localizer
+ // itself.
+ const Eigen::Vector2d linear_angular =
+ drivetrain::GetDrivetrainConfig().Tlr_to_la() *
+ Eigen::Vector2d(status->localizer()->left_velocity(),
+ status->localizer()->right_velocity());
+ const double xdot = linear_angular(0) * std::cos(status->theta());
+ const double ydot = linear_angular(0) * std::sin(status->theta());
+
+ const double inner_port_angle = robot_pose_from_inner_port.heading();
+ const double inner_port_distance = robot_pose_from_inner_port.xy_norm();
+ aiming_for_inner_port_ =
+ (std::abs(inner_port_angle) < kMaxInnerPortAngle) &&
+ (inner_port_distance > kMinimumInnerPortShotDistance);
+
+ // This code manages compensating the goal turret heading for the robot's
+ // current velocity, to allow for shooting on-the-fly.
+ // This works by solving for the correct turret angle numerically, since while
+ // we technically could do it analytically, doing so would both make it hard
+ // to make small changes (since it would force us to redo the math) and be
+ // error-prone since it'd be easy to make typos or other minor math errors.
+ Pose virtual_goal;
+ {
+ const Pose goal = aiming_for_inner_port_ ? inner_port : outer_port;
+ virtual_goal = goal;
+ if (shot_mode == ShotMode::kShootOnTheFly) {
+ for (int ii = 0; ii < 3; ++ii) {
+ virtual_goal =
+ IterateVirtualGoal(robot_pose, {xdot, ydot, 0}, goal, virtual_goal);
+ }
+ VLOG(1) << "Shooting-on-the-fly target position: "
+ << virtual_goal.abs_pos().transpose();
+ }
+ virtual_goal = virtual_goal.Rebase(&robot_pose);
+ }
+
+ const double heading_to_goal = virtual_goal.heading();
+ CHECK(status->has_localizer());
+ distance_ = virtual_goal.xy_norm();
+
+ // The following code all works to calculate what the rate of turn of the
+ // turret should be. The code only accounts for the rate of turn if we are
+ // aiming at a static target, which should be close enough to correct that it
+ // doesn't matter that it fails to account for the
+ // shooting-on-the-fly compensation.
+ const double rel_x = virtual_goal.rel_pos().x();
+ const double rel_y = virtual_goal.rel_pos().y();
+ const double squared_norm = rel_x * rel_x + rel_y * rel_y;
+
+ // If squared_norm gets to be too close to zero, just zero out the relevant
+ // term to prevent NaNs. Note that this doesn't address the chattering that
+ // would likely occur if we were to get excessively close to the target.
+ // Note that x and y terms are swapped relative to what you would normally see
+ // in the derivative of atan because xdot and ydot are the derivatives of
+ // robot_pos and we are working with the atan of (target_pos - robot_pos).
+ const double atan_diff = (squared_norm < 1e-3)
+ ? 0.0
+ : (rel_y * xdot - rel_x * ydot) / squared_norm;
+ // heading = atan2(relative_y, relative_x) - robot_theta
+ // dheading / dt = (rel_x * rel_y' - rel_y * rel_x') / (rel_x^2 + rel_y^2) - dtheta / dt
+ const double dheading_dt = atan_diff - linear_angular(1);
+
+ double range = kTurretRange;
+ if (wrap_mode == WrapMode::kAvoidEdges) {
+ range -= 2.0 * kAntiWrapBuffer;
+ }
+ // Calculate a goal turret heading such that it is within +/- pi of the
+ // current position (i.e., a goal that would minimize the amount the turret
+ // would have to travel).
+ // We then check if this goal would bring us out of range of the valid angles,
+ // and if it would, we reset to be within +/- pi of zero.
+ double turret_heading =
+ goal_.message().unsafe_goal() +
+ aos::math::NormalizeAngle(heading_to_goal - kTurretZeroOffset -
+ goal_.message().unsafe_goal());
+ if (std::abs(turret_heading - constants::Values::kTurretRange().middle()) >
+ range / 2.0) {
+ turret_heading = aos::math::NormalizeAngle(turret_heading);
+ }
+
+ goal_.mutable_message()->mutate_unsafe_goal(turret_heading);
+ goal_.mutable_message()->mutate_goal_velocity(dheading_dt);
+}
+
+flatbuffers::Offset<AimerStatus> Aimer::PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ AimerStatus::Builder builder(*fbb);
+ builder.add_turret_position(goal_.message().unsafe_goal());
+ builder.add_turret_velocity(goal_.message().goal_velocity());
+ builder.add_aiming_for_inner_port(aiming_for_inner_port_);
+ return builder.Finish();
+}
+
+} // namespace turret
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
diff --git a/y2020/control_loops/superstructure/turret/aiming.h b/y2020/control_loops/superstructure/turret/aiming.h
new file mode 100644
index 0000000..3b3071e
--- /dev/null
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -0,0 +1,74 @@
+#ifndef y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
+#define y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
+
+#include "aos/flatbuffers.h"
+#include "aos/robot_state/joystick_state_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+
+// Returns the port that we want to score on given our current alliance. The yaw
+// of the port will be such that the positive x axis points out the back of the
+// target.
+frc971::control_loops::Pose InnerPortPose(aos::Alliance alliance);
+frc971::control_loops::Pose OuterPortPose(aos::Alliance alliance);
+
+// This class manages taking in drivetrain status messages and generating turret
+// goals so that it gets aimed at the goal.
+class Aimer {
+ public:
+ typedef frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ Goal;
+ typedef frc971::control_loops::drivetrain::Status Status;
+ // Mode to run the aimer in, to control how we manage wrapping the turret
+ // angle.
+ enum class WrapMode {
+ // Keep the turret as far away from the edges of the range of motion as
+ // reasonable, to minimize the odds that we will hit the hardstops once we
+ // start shooting.
+ kAvoidEdges,
+ // Do everything reasonable to avoid having to wrap the shooter--set this
+ // while shooting so that we don't randomly spin the shooter 360 while
+ // shooting.
+ kAvoidWrapping,
+ };
+
+ // Control modes for managing how we manage shooting on the fly.
+ enum class ShotMode {
+ // Don't do any shooting-on-the-fly compensation--just point straight at the
+ // target. Primarily used in tests.
+ kStatic,
+ // Do do shooting-on-the-fly compensation.
+ kShootOnTheFly,
+ };
+
+ Aimer();
+
+ void Update(const Status *status, aos::Alliance alliance, WrapMode wrap_mode,
+ ShotMode shot_mode);
+
+ const Goal *TurretGoal() const { return &goal_.message(); }
+
+ // Returns the distance to the goal, in meters.
+ double DistanceToGoal() const { return distance_; }
+
+ flatbuffers::Offset<AimerStatus> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const;
+
+ private:
+ aos::FlatbufferDetachedBuffer<Goal> goal_;
+ bool aiming_for_inner_port_ = false;
+ double distance_ = 0.0;
+};
+
+} // namespace turret
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
+#endif // y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
diff --git a/y2020/control_loops/superstructure/turret/aiming_test.cc b/y2020/control_loops/superstructure/turret/aiming_test.cc
new file mode 100644
index 0000000..cb95b56
--- /dev/null
+++ b/y2020/control_loops/superstructure/turret/aiming_test.cc
@@ -0,0 +1,249 @@
+#include "y2020/control_loops/superstructure/turret/aiming.h"
+
+#include "frc971/control_loops/pose.h"
+#include "gtest/gtest.h"
+#include "y2020/constants.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+namespace testing {
+
+using frc971::control_loops::Pose;
+
+class AimerTest : public ::testing::Test {
+ public:
+ typedef Aimer::Goal Goal;
+ typedef Aimer::Status Status;
+ struct StatusData {
+ double x;
+ double y;
+ double theta;
+ double linear;
+ double angular;
+ };
+ aos::FlatbufferDetachedBuffer<Status> MakeStatus(const StatusData &data) {
+ flatbuffers::FlatBufferBuilder fbb;
+ frc971::control_loops::drivetrain::LocalizerState::Builder state_builder(
+ fbb);
+ state_builder.add_left_velocity(
+ data.linear -
+ data.angular * drivetrain::GetDrivetrainConfig().robot_radius);
+ state_builder.add_right_velocity(
+ data.linear +
+ data.angular * drivetrain::GetDrivetrainConfig().robot_radius);
+ const auto state_offset = state_builder.Finish();
+ Status::Builder builder(fbb);
+ builder.add_x(data.x);
+ builder.add_y(data.y);
+ builder.add_theta(data.theta);
+ builder.add_localizer(state_offset);
+ fbb.Finish(builder.Finish());
+ return fbb.Release();
+ }
+
+ const Goal *Update(
+ const StatusData &data, aos::Alliance alliance = aos::Alliance::kBlue,
+ Aimer::WrapMode wrap_mode = Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode shot_mode = Aimer::ShotMode::kShootOnTheFly) {
+ const auto buffer = MakeStatus(data);
+ aimer_.Update(&buffer.message(), alliance, wrap_mode, shot_mode);
+ const Goal *goal = aimer_.TurretGoal();
+ EXPECT_TRUE(goal->ignore_profile());
+ return goal;
+ }
+
+ protected:
+ Aimer aimer_;
+};
+
+TEST_F(AimerTest, StandingStill) {
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 0.0});
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+ goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 1.0,
+ .linear = 0.0,
+ .angular = 0.0});
+ EXPECT_EQ(-1.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = -1.0,
+ .linear = 0.0,
+ .angular = 0.0});
+ EXPECT_EQ(1.0, aos::math::NormalizeAngle(goal->unsafe_goal()));
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+ // Test that we handle the case that where we are right on top of the target.
+ goal = Update({.x = target.abs_pos().x() + 0.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 0.0});
+ EXPECT_EQ(M_PI, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(0.0, aimer_.DistanceToGoal());
+}
+
+TEST_F(AimerTest, SpinningRobot) {
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 1.0});
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(-1.0, goal->goal_velocity());
+}
+
+// Tests that when we drive straight away from the target we don't have to spin
+// the turret.
+TEST_F(AimerTest, DrivingAwayFromTarget) {
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ // To keep the test simple, disable shooting on the fly so that the
+ // goal distance comes out in an easy to calculate number.
+ const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 1.0,
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kStatic);
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+ // Next, try with shooting-on-the-fly enabled--because we are driving straight
+ // towards the target, only the goal distance should be impacted.
+ goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 1.0,
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kShootOnTheFly);
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(0.0, goal->goal_velocity());
+ EXPECT_LT(1.0001, aimer_.DistanceToGoal());
+ EXPECT_GT(1.1, aimer_.DistanceToGoal());
+}
+
+// Tests that when we drive perpendicular to the target, we do have to spin.
+TEST_F(AimerTest, DrivingLateralToTarget) {
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ // To keep the test simple, disable shooting on the fly so that the
+ // goal_velocity comes out in an easy to calculate number.
+ const Goal *goal = Update({.x = target.abs_pos().x() + 0.0,
+ .y = target.abs_pos().y() + 1.0,
+ .theta = 0.0,
+ .linear = 1.0,
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kStatic);
+ EXPECT_EQ(M_PI_2, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(-1.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+ // Next, test with shooting-on-the-fly enabled, The goal numbers should all be
+ // slightly offset due to the robot velocity.
+ goal = Update({.x = target.abs_pos().x() + 0.0,
+ .y = target.abs_pos().y() + 1.0,
+ .theta = 0.0,
+ .linear = 1.0,
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kShootOnTheFly);
+ // Confirm that the turret heading goal is less then -pi / 2, but not by too
+ // much.
+ EXPECT_GT(M_PI_2 - 0.001, goal->unsafe_goal());
+ EXPECT_LT(M_PI_2 - 0.1, goal->unsafe_goal());
+ // Similarly, the turret velocity goal should be a bit greater than -1.0,
+ // since the turret is no longer at exactly a right angle.
+ EXPECT_LT(-1.0, goal->goal_velocity());
+ EXPECT_GT(-0.95, goal->goal_velocity());
+ // And the distance to the goal should be a bit greater than 1.0.
+ EXPECT_LT(1.0001, aimer_.DistanceToGoal());
+ EXPECT_GT(1.1, aimer_.DistanceToGoal());
+}
+
+// Confirms that we will indeed shoot at the inner port when we have a good shot
+// angle on it.
+TEST_F(AimerTest, InnerPort) {
+ const Pose target = InnerPortPose(aos::Alliance::kRed);
+ const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 0.0},
+ aos::Alliance::kRed);
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+}
+
+// Confirms that when we move the turret heading so that it would be entirely
+// out of the normal range of motion that we send a valid (in-range) goal.
+TEST_F(AimerTest, WrapWhenOutOfRange) {
+ // Start ourselves needing a turret angle of M_PI.
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ StatusData status{.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 0.0};
+ const Goal *goal = Update(status);
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ // Move the robot a small amount--we should go past pi and not wrap yet.
+ status.theta = -0.1;
+ goal = Update(status);
+ EXPECT_FLOAT_EQ(0.1, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ // Move the robot so that, if we had no hard-stops, we would go past it.
+ status.theta = -2.0;
+ goal = Update(status);
+ EXPECT_FLOAT_EQ(2.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+}
+
+// Confirms that the avoid edges turret mode doesn't let us get all the way to
+// the turret hard-stops but that the avoid wrapping mode does.
+TEST_F(AimerTest, WrappingModes) {
+ // Start ourselves needing a turret angle of M_PI.
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ StatusData status{.x = target.abs_pos().x() - 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 0.0};
+ const Goal *goal =
+ Update(status, aos::Alliance::kBlue, Aimer::WrapMode::kAvoidWrapping);
+ EXPECT_EQ(M_PI, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ constexpr double kUpperLimit = constants::Values::kTurretRange().upper;
+ // Move the robot to the upper limit with AvoidWrapping set--we should be at
+ // the upper limit and not wrapped.
+ status.theta = goal->unsafe_goal() - kUpperLimit;
+ goal = Update(status, aos::Alliance::kBlue, Aimer::WrapMode::kAvoidWrapping);
+ EXPECT_FLOAT_EQ(kUpperLimit, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ // Enter kAvoidEdges mode--we should wrap around.
+ goal = Update(status, aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges);
+ // confirm that this test is actually testing something...
+ ASSERT_NE(aos::math::NormalizeAngle(kUpperLimit), kUpperLimit);
+ EXPECT_FLOAT_EQ(aos::math::NormalizeAngle(kUpperLimit), goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+}
+
+} // namespace testing
+} // namespace turret
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 026a50b..2049e83 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -13,10 +13,11 @@
#include "aos/network/team_number.h"
#include "aos/util/log_interval.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "y2020/control_loops/drivetrain/drivetrain_base.h"
#include "y2020/constants.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2020/setpoint_generated.h"
using aos::input::driver_station::ButtonLocation;
using aos::input::driver_station::ControlBit;
@@ -57,8 +58,9 @@
superstructure_goal_sender_(
event_loop->MakeSender<superstructure::Goal>("/superstructure")),
superstructure_status_fetcher_(
- event_loop->MakeFetcher<superstructure::Status>(
- "/superstructure")) {}
+ event_loop->MakeFetcher<superstructure::Status>("/superstructure")),
+ setpoint_fetcher_(event_loop->MakeFetcher<y2020::joysticks::Setpoint>(
+ "/superstructure")) {}
void AutoEnded() override {
AOS_LOG(INFO, "Auto ended, assuming disc and have piece\n");
@@ -71,6 +73,8 @@
return;
}
+ setpoint_fetcher_.Fetch();
+
double hood_pos = constants::Values::kHoodRange().middle();
double intake_pos = -0.89;
double turret_pos = 0.0;
@@ -84,15 +88,26 @@
}
if (data.IsPressed(kHood)) {
- hood_pos = 0.05;
+ hood_pos = 0.45;
+ } else {
+ if (setpoint_fetcher_.get()) {
+ hood_pos = setpoint_fetcher_->hood();
+ } else {
+ hood_pos = 0.58;
+ }
}
if (data.IsPressed(kShootFast)) {
- accelerator_speed = 500.0;
- finisher_speed = 500.0;
+ if (setpoint_fetcher_.get()) {
+ accelerator_speed = setpoint_fetcher_->accelerator();
+ finisher_speed = setpoint_fetcher_->finisher();
+ } else {
+ accelerator_speed = 250.0;
+ finisher_speed = 500.0;
+ }
} else if (data.IsPressed(kShootSlow)) {
- accelerator_speed = 30.0;
- finisher_speed = 30.0;
+ accelerator_speed = 180.0;
+ finisher_speed = 375.0;
}
if (data.IsPressed(kIntakeExtend)) {
@@ -162,6 +177,8 @@
::aos::Sender<superstructure::Goal> superstructure_goal_sender_;
::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
+
+ ::aos::Fetcher<y2020::joysticks::Setpoint> setpoint_fetcher_;
};
} // namespace joysticks
diff --git a/y2020/setpoint.fbs b/y2020/setpoint.fbs
new file mode 100644
index 0000000..5f1af72
--- /dev/null
+++ b/y2020/setpoint.fbs
@@ -0,0 +1,11 @@
+namespace y2020.joysticks;
+
+table Setpoint {
+ accelerator:double;
+
+ finisher:double;
+
+ hood:double;
+}
+
+root_type Setpoint;
diff --git a/y2020/setpoint_setter.cc b/y2020/setpoint_setter.cc
new file mode 100644
index 0000000..1cbc518
--- /dev/null
+++ b/y2020/setpoint_setter.cc
@@ -0,0 +1,34 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "gflags/gflags.h"
+#include "y2020/setpoint_generated.h"
+
+DEFINE_double(accelerator, 250.0, "Accelerator speed");
+DEFINE_double(finisher, 500.0, "Finsher speed");
+DEFINE_double(hood, 0.45, "Hood setpoint");
+
+int main(int argc, char ** argv) {
+ aos::InitGoogle(&argc, &argv);
+ aos::InitNRT();
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ ::aos::Sender<y2020::joysticks::Setpoint> setpoint_sender =
+ event_loop.MakeSender<y2020::joysticks::Setpoint>("/superstructure");
+
+ aos::Sender<y2020::joysticks::Setpoint>::Builder builder =
+ setpoint_sender.MakeBuilder();
+
+ y2020::joysticks::Setpoint::Builder setpoint_builder =
+ builder.MakeBuilder<y2020::joysticks::Setpoint>();
+
+ setpoint_builder.add_accelerator(FLAGS_accelerator);
+ setpoint_builder.add_finisher(FLAGS_finisher);
+ setpoint_builder.add_hood(FLAGS_hood);
+ builder.Send(setpoint_builder.Finish());
+
+ aos::Cleanup();
+}
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index 5e65387..3baa61b 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -59,3 +59,46 @@
srcs = ["vision.fbs"],
visibility = ["//y2020:__subpackages__"],
)
+
+cc_binary(
+ name = "viewer",
+ srcs = [
+ "viewer.cc",
+ ],
+ data = [
+ "//y2020:config.json",
+ ],
+ restricted_to = [
+ "//tools:k8",
+ "//tools:armhf-debian",
+ ],
+ visibility = ["//y2020:__subpackages__"],
+ deps = [
+ ":vision_fbs",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//third_party:opencv",
+ ],
+)
+
+cc_binary(
+ name = "viewer_replay",
+ srcs = [
+ "viewer_replay.cc",
+ ],
+ data = [
+ "//y2020:config.json",
+ ],
+ restricted_to = [
+ "//tools:k8",
+ "//tools:armhf-debian",
+ ],
+ visibility = ["//y2020:__subpackages__"],
+ deps = [
+ ":vision_fbs",
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:logger",
+ "//third_party:opencv",
+ ],
+)
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index c47e42b..f38a40e 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -189,12 +189,14 @@
// avoid crashes that only occur when specific features are matched.
CHECK(feature_table->has_field_location());
- const flatbuffers::Vector<float> *const descriptor =
+ const flatbuffers::Vector<uint8_t> *const descriptor =
feature_table->descriptor();
CHECK_EQ(descriptor->size(), 128u) << ": Unsupported feature size";
- cv::Mat(1, descriptor->size(), CV_32F,
- const_cast<void *>(static_cast<const void *>(descriptor->data())))
- .copyTo(features(cv::Range(i, i + 1), cv::Range(0, 128)));
+ const auto in_mat = cv::Mat(
+ 1, descriptor->size(), CV_8U,
+ const_cast<void *>(static_cast<const void *>(descriptor->data())));
+ const auto out_mat = features(cv::Range(i, i + 1), cv::Range(0, 128));
+ in_mat.convertTo(out_mat, CV_32F);
}
matcher_->add(features);
}
@@ -552,13 +554,21 @@
const cv::Mat &descriptors) {
const int number_features = keypoints.size();
CHECK_EQ(descriptors.rows, number_features);
+ CHECK_EQ(descriptors.cols, 128);
std::vector<flatbuffers::Offset<sift::Feature>> features_vector(
number_features);
for (int i = 0; i < number_features; ++i) {
- const auto submat = descriptors(cv::Range(i, i + 1), cv::Range(0, 128));
+ const auto submat =
+ descriptors(cv::Range(i, i + 1), cv::Range(0, descriptors.cols));
CHECK(submat.isContinuous());
- const auto descriptor_offset =
- fbb->CreateVector(reinterpret_cast<float *>(submat.data), 128);
+ flatbuffers::Offset<flatbuffers::Vector<uint8_t>> descriptor_offset;
+ {
+ uint8_t *data;
+ descriptor_offset = fbb->CreateUninitializedVector(128, &data);
+ submat.convertTo(
+ cv::Mat(1, descriptors.cols, CV_8U, static_cast<void *>(data)),
+ CV_8U);
+ }
sift::Feature::Builder feature_builder(*fbb);
feature_builder.add_descriptor(descriptor_offset);
feature_builder.add_x(keypoints[i].pt.x);
diff --git a/y2020/vision/rootfs/target_configure.sh b/y2020/vision/rootfs/target_configure.sh
index 032764a..193f8b5 100755
--- a/y2020/vision/rootfs/target_configure.sh
+++ b/y2020/vision/rootfs/target_configure.sh
@@ -37,7 +37,20 @@
libopencv-videoio3.2 \
libopencv-videostab3.2 \
libopencv-viz3.2 \
- python3-opencv
+ python3-opencv \
+ gstreamer1.0-plugins-bad \
+ gstreamer1.0-plugins-base \
+ gstreamer1.0-plugins-good \
+ gstreamer1.0-plugins-ugly \
+ gstreamer1.0-x \
+ gstreamer1.0-nice \
+ gstreamer1.0-gl \
+ libgstreamer-plugins-bad1.0-0 \
+ libgstreamer-plugins-base1.0-0 \
+ libgstreamer1.0-0 \
+ libgstreamer-gl1.0-0 \
+ libnice10 \
+ libnice-dev
echo 'GOVERNOR="performance"' > /etc/default/cpufrequtils
diff --git a/y2020/vision/sift/demo_sift_training.py b/y2020/vision/sift/demo_sift_training.py
index 3fa33cf..c78a44a 100644
--- a/y2020/vision/sift/demo_sift_training.py
+++ b/y2020/vision/sift/demo_sift_training.py
@@ -26,7 +26,8 @@
for keypoint, descriptor in zip(keypoints, descriptors):
Feature.FeatureStartDescriptorVector(fbb, len(descriptor))
for n in reversed(descriptor):
- fbb.PrependFloat32(n)
+ assert n == round(n)
+ fbb.PrependUint8(int(round(n)))
descriptor_vector = fbb.EndVector(len(descriptor))
Feature.FeatureStart(fbb)
diff --git a/y2020/vision/sift/sift.fbs b/y2020/vision/sift/sift.fbs
index 3e2daaf..24fd64d 100644
--- a/y2020/vision/sift/sift.fbs
+++ b/y2020/vision/sift/sift.fbs
@@ -9,7 +9,8 @@
// Represents a single feature extracted from an image.
table Feature {
- // Contains the descriptor data.
+ // Contains the descriptor data. OpenCV likes to represent them as floats, but
+ // they're really ubytes.
//
// TODO(Brian): These are scaled to be convertible to chars. Should we do
// that to minimize storage space? Or maybe int16?
@@ -17,7 +18,7 @@
// The size of this depends on the parameters. It is width*width*hist_bins.
// Currently we have width=4 and hist_bins=8, which results in a size of
// 4*4*8=128.
- descriptor:[float];
+ descriptor:[ubyte];
// Location of the keypoint.
x:float;
diff --git a/y2020/vision/tools/python_code/camera_definition.py b/y2020/vision/tools/python_code/camera_definition.py
index 194ddd3..0667b16 100644
--- a/y2020/vision/tools/python_code/camera_definition.py
+++ b/y2020/vision/tools/python_code/camera_definition.py
@@ -6,7 +6,7 @@
class CameraIntrinsics:
def __init__(self):
self.camera_matrix = []
- self.distortion_coeffs = []
+ self.dist_coeffs = []
pass
@@ -40,7 +40,7 @@
# Define a web_cam
web_cam_int = CameraIntrinsics()
web_cam_int.camera_matrix = np.asarray([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
-web_cam_int.distortion_coeffs = np.zeros((5, 1))
+web_cam_int.dist_coeffs = np.zeros((5, 1))
web_cam_ext = CameraExtrinsics()
# Camera rotation from robot x,y,z to opencv (z, -x, -y)
diff --git a/y2020/vision/tools/python_code/camera_definition_test.py b/y2020/vision/tools/python_code/camera_definition_test.py
index 65d1b68..732dbb8 100644
--- a/y2020/vision/tools/python_code/camera_definition_test.py
+++ b/y2020/vision/tools/python_code/camera_definition_test.py
@@ -6,7 +6,7 @@
class CameraIntrinsics:
def __init__(self):
self.camera_matrix = []
- self.distortion_coeffs = []
+ self.dist_coeffs = []
class CameraExtrinsics:
@@ -38,7 +38,7 @@
# Define a web_cam
web_cam_int = CameraIntrinsics()
web_cam_int.camera_matrix = np.asarray([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
-web_cam_int.distortion_coeffs = np.zeros((5, 1))
+web_cam_int.dist_coeffs = np.zeros((5, 1))
web_cam_ext = CameraExtrinsics()
# Camera rotation from robot x,y,z to opencv (z, -x, -y)
diff --git a/y2020/vision/tools/python_code/camera_param_test.cc b/y2020/vision/tools/python_code/camera_param_test.cc
index 5b959cd..483fe75 100644
--- a/y2020/vision/tools/python_code/camera_param_test.cc
+++ b/y2020/vision/tools/python_code/camera_param_test.cc
@@ -143,12 +143,14 @@
cv::Mat features(training_image->features()->size(), 128, CV_32F);
for (size_t i = 0; i < training_image->features()->size(); ++i) {
const sift::Feature *feature_table = training_image->features()->Get(i);
- const flatbuffers::Vector<float> *const descriptor =
+ const flatbuffers::Vector<uint8_t> *const descriptor =
feature_table->descriptor();
CHECK_EQ(descriptor->size(), 128u) << ": Unsupported feature size";
- cv::Mat(1, descriptor->size(), CV_32F,
- const_cast<void *>(static_cast<const void *>(descriptor->data())))
- .copyTo(features(cv::Range(i, i + 1), cv::Range(0, 128)));
+ const auto in_mat = cv::Mat(
+ 1, descriptor->size(), CV_8U,
+ const_cast<void *>(static_cast<const void *>(descriptor->data())));
+ const auto out_mat = features(cv::Range(i, i + 1), cv::Range(0, 128));
+ in_mat.convertTo(out_mat, CV_32F);
cv::Point2f point_2d = Training2dPoint(train_image_index, i);
point_list_2d_.push_back(point_2d);
@@ -252,12 +254,12 @@
<< "Mismatch on camera intrinsic matrix: " << intrinsic << "\nvs.\n"
<< camera_params.camera_intrinsic_matrix_;
- float dist_coeff_mat[5] = {0., 0., 0., 0., 0.};
- cv::Mat dist_coeff = cv::Mat(5, 1, CV_32F, dist_coeff_mat);
- cv::Mat dist_coeff_diff = (dist_coeff != camera_params.camera_dist_coeffs_);
- bool dist_coeff_eq = (cv::countNonZero(dist_coeff_diff) == 0);
- ASSERT_TRUE(dist_coeff_eq)
- << "Mismatch on camera distortion coefficients: " << dist_coeff
+ float dist_coeffs_mat[5] = {0., 0., 0., 0., 0.};
+ cv::Mat dist_coeffs = cv::Mat(5, 1, CV_32F, dist_coeffs_mat);
+ cv::Mat dist_coeffs_diff = (dist_coeffs != camera_params.camera_dist_coeffs_);
+ bool dist_coeffs_eq = (cv::countNonZero(dist_coeffs_diff) == 0);
+ ASSERT_TRUE(dist_coeffs_eq)
+ << "Mismatch on camera distortion coefficients: " << dist_coeffs
<< "\nvs.\n"
<< camera_params.camera_dist_coeffs_;
diff --git a/y2020/vision/tools/python_code/define_training_data.py b/y2020/vision/tools/python_code/define_training_data.py
index 9894ee2..8116fb7 100644
--- a/y2020/vision/tools/python_code/define_training_data.py
+++ b/y2020/vision/tools/python_code/define_training_data.py
@@ -216,16 +216,16 @@
# Given 2d and 3d locations, and camera location and projection model,
# display locations on an image
-def visualize_reprojections(img, pts_2d, pts_3d, cam_mat, distortion_coeffs):
+def visualize_reprojections(img, pts_2d, pts_3d, cam_mat, dist_coeffs):
# Compute camera location
# TODO: Warn on bad inliers
# TODO: Change this to not have to recast to np
pts_2d_np = np.asarray(np.float32(pts_2d)).reshape(-1, 1, 2)
pts_3d_np = np.asarray(np.float32(pts_3d)).reshape(-1, 1, 3)
retval, R, T, inliers = cv2.solvePnPRansac(pts_3d_np, pts_2d_np, cam_mat,
- distortion_coeffs)
+ dist_coeffs)
pts_3d_proj_2d, jac_2d = cv2.projectPoints(pts_3d_np, R, T, cam_mat,
- distortion_coeffs)
+ dist_coeffs)
if inliers is None:
glog.warn("WARNING: Didn't get any inliers when reprojecting polygons")
return img
diff --git a/y2020/vision/tools/python_code/image_capture.py b/y2020/vision/tools/python_code/image_capture.py
new file mode 100644
index 0000000..d6ce6d7
--- /dev/null
+++ b/y2020/vision/tools/python_code/image_capture.py
@@ -0,0 +1,34 @@
+import cv2
+import datetime
+# Open the device at the ID X for /dev/videoX
+CAMERA_INDEX = 0
+cap = cv2.VideoCapture(CAMERA_INDEX)
+
+#Check whether user selected camera is opened successfully.
+if not (cap.isOpened()):
+ print("Could not open video device /dev/video%d" % CAMERA_INDEX)
+ quit()
+
+while True:
+ # Capture frame-by-frame
+ ret, frame = cap.read()
+
+ exp = cap.get(cv2.CAP_PROP_EXPOSURE)
+ #print("Exposure:", exp)
+ # Display the resulting frame
+ cv2.imshow('preview', frame)
+
+ #Waits for a user input to capture image or quit the application
+ keystroke = cv2.waitKey(1)
+
+ if keystroke & 0xFF == ord('q'):
+ break
+ elif keystroke & 0xFF == ord('c'):
+ image_name = datetime.datetime.today().strftime(
+ "capture-%Y-%m-%d-%H-%M-%S.png")
+ print("Capturing image as %s" % image_name)
+ cv2.imwrite(image_name, frame)
+
+# When everything's done, release the capture
+cap.release()
+cv2.destroyAllWindows()
diff --git a/y2020/vision/tools/python_code/image_match_test.py b/y2020/vision/tools/python_code/image_match_test.py
index 09d2764..455bc30 100644
--- a/y2020/vision/tools/python_code/image_match_test.py
+++ b/y2020/vision/tools/python_code/image_match_test.py
@@ -173,7 +173,7 @@
# Load from camera parameters
cam_mat = camera_params.camera_int.camera_matrix
- distortion_coeffs = camera_params.camera_int.distortion_coeffs
+ dist_coeffs = camera_params.camera_int.dist_coeffs
# Create list of matching query point locations
dst_pts = np.float32([
@@ -184,7 +184,7 @@
# TODO: May want to supply it with estimated guess as starting point
# Find offset from camera to original location of camera relative to target
retval, R_ci_w_estj, T_ci_w_estj, inliers = cv2.solvePnPRansac(
- src_pts_3d_array, dst_pts, cam_mat, distortion_coeffs)
+ src_pts_3d_array, dst_pts, cam_mat, dist_coeffs)
tf = time.monotonic()
print("Solving Pose took ", tf - ts, " secs")
diff --git a/y2020/vision/tools/python_code/load_sift_training.py b/y2020/vision/tools/python_code/load_sift_training.py
index 6db3155..0ce70b7 100644
--- a/y2020/vision/tools/python_code/load_sift_training.py
+++ b/y2020/vision/tools/python_code/load_sift_training.py
@@ -71,7 +71,8 @@
# Build the Descriptor vector
Feature.FeatureStartDescriptorVector(fbb, len(descriptor))
for n in reversed(descriptor):
- fbb.PrependFloat32(n)
+ assert n == round(n)
+ fbb.PrependUint8(int(round(n)))
descriptor_vector = fbb.EndVector(len(descriptor))
# Add all the components to the each Feature
@@ -160,13 +161,12 @@
fbb.PrependFloat32(n)
intrinsics_vector = fbb.EndVector(len(camera_int_list))
- dist_coeff_list = camera_calib.camera_int.distortion_coeffs.ravel(
- ).tolist()
+ dist_coeffs_list = camera_calib.camera_int.dist_coeffs.ravel().tolist()
CameraCalibration.CameraCalibrationStartDistCoeffsVector(
- fbb, len(dist_coeff_list))
- for n in reversed(dist_coeff_list):
+ fbb, len(dist_coeffs_list))
+ for n in reversed(dist_coeffs_list):
fbb.PrependFloat32(n)
- dist_coeff_vector = fbb.EndVector(len(dist_coeff_list))
+ dist_coeffs_vector = fbb.EndVector(len(dist_coeffs_list))
node_name_offset = fbb.CreateString(camera_calib.node_name)
CameraCalibration.CameraCalibrationStart(fbb)
@@ -176,7 +176,7 @@
CameraCalibration.CameraCalibrationAddIntrinsics(
fbb, intrinsics_vector)
CameraCalibration.CameraCalibrationAddDistCoeffs(
- fbb, dist_coeff_vector)
+ fbb, dist_coeffs_vector)
CameraCalibration.CameraCalibrationAddFixedExtrinsics(
fbb, fixed_extrinsics_vector)
camera_calibration_vector.append(
diff --git a/y2020/vision/tools/python_code/target_definition.py b/y2020/vision/tools/python_code/target_definition.py
index eae358c..fb2f03a 100644
--- a/y2020/vision/tools/python_code/target_definition.py
+++ b/y2020/vision/tools/python_code/target_definition.py
@@ -384,7 +384,7 @@
dtd.visualize_reprojections(
img_copy, ideal_pts_tmp, ideal_pts_3d_tmp,
camera_params.camera_int.camera_matrix,
- camera_params.camera_int.distortion_coeffs)
+ camera_params.camera_int.dist_coeffs)
for polygon in ideal_target.polygon_list:
img_copy = ideal_target.image.copy()
@@ -401,7 +401,7 @@
np.asarray(kp_in_poly2d).reshape(-1, 2),
np.asarray(kp_in_poly3d).reshape(
-1, 3), camera_params.camera_int.camera_matrix,
- camera_params.camera_int.distortion_coeffs)
+ camera_params.camera_int.dist_coeffs)
###############
### Compute 3D points on actual training images
@@ -510,7 +510,7 @@
dtd.visualize_reprojections(
img_copy, kp_tmp, training_target.keypoint_list_3d,
camera_params.camera_int.camera_matrix,
- camera_params.camera_int.distortion_coeffs)
+ camera_params.camera_int.dist_coeffs)
y2020_target_list = training_target_list
return y2020_target_list
diff --git a/y2020/vision/tools/python_code/train_and_match.py b/y2020/vision/tools/python_code/train_and_match.py
index f139e71..1def399 100644
--- a/y2020/vision/tools/python_code/train_and_match.py
+++ b/y2020/vision/tools/python_code/train_and_match.py
@@ -41,7 +41,7 @@
# Load image (in color; let opencv convert to B&W for features)
img_data = cv2.imread(im)
if img_data is None:
- glog.error("Failed to load image: ", im)
+ glog.error("Failed to load image: '%s'" % im)
exit()
else:
image_list.append(img_data)
diff --git a/y2020/vision/tools/python_code/transform_projection_test.py b/y2020/vision/tools/python_code/transform_projection_test.py
index 00f68bb..01fe695 100644
--- a/y2020/vision/tools/python_code/transform_projection_test.py
+++ b/y2020/vision/tools/python_code/transform_projection_test.py
@@ -8,7 +8,7 @@
# Import camera from camera_definition
camera_params = camera_definition.web_cam_params
cam_mat = camera_params.camera_int.camera_matrix
-distortion_coeffs = camera_params.camera_int.distortion_coeffs
+dist_coeffs = camera_params.camera_int.dist_coeffs
# Height of camera on robot above ground
cam_above_ground = 0.5
@@ -48,11 +48,11 @@
pts_proj_2d_cam2, jac_2d = cv2.projectPoints(
pts_3d_target, R_cam_cam2_gt_mat_inv, T_cam_cam2_gt_inv, cam_mat,
- distortion_coeffs)
+ dist_coeffs)
# Now, solve for the pose using the original 3d points (pts_3d_T_t) and the projections from the new location
retval, R_cam2_cam_est, T_cam2_cam_est, inliers = cv2.solvePnPRansac(
- pts_3d_target, pts_proj_2d_cam2, cam_mat, distortion_coeffs)
+ pts_3d_target, pts_proj_2d_cam2, cam_mat, dist_coeffs)
# This is the pose from camera to original target spot. We need to invert to get back to the pose we want
R_cam_cam2_est_mat = cv2.Rodrigues(R_cam2_cam_est)[0].T
diff --git a/y2020/vision/tools/python_code/usb_camera_stream.py b/y2020/vision/tools/python_code/usb_camera_stream.py
deleted file mode 100644
index 5d3ae91..0000000
--- a/y2020/vision/tools/python_code/usb_camera_stream.py
+++ /dev/null
@@ -1,25 +0,0 @@
-import cv2
-# Open the device at the ID X for /dev/videoX
-cap = cv2.VideoCapture(2)
-
-#Check whether user selected camera is opened successfully.
-if not (cap.isOpened()):
- print("Could not open video device")
- quit()
-
-while True:
- # Capture frame-by-frame
- ret, frame = cap.read()
-
- exp = cap.get(cv2.CAP_PROP_EXPOSURE)
- print("Exposure:", exp)
- # Display the resulting frame
- cv2.imshow('preview', frame)
-
- #Waits for a user input to quit the application
- if cv2.waitKey(1) & 0xFF == ord('q'):
- break
-
-# When everything done, release the capture
-cap.release()
-cv2.destroyAllWindows()
diff --git a/y2020/vision/viewer.cc b/y2020/vision/viewer.cc
new file mode 100644
index 0000000..be9b980
--- /dev/null
+++ b/y2020/vision/viewer.cc
@@ -0,0 +1,46 @@
+#include <opencv2/calib3d.hpp>
+#include <opencv2/features2d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2020/vision/vision_generated.h"
+
+DEFINE_string(config, "config.json", "Path to the config file to use.");
+
+namespace frc971 {
+namespace vision {
+namespace {
+
+void ViewerMain() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ event_loop.MakeWatcher("/camera", [](const CameraImage &image) {
+ cv::Mat image_mat(image.rows(), image.cols(), CV_8U);
+ CHECK(image_mat.isContinuous());
+ const int number_pixels = image.rows() * image.cols();
+ for (int i = 0; i < number_pixels; ++i) {
+ reinterpret_cast<uint8_t *>(image_mat.data)[i] =
+ image.data()->data()[i * 2];
+ }
+
+ cv::imshow("Display", image_mat);
+ cv::waitKey(1);
+ });
+
+ event_loop.Run();
+}
+
+} // namespace
+} // namespace vision
+} // namespace frc971
+
+// Quick and lightweight grayscale viewer for images
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ frc971::vision::ViewerMain();
+}
diff --git a/y2020/vision/viewer_replay.cc b/y2020/vision/viewer_replay.cc
new file mode 100644
index 0000000..82ab11a
--- /dev/null
+++ b/y2020/vision/viewer_replay.cc
@@ -0,0 +1,65 @@
+#include <opencv2/calib3d.hpp>
+#include <opencv2/features2d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "aos/events/logging/logger.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "y2020/vision/vision_generated.h"
+
+DEFINE_string(config, "y2020/config.json", "Path to the config file to use.");
+DEFINE_string(logfile, "", "Path to the log file to use.");
+DEFINE_string(node, "pi1", "Node name to replay.");
+DEFINE_string(image_save_prefix, "/tmp/img",
+ "Prefix to use for saving images from the logfile.");
+
+namespace frc971 {
+namespace vision {
+namespace {
+
+void ViewerMain() {
+ CHECK(!FLAGS_logfile.empty()) << "You forgot to specify a logfile.";
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::logger::LogReader reader(FLAGS_logfile, &config.message());
+ reader.Register();
+ const aos::Node *node = nullptr;
+ if (aos::configuration::MultiNode(reader.configuration())) {
+ node = aos::configuration::GetNode(reader.configuration(), FLAGS_node);
+ }
+ std::unique_ptr<aos::EventLoop> event_loop =
+ reader.event_loop_factory()->MakeEventLoop("player", node);
+
+ int image_count = 0;
+ event_loop->MakeWatcher("/camera", [&image_count](const CameraImage &image) {
+ cv::Mat image_mat(image.rows(), image.cols(), CV_8U);
+ CHECK(image_mat.isContinuous());
+ const int number_pixels = image.rows() * image.cols();
+ for (int i = 0; i < number_pixels; ++i) {
+ reinterpret_cast<uint8_t *>(image_mat.data)[i] =
+ image.data()->data()[i * 2];
+ }
+
+ cv::imshow("Display", image_mat);
+ if (!FLAGS_image_save_prefix.empty()) {
+ cv::imwrite("/tmp/img" + std::to_string(image_count++) + ".png",
+ image_mat);
+ }
+ cv::waitKey(1);
+ });
+
+ reader.event_loop_factory()->Run();
+}
+
+} // namespace
+} // namespace vision
+} // namespace frc971
+
+// Quick and lightweight grayscale viewer for images
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ frc971::vision::ViewerMain();
+}
diff --git a/y2020/www/BUILD b/y2020/www/BUILD
index e8c227d..a035d84 100644
--- a/y2020/www/BUILD
+++ b/y2020/www/BUILD
@@ -25,6 +25,7 @@
],
deps = [
"//aos/network/www:proxy",
+ "//y2020/vision/sift:sift_ts_fbs",
],
)
diff --git a/y2020/www/field.html b/y2020/www/field.html
index 37452a3..ad449db 100644
--- a/y2020/www/field.html
+++ b/y2020/www/field.html
@@ -5,8 +5,6 @@
<link rel="stylesheet" href="styles.css">
</head>
<body>
- <div id="config">
- </div>
</body>
</html>
diff --git a/y2020/www/field_handler.ts b/y2020/www/field_handler.ts
index a960a63..f05f5d5 100644
--- a/y2020/www/field_handler.ts
+++ b/y2020/www/field_handler.ts
@@ -1,33 +1,36 @@
+import {Configuration, Channel} from 'aos/configuration_generated';
+import {Connection} from 'aos/network/www/proxy';
+import {Connect} from 'aos/network/connect_generated';
import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
+import {ImageMatchResult} from 'y2020/vision/sift/sift_generated'
+// (0,0) is field center, +X is toward red DS
const FIELD_SIDE_Y = FIELD_WIDTH / 2;
-const FIELD_CENTER_X = (198.75 + 116) * IN_TO_M;
+const FIELD_EDGE_X = FIELD_LENGTH / 2;
const DS_WIDTH = 69 * IN_TO_M;
const DS_ANGLE = 20 * Math.PI / 180;
-const DS_END_X = DS_WIDTH * Math.sin(DS_ANGLE);
-const OTHER_DS_X = FIELD_LENGTH - DS_END_X;
+const DS_END_X = FIELD_EDGE_X - DS_WIDTH * Math.sin(DS_ANGLE);
const DS_INSIDE_Y = FIELD_SIDE_Y - DS_WIDTH * Math.cos(DS_ANGLE);
-const TRENCH_START_X = 206.57 * IN_TO_M;
-const TRENCH_END_X = FIELD_LENGTH - TRENCH_START_X;
+const TRENCH_X = 108 * IN_TO_M;
const TRENCH_WIDTH = 55.5 * IN_TO_M;
const TRENCH_INSIDE = FIELD_SIDE_Y - TRENCH_WIDTH;
const SPINNER_LENGTH = 30 * IN_TO_M;
-const SPINNER_TOP_X = 374.59 * IN_TO_M;
+const SPINNER_TOP_X = 374.59 * IN_TO_M - FIELD_EDGE_X;
const SPINNER_BOTTOM_X = SPINNER_TOP_X - SPINNER_LENGTH;
-const SHIELD_BOTTOM_X = FIELD_CENTER_X - 116 * IN_TO_M;
+const SHIELD_BOTTOM_X = -116 * IN_TO_M;
const SHIELD_BOTTOM_Y = 43.75 * IN_TO_M;
-const SHIELD_TOP_X = FIELD_CENTER_X + 116 * IN_TO_M;
+const SHIELD_TOP_X = 116 * IN_TO_M;
const SHIELD_TOP_Y = -43.75 * IN_TO_M;
-const SHIELD_RIGHT_X = FIELD_CENTER_X - 51.06 * IN_TO_M;
+const SHIELD_RIGHT_X = -51.06 * IN_TO_M;
const SHIELD_RIGHT_Y = -112.88 * IN_TO_M;
-const SHIELD_LEFT_X = FIELD_CENTER_X + 51.06 * IN_TO_M;
+const SHIELD_LEFT_X = 51.06 * IN_TO_M;
const SHIELD_LEFT_Y = 112.88 * IN_TO_M;
const SHIELD_CENTER_TOP_X = (SHIELD_TOP_X + SHIELD_LEFT_X) / 2
@@ -36,18 +39,78 @@
const SHIELD_CENTER_BOTTOM_X = (SHIELD_BOTTOM_X + SHIELD_RIGHT_X) / 2
const SHIELD_CENTER_BOTTOM_Y = (SHIELD_BOTTOM_Y + SHIELD_RIGHT_Y) / 2
-const INITIATION_X = 120 * IN_TO_M;
-const FAR_INITIATION_X = FIELD_LENGTH - 120 * IN_TO_M;
+const INITIATION_X = FIELD_EDGE_X - 120 * IN_TO_M;
-const TARGET_ZONE_TIP_X = 30 * IN_TO_M;
+const TARGET_ZONE_TIP_X = FIELD_EDGE_X - 30 * IN_TO_M;
const TARGET_ZONE_WIDTH = 48 * IN_TO_M;
const LOADING_ZONE_WIDTH = 60 * IN_TO_M;
+/**
+ * All the messages that are required to display camera information on the field.
+ * Messages not readable on the server node are ignored.
+ */
+const REQUIRED_CHANNELS = [
+ {
+ name: '/pi1/camera',
+ type: 'frc971.vision.sift.ImageMatchResult',
+ },
+ {
+ name: '/pi2/camera',
+ type: 'frc971.vision.sift.ImageMatchResult',
+ },
+ {
+ name: '/pi3/camera',
+ type: 'frc971.vision.sift.ImageMatchResult',
+ },
+ {
+ name: '/pi4/camera',
+ type: 'frc971.vision.sift.ImageMatchResult',
+ },
+ {
+ name: '/pi5/camera',
+ type: 'frc971.vision.sift.ImageMatchResult',
+ },
+];
+
export class FieldHandler {
private canvas = document.createElement('canvas');
+ private imageMatchResult :ImageMatchResult|null = null
- constructor() {
+ constructor(private readonly connection: Connection) {
document.body.appendChild(this.canvas);
+
+ this.connection.addConfigHandler(() => {
+ this.sendConnect();
+ });
+ this.connection.addHandler(ImageMatchResult.getFullyQualifiedName(), (res) => {
+ this.handleImageMatchResult(res);
+ });
+ }
+
+ private handleImageMatchResult(data: Uint8Array): void {
+ const fbBuffer = new flatbuffers.ByteBuffer(data);
+ this.imageMatchResult = ImageMatchResult.getRootAsImageMatchResult(fbBuffer);
+ }
+
+ private sendConnect(): void {
+ const builder = new flatbuffers.Builder(512);
+ const channels: flatbuffers.Offset[] = [];
+ for (const channel of REQUIRED_CHANNELS) {
+ const nameFb = builder.createString(channel.name);
+ const typeFb = builder.createString(channel.type);
+ Channel.startChannel(builder);
+ Channel.addName(builder, nameFb);
+ Channel.addType(builder, typeFb);
+ const channelFb = Channel.endChannel(builder);
+ channels.push(channelFb);
+ }
+
+ const channelsFb = Connect.createChannelsToTransferVector(builder, channels);
+ Connect.startConnect(builder);
+ Connect.addChannelsToTransfer(builder, channelsFb);
+ const connect = Connect.endConnect(builder);
+ builder.finish(connect);
+ this.connection.sendConnectMessage(builder);
}
drawField(): void {
@@ -56,15 +119,15 @@
const ctx = this.canvas.getContext('2d');
// draw perimiter
ctx.beginPath();
- ctx.moveTo(0, DS_INSIDE_Y);
+ ctx.moveTo(FIELD_EDGE_X, DS_INSIDE_Y);
ctx.lineTo(DS_END_X, FIELD_SIDE_Y);
- ctx.lineTo(OTHER_DS_X, FIELD_SIDE_Y);
- ctx.lineTo(FIELD_LENGTH, DS_INSIDE_Y);
- ctx.lineTo(FIELD_LENGTH, -DS_INSIDE_Y);
- ctx.lineTo(OTHER_DS_X, -FIELD_SIDE_Y);
+ ctx.lineTo(-DS_END_X, FIELD_SIDE_Y);
+ ctx.lineTo(-FIELD_EDGE_X, DS_INSIDE_Y);
+ ctx.lineTo(-FIELD_EDGE_X, -DS_INSIDE_Y);
+ ctx.lineTo(-DS_END_X, -FIELD_SIDE_Y);
ctx.lineTo(DS_END_X, -FIELD_SIDE_Y);
- ctx.lineTo(0, -DS_INSIDE_Y);
- ctx.lineTo(0, DS_INSIDE_Y);
+ ctx.lineTo(FIELD_EDGE_X, -DS_INSIDE_Y);
+ ctx.lineTo(FIELD_EDGE_X, DS_INSIDE_Y);
ctx.stroke();
// draw shield generator
@@ -78,21 +141,20 @@
ctx.lineTo(SHIELD_CENTER_BOTTOM_X, SHIELD_CENTER_BOTTOM_Y);
ctx.stroke();
- // draw trenches
- ctx.strokeStyle = MY_COLOR;
- ctx.beginPath();
- ctx.moveTo(TRENCH_START_X, FIELD_SIDE_Y);
- ctx.lineTo(TRENCH_START_X, TRENCH_INSIDE);
- ctx.lineTo(TRENCH_END_X, TRENCH_INSIDE);
- ctx.lineTo(TRENCH_END_X, FIELD_SIDE_Y);
- ctx.stroke();
+ this.drawHalfField(ctx, 'red');
+ ctx.rotate(Math.PI);
+ this.drawHalfField(ctx, 'blue');
+ ctx.rotate(Math.PI);
+ }
- ctx.strokeStyle = OTHER_COLOR;
+ drawHalfField(ctx, color: string): void {
+ // trenches
+ ctx.strokeStyle = color;
ctx.beginPath();
- ctx.moveTo(TRENCH_START_X, -FIELD_SIDE_Y);
- ctx.lineTo(TRENCH_START_X, -TRENCH_INSIDE);
- ctx.lineTo(TRENCH_END_X, -TRENCH_INSIDE);
- ctx.lineTo(TRENCH_END_X, -FIELD_SIDE_Y);
+ ctx.moveTo(TRENCH_X, FIELD_SIDE_Y);
+ ctx.lineTo(TRENCH_X, TRENCH_INSIDE);
+ ctx.lineTo(-TRENCH_X, TRENCH_INSIDE);
+ ctx.lineTo(-TRENCH_X, FIELD_SIDE_Y);
ctx.stroke();
ctx.strokeStyle = 'black';
@@ -101,46 +163,59 @@
ctx.lineTo(SPINNER_TOP_X, TRENCH_INSIDE);
ctx.lineTo(SPINNER_BOTTOM_X, TRENCH_INSIDE);
ctx.lineTo(SPINNER_BOTTOM_X, FIELD_SIDE_Y);
- ctx.moveTo(FIELD_LENGTH - SPINNER_TOP_X, -FIELD_SIDE_Y);
- ctx.lineTo(FIELD_LENGTH - SPINNER_TOP_X, -TRENCH_INSIDE);
- ctx.lineTo(FIELD_LENGTH - SPINNER_BOTTOM_X, -TRENCH_INSIDE);
- ctx.lineTo(FIELD_LENGTH - SPINNER_BOTTOM_X, -FIELD_SIDE_Y);
ctx.stroke();
- // draw initiation lines
ctx.beginPath();
ctx.moveTo(INITIATION_X, FIELD_SIDE_Y);
ctx.lineTo(INITIATION_X, -FIELD_SIDE_Y);
- ctx.moveTo(FAR_INITIATION_X, FIELD_SIDE_Y);
- ctx.lineTo(FAR_INITIATION_X, -FIELD_SIDE_Y);
ctx.stroke();
- // draw target/loading zones
- ctx.strokeStyle = MY_COLOR;
+ // target/loading
+ ctx.strokeStyle = color;
ctx.beginPath();
- ctx.moveTo(0, DS_INSIDE_Y);
+ ctx.moveTo(FIELD_EDGE_X, DS_INSIDE_Y);
ctx.lineTo(TARGET_ZONE_TIP_X, DS_INSIDE_Y - 0.5 * TARGET_ZONE_WIDTH);
- ctx.lineTo(0, DS_INSIDE_Y - TARGET_ZONE_WIDTH);
+ ctx.lineTo(FIELD_EDGE_X, DS_INSIDE_Y - TARGET_ZONE_WIDTH);
- ctx.moveTo(FIELD_LENGTH, DS_INSIDE_Y);
- ctx.lineTo(
- FIELD_LENGTH - TARGET_ZONE_TIP_X,
- DS_INSIDE_Y - 0.5 * LOADING_ZONE_WIDTH);
- ctx.lineTo(FIELD_LENGTH, DS_INSIDE_Y - LOADING_ZONE_WIDTH);
+ ctx.moveTo(-FIELD_EDGE_X, DS_INSIDE_Y);
+ ctx.lineTo(-TARGET_ZONE_TIP_X, DS_INSIDE_Y - 0.5 * LOADING_ZONE_WIDTH);
+ ctx.lineTo(-FIELD_EDGE_X, DS_INSIDE_Y - LOADING_ZONE_WIDTH);
ctx.stroke();
+ }
- ctx.strokeStyle = OTHER_COLOR;
+ drawCamera(x: number, y: number, theta: number): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.translate(x, y);
+ ctx.rotate(theta);
ctx.beginPath();
- ctx.moveTo(0, -DS_INSIDE_Y);
- ctx.lineTo(TARGET_ZONE_TIP_X, -(DS_INSIDE_Y - 0.5 * LOADING_ZONE_WIDTH));
- ctx.lineTo(0, -(DS_INSIDE_Y - LOADING_ZONE_WIDTH));
-
- ctx.moveTo(FIELD_LENGTH, -DS_INSIDE_Y);
- ctx.lineTo(
- FIELD_LENGTH - TARGET_ZONE_TIP_X,
- -(DS_INSIDE_Y - 0.5 * TARGET_ZONE_WIDTH));
- ctx.lineTo(FIELD_LENGTH, -(DS_INSIDE_Y - TARGET_ZONE_WIDTH));
+ ctx.moveTo(0.5, 0.5);
+ ctx.lineTo(0, 0);
+ ctx.lineTo(0.5, -0.5);
ctx.stroke();
+ ctx.beginPath();
+ ctx.arc(0, 0, 0.25, -Math.PI/4, Math.PI/4);
+ ctx.stroke();
+ ctx.restore();
+ }
+
+ draw(): void {
+ this.reset();
+ this.drawField();
+ //draw cameras
+ if (this.imageMatchResult) {
+ console.log(this.imageMatchResult.cameraPosesLength());
+ for (const i = 0; i < this.imageMatchResult.cameraPosesLength(); i++) {
+ const pose = this.imageMatchResult.cameraPoses(i);
+ const mat = pose.fieldToCamera();
+ const x = mat.data(3);
+ const y = mat.data(7);
+ this.drawCamera(x, y, 0);
+ console.log(x, y);
+ }
+ }
+
+ window.requestAnimationFrame(() => this.draw());
}
reset(): void {
@@ -148,17 +223,17 @@
ctx.setTransform(1, 0, 0, 1, 0, 0);
const size = window.innerHeight * 0.9;
ctx.canvas.height = size;
- ctx.canvas.width = size / 2 + 10;
- ctx.clearRect(0, 0, size, size / 2 + 10);
+ const width = size / 2 + 20;
+ ctx.canvas.width = width;
+ ctx.clearRect(0, 0, size, width);
- // Translate to center of bottom of display.
- ctx.translate(size / 4, size);
+ // Translate to center of display.
+ ctx.translate(width / 2, size / 2);
// Coordinate system is:
// x -> forward.
// y -> to the left.
ctx.rotate(-Math.PI / 2);
ctx.scale(1, -1);
- ctx.translate(5, 0);
const M_TO_PX = (size - 10) / FIELD_LENGTH;
ctx.scale(M_TO_PX, M_TO_PX);
diff --git a/y2020/www/field_main.ts b/y2020/www/field_main.ts
index adcaa27..163c817 100644
--- a/y2020/www/field_main.ts
+++ b/y2020/www/field_main.ts
@@ -6,8 +6,7 @@
conn.connect();
-const fieldHandler = new FieldHandler();
+const fieldHandler = new FieldHandler(conn);
-fieldHandler.reset();
-fieldHandler.drawField();
+fieldHandler.draw();
diff --git a/y2020/www/image_handler.ts b/y2020/www/image_handler.ts
index ae530ef..e2ba0b9 100644
--- a/y2020/www/image_handler.ts
+++ b/y2020/www/image_handler.ts
@@ -4,6 +4,7 @@
export class ImageHandler {
private canvas = document.createElement('canvas');
private imageBuffer: Uint8ClampedArray|null = null;
+ private image: CameraImage|null = null;
private imageTimestamp: flatbuffers.Long|null = null;
private result: ImageMatchResult|null = null;
private resultTimestamp: flatbuffers.Long|null = null;
@@ -16,6 +17,7 @@
}
handleImage(data: Uint8Array): void {
+ console.log('got an image to process');
if (this.imageSkipCount != 0) {
this.imageSkipCount--;
return;
@@ -24,24 +26,28 @@
}
const fbBuffer = new flatbuffers.ByteBuffer(data);
- const image = CameraImage.getRootAsCameraImage(fbBuffer);
- this.imageTimestamp = image.monotonicTimestampNs();
+ this.image = CameraImage.getRootAsCameraImage(fbBuffer);
+ this.imageTimestamp = this.image.monotonicTimestampNs();
- this.width = image.cols();
- this.height = image.rows();
+ this.width = this.image.cols();
+ this.height = this.image.rows();
if (this.width === 0 || this.height === 0) {
return;
}
- this.imageBuffer = new Uint8ClampedArray(this.width * this.height * 4); // RGBA
+ this.draw();
+ }
+
+ convertImage(): void {
+ this.imageBuffer = new Uint8ClampedArray(this.width * this.height * 4); // RGBA
// Read four bytes (YUYV) from the data and transform into two pixels of
// RGBA for canvas
for (const j = 0; j < this.height; j++) {
for (const i = 0; i < this.width; i += 2) {
- const y1 = image.data((j * this.width + i) * 2);
- const u = image.data((j * this.width + i) * 2 + 1);
- const y2 = image.data((j * this.width + i + 1) * 2);
- const v = image.data((j * this.width + i + 1) * 2 + 1);
+ const y1 = this.image.data((j * this.width + i) * 2);
+ const u = this.image.data((j * this.width + i) * 2 + 1);
+ const y2 = this.image.data((j * this.width + i + 1) * 2);
+ const v = this.image.data((j * this.width + i + 1) * 2 + 1);
// Based on https://en.wikipedia.org/wiki/YUV#Converting_between_Y%E2%80%B2UV_and_RGB
const c1 = y1 - 16;
@@ -59,11 +65,10 @@
this.imageBuffer[(j * this.width + i) * 4 + 7] = 255;
}
}
-
- this.draw();
}
handleImageMetadata(data: Uint8Array): void {
+ console.log('got an image match result to process');
const fbBuffer = new flatbuffers.ByteBuffer(data);
this.result = ImageMatchResult.getRootAsImageMatchResult(fbBuffer);
this.resultTimestamp = this.result.imageMonotonicTimestampNs();
@@ -74,8 +79,12 @@
if (!this.imageTimestamp || !this.resultTimestamp ||
this.imageTimestamp.low !== this.resultTimestamp.low ||
this.imageTimestamp.high !== this.resultTimestamp.high) {
+ console.log('image and result do not match');
+ console.log(this.imageTimestamp.low, this.resultTimestamp.low);
+ console.log(this.imageTimestamp.high, this.resultTimestamp.high);
return;
}
+ this.convertImage();
const ctx = this.canvas.getContext('2d');
this.canvas.width = this.width;
diff --git a/y2020/www/index.html b/y2020/www/index.html
index 97e16d4..20b9785 100644
--- a/y2020/www/index.html
+++ b/y2020/www/index.html
@@ -5,7 +5,5 @@
<link rel="stylesheet" href="styles.css">
</head>
<body>
- <div id="config">
- </div>
</body>
</html>
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 3d966b8..b40ac87 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -248,6 +248,12 @@
"num_senders": 2
},
{
+ "name": "/superstructure",
+ "type": "y2020.joysticks.Setpoint",
+ "source_node": "roborio",
+ "num_senders": 2
+ },
+ {
"name": "/drivetrain",
"type": "frc971.IMUValues",
"source_node": "roborio",
@@ -334,7 +340,7 @@
"type": "frc971.vision.sift.ImageMatchResult",
"source_node": "pi1",
"frequency": 25,
- "max_size": 300000
+ "max_size": 1000000
},
{
"name": "/pi1/camera",
@@ -370,7 +376,7 @@
"type": "frc971.vision.sift.ImageMatchResult",
"source_node": "pi2",
"frequency": 25,
- "max_size": 300000
+ "max_size": 1000000
},
{
"name": "/pi2/camera",
@@ -406,7 +412,7 @@
"type": "frc971.vision.sift.ImageMatchResult",
"source_node": "pi3",
"frequency": 25,
- "max_size": 300000
+ "max_size": 1000000
},
{
"name": "/pi3/camera",