Merge changes I1132152d,I2ee8fa9d,I9c11b56d,I04de0d01,If4fc6249
* changes:
Make Aimer use correct turret zero convention
Add basic shooting-on-the-fly implementation
Handle turret wrapping more intelligently in Aimer
Add targets for the turret aiming to shoot at
Perform basic turret auto-aiming
diff --git a/aos/network/www/config_handler.ts b/aos/network/www/config_handler.ts
index 72b496e..3d20f0d 100644
--- a/aos/network/www/config_handler.ts
+++ b/aos/network/www/config_handler.ts
@@ -1,18 +1,35 @@
import {Configuration, Channel} from 'aos/configuration_generated';
import {Connect} from 'aos/network/connect_generated';
+import {Connection} from './proxy';
export class ConfigHandler {
private readonly root_div = document.getElementById('config');
+ private readonly tree_div;
+ private config: Configuration|null = null
- constructor(
- private readonly config: Configuration,
- private readonly dataChannel: RTCDataChannel) {}
+ constructor(private readonly connection: Connection) {
+ this.connection.addConfigHandler((config) => this.handleConfig(config));
+
+ const show_button = document.createElement('button');
+ show_button.addEventListener('click', () => this.toggleConfig());
+ const show_text = document.createTextNode('Show/Hide Config');
+ show_button.appendChild(show_text);
+ this.tree_div = document.createElement('div');
+ this.tree_div.hidden = true;
+ this.root_div.appendChild(show_button);
+ this.root_div.appendChild(this.tree_div);
+ }
+
+ handleConfig(config: Configuration) {
+ this.config = config;
+ this.printConfig();
+ }
printConfig() {
for (const i = 0; i < this.config.channelsLength(); i++) {
const channel_div = document.createElement('div');
channel_div.classList.add('channel');
- this.root_div.appendChild(channel_div);
+ this.tree_div.appendChild(channel_div);
const input_el = document.createElement('input');
input_el.setAttribute('data-index', i);
@@ -60,8 +77,10 @@
Connect.addChannelsToTransfer(builder, channelsfb);
const connect = Connect.endConnect(builder);
builder.finish(connect);
- const array = builder.asUint8Array();
- console.log('connect', array);
- this.dataChannel.send(array.buffer.slice(array.byteOffset));
+ this.connection.sendConnectMessage(builder);
+ }
+
+ toggleConfig() {
+ this.tree_div.hidden = !this.tree_div.hidden;
}
}
diff --git a/aos/network/www/proxy.ts b/aos/network/www/proxy.ts
index 704fc85..9eb0bb6 100644
--- a/aos/network/www/proxy.ts
+++ b/aos/network/www/proxy.ts
@@ -49,8 +49,11 @@
private rtcPeerConnection: RTCPeerConnection|null = null;
private dataChannel: DataChannel|null = null;
private webSocketUrl: string;
- private configHandler: ConfigHandler|null = null;
- private config: Configuration|null = null;
+
+ private configInternal: Configuration|null = null;
+ // A set of functions that accept the config to handle.
+ private readonly configHandlers = new Set<(config: Configuration) => void>();
+
private readonly handlerFuncs = new Map<string, (data: Uint8Array) => void>();
private readonly handlers = new Set<Handler>();
@@ -59,6 +62,10 @@
this.webSocketUrl = `ws://${server}/ws`;
}
+ addConfigHandler(handler: (config: Configuration) => void): void {
+ this.configHandlers.add(handler);
+ }
+
addHandler(id: string, handler: (data: Uint8Array) => void): void {
this.handlerFuncs.set(id, handler);
}
@@ -72,17 +79,17 @@
'message', (e) => this.onWebSocketMessage(e));
}
+ get config() {
+ return this.config_internal;
+ }
+
// Handle messages on the DataChannel. Handles the Configuration message as
// all other messages are sent on specific DataChannels.
onDataChannelMessage(e: MessageEvent): void {
const fbBuffer = new flatbuffers.ByteBuffer(new Uint8Array(e.data));
- // TODO(alex): handle config updates if/when required
- if (!this.configHandler) {
- const config = Configuration.getRootAsConfiguration(fbBuffer);
- this.config = config;
- this.configHandler = new ConfigHandler(config, this.dataChannel);
- this.configHandler.printConfig();
- return;
+ this.configInternal = Configuration.getRootAsConfiguration(fbBuffer);
+ for (handler of this.configHandlers) {
+ handler(this.configInternal);
}
}
@@ -174,4 +181,13 @@
break;
}
}
+
+ /**
+ * Subscribes to messages.
+ * @param a Finished flatbuffer.Builder containing a Connect message to send.
+ */
+ sendConnectMessage(builder: any) {
+ const array = builder.assUint8Array();
+ 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/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/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/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index d769541..f033a89 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -142,7 +142,7 @@
A_.setZero();
B_.setZero();
DelayedU_.setZero();
- UpdateAB(::std::chrono::milliseconds(5));
+ UpdateAB(::std::chrono::microseconds(5050));
}
// Assert that U is within the hardware range.
@@ -158,6 +158,8 @@
// Computes the new X and Y given the control input.
void Update(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
::std::chrono::nanoseconds dt, Scalar voltage_battery) {
+ CHECK_NE(dt, std::chrono::nanoseconds(0));
+
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU(U);
@@ -178,7 +180,11 @@
// or the unit delay... Might want to do that if we care about performance
// again.
UpdateAB(dt);
- return A() * X + B() * U;
+ const Eigen::Matrix<Scalar, number_of_states, 1> new_X =
+ A() * X + B() * DelayedU_;
+ DelayedU_ = U;
+
+ return new_X;
}
protected:
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 27a659a..9c36c90 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -284,9 +284,9 @@
def InitializeState(self):
"""Sets X, Y, and X_hat to zero defaults."""
- self.X = numpy.zeros((self.A.shape[0], 1))
+ self.X = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
self.Y = self.C * self.X
- self.X_hat = numpy.zeros((self.A.shape[0], 1))
+ self.X_hat = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
def PlaceControllerPoles(self, poles):
"""Places the controller poles.
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index d63bca7..168df43 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -108,6 +108,8 @@
}
Scalar U_max(int i, int j = 0) const { return U_max()(i, j); }
+ const std::chrono::nanoseconds dt() const { return coefficients().dt; }
+
const Eigen::Matrix<Scalar, number_of_states, 1> &X() const { return X_; }
Scalar X(int i, int j = 0) const { return X()(i, j); }
const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y() const { return Y_; }
@@ -503,9 +505,7 @@
return controller().Kff() * (next_R() - plant().A() * R());
}
- // stop_motors is whether or not to output all 0s.
- void Update(bool stop_motors,
- ::std::chrono::nanoseconds dt = ::std::chrono::milliseconds(5)) {
+ void UpdateController(bool stop_motors) {
if (stop_motors) {
U_.setZero();
U_uncapped_.setZero();
@@ -514,10 +514,15 @@
U_ = U_uncapped_ = ControllerOutput();
CapU();
}
+ UpdateFFReference();
+ }
+
+ // stop_motors is whether or not to output all 0s.
+ void Update(bool stop_motors,
+ ::std::chrono::nanoseconds dt = ::std::chrono::milliseconds(0)) {
+ UpdateController(stop_motors);
UpdateObserver(U_, dt);
-
- UpdateFFReference();
}
// Updates R() after any CapU operations happen on U().
diff --git a/frc971/downloader/downloader.py b/frc971/downloader/downloader.py
index 5ebf417..42d4a7c 100644
--- a/frc971/downloader/downloader.py
+++ b/frc971/downloader/downloader.py
@@ -82,6 +82,8 @@
ssh_path = "external/ssh/ssh"
scp_path = "external/ssh/scp"
+ subprocess.check_call([ssh_path, ssh_target, "mkdir", "-p", target_dir])
+
rsync_cmd = ([
"external/rsync/usr/bin/rsync", "-e", ssh_path, "-c", "-v", "-z",
"--copy-links"
diff --git a/y2020/BUILD b/y2020/BUILD
index f3ea4d6..4aa851f 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -166,6 +166,7 @@
srcs = ["web_proxy.sh"],
data = [
":config.json",
+ "//y2020/www:field_main_bundle",
"//aos/network:web_proxy_main",
"//y2020/www:files",
"//y2020/www:flatbuffers",
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/constants.cc b/y2020/constants.cc
index f0610a0..463919a 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -102,15 +102,15 @@
break;
case kCompTeamNumber:
- hood->zeroing_constants.measured_absolute_position = 0.432541963515072;
+ hood->zeroing_constants.measured_absolute_position = 0.266691815725476;
intake->zeroing_constants.measured_absolute_position =
1.42977866919024 - Values::kIntakeZero();
- turret->potentiometer_offset =
- 5.52519370141247 + 0.00853506822980376 + 0.0109413725126625;
+ turret->potentiometer_offset = 5.52519370141247 + 0.00853506822980376 +
+ 0.0109413725126625 - 0.223719825811759;
turret_params->zeroing_constants.measured_absolute_position =
- 0.251065633255048;
+ 0.547478339799516;
break;
case kPracticeTeamNumber:
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/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 4b164b5..d26552a 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -23,6 +23,9 @@
return result;
}
+// Indices of the pis to use.
+const std::array<std::string, 3> kPisToUse{"pi1", "pi2", "pi3"};
+
} // namespace
Localizer::Localizer(
@@ -51,9 +54,11 @@
ekf_.P());
});
- image_fetchers_.emplace_back(
- event_loop_->MakeFetcher<frc971::vision::sift::ImageMatchResult>(
- "/pi1/camera"));
+ for (const auto &pi : kPisToUse) {
+ image_fetchers_.emplace_back(
+ event_loop_->MakeFetcher<frc971::vision::sift::ImageMatchResult>(
+ "/" + pi + "/camera"));
+ }
target_selector_.set_has_target(false);
}
@@ -89,9 +94,10 @@
aos::monotonic_clock::time_point now,
double left_encoder, double right_encoder,
double gyro_rate, const Eigen::Vector3d &accel) {
- for (auto &image_fetcher : image_fetchers_) {
+ for (size_t ii = 0; ii < kPisToUse.size(); ++ii) {
+ auto &image_fetcher = image_fetchers_[ii];
while (image_fetcher.FetchNext()) {
- HandleImageMatch(*image_fetcher);
+ HandleImageMatch(kPisToUse[ii], *image_fetcher);
}
}
ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, accel,
@@ -99,13 +105,13 @@
}
void Localizer::HandleImageMatch(
- const frc971::vision::sift::ImageMatchResult &result) {
+ std::string_view pi, const frc971::vision::sift::ImageMatchResult &result) {
std::chrono::nanoseconds monotonic_offset(0);
clock_offset_fetcher_.Fetch();
if (clock_offset_fetcher_.get() != nullptr) {
for (const auto connection : *clock_offset_fetcher_->connections()) {
if (connection->has_node() && connection->node()->has_name() &&
- connection->node()->name()->string_view() == "pi1") {
+ connection->node()->name()->string_view() == pi) {
monotonic_offset =
std::chrono::nanoseconds(connection->monotonic_offset());
break;
@@ -193,6 +199,12 @@
H(0, StateIdx::kX) = 1;
H(1, StateIdx::kY) = 1;
H(2, StateIdx::kTheta) = 1;
+ // If the heading is off by too much, assume that we got a false-positive
+ // and don't use the correction.
+ if (std::abs(aos::math::DiffAngle(theta(), Z(2))) > M_PI_2) {
+ AOS_LOG(WARNING, "Dropped image match due to heading mismatch.\n");
+ return;
+ }
ekf_.Correct(Z, nullptr, {}, [H, Z](const State &X, const Input &) {
Eigen::Vector3d Zhat = H * X;
// In order to deal with wrapping of the
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 8636a7a..1d6f816 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -1,6 +1,8 @@
#ifndef Y2020_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
#define Y2020_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
+#include <string_view>
+
#include "aos/containers/ring_buffer.h"
#include "aos/events/event_loop.h"
#include "aos/network/message_bridge_server_generated.h"
@@ -69,8 +71,9 @@
double velocity = 0.0; // rad/sec
};
- // Processes new image data and updates the EKF.
- void HandleImageMatch(const frc971::vision::sift::ImageMatchResult &result);
+ // Processes new image data from the given pi and updates the EKF.
+ void HandleImageMatch(std::string_view pi,
+ const frc971::vision::sift::ImageMatchResult &result);
// Processes the most recent turret position and stores it in the turret_data_
// buffer.
diff --git a/y2020/control_loops/python/accelerator.py b/y2020/control_loops/python/accelerator.py
index 1746589..48c9098 100644
--- a/y2020/control_loops/python/accelerator.py
+++ b/y2020/control_loops/python/accelerator.py
@@ -28,20 +28,22 @@
name='Accelerator',
motor=control_loop.Falcon(),
G=G,
- J=J,
- q_pos=0.08,
- q_vel=4.00,
- q_voltage=0.4,
+ J=J + 0.0015,
+ q_pos=0.01,
+ q_vel=40.0,
+ q_voltage=2.0,
r_pos=0.05,
- controller_poles=[.84])
+ controller_poles=[.86])
def main(argv):
if FLAGS.plot:
R = numpy.matrix([[0.0], [500.0], [0.0]])
- flywheel.PlotSpinup(kAccelerator, goal=R, iterations=200)
+ flywheel.PlotSpinup(kAccelerator, goal=R, iterations=400)
return 0
+ glog.debug("J is %f" % J)
+
if len(argv) != 5:
glog.fatal('Expected .h file name and .cc file name')
else:
@@ -53,4 +55,5 @@
if __name__ == '__main__':
argv = FLAGS(sys.argv)
+ glog.init()
sys.exit(main(argv))
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 5067a16..9381630 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -14,7 +14,7 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
# Inertia for a single 4" diameter, 2" wide neopreme wheel.
-J_wheel = 0.000319 * 2.0
+J_wheel = 0.000319 * 2.0 * 6.0
# Gear ratio to the final wheel.
# 40 tooth on the flywheel
# 48 for the falcon.
@@ -29,17 +29,17 @@
motor=control_loop.Falcon(),
G=G,
J=J,
- q_pos=0.08,
- q_vel=4.00,
- q_voltage=0.4,
+ q_pos=0.01,
+ q_vel=100.0,
+ q_voltage=6.0,
r_pos=0.05,
- controller_poles=[.84])
+ controller_poles=[.92])
def main(argv):
if FLAGS.plot:
R = numpy.matrix([[0.0], [500.0], [0.0]])
- flywheel.PlotSpinup(params=kFinisher, goal=R, iterations=200)
+ flywheel.PlotSpinup(params=kFinisher, goal=R, iterations=400)
return 0
if len(argv) != 5:
diff --git a/y2020/control_loops/python/flywheel.py b/y2020/control_loops/python/flywheel.py
index 630d223..451788a 100755
--- a/y2020/control_loops/python/flywheel.py
+++ b/y2020/control_loops/python/flywheel.py
@@ -30,7 +30,7 @@
self.controller_poles = controller_poles
-class VelocityFlywheel(control_loop.ControlLoop):
+class VelocityFlywheel(control_loop.HybridControlLoop):
def __init__(self, params, name="Flywheel"):
super(VelocityFlywheel, self).__init__(name=name)
self.params = params
@@ -121,26 +121,44 @@
self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+
# states
# [position, velocity, voltage_error]
self.C_unaugmented = self.C
self.C = numpy.matrix(numpy.zeros((1, 3)))
self.C[0:1, 0:2] = self.C_unaugmented
+ glog.debug('A_continuous %s' % str(self.A_continuous))
+ glog.debug('B_continuous %s' % str(self.B_continuous))
+ glog.debug('C %s' % str(self.C))
+
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
self.B_continuous, self.dt)
+ glog.debug('A %s' % str(self.A))
+ glog.debug('B %s' % str(self.B))
+
q_pos = self.params.q_pos
q_vel = self.params.q_vel
q_voltage = self.params.q_voltage
- self.Q = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
+ self.Q_continuous = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
[0.0, (q_vel**2.0), 0.0],
[0.0, 0.0, (q_voltage**2.0)]])
r_pos = self.params.r_pos
- self.R = numpy.matrix([[(r_pos**2.0)]])
+ self.R_continuous = numpy.matrix([[(r_pos**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
+ _, _, self.Q, self.R = controls.kalmd(
+ A_continuous=self.A_continuous,
+ B_continuous=self.B_continuous,
+ Q_continuous=self.Q_continuous,
+ R_continuous=self.R_continuous,
+ dt=self.dt)
+
+ glog.debug('Q_discrete %s' % (str(self.Q)))
+ glog.debug('R_discrete %s' % (str(self.R)))
+
+ self.KalmanGain, self.P_steady_state = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
self.L = self.A * self.KalmanGain
@@ -155,7 +173,7 @@
self.InitializeState()
-def PlotSpinup(params, goal, iterations=200):
+def PlotSpinup(params, goal, iterations=400):
"""Runs the flywheel plant with an initial condition and goal.
Args:
@@ -210,21 +228,20 @@
if observer_flywheel is not None:
observer_flywheel.Y = flywheel.Y
- observer_flywheel.CorrectObserver(U)
+ observer_flywheel.CorrectHybridObserver(U)
offset.append(observer_flywheel.X_hat[2, 0])
applied_U = U.copy()
- if i > 100:
+ if i > 200:
applied_U += 2
flywheel.Update(applied_U)
if observer_flywheel is not None:
- observer_flywheel.PredictObserver(U)
+ observer_flywheel.PredictHybridObserver(U, flywheel.dt)
t.append(initial_t + i * flywheel.dt)
u.append(U[0, 0])
- glog.debug('Time: %f', t[-1])
pylab.subplot(3, 1, 1)
pylab.plot(t, v, label='x')
pylab.plot(t, x_hat, label='x_hat')
@@ -281,5 +298,9 @@
loop_writer.Write(plant_files[0], plant_files[1])
integral_loop_writer = control_loop.ControlLoopWriter(
- 'Integral' + name, integral_flywheels, namespaces=namespace)
+ 'Integral' + name,
+ integral_flywheels,
+ namespaces=namespace,
+ plant_type='StateFeedbackHybridPlant',
+ observer_type='HybridKalman')
integral_loop_writer.Write(controller_files[0], controller_files[1])
diff --git a/y2020/control_loops/python/hood.py b/y2020/control_loops/python/hood.py
index b4d8c84..7e75dce 100644
--- a/y2020/control_loops/python/hood.py
+++ b/y2020/control_loops/python/hood.py
@@ -31,13 +31,13 @@
kHood = angular_system.AngularSystemParams(
name='Hood',
motor=control_loop.BAG(),
- G=radians_per_turn,
- J=0.08389,
- q_pos=0.55,
- q_vel=14.0,
+ G=radians_per_turn / (2.0 * numpy.pi),
+ J=4.0,
+ q_pos=0.15,
+ q_vel=5.0,
kalman_q_pos=0.12,
- kalman_q_vel=2.0,
- kalman_q_voltage=2.5,
+ kalman_q_vel=10.0,
+ kalman_q_voltage=15.0,
kalman_r_position=0.05)
@@ -47,6 +47,8 @@
angular_system.PlotKick(kHood, R)
angular_system.PlotMotion(kHood, R)
+ glog.info("Radians per turn: %f\n", radians_per_turn)
+
# Write the generated constants out to a file.
if len(argv) != 5:
glog.fatal(
diff --git a/y2020/control_loops/python/turret.py b/y2020/control_loops/python/turret.py
index e276457..5a27afb 100644
--- a/y2020/control_loops/python/turret.py
+++ b/y2020/control_loops/python/turret.py
@@ -22,12 +22,12 @@
name='Turret',
motor=control_loop.Vex775Pro(),
G=(6.0 / 60.0) * (26.0 / 150.0),
- J=0.11,
- q_pos=0.40,
- q_vel=7.0,
+ J=0.20,
+ q_pos=0.30,
+ q_vel=4.5,
kalman_q_pos=0.12,
- kalman_q_vel=2.0,
- kalman_q_voltage=3.0,
+ kalman_q_vel=10.0,
+ kalman_q_voltage=12.0,
kalman_r_position=0.05)
def main(argv):
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
index 77fb769..07c3d99 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
@@ -11,8 +11,12 @@
namespace superstructure {
namespace shooter {
-FlywheelController::FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop)
- : loop_(new StateFeedbackLoop<3, 1, 1>(std::move(loop))) {
+FlywheelController::FlywheelController(
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>> &&loop)
+ : loop_(new StateFeedbackLoop<3, 1, 1, double,
+ StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>(std::move(loop))) {
history_.fill(std::pair<double, ::aos::monotonic_clock::time_point>(
0, ::aos::monotonic_clock::epoch()));
Y_.setZero();
@@ -26,6 +30,18 @@
void FlywheelController::set_position(
double current_position,
const aos::monotonic_clock::time_point position_timestamp) {
+ // Project time forwards.
+ const int newest_history_position =
+ ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
+
+ if (!first_) {
+ loop_->UpdateObserver(
+ loop_->U(),
+ position_timestamp - std::get<1>(history_[newest_history_position]));
+ } else {
+ first_ = false;
+ }
+
// Update position in the model.
Y_ << current_position;
@@ -34,6 +50,8 @@
std::pair<double, ::aos::monotonic_clock::time_point>(current_position,
position_timestamp);
history_position_ = (history_position_ + 1) % kHistoryLength;
+
+ loop_->Correct(Y_);
}
double FlywheelController::voltage() const { return loop_->U(0, 0); }
@@ -45,22 +63,22 @@
disabled = true;
}
- loop_->Correct(Y_);
- loop_->Update(disabled);
+ loop_->UpdateController(disabled);
}
flatbuffers::Offset<FlywheelControllerStatus> FlywheelController::SetStatus(
flatbuffers::FlatBufferBuilder *fbb) {
// Compute the oldest point in the history.
- const int oldest_history_position =
+ const int oldest_history_position = history_position_;
+ const int newest_history_position =
((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
const double total_loop_time = ::aos::time::DurationInSeconds(
- std::get<1>(history_[history_position_]) -
+ std::get<1>(history_[newest_history_position]) -
std::get<1>(history_[oldest_history_position]));
const double distance_traveled =
- std::get<0>(history_[history_position_]) -
+ std::get<0>(history_[newest_history_position]) -
std::get<0>(history_[oldest_history_position]);
// Compute the distance moved over that time period.
@@ -70,6 +88,7 @@
builder.add_avg_angular_velocity(avg_angular_velocity_);
builder.add_angular_velocity(loop_->X_hat(1, 0));
+ builder.add_voltage_error(loop_->X_hat(2, 0));
builder.add_angular_velocity_goal(last_goal_);
return builder.Finish();
}
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
index a3e9bdb..e130389 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -18,7 +18,9 @@
// Handles the velocity control of each flywheel.
class FlywheelController {
public:
- FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop);
+ FlywheelController(
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>> &&loop);
// Sets the velocity goal in radians/sec
void set_goal(double angular_velocity_goal);
@@ -45,7 +47,10 @@
// The current sensor measurement.
Eigen::Matrix<double, 1, 1> Y_;
// The control loop.
- ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
+ ::std::unique_ptr<
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>>
+ loop_;
// History array for calculating a filtered angular velocity.
static constexpr int kHistoryLength = 10;
@@ -59,6 +64,8 @@
double last_goal_ = 0;
+ bool first_ = true;
+
DISALLOW_COPY_AND_ASSIGN(FlywheelController);
};
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 25f6a6a..a9f1c4c 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -12,7 +12,7 @@
namespace shooter {
namespace {
-const double kVelocityTolerance = 0.01;
+const double kVelocityTolerance = 20.0;
} // namespace
Shooter::Shooter()
@@ -46,16 +46,18 @@
accelerator_right_.set_position(position->theta_accelerator_right(),
position_timestamp);
- finisher_.Update(output == nullptr);
- accelerator_left_.Update(output == nullptr);
- accelerator_right_.Update(output == nullptr);
-
// Update goal.
if (goal) {
finisher_.set_goal(goal->velocity_finisher());
accelerator_left_.set_goal(goal->velocity_accelerator());
accelerator_right_.set_goal(goal->velocity_accelerator());
+ }
+ finisher_.Update(output == nullptr);
+ accelerator_left_.Update(output == nullptr);
+ accelerator_right_.Update(output == nullptr);
+
+ if (goal) {
if (UpToSpeed(goal) && goal->velocity_finisher() > kVelocityTolerance &&
goal->velocity_accelerator() > kVelocityTolerance) {
ready_ = true;
diff --git a/y2020/control_loops/superstructure/shooter_plot.pb b/y2020/control_loops/superstructure/shooter_plot.pb
index 01a1e20..16ab5f4 100644
--- a/y2020/control_loops/superstructure/shooter_plot.pb
+++ b/y2020/control_loops/superstructure/shooter_plot.pb
@@ -24,19 +24,87 @@
line {
y_signal {
channel: "Status"
- field: "hood.position"
+ field: "shooter.accelerator_left.avg_angular_velocity"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "shooter.accelerator_right.avg_angular_velocity"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "shooter.accelerator_left.angular_velocity"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "shooter.accelerator_right.angular_velocity"
}
}
line {
y_signal {
channel: "Goal"
- field: "hood.unsafe_goal"
+ field: "shooter.velocity_accelerator"
}
}
line {
y_signal {
- channel: "Position"
- field: "hood.encoder"
+ channel: "Status"
+ field: "shooter.finisher.avg_angular_velocity"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "shooter.finisher.angular_velocity"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Goal"
+ field: "shooter.velocity_finisher"
+ }
+ }
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "Output"
+ field: "accelerator_left_voltage"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Output"
+ field: "accelerator_right_voltage"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "shooter.accelerator_left.voltage_error"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "shooter.accelerator_right.voltage_error"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Output"
+ field: "finisher_voltage"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "shooter.finisher.voltage_error"
}
}
ylabel: "hood position"
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index d5468e9..9ea9eb2 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -62,6 +62,30 @@
output != nullptr ? &(output_struct.hood_voltage) : nullptr,
status->fbb());
+ if (unsafe_goal != nullptr) {
+ if (unsafe_goal->shooting() &&
+ shooting_start_time_ == aos::monotonic_clock::min_time) {
+ shooting_start_time_ = position_timestamp;
+ }
+
+ if (unsafe_goal->shooting()) {
+ constexpr std::chrono::milliseconds kPeriod =
+ std::chrono::milliseconds(250);
+ if ((position_timestamp - shooting_start_time_) % (kPeriod * 2) <
+ kPeriod) {
+ intake_joint_.set_min_position(-0.25);
+ } else {
+ intake_joint_.set_min_position(-0.75);
+ }
+ } else {
+ intake_joint_.clear_min_position();
+ }
+
+ if (!unsafe_goal->shooting()) {
+ shooting_start_time_ = aos::monotonic_clock::min_time;
+ }
+ }
+
flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> intake_status_offset =
intake_joint_.Iterate(
unsafe_goal != nullptr ? unsafe_goal->intake() : nullptr,
@@ -88,19 +112,37 @@
climber_.Iterate(unsafe_goal, output != nullptr ? &(output_struct) : nullptr);
+ const AbsoluteEncoderProfiledJointStatus *const hood_status =
+ GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
+
+ const PotAndAbsoluteEncoderProfiledJointStatus *const turret_status =
+ GetMutableTemporaryPointer(*status->fbb(), turret_status_offset);
+
+ if (output != nullptr) {
+ // Friction is a pain and putting a really high burden on the integrator.
+ const double turret_velocity_sign = turret_status->velocity() * kTurretFrictionGain;
+ output_struct.turret_voltage +=
+ std::clamp(turret_velocity_sign, -kTurretFrictionVoltageLimit,
+ kTurretFrictionVoltageLimit);
+
+ // Friction is a pain and putting a really high burden on the integrator.
+ const double hood_velocity_sign = hood_status->velocity() * kHoodFrictionGain;
+ output_struct.hood_voltage +=
+ std::clamp(hood_velocity_sign, -kHoodFrictionVoltageLimit,
+ kHoodFrictionVoltageLimit);
+
+ // And dither the output.
+ time_ += 0.00505;
+ output_struct.hood_voltage += 1.3 * std::sin(time_ * 2.0 * M_PI * 30.0);
+ }
+
bool zeroed;
bool estopped;
{
- const AbsoluteEncoderProfiledJointStatus *const hood_status =
- GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
-
const AbsoluteEncoderProfiledJointStatus *const intake_status =
GetMutableTemporaryPointer(*status->fbb(), intake_status_offset);
- const PotAndAbsoluteEncoderProfiledJointStatus *const turret_status =
- GetMutableTemporaryPointer(*status->fbb(), turret_status_offset);
-
zeroed = hood_status->zeroed() && intake_status->zeroed() &&
turret_status->zeroed();
estopped = hood_status->estopped() || intake_status->estopped() ||
@@ -122,13 +164,21 @@
if (output != nullptr) {
if (unsafe_goal) {
- output_struct.washing_machine_spinner_voltage = 6.0;
+ output_struct.washing_machine_spinner_voltage = 0.0;
if (unsafe_goal->shooting()) {
- output_struct.feeder_voltage = 6.0;
+ if (shooter_.ready() &&
+ unsafe_goal->shooter()->velocity_accelerator() > 10.0 &&
+ unsafe_goal->shooter()->velocity_finisher() > 10.0) {
+ output_struct.feeder_voltage = 9.0;
+ } else {
+ output_struct.feeder_voltage = 0.0;
+ }
+ output_struct.washing_machine_spinner_voltage = 5.0;
+ output_struct.intake_roller_voltage = 3.0;
} else {
output_struct.feeder_voltage = 0.0;
+ output_struct.intake_roller_voltage = unsafe_goal->roller_voltage();
}
- output_struct.intake_roller_voltage = unsafe_goal->roller_voltage();
} else {
output_struct.intake_roller_voltage = 0.0;
}
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index cea7c6a..1a9d401 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -24,6 +24,14 @@
explicit Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name = "/superstructure");
+ // Terms to control the velocity gain for the friction compensation, and the
+ // voltage cap.
+ static constexpr double kTurretFrictionGain = 10.0;
+ static constexpr double kTurretFrictionVoltageLimit = 1.5;
+
+ static constexpr double kHoodFrictionGain = 40.0;
+ static constexpr double kHoodFrictionVoltageLimit = 1.5;
+
using PotAndAbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
@@ -55,6 +63,12 @@
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
Climber climber_;
+
+ aos::monotonic_clock::time_point shooting_start_time_ =
+ aos::monotonic_clock::min_time;
+
+ double time_ = 0.0;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2020/control_loops/superstructure/superstructure_goal.fbs b/y2020/control_loops/superstructure/superstructure_goal.fbs
index f066ab0..c7fe643 100644
--- a/y2020/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2020/control_loops/superstructure/superstructure_goal.fbs
@@ -27,9 +27,7 @@
// Positive is rollers intaking to Washing Machine.
roller_voltage:float;
- // 0 = facing the front of the robot. Positive rotates counterclockwise.
- // TODO(Kai): Define which wrap of the shooter is 0 if it can rotate past
- // forward more than once.
+ // 0 = facing the back of the robot. Positive rotates counterclockwise.
turret:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
// Only applies if shooter_tracking = false.
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 9a86788..b461ec6 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -235,17 +235,29 @@
EXPECT_NEAR(superstructure_output_fetcher_->turret_voltage(), 0.0,
voltage_check_turret);
+ // Invert the friction model.
+ const double hood_velocity_sign =
+ hood_plant_->X(1) * Superstructure::kHoodFrictionGain;
::Eigen::Matrix<double, 1, 1> hood_U;
hood_U << superstructure_output_fetcher_->hood_voltage() +
- hood_plant_->voltage_offset();
+ hood_plant_->voltage_offset() -
+ std::clamp(hood_velocity_sign,
+ -Superstructure::kHoodFrictionVoltageLimit,
+ Superstructure::kHoodFrictionVoltageLimit);
::Eigen::Matrix<double, 1, 1> intake_U;
intake_U << superstructure_output_fetcher_->intake_joint_voltage() +
intake_plant_->voltage_offset();
+ // Invert the friction model.
+ const double turret_velocity_sign =
+ turret_plant_->X(1) * Superstructure::kTurretFrictionGain;
::Eigen::Matrix<double, 1, 1> turret_U;
turret_U << superstructure_output_fetcher_->turret_voltage() +
- turret_plant_->voltage_offset();
+ turret_plant_->voltage_offset() -
+ std::clamp(turret_velocity_sign,
+ -Superstructure::kTurretFrictionVoltageLimit,
+ Superstructure::kTurretFrictionVoltageLimit);
::Eigen::Matrix<double, 1, 1> accelerator_left_U;
accelerator_left_U
@@ -673,7 +685,8 @@
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_hood_velocity(23.0);
- superstructure_plant_.set_peak_hood_acceleration(0.2);
+ // 30 hz sin wave on the hood causes acceleration to be ignored.
+ superstructure_plant_.set_peak_hood_acceleration(5.5);
superstructure_plant_.set_peak_intake_velocity(23.0);
superstructure_plant_.set_peak_intake_acceleration(0.2);
@@ -756,7 +769,7 @@
}
// Give it a lot of time to get there.
- RunFor(chrono::seconds(9));
+ RunFor(chrono::seconds(15));
VerifyNearGoal();
}
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 5ae287c..81293d3 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -14,6 +14,9 @@
// The target speed selected by the lookup table or from manual override
// Can be compared to velocity to determine if ready.
angular_velocity_goal:double;
+
+ // Voltage error.
+ voltage_error:double;
}
table ShooterStatus {
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 75a4b26..026a50b 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -96,7 +96,13 @@
}
if (data.IsPressed(kIntakeExtend)) {
- intake_pos = 1.0;
+ intake_pos = 1.2;
+ roller_speed = 9.0f;
+ }
+
+ if (superstructure_status_fetcher_.get() &&
+ superstructure_status_fetcher_->intake()->position() > -0.5) {
+ roller_speed = std::max(roller_speed, 6.0f);
}
if (data.IsPressed(kFeed)) {
@@ -119,7 +125,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), hood_pos,
- CreateProfileParameters(*builder.fbb(), 0.5, 1.0));
+ CreateProfileParameters(*builder.fbb(), 0.7, 3.0));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index b643fcb..5e65387 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -31,14 +31,14 @@
srcs = [
"camera_reader.cc",
],
+ data = [
+ "//y2020:config.json",
+ ],
restricted_to = [
"//tools:k8",
"//tools:armhf-debian",
],
visibility = ["//y2020:__subpackages__"],
- data = [
- "//y2020:config.json",
- ],
deps = [
":v4l2_reader",
":vision_fbs",
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index d45ec3f..c47e42b 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -76,6 +76,8 @@
const std::vector<std::vector<cv::DMatch>> &matches,
const std::vector<cv::Mat> &camera_target_list,
const std::vector<cv::Mat> &field_camera_list,
+ const std::vector<cv::Point2f> &target_point_vector,
+ const std::vector<float> &target_radius_vector,
aos::Sender<sift::ImageMatchResult> *result_sender,
bool send_details);
@@ -113,6 +115,17 @@
->field_to_target();
}
+ void TargetLocation(int training_image_index, cv::Point2f &target_location,
+ float &target_radius) {
+ target_location.x =
+ training_data_->images()->Get(training_image_index)->target_point_x();
+ target_location.y =
+ training_data_->images()->Get(training_image_index)->target_point_y();
+ target_radius = training_data_->images()
+ ->Get(training_image_index)
+ ->target_point_radius();
+ }
+
int number_training_images() const {
return training_data_->images()->size();
}
@@ -193,6 +206,8 @@
const std::vector<std::vector<cv::DMatch>> &matches,
const std::vector<cv::Mat> &camera_target_list,
const std::vector<cv::Mat> &field_camera_list,
+ const std::vector<cv::Point2f> &target_point_vector,
+ const std::vector<float> &target_radius_vector,
aos::Sender<sift::ImageMatchResult> *result_sender, bool send_details) {
auto builder = result_sender->MakeBuilder();
const auto camera_calibration_offset =
@@ -234,6 +249,9 @@
pose_builder.add_camera_to_target(transform_offset);
pose_builder.add_field_to_camera(fc_transform_offset);
pose_builder.add_field_to_target(field_to_target_offset);
+ pose_builder.add_query_target_point_x(target_point_vector[i].x);
+ pose_builder.add_query_target_point_y(target_point_vector[i].y);
+ pose_builder.add_query_target_point_radius(target_radius_vector[i]);
camera_poses.emplace_back(pose_builder.Finish());
}
const auto camera_poses_offset = builder.fbb()->CreateVector(camera_poses);
@@ -317,8 +335,12 @@
std::vector<cv::Mat> camera_target_list;
std::vector<cv::Mat> field_camera_list;
- std::vector<PerImageMatches> per_image_good_matches;
+ // Rebuild the matches and store them here
std::vector<std::vector<cv::DMatch>> all_good_matches;
+ // Build list of target point and radius for each good match
+ std::vector<cv::Point2f> target_point_vector;
+ std::vector<float> target_radius_vector;
+
// Iterate through matches for each training image
for (size_t i = 0; i < per_image_matches.size(); ++i) {
const PerImageMatches &per_image = per_image_matches[i];
@@ -343,8 +365,8 @@
VLOG(2) << "Number of matches after homography: " << cv::countNonZero(mask)
<< "\n";
- // Fill our match info for each good match
- // TODO<Jim>: Could probably optimize some of the copies here
+
+ // Fill our match info for each good match based on homography result
PerImageMatches per_image_good_match;
CHECK_EQ(per_image.training_points.size(),
(unsigned long)mask.size().height);
@@ -359,22 +381,46 @@
static_cast<std::vector<cv::DMatch>>(*per_image.matches[j]));
// Fill out the data for matches per image that made it past
- // homography check
+ // homography check, for later use
per_image_good_match.matches.push_back(per_image.matches[j]);
per_image_good_match.training_points.push_back(
per_image.training_points[j]);
per_image_good_match.training_points_3d.push_back(
per_image.training_points_3d[j]);
per_image_good_match.query_points.push_back(per_image.query_points[j]);
- per_image_good_match.homography = homography;
}
- per_image_good_matches.push_back(per_image_good_match);
- // TODO: Use homography to compute target point on query image
+ // Returns from opencv are doubles (CV_64F), which don't play well
+ // with our floats
+ homography.convertTo(homography, CV_32F);
+ per_image_good_match.homography = homography;
+
+ CHECK_GT(per_image_good_match.matches.size(), 0u);
+
+ // Collect training target location, so we can map it to matched image
+ cv::Point2f target_point;
+ float target_radius;
+ TargetLocation((*(per_image_good_match.matches[0]))[0].imgIdx, target_point,
+ target_radius);
+
+ // Store target_point in vector for use by perspectiveTransform
+ std::vector<cv::Point2f> src_target_pt;
+ src_target_pt.push_back(target_point);
+ std::vector<cv::Point2f> query_target_pt;
+
+ cv::perspectiveTransform(src_target_pt, query_target_pt, homography);
+
+ float query_target_radius =
+ target_radius *
+ abs(homography.at<float>(0, 0) + homography.at<float>(1, 1)) / 2.;
+
+ CHECK_EQ(query_target_pt.size(), 1u);
+ target_point_vector.push_back(query_target_pt[0]);
+ target_radius_vector.push_back(query_target_radius);
// Pose transformations (rotations and translations) for various
// coordinate frames. R_X_Y_vec is the Rodrigues (angle-axis)
- // representation the 3x3 rotation R_X_Y from frame X to frame Y
+ // representation of the 3x3 rotation R_X_Y from frame X to frame Y
// Tranform from camera to target frame
cv::Mat R_camera_target_vec, R_camera_target, T_camera_target;
@@ -444,10 +490,12 @@
// debugging(features), and one smaller
SendImageMatchResult(image, keypoints, descriptors, all_good_matches,
camera_target_list, field_camera_list,
+ target_point_vector, target_radius_vector,
&detailed_result_sender_, true);
SendImageMatchResult(image, keypoints, descriptors, all_good_matches,
- camera_target_list, field_camera_list, &result_sender_,
- false);
+ camera_target_list, field_camera_list,
+ target_point_vector, target_radius_vector,
+ &result_sender_, false);
}
void CameraReader::ReadImage() {
diff --git a/y2020/vision/sift/sift.fbs b/y2020/vision/sift/sift.fbs
index 43ea152..3e2daaf 100644
--- a/y2020/vision/sift/sift.fbs
+++ b/y2020/vision/sift/sift.fbs
@@ -133,6 +133,12 @@
// The pose of the camera in the field coordinate frame
field_to_camera:TransformationMatrix;
+
+ // 2D image coordinate representing target location on the matched image
+ query_target_point_x:float;
+ query_target_point_y:float;
+ // Perceived radius of target circle
+ query_target_point_radius:float;
}
table ImageMatchResult {
@@ -153,10 +159,6 @@
// Information about the camera which took this image.
camera_calibration:CameraCalibration;
-
- // 2D image coordinate representing target location on the matched image
- target_point_x:float;
- target_point_y:float;
}
root_type ImageMatchResult;
diff --git a/y2020/vision/sift/sift_training.fbs b/y2020/vision/sift/sift_training.fbs
index d4fa740..12ad9b4 100644
--- a/y2020/vision/sift/sift_training.fbs
+++ b/y2020/vision/sift/sift_training.fbs
@@ -14,6 +14,8 @@
// 2D image coordinate representing target location on the training image
target_point_x:float;
target_point_y:float;
+ // Radius of target circle
+ target_point_radius:float;
}
// Represents the information used to match incoming images against.
diff --git a/y2020/vision/tools/python_code/define_training_data.py b/y2020/vision/tools/python_code/define_training_data.py
index 157f9e6..9894ee2 100644
--- a/y2020/vision/tools/python_code/define_training_data.py
+++ b/y2020/vision/tools/python_code/define_training_data.py
@@ -18,7 +18,7 @@
global current_mouse
current_mouse = (x, y)
if event == cv2.EVENT_LBUTTONUP:
- glog.debug("Adding point at %d, %d" % (x,y))
+ glog.debug("Adding point at %d, %d" % (x, y))
point_list.append([x, y])
pass
@@ -143,20 +143,22 @@
return point_list
+
# Determine whether a given point lies within (or on border of) a set of polygons
# Return true if it does
def point_in_polygons(point, polygons):
for poly in polygons:
np_poly = np.asarray(poly)
dist = cv2.pointPolygonTest(np_poly, (point[0], point[1]), True)
- if dist >=0:
+ if dist >= 0:
return True
return False
+
## Filter keypoints by polygons
def filter_keypoints_by_polygons(keypoint_list, descriptor_list, polygons):
- # TODO: Need to make sure we've got the right numpy array / list
+ # TODO: Need to make sure we've got the right numpy array / list
keep_keypoint_list = []
keep_descriptor_list = []
reject_keypoint_list = []
@@ -166,7 +168,8 @@
# For now, pretend keypoints are just points
for i in range(len(keypoint_list)):
- if point_in_polygons((keypoint_list[i].pt[0], keypoint_list[i].pt[1]), polygons):
+ if point_in_polygons((keypoint_list[i].pt[0], keypoint_list[i].pt[1]),
+ polygons):
keep_list.append(i)
else:
reject_list.append(i)
@@ -175,37 +178,42 @@
reject_keypoint_list = [keypoint_list[kp_ind] for kp_ind in reject_list]
# Allow function to be called with no descriptors, and just return empty list
if descriptor_list is not None:
- keep_descriptor_list = descriptor_list[keep_list,:]
- reject_descriptor_list = descriptor_list[reject_list,:]
+ keep_descriptor_list = descriptor_list[keep_list, :]
+ reject_descriptor_list = descriptor_list[reject_list, :]
return keep_keypoint_list, keep_descriptor_list, reject_keypoint_list, reject_descriptor_list, keep_list
+
# Helper function that appends a column of ones to a list (of 2d points)
def append_ones(point_list):
- return np.hstack([point_list, np.ones((len(point_list),1))])
+ return np.hstack([point_list, np.ones((len(point_list), 1))])
+
# Given a list of 2d points and thei corresponding 3d locations, compute map
# between them.
# pt_3d = (pt_2d, 1) * reprojection_map
# TODO: We should really look at residuals to see if things are messed up
def compute_reprojection_map(polygon_2d, polygon_3d):
- pts_2d = np.asarray(np.float32(polygon_2d)).reshape(-1,2)
+ pts_2d = np.asarray(np.float32(polygon_2d)).reshape(-1, 2)
pts_2d_lstsq = append_ones(pts_2d)
- pts_3d_lstsq = np.asarray(np.float32(polygon_3d)).reshape(-1,3)
+ pts_3d_lstsq = np.asarray(np.float32(polygon_3d)).reshape(-1, 3)
reprojection_map = np.linalg.lstsq(pts_2d_lstsq, pts_3d_lstsq, rcond=-1)[0]
return reprojection_map
+
# Given set of keypoints (w/ 2d location), a reprojection map, and a polygon
# that defines regions for where this is valid
# Returns a numpy array of 3d locations the same size as the keypoint list
def compute_3d_points(keypoint_2d_list, reprojection_map):
- pt_2d_lstsq = append_ones(np.asarray(np.float32(keypoint_2d_list)).reshape(-1,2))
+ pt_2d_lstsq = append_ones(
+ np.asarray(np.float32(keypoint_2d_list)).reshape(-1, 2))
pt_3d = pt_2d_lstsq.dot(reprojection_map)
return pt_3d
+
# 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):
@@ -246,11 +254,8 @@
def sample_define_points_by_list_usage():
image = cv2.imread("test_images/train_power_port_red.png")
- test_points = [(451, 679), (451, 304),
- (100, 302), (451, 74),
- (689, 74), (689, 302),
- (689, 679)]
+ test_points = [(451, 679), (451, 304), (100, 302), (451, 74), (689, 74),
+ (689, 302), (689, 679)]
polygon_list = define_points_by_list(image, test_points)
glog.debug(polygon_list)
-
diff --git a/y2020/vision/tools/python_code/load_sift_training.py b/y2020/vision/tools/python_code/load_sift_training.py
index 651efe2..6db3155 100644
--- a/y2020/vision/tools/python_code/load_sift_training.py
+++ b/y2020/vision/tools/python_code/load_sift_training.py
@@ -107,28 +107,27 @@
fbb)
# Create the TrainingImage feature vector
- TrainingImage.TrainingImageStartFeaturesVector(
- fbb, len(features_vector))
+ TrainingImage.TrainingImageStartFeaturesVector(fbb,
+ len(features_vector))
for feature in reversed(features_vector):
fbb.PrependUOffsetTRelative(feature)
features_vector_offset = fbb.EndVector(len(features_vector))
# Add the TrainingImage data
TrainingImage.TrainingImageStart(fbb)
- TrainingImage.TrainingImageAddFeatures(fbb,
- features_vector_offset)
- TrainingImage.TrainingImageAddFieldToTarget(
- fbb, transformation_mat_offset)
+ TrainingImage.TrainingImageAddFeatures(fbb, features_vector_offset)
+ TrainingImage.TrainingImageAddFieldToTarget(fbb,
+ transformation_mat_offset)
TrainingImage.TrainingImageAddTargetPointX(
fbb, target_data.target_point_2d[0][0][0])
TrainingImage.TrainingImageAddTargetPointY(
fbb, target_data.target_point_2d[0][0][1])
- images_vector.append(
- TrainingImage.TrainingImageEnd(fbb))
+ TrainingImage.TrainingImageAddTargetPointRadius(
+ fbb, target_data.target_radius)
+ images_vector.append(TrainingImage.TrainingImageEnd(fbb))
# Create and add Training Data of all targets
- TrainingData.TrainingDataStartImagesVector(fbb,
- len(images_vector))
+ TrainingData.TrainingDataStartImagesVector(fbb, len(images_vector))
for training_image in reversed(images_vector):
fbb.PrependUOffsetTRelative(training_image)
images_vector_table = fbb.EndVector(len(images_vector))
diff --git a/y2020/vision/tools/python_code/target_definition.py b/y2020/vision/tools/python_code/target_definition.py
index 4c3f36b..eae358c 100644
--- a/y2020/vision/tools/python_code/target_definition.py
+++ b/y2020/vision/tools/python_code/target_definition.py
@@ -16,6 +16,9 @@
USE_BAZEL = True
VISUALIZE_KEYPOINTS = False
+# For now, just have a 32 pixel radius, based on original training image
+target_radius_default = 32.
+
def bazel_name_fix(filename):
ret_name = filename
@@ -44,6 +47,7 @@
self.target_rotation = None
self.target_position = None
self.target_point_2d = None
+ self.target_radius = target_radius_default
def extract_features(self, feature_extractor=None):
if feature_extractor is None:
@@ -73,8 +77,8 @@
# Filter and project points for each polygon in the list
filtered_keypoints, _, _, _, keep_list = dtd.filter_keypoints_by_polygons(
keypoint_list, None, [self.polygon_list[poly_ind]])
- glog.info("Filtering kept %d of %d features" % (len(keep_list),
- len(keypoint_list)))
+ glog.info("Filtering kept %d of %d features" %
+ (len(keep_list), len(keypoint_list)))
filtered_point_array = np.asarray(
[(keypoint.pt[0], keypoint.pt[1])
for keypoint in filtered_keypoints]).reshape(-1, 2)
@@ -114,7 +118,8 @@
wing_angle = 20. * math.pi / 180.
# Pick the target center location at halfway between top and bottom of the top panel
- power_port_target_height = (power_port_total_height + power_port_bottom_wing_height) / 2.
+ power_port_target_height = (
+ power_port_total_height + power_port_bottom_wing_height) / 2.
###
### Red Power Port
@@ -132,32 +137,32 @@
# These are "virtual" 3D points based on the expected geometry
power_port_red_main_panel_polygon_points_3d = [
- (-field_length/2., power_port_edge_y, 0.),
- (-field_length/2., power_port_edge_y,
- power_port_bottom_wing_height),
- (-field_length/2., power_port_edge_y
- - power_port_wing_width, power_port_bottom_wing_height),
- (-field_length/2., power_port_edge_y,
- power_port_total_height),
- (-field_length/2., power_port_edge_y + power_port_width,
- power_port_total_height),
- (-field_length/2., power_port_edge_y + power_port_width,
- power_port_bottom_wing_height),
- (-field_length/2., power_port_edge_y + power_port_width, 0.)
+ (-field_length / 2., power_port_edge_y,
+ 0.), (-field_length / 2., power_port_edge_y,
+ power_port_bottom_wing_height),
+ (-field_length / 2., power_port_edge_y - power_port_wing_width,
+ power_port_bottom_wing_height), (-field_length / 2.,
+ power_port_edge_y,
+ power_port_total_height),
+ (-field_length / 2., power_port_edge_y + power_port_width,
+ power_port_total_height), (-field_length / 2.,
+ power_port_edge_y + power_port_width,
+ power_port_bottom_wing_height),
+ (-field_length / 2., power_port_edge_y + power_port_width, 0.)
]
power_port_red_wing_panel_polygon_points_2d = [(689, 74), (1022, 302),
(689, 302)]
# These are "virtual" 3D points based on the expected geometry
power_port_red_wing_panel_polygon_points_3d = [
- (-field_length/2., power_port_edge_y + power_port_width,
+ (-field_length / 2., power_port_edge_y + power_port_width,
power_port_total_height),
- (-field_length/2. + power_port_wing_width * math.sin(wing_angle),
- power_port_edge_y + power_port_width
- + power_port_wing_width * math.cos(wing_angle),
- power_port_bottom_wing_height),
- (-field_length/2., power_port_edge_y + power_port_width,
- power_port_bottom_wing_height)
+ (-field_length / 2. + power_port_wing_width * math.sin(wing_angle),
+ power_port_edge_y + power_port_width +
+ power_port_wing_width * math.cos(wing_angle),
+ power_port_bottom_wing_height), (-field_length / 2.,
+ power_port_edge_y + power_port_width,
+ power_port_bottom_wing_height)
]
# Populate the red power port
@@ -173,16 +178,17 @@
# and the skew to target is zero
ideal_power_port_red.target_rotation = -np.identity(3, np.double)
ideal_power_port_red.target_rotation[2][2] = 1.
- ideal_power_port_red.target_position = np.array([-field_length/2.,
- power_port_edge_y + power_port_width/2.,
- power_port_target_height])
+ ideal_power_port_red.target_position = np.array([
+ -field_length / 2., power_port_edge_y + power_port_width / 2.,
+ power_port_target_height
+ ])
# Target point on the image -- needs to match training image
# These are manually captured by examining the images,
# and entering the pixel values from the target center for each image.
# These are currently only used for visualization of the target
- ideal_power_port_red.target_point_2d = np.float32([[570,192]]).reshape(-1,1,2) # train_power_port_red.png
-
+ ideal_power_port_red.target_point_2d = np.float32([[570, 192]]).reshape(
+ -1, 1, 2) # train_power_port_red.png
# np.float32([[305, 97]]).reshape(-1, 1, 2), #train_power_port_red_webcam.png
# Add the ideal 3D target to our list
@@ -192,6 +198,8 @@
'test_images/train_power_port_red_webcam.png')
training_target_power_port_red.target_rotation = ideal_power_port_red.target_rotation
training_target_power_port_red.target_position = ideal_power_port_red.target_position
+ training_target_power_port_red.target_radius = target_radius_default
+
training_target_list.append(training_target_power_port_red)
###
@@ -207,10 +215,11 @@
# These are "virtual" 3D points based on the expected geometry
loading_bay_red_polygon_points_3d = [
- (field_length/2., loading_bay_edge_y + loading_bay_width, 0.),
- (field_length/2., loading_bay_edge_y + loading_bay_width, loading_bay_height),
- (field_length/2., loading_bay_edge_y, loading_bay_height),
- (field_length/2., loading_bay_edge_y, 0.)
+ (field_length / 2., loading_bay_edge_y + loading_bay_width, 0.),
+ (field_length / 2., loading_bay_edge_y + loading_bay_width,
+ loading_bay_height), (field_length / 2., loading_bay_edge_y,
+ loading_bay_height), (field_length / 2.,
+ loading_bay_edge_y, 0.)
]
ideal_loading_bay_red.polygon_list.append(
@@ -219,16 +228,19 @@
loading_bay_red_polygon_points_3d)
# Location of target
ideal_loading_bay_red.target_rotation = np.identity(3, np.double)
- ideal_loading_bay_red.target_position = np.array([field_length/2.,
- loading_bay_edge_y + loading_bay_width/2.,
- loading_bay_height/2.])
- ideal_loading_bay_red.target_point_2d = np.float32([[366, 236]]).reshape(-1, 1, 2) # train_loading_bay_red.png
+ ideal_loading_bay_red.target_position = np.array([
+ field_length / 2., loading_bay_edge_y + loading_bay_width / 2.,
+ loading_bay_height / 2.
+ ])
+ ideal_loading_bay_red.target_point_2d = np.float32([[366, 236]]).reshape(
+ -1, 1, 2) # train_loading_bay_red.png
ideal_target_list.append(ideal_loading_bay_red)
training_target_loading_bay_red = TargetData(
'test_images/train_loading_bay_red.png')
training_target_loading_bay_red.target_rotation = ideal_loading_bay_red.target_rotation
training_target_loading_bay_red.target_position = ideal_loading_bay_red.target_position
+ training_target_loading_bay_red.target_radius = target_radius_default
training_target_list.append(training_target_loading_bay_red)
###
@@ -246,31 +258,31 @@
# These are "virtual" 3D points based on the expected geometry
power_port_blue_main_panel_polygon_points_3d = [
- (field_length/2., -power_port_edge_y, 0.),
- (field_length/2., -power_port_edge_y,
- power_port_bottom_wing_height),
- (field_length/2., -power_port_edge_y + power_port_wing_width,
- power_port_bottom_wing_height),
- (field_length/2., -power_port_edge_y,
- power_port_total_height),
- (field_length/2., -power_port_edge_y - power_port_width,
- power_port_total_height),
- (field_length/2., -power_port_edge_y - power_port_width,
- power_port_bottom_wing_height),
- (field_length/2., -power_port_edge_y - power_port_width, 0.)
+ (field_length / 2., -power_port_edge_y,
+ 0.), (field_length / 2., -power_port_edge_y,
+ power_port_bottom_wing_height),
+ (field_length / 2., -power_port_edge_y + power_port_wing_width,
+ power_port_bottom_wing_height), (field_length / 2.,
+ -power_port_edge_y,
+ power_port_total_height),
+ (field_length / 2., -power_port_edge_y - power_port_width,
+ power_port_total_height), (field_length / 2.,
+ -power_port_edge_y - power_port_width,
+ power_port_bottom_wing_height),
+ (field_length / 2., -power_port_edge_y - power_port_width, 0.)
]
power_port_blue_wing_panel_polygon_points_2d = [(692, 50), (1047, 285),
(692, 285)]
# These are "virtual" 3D points based on the expected geometry
power_port_blue_wing_panel_polygon_points_3d = [
- (field_length/2., -power_port_edge_y - power_port_width,
+ (field_length / 2., -power_port_edge_y - power_port_width,
power_port_total_height),
- (field_length/2. - power_port_wing_width * math.sin(wing_angle),
+ (field_length / 2. - power_port_wing_width * math.sin(wing_angle),
-power_port_edge_y - power_port_width -
power_port_wing_width * math.cos(wing_angle),
power_port_bottom_wing_height),
- (field_length/2., -power_port_edge_y - power_port_width,
+ (field_length / 2., -power_port_edge_y - power_port_width,
power_port_bottom_wing_height)
]
@@ -282,16 +294,19 @@
# Location of target. Rotation is pointing in -x direction
ideal_power_port_blue.target_rotation = np.identity(3, np.double)
- ideal_power_port_blue.target_position = np.array([field_length/2.,
- -power_port_edge_y - power_port_width/2.,
- power_port_target_height])
- ideal_power_port_blue.target_point_2d = np.float32([[567, 180]]).reshape(-1, 1, 2) # train_power_port_blue.png
+ ideal_power_port_blue.target_position = np.array([
+ field_length / 2., -power_port_edge_y - power_port_width / 2.,
+ power_port_target_height
+ ])
+ ideal_power_port_blue.target_point_2d = np.float32([[567, 180]]).reshape(
+ -1, 1, 2) # train_power_port_blue.png
ideal_target_list.append(ideal_power_port_blue)
training_target_power_port_blue = TargetData(
'test_images/train_power_port_blue.png')
training_target_power_port_blue.target_rotation = ideal_power_port_blue.target_rotation
training_target_power_port_blue.target_position = ideal_power_port_blue.target_position
+ training_target_power_port_blue.target_radius = target_radius_default
training_target_list.append(training_target_power_port_blue)
###
@@ -308,10 +323,11 @@
# These are "virtual" 3D points based on the expected geometry
loading_bay_blue_polygon_points_3d = [
- (-field_length/2., -loading_bay_edge_y - loading_bay_width, 0.),
- (-field_length/2., -loading_bay_edge_y - loading_bay_width, loading_bay_height),
- (-field_length/2., -loading_bay_edge_y, loading_bay_height),
- (-field_length/2., -loading_bay_edge_y, 0.)
+ (-field_length / 2., -loading_bay_edge_y - loading_bay_width, 0.),
+ (-field_length / 2., -loading_bay_edge_y - loading_bay_width,
+ loading_bay_height), (-field_length / 2., -loading_bay_edge_y,
+ loading_bay_height), (-field_length / 2.,
+ -loading_bay_edge_y, 0.)
]
ideal_loading_bay_blue.polygon_list.append(
@@ -322,16 +338,19 @@
# Location of target
ideal_loading_bay_blue.target_rotation = -np.identity(3, np.double)
ideal_loading_bay_blue.target_rotation[2][2] = 1.
- ideal_loading_bay_blue.target_position = np.array([-field_length/2.,
- -loading_bay_edge_y - loading_bay_width/2.,
- loading_bay_height/2.])
- ideal_loading_bay_blue.target_point_2d = np.float32([[366, 236]]).reshape(-1, 1, 2) # train_loading_bay_blue.png
+ ideal_loading_bay_blue.target_position = np.array([
+ -field_length / 2., -loading_bay_edge_y - loading_bay_width / 2.,
+ loading_bay_height / 2.
+ ])
+ ideal_loading_bay_blue.target_point_2d = np.float32([[366, 236]]).reshape(
+ -1, 1, 2) # train_loading_bay_blue.png
ideal_target_list.append(ideal_loading_bay_blue)
training_target_loading_bay_blue = TargetData(
'test_images/train_loading_bay_blue.png')
training_target_loading_bay_blue.target_rotation = ideal_loading_bay_blue.target_rotation
training_target_loading_bay_blue.target_position = ideal_loading_bay_blue.target_position
+ training_target_loading_bay_blue.target_radius = target_radius_default
training_target_list.append(training_target_loading_bay_blue)
# Create feature extractor
@@ -341,7 +360,8 @@
camera_params = camera_definition.web_cam_params
for ideal_target in ideal_target_list:
- glog.info("\nPreparing target for image %s" % ideal_target.image_filename)
+ glog.info(
+ "\nPreparing target for image %s" % ideal_target.image_filename)
ideal_target.extract_features(feature_extractor)
ideal_target.filter_keypoints_by_polygons()
ideal_target.compute_reprojection_maps()
@@ -390,7 +410,9 @@
AUTO_PROJECTION = True
if AUTO_PROJECTION:
- glog.info("\n\nAuto projection of training keypoints to 3D using ideal images")
+ glog.info(
+ "\n\nAuto projection of training keypoints to 3D using ideal images"
+ )
# Match the captured training image against the "ideal" training image
# and use those matches to pin down the 3D locations of the keypoints
@@ -399,7 +421,8 @@
training_target = training_target_list[target_ind]
ideal_target = ideal_target_list[target_ind]
- glog.info("\nPreparing target for image %s" % training_target.image_filename)
+ glog.info("\nPreparing target for image %s" %
+ training_target.image_filename)
# Extract keypoints and descriptors for model
training_target.extract_features(feature_extractor)
@@ -419,7 +442,8 @@
H_inv = np.linalg.inv(homography_list[0])
for polygon in ideal_target.polygon_list:
- ideal_pts_2d = np.asarray(np.float32(polygon)).reshape(-1, 1, 2)
+ ideal_pts_2d = np.asarray(np.float32(polygon)).reshape(
+ -1, 1, 2)
# We use the ideal target's polygons to define the polygons on
# the training target
transformed_polygon = cv2.perspectiveTransform(
@@ -427,10 +451,14 @@
training_target.polygon_list.append(transformed_polygon)
# Also project the target point from the ideal to training image
- training_target_point_2d = cv2.perspectiveTransform(np.asarray(ideal_target.target_point_2d).reshape(-1,1,2), H_inv)
- training_target.target_point_2d = training_target_point_2d.reshape(-1,1,2)
+ training_target_point_2d = cv2.perspectiveTransform(
+ np.asarray(ideal_target.target_point_2d).reshape(-1, 1, 2),
+ H_inv)
+ training_target.target_point_2d = training_target_point_2d.reshape(
+ -1, 1, 2)
- glog.info("Started with %d keypoints" % len(training_target.keypoint_list))
+ glog.info("Started with %d keypoints" % len(
+ training_target.keypoint_list))
training_target.keypoint_list, training_target.descriptor_list, rejected_keypoint_list, rejected_descriptor_list, _ = dtd.filter_keypoints_by_polygons(
training_target.keypoint_list, training_target.descriptor_list,
@@ -490,8 +518,17 @@
if __name__ == '__main__':
ap = argparse.ArgumentParser()
- ap.add_argument("--visualize", help="Whether to visualize the results", default=False, action='store_true')
- ap.add_argument("-n", "--no_bazel", help="Don't run using Bazel", default=True, action='store_false')
+ ap.add_argument(
+ "--visualize",
+ help="Whether to visualize the results",
+ default=False,
+ action='store_true')
+ ap.add_argument(
+ "-n",
+ "--no_bazel",
+ help="Don't run using Bazel",
+ default=True,
+ action='store_false')
args = vars(ap.parse_args())
VISUALIZE_KEYPOINTS = args["visualize"]
diff --git a/y2020/vision/tools/python_code/target_definition_test.py b/y2020/vision/tools/python_code/target_definition_test.py
index 18df1e9..3c4d308 100644
--- a/y2020/vision/tools/python_code/target_definition_test.py
+++ b/y2020/vision/tools/python_code/target_definition_test.py
@@ -26,5 +26,6 @@
target_data_test_1.target_rotation = np.identity(3, np.double)
target_data_test_1.target_position = np.array([0., 1., 2.])
target_data_test_1.target_point_2d = np.array([10., 20.]).reshape(-1, 1, 2)
+ target_data_test_1.target_radius = 32
target_data_list.append(target_data_test_1)
return target_data_list
diff --git a/y2020/vision/tools/python_code/train_and_match.py b/y2020/vision/tools/python_code/train_and_match.py
index 93c607c..f139e71 100644
--- a/y2020/vision/tools/python_code/train_and_match.py
+++ b/y2020/vision/tools/python_code/train_and_match.py
@@ -11,6 +11,7 @@
glog.setLevel("WARN")
+
# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
# of the euler angles ( x and z are swapped ).
@@ -175,13 +176,16 @@
for i in range(len(train_keypoint_lists)):
good_matches = good_matches_list[i]
if len(good_matches) < MIN_MATCH_COUNT:
- glog.warn("Not enough matches are for model %d: %d out of needed #: %d" % (i, len(good_matches), MIN_MATCH_COUNT))
+ glog.warn(
+ "Not enough matches are for model %d: %d out of needed #: %d" %
+ (i, len(good_matches), MIN_MATCH_COUNT))
homography_list.append([])
matches_mask_list.append([])
continue
- glog.info("Got good number of matches for model %d: %d (needed only %d)" %
- (i, len(good_matches), MIN_MATCH_COUNT))
+ glog.info(
+ "Got good number of matches for model %d: %d (needed only %d)" %
+ (i, len(good_matches), MIN_MATCH_COUNT))
# Extract and bundle keypoint locations for computations
src_pts = np.float32([
train_keypoint_lists[i][m.trainIdx].pt for m in good_matches
@@ -216,8 +220,8 @@
matches_mask_count = matches_mask_list[i].count(1)
if matches_mask_count != len(good_matches):
glog.info("Homography rejected some matches! From ",
- len(good_matches), ", only ", matches_mask_count,
- " were used")
+ len(good_matches), ", only ", matches_mask_count,
+ " were used")
if matches_mask_count < MIN_MATCH_COUNT:
glog.info(
@@ -233,7 +237,8 @@
query_box_pts = cv2.perspectiveTransform(train_box_pts, H)
# Figure out where the training target goes on the query img
- transformed_target = cv2.perspectiveTransform(target_point_list[i].reshape(-1,1,2), H)
+ transformed_target = cv2.perspectiveTransform(
+ target_point_list[i].reshape(-1, 1, 2), H)
# Ballpark the size of the circle so it looks right on image
radius = int(
32 * abs(H[0][0] + H[1][1]) / 2) # Average of scale factors
@@ -276,9 +281,11 @@
# keypoint_list: List of opencv keypoints
def show_keypoints(image, keypoint_list):
ret_img = image.copy()
- ret_img = cv2.drawKeypoints(ret_img, keypoint_list, ret_img,
- flags=cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
+ ret_img = cv2.drawKeypoints(
+ ret_img,
+ keypoint_list,
+ ret_img,
+ flags=cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
cv2.imshow("Keypoints", ret_img)
cv2.waitKey(0)
return ret_img
-
diff --git a/y2020/www/BUILD b/y2020/www/BUILD
index 292618b..e8c227d 100644
--- a/y2020/www/BUILD
+++ b/y2020/www/BUILD
@@ -16,6 +16,18 @@
],
)
+ts_library(
+ name = "field_main",
+ srcs = [
+ "field_main.ts",
+ "field_handler.ts",
+ "constants.ts",
+ ],
+ deps = [
+ "//aos/network/www:proxy",
+ ],
+)
+
rollup_bundle(
name = "main_bundle",
entry_point = "y2020/www/main",
@@ -25,6 +37,15 @@
],
)
+rollup_bundle(
+ name = "field_main_bundle",
+ entry_point = "y2020/www/field_main",
+ deps = [
+ "field_main",
+ ],
+ visibility = ["//y2020:__subpackages__"],
+)
+
filegroup(
name = "files",
srcs = glob([
diff --git a/y2020/www/constants.ts b/y2020/www/constants.ts
new file mode 100644
index 0000000..b94d7a7
--- /dev/null
+++ b/y2020/www/constants.ts
@@ -0,0 +1,7 @@
+// Conversion constants to meters
+export const IN_TO_M = 0.0254;
+export const FT_TO_M = 0.3048;
+// Dimensions of the field in meters
+export const FIELD_WIDTH = 26 * FT_TO_M + 11.25 * IN_TO_M;
+export const FIELD_LENGTH = 52 * FT_TO_M + 5.25 * IN_TO_M;
+
diff --git a/y2020/www/field.html b/y2020/www/field.html
new file mode 100644
index 0000000..37452a3
--- /dev/null
+++ b/y2020/www/field.html
@@ -0,0 +1,12 @@
+<html>
+ <head>
+ <script src="flatbuffers.js"></script>
+ <script src="field_main_bundle.min.js" defer></script>
+ <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
new file mode 100644
index 0000000..a960a63
--- /dev/null
+++ b/y2020/www/field_handler.ts
@@ -0,0 +1,167 @@
+import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
+
+const FIELD_SIDE_Y = FIELD_WIDTH / 2;
+const FIELD_CENTER_X = (198.75 + 116) * IN_TO_M;
+
+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_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_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_BOTTOM_X = SPINNER_TOP_X - SPINNER_LENGTH;
+
+const SHIELD_BOTTOM_X = FIELD_CENTER_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_Y = -43.75 * IN_TO_M;
+
+const SHIELD_RIGHT_X = FIELD_CENTER_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_Y = 112.88 * IN_TO_M;
+
+const SHIELD_CENTER_TOP_X = (SHIELD_TOP_X + SHIELD_LEFT_X) / 2
+const SHIELD_CENTER_TOP_Y = (SHIELD_TOP_Y + SHIELD_LEFT_Y) / 2
+
+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 TARGET_ZONE_TIP_X = 30 * IN_TO_M;
+const TARGET_ZONE_WIDTH = 48 * IN_TO_M;
+const LOADING_ZONE_WIDTH = 60 * IN_TO_M;
+
+export class FieldHandler {
+ private canvas = document.createElement('canvas');
+
+ constructor() {
+ document.body.appendChild(this.canvas);
+ }
+
+ drawField(): void {
+ const MY_COLOR = 'red';
+ const OTHER_COLOR = 'blue';
+ const ctx = this.canvas.getContext('2d');
+ // draw perimiter
+ ctx.beginPath();
+ ctx.moveTo(0, 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(0, -DS_INSIDE_Y);
+ ctx.lineTo(0, DS_INSIDE_Y);
+ ctx.stroke();
+
+ // draw shield generator
+ ctx.beginPath();
+ ctx.moveTo(SHIELD_BOTTOM_X, SHIELD_BOTTOM_Y);
+ ctx.lineTo(SHIELD_RIGHT_X, SHIELD_RIGHT_Y);
+ ctx.lineTo(SHIELD_TOP_X, SHIELD_TOP_Y);
+ ctx.lineTo(SHIELD_LEFT_X, SHIELD_LEFT_Y);
+ ctx.lineTo(SHIELD_BOTTOM_X, SHIELD_BOTTOM_Y);
+ ctx.moveTo(SHIELD_CENTER_TOP_X, SHIELD_CENTER_TOP_Y);
+ 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();
+
+ ctx.strokeStyle = OTHER_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();
+
+ ctx.strokeStyle = 'black';
+ ctx.beginPath();
+ ctx.moveTo(SPINNER_TOP_X, FIELD_SIDE_Y);
+ 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;
+ ctx.beginPath();
+ ctx.moveTo(0, 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.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.stroke();
+
+ ctx.strokeStyle = OTHER_COLOR;
+ 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.stroke();
+ }
+
+ reset(): void {
+ const ctx = this.canvas.getContext('2d');
+ 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);
+
+ // Translate to center of bottom of display.
+ ctx.translate(size / 4, size);
+ // 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);
+ ctx.lineWidth = 1 / M_TO_PX;
+ }
+}
diff --git a/y2020/www/field_main.ts b/y2020/www/field_main.ts
new file mode 100644
index 0000000..adcaa27
--- /dev/null
+++ b/y2020/www/field_main.ts
@@ -0,0 +1,13 @@
+import {Connection} from 'aos/network/www/proxy';
+
+import {FieldHandler} from './field_handler';
+
+const conn = new Connection();
+
+conn.connect();
+
+const fieldHandler = new FieldHandler();
+
+fieldHandler.reset();
+fieldHandler.drawField();
+
diff --git a/y2020/www/main.ts b/y2020/www/main.ts
index d414eac..46a0e8a 100644
--- a/y2020/www/main.ts
+++ b/y2020/www/main.ts
@@ -1,9 +1,12 @@
import {Connection} from 'aos/network/www/proxy';
import {ImageHandler} from './image_handler';
+import {ConfigHandler} from 'aos/network/www/config_handler';
const conn = new Connection();
+const configPrinter = new ConfigHandler(conn);
+
conn.connect();
const iHandler = new ImageHandler();
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 0e1a6ba..3d966b8 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -380,6 +380,42 @@
"max_size": 2000000
},
{
+ "name": "/pi3/camera",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "pi3",
+ "frequency": 25,
+ "max_size": 620000,
+ "num_senders": 18
+ },
+ {
+ "name": "/pi3/camera",
+ "type": "frc971.vision.sift.ImageMatchResult",
+ "source_node": "pi3",
+ "frequency": 25,
+ "max_size": 10000,
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 1,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/pi3/camera/detailed",
+ "type": "frc971.vision.sift.ImageMatchResult",
+ "source_node": "pi3",
+ "frequency": 25,
+ "max_size": 300000
+ },
+ {
+ "name": "/pi3/camera",
+ "type": "frc971.vision.sift.TrainingData",
+ "source_node": "pi3",
+ "frequency": 2,
+ "max_size": 2000000
+ },
+ {
"name": "/autonomous",
"type": "aos.common.actions.Status",
"source_node": "roborio"
@@ -491,6 +527,24 @@
},
{
"match": {
+ "name": "/camera",
+ "source_node": "pi3"
+ },
+ "rename": {
+ "name": "/pi3/camera"
+ }
+ },
+ {
+ "match": {
+ "name": "/camera/detailed",
+ "source_node": "pi3"
+ },
+ "rename": {
+ "name": "/pi3/camera/detailed"
+ }
+ },
+ {
+ "match": {
"name": "/aos",
"type": "aos.RobotState"
},