Merge changes I6eb64c28,Idea8cd7c
* changes:
Add status data for line follow drivetrain
Make 2019 target selector
diff --git a/y2019/BUILD b/y2019/BUILD
index e3bac02..4f11c36 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -1,4 +1,5 @@
load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos/build:queues.bzl", "queue_library")
robot_downloader(
start_binaries = [
@@ -44,6 +45,7 @@
restricted_to = ["//tools:roborio"],
deps = [
":constants",
+ ":status_light",
"//aos:init",
"//aos:make_unique",
"//aos:math",
@@ -110,6 +112,7 @@
":joystick_reader.cc",
],
deps = [
+ ":status_light",
"//aos:init",
"//aos/actions:action_lib",
"//aos/input:action_joystick_input",
@@ -129,6 +132,14 @@
],
)
+queue_library(
+ name = "status_light",
+ srcs = [
+ "status_light.q",
+ ],
+ visibility = ["//visibility:public"],
+)
+
py_library(
name = "python_init",
srcs = ["__init__.py"],
diff --git a/y2019/constants.cc b/y2019/constants.cc
index f84e945..0a9a429 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -196,8 +196,8 @@
intake->zeroing_constants.measured_absolute_position = 1.756847;
- wrist_params->zeroing_constants.measured_absolute_position = 0.357394;
- wrist->potentiometer_offset = -1.479097 - 2.740303;
+ wrist_params->zeroing_constants.measured_absolute_position = 0.192576;
+ wrist->potentiometer_offset = -4.200894 - 0.187134;
stilts_params->zeroing_constants.measured_absolute_position = 0.043580;
stilts->potentiometer_offset = -0.093820 + 0.0124 - 0.008334 + 0.004507;
diff --git a/y2019/control_loops/drivetrain/camera.h b/y2019/control_loops/drivetrain/camera.h
index 2ab4b4f..d59ac21 100644
--- a/y2019/control_loops/drivetrain/camera.h
+++ b/y2019/control_loops/drivetrain/camera.h
@@ -213,14 +213,18 @@
// This number is unitless and if greater than 1, implies that the target is
// visible to the camera and if less than 1 implies it is too small to be
// registered on the camera.
- Scalar apparent_width =
- ::std::max<Scalar>(0.0, ::std::cos(view->reading.skew) *
- noise_parameters_.max_viewable_distance /
- view->reading.distance);
+ const Scalar cosskew = ::std::cos(view->reading.skew);
+ Scalar apparent_width = ::std::max<Scalar>(
+ 0.0, cosskew * noise_parameters_.max_viewable_distance /
+ view->reading.distance);
+ // If we got wildly invalid distance or skew measurements, then set a very
+ // small apparent width.
+ if (view->reading.distance < 0 || cosskew < 0) {
+ apparent_width = 0.01;
+ }
// As both a sanity check and for the sake of numerical stability, manually
- // set apparent_width to something "very small" if the distance is too
- // close.
- if (view->reading.distance < 0.01) {
+ // set apparent_width to something "very small" if it is near zero.
+ if (apparent_width < 0.01) {
apparent_width = 0.01;
}
diff --git a/y2019/control_loops/drivetrain/camera_test.cc b/y2019/control_loops/drivetrain/camera_test.cc
index ce1a2a4..d0a4b85 100644
--- a/y2019/control_loops/drivetrain/camera_test.cc
+++ b/y2019/control_loops/drivetrain/camera_test.cc
@@ -154,10 +154,11 @@
EXPECT_EQ(1u, camera_.target_views().size());
}
+using Reading = TestCamera::TargetView::Reading;
+
// Checks that reading noises have the expected characteristics (mostly, going
// up linearly with distance):
TEST_F(CameraTest, DistanceNoiseTest) {
- using Reading = TestCamera::TargetView::Reading;
const Reading normal_noise = camera_.target_views()[0].noise;
// Get twice as close:
base_pose_.mutable_pos()->y() /= 2.0;
@@ -169,6 +170,40 @@
EXPECT_EQ(normal_noise.heading, closer_noise.heading);
}
+class CameraViewParamTest : public CameraTest,
+ public ::testing::WithParamInterface<Reading> {};
+
+// Checks that invalid or absurd measurements result in large but finite noises
+// and non-visible targets.
+TEST_P(CameraViewParamTest, InvalidReading) {
+ TestCamera::TargetView view;
+ view.reading = GetParam();
+ bool visible = true;
+ camera_.PopulateNoise(&view, &visible);
+ // Target should not be visible
+ EXPECT_FALSE(visible);
+ // We should end up with finite but very large noises when things are invalid.
+ EXPECT_TRUE(::std::isfinite(view.noise.heading));
+ EXPECT_TRUE(::std::isfinite(view.noise.distance));
+ EXPECT_TRUE(::std::isfinite(view.noise.skew));
+ EXPECT_TRUE(::std::isfinite(view.noise.height));
+ // Don't check heading noise because it is always constant.
+ EXPECT_LT(10, view.noise.distance);
+ EXPECT_LT(10, view.noise.skew);
+ EXPECT_LT(5, view.noise.height);
+}
+
+INSTANTIATE_TEST_CASE_P(
+ InvalidMeasurements, CameraViewParamTest,
+ ::testing::Values(
+ // heading, distance, height, skew
+ Reading({100.0, -10.0, -10.0, -3.0}), Reading({0.0, 1.0, 0.0, -3.0}),
+ Reading({0.0, 1.0, 0.0, 3.0}), Reading({0.0, 1.0, 0.0, 9.0}),
+ Reading({0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0, 0.0}),
+ Reading({0.0, ::std::numeric_limits<double>::infinity(), 0.0, 0.0}),
+ Reading({0.0, 1.0, 0.0, ::std::numeric_limits<double>::infinity()}),
+ Reading({0.0, 1.0, 0.0, ::std::numeric_limits<double>::quiet_NaN()})));
+
} // namespace testing
} // namespace control_loops
} // namespace y2019
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index 4656cb3..5a4264d 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -57,6 +57,16 @@
void EventLoopLocalizer::HandleFrame(const CameraFrame &frame) {
// We need to construct TargetView's and pass them to the localizer:
::aos::SizedArray<TargetView, kMaxTargetsPerFrame> views;
+ // Note: num_targets and camera are unsigned integers and so don't need to be
+ // checked for < 0.
+ if (frame.num_targets > kMaxTargetsPerFrame) {
+ LOG(ERROR, "Got bad num_targets %d\n", frame.num_targets);
+ return;
+ }
+ if (frame.camera > cameras_.size()) {
+ LOG(ERROR, "Got bad camera number %d\n", frame.camera);
+ return;
+ }
for (int ii = 0; ii < frame.num_targets; ++ii) {
TargetView view;
view.reading.heading = frame.targets[ii].heading;
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index 0ebfef1..8a65170 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -135,10 +135,18 @@
++jj) {
EventLoopLocalizer::TargetView view = target_views[jj];
++frame.num_targets;
- frame.targets[jj].heading = view.reading.heading;
- frame.targets[jj].distance = view.reading.distance;
- frame.targets[jj].skew = view.reading.skew;
- frame.targets[jj].height = view.reading.height;
+ const float nan = ::std::numeric_limits<float>::quiet_NaN();
+ if (send_bad_frames_) {
+ frame.targets[jj].heading = nan;
+ frame.targets[jj].distance = nan;
+ frame.targets[jj].skew = nan;
+ frame.targets[jj].height = nan;
+ } else {
+ frame.targets[jj].heading = view.reading.heading;
+ frame.targets[jj].distance = view.reading.distance;
+ frame.targets[jj].skew = view.reading.skew;
+ frame.targets[jj].height = view.reading.height;
+ }
}
camera_delay_queue_.emplace(monotonic_clock::now(), frame);
}
@@ -183,9 +191,11 @@
camera_delay_queue_;
void set_enable_cameras(bool enable) { enable_cameras_ = enable; }
+ void set_bad_frames(bool enable) { send_bad_frames_ = enable; }
private:
bool enable_cameras_ = false;
+ bool send_bad_frames_ = false;
};
// Tests that no camera updates, combined with a perfect model, results in no
@@ -202,6 +212,20 @@
VerifyEstimatorAccurate(1e-10);
}
+// Bad camera updates (NaNs) should have no effect.
+TEST_F(LocalizedDrivetrainTest, BadCameraUpdate) {
+ set_enable_cameras(true);
+ set_bad_frames(true);
+ my_drivetrain_queue_.goal.MakeWithBuilder()
+ .controller_type(1)
+ .left_goal(-1.0)
+ .right_goal(1.0)
+ .Send();
+ RunForTime(chrono::seconds(3));
+ VerifyNearGoal();
+ VerifyEstimatorAccurate(1e-10);
+}
+
// Tests that camera udpates with a perfect models results in no errors.
TEST_F(LocalizedDrivetrainTest, PerfectCameraUpdate) {
set_enable_cameras(true);
diff --git a/y2019/control_loops/drivetrain/localizer.h b/y2019/control_loops/drivetrain/localizer.h
index 095c8a9..f8849ce 100644
--- a/y2019/control_loops/drivetrain/localizer.h
+++ b/y2019/control_loops/drivetrain/localizer.h
@@ -53,6 +53,11 @@
return;
}
+ if (!SanitizeTargets(targets)) {
+ LOG(ERROR, "Throwing out targets due to in insane values.\n");
+ return;
+ }
+
if (t > HybridEkf::latest_t()) {
LOG(ERROR,
"target observations must be older than most recent encoder/gyro "
@@ -97,11 +102,36 @@
// TODO(james): Tune
static constexpr Scalar kRejectionScore = 1.0;
+ // Checks that the targets coming in make some sense--mostly to prevent NaNs
+ // or the such from propagating.
+ bool SanitizeTargets(
+ const ::aos::SizedArray<TargetView, max_targets_per_frame> &targets) {
+ for (const TargetView &view : targets) {
+ const typename TargetView::Reading reading = view.reading;
+ if (!(::std::isfinite(reading.heading) &&
+ ::std::isfinite(reading.distance) &&
+ ::std::isfinite(reading.skew) && ::std::isfinite(reading.height))) {
+ LOG(ERROR, "Got non-finite values in target.\n");
+ return false;
+ }
+ if (reading.distance < 0) {
+ LOG(ERROR, "Got negative distance.\n");
+ return false;
+ }
+ if (::std::abs(::aos::math::NormalizeAngle(reading.skew)) > M_PI_2) {
+ LOG(ERROR, "Got skew > pi / 2.\n");
+ return false;
+ }
+ }
+ return true;
+ }
+
// Computes the measurement (z) and noise covariance (R) matrices for a given
// TargetView.
void TargetViewToMatrices(const TargetView &view, Output *z,
Eigen::Matrix<Scalar, kNOutputs, kNOutputs> *R) {
- *z << view.reading.heading, view.reading.distance, view.reading.skew;
+ *z << view.reading.heading, view.reading.distance,
+ ::aos::math::NormalizeAngle(view.reading.skew);
// TODO(james): R should account as well for our confidence in the target
// matching. However, handling that properly requires thing a lot more about
// the probabilities.
@@ -273,7 +303,11 @@
::std::array<int, max_targets_per_frame> best_frames =
MatchFrames(scores, best_matches, target_views.size());
for (size_t ii = 0; ii < target_views.size(); ++ii) {
- int view_idx = best_frames[ii];
+ size_t view_idx = best_frames[ii];
+ if (view_idx < 0 || view_idx >= camera_views.size()) {
+ LOG(ERROR, "Somehow, the view scorer failed.\n");
+ continue;
+ }
const Eigen::Matrix<Scalar, kNOutputs, kNStates> best_H =
all_H_matrices[ii][view_idx];
const TargetView best_view = camera_views[view_idx];
@@ -364,6 +398,7 @@
best_matches,
int n_views) {
::std::array<int, max_targets_per_frame> best_set;
+ best_set.fill(-1);
Scalar best_score;
// We start out without having "used" any views/targets:
::aos::SizedArray<bool, max_targets_per_frame> used_views;
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index 3b09e64..8b6f7ed 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -327,6 +327,8 @@
::std::normal_distribution<> normal_;
};
+using ::std::chrono::milliseconds;
+
// Tests that, when we attempt to follow a spline and use the localizer to
// perform the state estimation, we end up roughly where we should and that
// the localizer has a valid position estimate.
@@ -340,15 +342,20 @@
// we just trigger all the cameras at once, rather than offsetting them or
// anything.
const int camera_period = 5; // cycles
- // The amount of time to delay the camera images from when they are taken.
- const ::std::chrono::milliseconds camera_latency(50);
+ // The amount of time to delay the camera images from when they are taken, for
+ // each camera.
+ const ::std::array<milliseconds, 4> camera_latencies{
+ {milliseconds(45), milliseconds(50), milliseconds(55),
+ milliseconds(100)}};
- // A queue of camera frames so that we can add a time delay to the data
- // coming from the cameras.
- ::std::queue<::std::tuple<
- ::aos::monotonic_clock::time_point, const TestCamera *,
- ::aos::SizedArray<TestCamera::TargetView, kNumTargetsPerFrame>>>
- camera_queue;
+ // A queue of camera frames for each camera so that we can add a time delay to
+ // the data coming from the cameras.
+ ::std::array<
+ ::std::queue<::std::tuple<
+ ::aos::monotonic_clock::time_point, const TestCamera *,
+ ::aos::SizedArray<TestCamera::TargetView, kNumTargetsPerFrame>>>,
+ 4>
+ camera_queues;
// Start time, for debugging.
const auto begin = ::std::chrono::steady_clock::now();
@@ -431,34 +438,37 @@
right_enc + Normal(encoder_noise_),
gyro + Normal(gyro_noise_), U, t);
- // Clear out the camera frames that we are ready to pass to the localizer.
- while (!camera_queue.empty() &&
- ::std::get<0>(camera_queue.front()) < t - camera_latency) {
- const auto tuple = camera_queue.front();
- camera_queue.pop();
- ::aos::monotonic_clock::time_point t_obs = ::std::get<0>(tuple);
- const TestCamera *camera = ::std::get<1>(tuple);
- ::aos::SizedArray<TestCamera::TargetView, kNumTargetsPerFrame> views =
- ::std::get<2>(tuple);
- localizer_.UpdateTargets(*camera, views, t_obs);
- }
+ for (size_t cam_idx = 0; cam_idx < camera_queues.size(); ++cam_idx) {
+ auto &camera_queue = camera_queues[cam_idx];
+ // Clear out the camera frames that we are ready to pass to the localizer.
+ while (!camera_queue.empty() && ::std::get<0>(camera_queue.front()) <
+ t - camera_latencies[cam_idx]) {
+ const auto tuple = camera_queue.front();
+ camera_queue.pop();
+ ::aos::monotonic_clock::time_point t_obs = ::std::get<0>(tuple);
+ const TestCamera *camera = ::std::get<1>(tuple);
+ ::aos::SizedArray<TestCamera::TargetView, kNumTargetsPerFrame> views =
+ ::std::get<2>(tuple);
+ localizer_.UpdateTargets(*camera, views, t_obs);
+ }
- // Actually take all the images and store them in the queue.
- if (i % camera_period == 0) {
- for (size_t jj = 0; jj < true_cameras_.size(); ++jj) {
- const auto target_views = true_cameras_[jj].target_views();
- ::aos::SizedArray<TestCamera::TargetView, kNumTargetsPerFrame>
- pass_views;
- for (size_t ii = 0;
- ii < ::std::min(kNumTargetsPerFrame, target_views.size()); ++ii) {
- TestCamera::TargetView view = target_views[ii];
- Noisify(&view);
- pass_views.push_back(view);
+ // Actually take all the images and store them in the queue.
+ if (i % camera_period == 0) {
+ for (size_t jj = 0; jj < true_cameras_.size(); ++jj) {
+ const auto target_views = true_cameras_[jj].target_views();
+ ::aos::SizedArray<TestCamera::TargetView, kNumTargetsPerFrame>
+ pass_views;
+ for (size_t ii = 0;
+ ii < ::std::min(kNumTargetsPerFrame, target_views.size());
+ ++ii) {
+ TestCamera::TargetView view = target_views[ii];
+ Noisify(&view);
+ pass_views.push_back(view);
+ }
+ camera_queue.emplace(t, &robot_cameras_[jj], pass_views);
}
- camera_queue.emplace(t, &robot_cameras_[jj], pass_views);
}
}
-
}
const auto end = ::std::chrono::steady_clock::now();
@@ -604,7 +614,7 @@
.finished(),
/*noisify=*/true,
/*disturb=*/false,
- /*estimate_tolerance=*/0.1,
+ /*estimate_tolerance=*/0.15,
/*goal_tolerance=*/0.5,
})));
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 00bacae..524a1ba 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -24,10 +24,11 @@
],
deps = [
":collision_avoidance",
- ":vacuum",
":superstructure_queue",
+ ":vacuum",
"//aos/controls:control_loop",
"//y2019:constants",
+ "//y2019:status_light",
],
)
@@ -47,6 +48,7 @@
"//frc971/control_loops:capped_test_plant",
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:team_number_test_environment",
+ "//y2019:status_light",
"//y2019/control_loops/superstructure/intake:intake_plants",
],
)
@@ -88,7 +90,7 @@
],
deps = [
":superstructure_queue",
- "//aos/controls:control_loop"
+ "//aos/controls:control_loop",
],
)
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index 3443d63..dd332b8 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -4,10 +4,26 @@
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "y2019/status_light.q.h"
+
namespace y2019 {
namespace control_loops {
namespace superstructure {
+namespace {
+
+void SendColors(float red, float green, float blue) {
+ auto new_status_light = status_light.MakeMessage();
+ new_status_light->red = red;
+ new_status_light->green = green;
+ new_status_light->blue = blue;
+
+ if (!new_status_light.Send()) {
+ LOG(ERROR, "Failed to send lights.\n");
+ }
+}
+} // namespace
+
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
: aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name),
@@ -75,6 +91,22 @@
wrist_.set_max_position(collision_avoidance_.max_wrist_goal());
intake_.set_min_position(collision_avoidance_.min_intake_goal());
intake_.set_max_position(collision_avoidance_.max_intake_goal());
+
+ if (status && unsafe_goal) {
+ // Light Logic
+ if (status->estopped) {
+ // Estop is red
+ SendColors(0.5, 0.0, 0.0);
+ } else if (unsafe_goal->suction.gamepiece_mode == 0) {
+ // Ball mode is blue
+ SendColors(0.0, 0.0, 0.5);
+ } else if (unsafe_goal->suction.gamepiece_mode == 1) {
+ // Disk mode is yellow
+ SendColors(0.5, 0.5, 0.0);
+ } else {
+ SendColors(0.0, 0.0, 0.0);
+ }
+ }
}
} // namespace superstructure
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index f87e4f2..d5c8a73 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -4,10 +4,13 @@
import "frc971/control_loops/profiled_subsystem.q";
struct SuctionGoal {
- // True = open solenoid (apply suction)
- // Top/bottom are when wrist is forward
- bool top;
- bool bottom;
+ // True = apply suction
+ bool grab_piece;
+
+ // 0 = ball mode
+ // 1 = disk mode
+
+ int32_t gamepiece_mode;
};
queue_group SuperstructureQueue {
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index 0cce4a6..4d3de51 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -10,6 +10,7 @@
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2019/constants.h"
+#include "y2019/status_light.q.h"
#include "y2019/control_loops/superstructure/elevator/elevator_plant.h"
#include "y2019/control_loops/superstructure/intake/intake_plant.h"
#include "y2019/control_loops/superstructure/stilts/stilts_plant.h"
@@ -288,6 +289,7 @@
".y2019.control_loops.superstructure.superstructure_queue."
"position"),
superstructure_(&event_loop_) {
+ status_light.Clear();
set_team_id(::frc971::control_loops::testing::kTeamNumber);
}
@@ -712,8 +714,7 @@
// Turn on suction
{
auto goal = superstructure_queue_.goal.MakeMessage();
- goal->suction.top = true;
- goal->suction.bottom = true;
+ goal->suction.grab_piece = true;
ASSERT_TRUE(goal.Send());
}
@@ -734,8 +735,7 @@
// Turn on suction
{
auto goal = superstructure_queue_.goal.MakeMessage();
- goal->suction.top = true;
- goal->suction.bottom = true;
+ goal->suction.grab_piece = true;
ASSERT_TRUE(goal.Send());
}
@@ -761,8 +761,7 @@
// Turn on suction
{
auto goal = superstructure_queue_.goal.MakeMessage();
- goal->suction.top = true;
- goal->suction.bottom = true;
+ goal->suction.grab_piece = true;
ASSERT_TRUE(goal.Send());
}
@@ -776,8 +775,7 @@
// Turn off suction
{
auto goal = superstructure_queue_.goal.MakeMessage();
- goal->suction.top = false;
- goal->suction.bottom = false;
+ goal->suction.grab_piece = false;
ASSERT_TRUE(goal.Send());
}
diff --git a/y2019/control_loops/superstructure/vacuum.cc b/y2019/control_loops/superstructure/vacuum.cc
index 7bf2e94..497ae5b 100644
--- a/y2019/control_loops/superstructure/vacuum.cc
+++ b/y2019/control_loops/superstructure/vacuum.cc
@@ -32,7 +32,7 @@
low_pump_voltage = *has_piece;
if (unsafe_goal && output) {
- const bool release = !unsafe_goal->top && !unsafe_goal->bottom;
+ const bool release = !unsafe_goal->grab_piece;
if (release) {
last_release_time_ = monotonic_now;
@@ -44,8 +44,16 @@
output->pump_voltage =
release ? 0 : (low_pump_voltage ? kPumpHasPieceVoltage : kPumpVoltage);
- output->intake_suction_top = unsafe_goal->top;
- output->intake_suction_bottom = unsafe_goal->bottom;
+ if (unsafe_goal->grab_piece && unsafe_goal->gamepiece_mode == 0) {
+ output->intake_suction_top = false;
+ output->intake_suction_bottom = true;
+ } else if (unsafe_goal->grab_piece && unsafe_goal->gamepiece_mode == 1) {
+ output->intake_suction_top = true;
+ output->intake_suction_bottom = true;
+ } else {
+ output->intake_suction_top = false;
+ output->intake_suction_bottom = false;
+ }
// If we intend to release, or recently released, set has_piece to false so
// that we give the part of the vacuum circuit with the pressure sensor time
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index bfc13e4..26a8ce1 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -18,6 +18,7 @@
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/status_light.q.h"
using ::y2019::control_loops::superstructure::superstructure_queue;
using ::aos::input::driver_station::ButtonLocation;
@@ -105,8 +106,7 @@
::y2019::control_loops::drivetrain::GetDrivetrainConfig()) {
superstructure_queue.goal.FetchLatest();
if (superstructure_queue.goal.get()) {
- top_ = superstructure_queue.goal->suction.top;
- bottom_ = superstructure_queue.goal->suction.bottom;
+ grab_piece_ = superstructure_queue.goal->suction.grab_piece;
}
}
@@ -122,13 +122,12 @@
auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
if (data.IsPressed(kSuctionBall)) {
- Ball();
+ grab_piece_ = true;
} else if (data.IsPressed(kSuctionHatch)) {
- Disc();
+ grab_piece_ = true;
} else if (data.IsPressed(kRelease) ||
!superstructure_queue.status->has_piece) {
- top_ = false;
- bottom_ = false;
+ grab_piece_ = false;
}
if (data.IsPressed(kRocketBackwardUnpressed)) {
@@ -178,10 +177,10 @@
// Go to intake position and apply vacuum
if (data.IsPressed(kBallHPIntakeForward)) {
- Ball();
+ grab_piece_ = true;
elevator_wrist_pos_ = kBallHPIntakeForwardPos;
} else if (data.IsPressed(kBallHPIntakeBackward)) {
- Ball();
+ grab_piece_ = true;
elevator_wrist_pos_ = kBallHPIntakeBackwardPos;
}
@@ -206,10 +205,10 @@
}
} else {
if (data.IsPressed(kPanelHPIntakeForward)) {
- Disc();
+ grab_piece_ = true;
elevator_wrist_pos_ = kPanelHPIntakeForwrdPos;
} else if (data.IsPressed(kPanelHPIntakeBackward)) {
- Disc();
+ grab_piece_ = true;
elevator_wrist_pos_ = kPanelHPIntakeBackwardPos;
}
@@ -243,7 +242,7 @@
if (kDoBallIntake && !superstructure_queue.status->has_piece) {
elevator_wrist_pos_ = kBallIntakePos;
new_superstructure_goal->roller_voltage = 9.0;
- Ball();
+ grab_piece_ = true;
} else {
if (kDoBallOutake) {
new_superstructure_goal->roller_voltage = -6.0;
@@ -255,12 +254,16 @@
}
if (data.IsPressed(kRelease)) {
- top_ = false;
- bottom_ = false;
+ grab_piece_ = false;
}
- new_superstructure_goal->suction.top = top_;
- new_superstructure_goal->suction.bottom = bottom_;
+ if (switch_ball_) {
+ new_superstructure_goal->suction.gamepiece_mode = 0;
+ } else {
+ new_superstructure_goal->suction.gamepiece_mode = 1;
+ }
+
+ new_superstructure_goal->suction.grab_piece = grab_piece_;
new_superstructure_goal->elevator.unsafe_goal =
elevator_wrist_pos_.elevator;
@@ -272,20 +275,10 @@
}
}
- void Disc() {
- top_ = true;
- bottom_ = true;
- }
- void Ball() {
- top_ = false;
- bottom_ = true;
- }
-
private:
// Current goals here.
ElevatorWristPosition elevator_wrist_pos_ = kStowPos;
- bool top_ = false;
- bool bottom_ = false;
+ bool grab_piece_ = false;
bool switch_ball_ = false;
bool stilts_was_above_ = false;
diff --git a/y2019/status_light.q b/y2019/status_light.q
new file mode 100644
index 0000000..f84ed28
--- /dev/null
+++ b/y2019/status_light.q
@@ -0,0 +1,10 @@
+package y2019;
+
+message StatusLight {
+ // How bright to make each one. 0 is off, 1 is full on.
+ float red;
+ float green;
+ float blue;
+};
+
+queue StatusLight status_light;
diff --git a/y2019/vision/calibrate.sh b/y2019/vision/calibrate.sh
new file mode 100755
index 0000000..24118ed
--- /dev/null
+++ b/y2019/vision/calibrate.sh
@@ -0,0 +1,70 @@
+#!/bin/bash
+
+set -e
+
+cd "$(dirname "${BASH_SOURCE[0]}")"
+
+# We need to rebuild to save the old constants since the .cc file is baked into
+# the executable.
+calibration() {
+ bazel build -c opt //y2019/vision:global_calibration
+
+ ../../bazel-bin/y2019/vision/global_calibration "$@"
+}
+
+# Calibrate the comp robot. This is assuming the data file captured has been
+# extracted into the data folder in //y2019/vision/
+calibration \
+ --camera_id 10 \
+ --tape_start_x=-12.5 \
+ --tape_start_y=0.0 \
+ --tape_direction_x=-1.0 \
+ --tape_direction_y=0.0 \
+ --beginning_tape_measure_reading=11 \
+ --image_count=75 \
+ --constants=constants.cc \
+ --prefix data/cam10_0/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 6 \
+ --tape_start_x=12.5 \
+ --tape_start_y=0.0 \
+ --tape_direction_x=1.0 \
+ --tape_direction_y=0.0 \
+ --beginning_tape_measure_reading=11 \
+ --image_count=75 \
+ --constants=constants.cc \
+ --prefix data/cam6_0/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 7 \
+ --tape_start_x=12.5 \
+ --tape_start_y=0.0 \
+ --tape_direction_x=1.0 \
+ --tape_direction_y=0.0 \
+ --beginning_tape_measure_reading=21 \
+ --image_count=65 \
+ --constants=constants.cc \
+ --prefix data/cam7_0/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 9 \
+ --tape_start_x=-6.5 \
+ --tape_start_y=11.0 \
+ --tape_direction_x=0.0 \
+ --tape_direction_y=-1.0 \
+ --beginning_tape_measure_reading=30 \
+ --image_count=56 \
+ --constants=constants.cc \
+ --prefix data/cam9_0/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 8 \
+ --tape_start_x=6.5 \
+ --tape_start_y=-11.0 \
+ --tape_direction_x=0.0 \
+ --tape_direction_y=1.0 \
+ --beginning_tape_measure_reading=25 \
+ --image_count=61 \
+ --constants=constants.cc \
+ --prefix data/cam8_0/debug_viewer_jpeg_
diff --git a/y2019/vision/calibrate_9971.sh b/y2019/vision/calibrate_9971.sh
new file mode 100755
index 0000000..9a05ce2
--- /dev/null
+++ b/y2019/vision/calibrate_9971.sh
@@ -0,0 +1,70 @@
+#!/bin/bash
+
+set -e
+
+cd "$(dirname "${BASH_SOURCE[0]}")"
+
+# We need to rebuild to save the old constants since the .cc file is baked into
+# the executable.
+calibration() {
+ bazel build -c opt //y2019/vision:global_calibration
+
+ ../../bazel-bin/y2019/vision/global_calibration "$@"
+}
+
+# Calibrate the comp robot. This is assuming the data file captured has been
+# extracted into the users home directory.
+calibration \
+ --camera_id 1 \
+ --tape_start_x=-12.5 \
+ --tape_start_y=0.0 \
+ --tape_direction_x=-1.0 \
+ --tape_direction_y=0.0 \
+ --beginning_tape_measure_reading=16 \
+ --image_count=45 \
+ --constants=constants.cc \
+ --prefix ~/cam1/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 14 \
+ --tape_start_x=12.5 \
+ --tape_start_y=0.0 \
+ --tape_direction_x=1.0 \
+ --tape_direction_y=0.0 \
+ --beginning_tape_measure_reading=16 \
+ --image_count=71 \
+ --constants=constants.cc \
+ --prefix ~/cam14/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 15 \
+ --tape_start_x=12.5 \
+ --tape_start_y=0.0 \
+ --tape_direction_x=1.0 \
+ --tape_direction_y=0.0 \
+ --beginning_tape_measure_reading=25 \
+ --image_count=62 \
+ --constants=constants.cc \
+ --prefix ~/cam15/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 17 \
+ --tape_start_x=-6.5 \
+ --tape_start_y=11.0 \
+ --tape_direction_x=0.0 \
+ --tape_direction_y=-1.0 \
+ --beginning_tape_measure_reading=29 \
+ --image_count=58 \
+ --constants=constants.cc \
+ --prefix ~/cam17/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 18 \
+ --tape_start_x=6.5 \
+ --tape_start_y=-11.0 \
+ --tape_direction_x=0.0 \
+ --tape_direction_y=1.0 \
+ --beginning_tape_measure_reading=27 \
+ --image_count=60 \
+ --constants=constants.cc \
+ --prefix ~/cam18/debug_viewer_jpeg_
diff --git a/y2019/vision/constants.cc b/y2019/vision/constants.cc
index e41a53b..0fa8eda 100644
--- a/y2019/vision/constants.cc
+++ b/y2019/vision/constants.cc
@@ -5,6 +5,24 @@
static constexpr double kInchesToMeters = 0.0254;
+CameraCalibration camera_1 = {
+ {
+ -0.874694 / 180.0 * M_PI, 338.619, 2.14651 / 180.0 * M_PI,
+ },
+ {
+ {{-5.44063 * kInchesToMeters, 2.83405 * kInchesToMeters,
+ 33.0386 * kInchesToMeters}},
+ 181.723 / 180.0 * M_PI,
+ },
+ {
+ 1,
+ {{-12.5 * kInchesToMeters, 0.0}},
+ {{-1 * kInchesToMeters, 0.0}},
+ 16,
+ "/home/alex/cam1/debug_viewer_jpeg_",
+ 45,
+ }};
+
CameraCalibration camera_4 = {
{
3.73623 / 180.0 * M_PI, 588.1, 0.269291 / 180.0 * M_PI,
@@ -41,22 +59,130 @@
59,
}};
-CameraCalibration camera_10 = {
+CameraCalibration camera_6 = {
{
- 0.0429164 / 180.0 * M_PI, 337.247, 0.865625 / 180.0 * M_PI,
+ -1.17595 / 180.0 * M_PI, 346.997, 0.987547 / 180.0 * M_PI,
},
{
- {{20.5486 * kInchesToMeters, -2.47638 * kInchesToMeters,
- 33.1395 * kInchesToMeters}},
- 2.08049 / 180.0 * M_PI,
+ {{4.88124 * kInchesToMeters, 2.15528 * kInchesToMeters,
+ 33.1686 * kInchesToMeters}},
+ -12.0018 / 180.0 * M_PI,
+ },
+ {
+ 6,
+ {{12.5 * kInchesToMeters, 0.0}},
+ {{1 * kInchesToMeters, 0.0}},
+ 11,
+ "data/cam6_0/debug_viewer_jpeg_",
+ 75,
+ }};
+
+CameraCalibration camera_7 = {
+ {
+ -2.30729 / 180.0 * M_PI, 339.894, 1.16684 / 180.0 * M_PI,
+ },
+ {
+ {{3.62399 * kInchesToMeters, 3.94792 * kInchesToMeters,
+ 33.3196 * kInchesToMeters}},
+ 18.5828 / 180.0 * M_PI,
+ },
+ {
+ 7,
+ {{12.5 * kInchesToMeters, 0.0}},
+ {{1 * kInchesToMeters, 0.0}},
+ 21,
+ "data/cam7_0/debug_viewer_jpeg_",
+ 65,
+ }};
+
+CameraCalibration camera_8 = {
+ {
+ 37.0966 / 180.0 * M_PI, 339.997, 0.265968 / 180.0 * M_PI,
+ },
+ {
+ {{3.53674 * kInchesToMeters, 5.25891 * kInchesToMeters,
+ 12.6869 * kInchesToMeters}},
+ 92.4773 / 180.0 * M_PI,
+ },
+ {
+ 8,
+ {{6.5 * kInchesToMeters, -11 * kInchesToMeters}},
+ {{0.0, 1 * kInchesToMeters}},
+ 25,
+ "data/cam8_0/debug_viewer_jpeg_",
+ 61,
+ }};
+
+CameraCalibration camera_9 = {
+ {
+ 35.3461 / 180.0 * M_PI, 337.599, 3.34351 / 180.0 * M_PI,
+ },
+ {
+ {{4.24216 * kInchesToMeters, -2.97032 * kInchesToMeters,
+ 11.323 * kInchesToMeters}},
+ -93.3026 / 180.0 * M_PI,
+ },
+ {
+ 9,
+ {{-6.5 * kInchesToMeters, 11 * kInchesToMeters}},
+ {{0.0, -1 * kInchesToMeters}},
+ 30,
+ "data/cam9_0/debug_viewer_jpeg_",
+ 56,
+ }};
+
+CameraCalibration camera_10 = {
+ {
+ -0.165199 / 180.0 * M_PI, 340.666, 0.596842 / 180.0 * M_PI,
+ },
+ {
+ {{-5.23103 * kInchesToMeters, 2.96098 * kInchesToMeters,
+ 33.2867 * kInchesToMeters}},
+ 182.121 / 180.0 * M_PI,
},
{
10,
- {{12.5 * kInchesToMeters, 0.5 * kInchesToMeters}},
- {{1 * kInchesToMeters, 0.0}},
- 26,
+ {{-12.5 * kInchesToMeters, 0.0}},
+ {{-1 * kInchesToMeters, 0.0}},
+ 11,
"data/cam10_0/debug_viewer_jpeg_",
- 59,
+ 75,
+ }};
+
+CameraCalibration camera_14 = {
+ {
+ -0.0729684 / 180.0 * M_PI, 343.569, 0.685893 / 180.0 * M_PI,
+ },
+ {
+ {{5.53867 * kInchesToMeters, 2.08897 * kInchesToMeters,
+ 33.1766 * kInchesToMeters}},
+ -12.4121 / 180.0 * M_PI,
+ },
+ {
+ 14,
+ {{12.5 * kInchesToMeters, 0.0}},
+ {{1 * kInchesToMeters, 0.0}},
+ 16,
+ "/home/alex/cam14/debug_viewer_jpeg_",
+ 71,
+ }};
+
+CameraCalibration camera_15 = {
+ {
+ -0.715538 / 180.0 * M_PI, 336.509, 1.54169 / 180.0 * M_PI,
+ },
+ {
+ {{4.57935 * kInchesToMeters, 4.16624 * kInchesToMeters,
+ 33.433 * kInchesToMeters}},
+ 20.9856 / 180.0 * M_PI,
+ },
+ {
+ 15,
+ {{12.5 * kInchesToMeters, 0.0}},
+ {{1 * kInchesToMeters, 0.0}},
+ 25,
+ "/home/alex/cam15/debug_viewer_jpeg_",
+ 62,
}};
CameraCalibration camera_16 = {
@@ -77,6 +203,42 @@
55,
}};
+CameraCalibration camera_17 = {
+ {
+ 34.7631 / 180.0 * M_PI, 337.946, 2.48943 / 180.0 * M_PI,
+ },
+ {
+ {{3.15808 * kInchesToMeters, -2.5734 * kInchesToMeters,
+ 11.8507 * kInchesToMeters}},
+ -92.1953 / 180.0 * M_PI,
+ },
+ {
+ 17,
+ {{-6.5 * kInchesToMeters, 11 * kInchesToMeters}},
+ {{0.0, -1 * kInchesToMeters}},
+ 29,
+ "/home/alex/cam17/debug_viewer_jpeg_",
+ 58,
+ }};
+
+CameraCalibration camera_18 = {
+ {
+ 33.9292 / 180.0 * M_PI, 338.44, -1.71889 / 180.0 * M_PI,
+ },
+ {
+ {{3.82945 * kInchesToMeters, 5.51444 * kInchesToMeters,
+ 12.3803 * kInchesToMeters}},
+ 96.0112 / 180.0 * M_PI,
+ },
+ {
+ 18,
+ {{6.5 * kInchesToMeters, -11 * kInchesToMeters}},
+ {{0.0, 1 * kInchesToMeters}},
+ 27,
+ "/home/alex/cam18/debug_viewer_jpeg_",
+ 60,
+ }};
+
CameraCalibration camera_19 = {
{
-0.341036 / 180.0 * M_PI, 324.626, 1.2545 / 180.0 * M_PI,
@@ -97,14 +259,32 @@
const CameraCalibration *GetCamera(int camera_id) {
switch (camera_id) {
+ case 1:
+ return &camera_1;
case 4:
return &camera_4;
case 5:
return &camera_5;
+ case 6:
+ return &camera_6;
+ case 7:
+ return &camera_7;
+ case 8:
+ return &camera_8;
+ case 9:
+ return &camera_9;
case 10:
return &camera_10;
+ case 14:
+ return &camera_14;
+ case 15:
+ return &camera_15;
case 16:
return &camera_16;
+ case 17:
+ return &camera_17;
+ case 18:
+ return &camera_18;
case 19:
return &camera_19;
default:
diff --git a/y2019/vision/constants.h b/y2019/vision/constants.h
index 3f07f87..faa16e1 100644
--- a/y2019/vision/constants.h
+++ b/y2019/vision/constants.h
@@ -12,14 +12,14 @@
struct CameraGeometry {
static constexpr size_t kNumParams = 4;
// In Meters from floor under imu center.
- std::array<double, 3> location{{0, 0, 0}};
+ ::std::array<double, 3> location{{0.0, 0.0, 0.0}};
double heading = 0.0;
void set(double *data) {
- location[0] = data[0];
- location[1] = data[1];
- location[2] = data[2];
- heading = data[3];
+ data[0] = location[0];
+ data[1] = location[1];
+ data[2] = location[2];
+ data[3] = heading;
}
static CameraGeometry get(const double *data) {
CameraGeometry out;
@@ -60,9 +60,9 @@
struct DatasetInfo {
int camera_id;
// In meters from IMU start.
- std::array<double, 2> to_tape_measure_start;
+ ::std::array<double, 2> to_tape_measure_start;
// In meters,
- std::array<double, 2> tape_measure_direction;
+ ::std::array<double, 2> tape_measure_direction;
// This will multiply tape_measure_direction and thus has no units.
double beginning_tape_measure_reading;
const char *filename_prefix;
@@ -81,14 +81,17 @@
// Serial number of the teensy for each robot.
constexpr uint32_t CodeBotTeensyId() { return 0xffff322e; }
+constexpr uint32_t PracticeBotTeensyId() { return 0xffff3215; }
// Get the IDs of the cameras in each port for a particular teensy board.
// inlined so that we don't have to deal with including it in the autogenerated
// constants.cc.
-inline std::array<int, 5> CameraSerialNumbers(uint32_t processor_id) {
+inline ::std::array<int, 5> CameraSerialNumbers(uint32_t processor_id) {
switch (processor_id) {
case CodeBotTeensyId():
return {{0, 0, 0, 16, 19}};
+ case PracticeBotTeensyId():
+ return {{14, 15, 18, 17, 1}};
default:
return {{0, 0, 0, 0, 0}};
}
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index c962fc9..d21e184 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -18,6 +18,27 @@
#include "ceres/ceres.h"
DEFINE_int32(camera_id, -1, "The camera ID to calibrate");
+DEFINE_string(prefix, "", "The image filename prefix");
+
+DEFINE_string(constants, "y2019/vision/constants.cc",
+ "Path to the constants file to update");
+
+DEFINE_double(beginning_tape_measure_reading, 11,
+ "The tape measure measurement (in inches) of the first image.");
+DEFINE_int32(image_count, 75, "The number of images to capture");
+DEFINE_double(
+ tape_start_x, -12.5,
+ "The starting location of the tape measure in x relative to the CG in inches.");
+DEFINE_double(
+ tape_start_y, -0.5,
+ "The starting location of the tape measure in y relative to the CG in inches.");
+
+DEFINE_double(
+ tape_direction_x, -1.0,
+ "The x component of \"1\" inch along the tape measure in meters.");
+DEFINE_double(
+ tape_direction_y, 0.0,
+ "The y component of \"1\" inch along the tape measure in meters.");
using ::aos::events::DataSocket;
using ::aos::events::RXUdpSocket;
@@ -36,19 +57,17 @@
namespace y2019 {
namespace vision {
+namespace {
constexpr double kInchesToMeters = 0.0254;
-template <size_t k, size_t n, size_t n2, typename T>
-T *MakeFunctor(T &&t) {
- return new T(std::move(t));
-}
+} // namespace
-std::array<double, 3> GetError(const DatasetInfo &info,
- const double *const extrinsics,
- const double *const geometry, int i) {
- auto extrinsic_params = ExtrinsicParams::get(extrinsics);
- auto geo = CameraGeometry::get(geometry);
+::std::array<double, 3> GetError(const DatasetInfo &info,
+ const double *const extrinsics,
+ const double *const geometry, int i) {
+ const ExtrinsicParams extrinsic_params = ExtrinsicParams::get(extrinsics);
+ const CameraGeometry geo = CameraGeometry::get(geometry);
const double s = sin(geo.heading + extrinsic_params.r2);
const double c = cos(geo.heading + extrinsic_params.r2);
@@ -56,142 +75,168 @@
// Take the tape measure starting point (this will be at the perimeter of the
// robot), add the offset to the first sample, and then add the per sample
// offset.
- const double dx =
- (info.to_tape_measure_start[0] +
- (info.beginning_tape_measure_reading + i) *
- info.tape_measure_direction[0]) /
- kInchesToMeters -
- (geo.location[0] + c * extrinsic_params.z) / kInchesToMeters;
- const double dy =
- (info.to_tape_measure_start[1] +
- (info.beginning_tape_measure_reading + i) *
- info.tape_measure_direction[1]) /
- kInchesToMeters -
- (geo.location[1] + s * extrinsic_params.z) / kInchesToMeters;
+ const double dx = (info.to_tape_measure_start[0] +
+ (info.beginning_tape_measure_reading + i) *
+ info.tape_measure_direction[0]) -
+ (geo.location[0] + c * extrinsic_params.z);
+ const double dy = (info.to_tape_measure_start[1] +
+ (info.beginning_tape_measure_reading + i) *
+ info.tape_measure_direction[1]) -
+ (geo.location[1] + s * extrinsic_params.z);
- constexpr double kCalibrationTargetHeight = 28.5;
- const double dz = kCalibrationTargetHeight -
- (geo.location[2] + extrinsic_params.y) / kInchesToMeters;
+ constexpr double kCalibrationTargetHeight = 28.5 * kInchesToMeters;
+ const double dz =
+ kCalibrationTargetHeight - (geo.location[2] + extrinsic_params.y);
return {{dx, dy, dz}};
}
void main(int argc, char **argv) {
using namespace y2019::vision;
- gflags::ParseCommandLineFlags(&argc, &argv, false);
-
- const char *base_directory = "/home/parker/data/frc/2019_calibration/";
+ ::gflags::ParseCommandLineFlags(&argc, &argv, false);
DatasetInfo info = {
FLAGS_camera_id,
- {{12.5 * kInchesToMeters, 0.5 * kInchesToMeters}},
- {{kInchesToMeters, 0.0}},
- 26,
- "cam5_0/debug_viewer_jpeg_",
- 59,
+ {{FLAGS_tape_start_x * kInchesToMeters,
+ FLAGS_tape_start_y * kInchesToMeters}},
+ {{FLAGS_tape_direction_x * kInchesToMeters,
+ FLAGS_tape_direction_y * kInchesToMeters}},
+ FLAGS_beginning_tape_measure_reading,
+ FLAGS_prefix.c_str(),
+ FLAGS_image_count,
};
::aos::logging::Init();
::aos::logging::AddImplementation(
new ::aos::logging::StreamLogImplementation(stderr));
- TargetFinder finder_;
+ TargetFinder target_finder;
ceres::Problem problem;
struct Sample {
- std::unique_ptr<double[]> extrinsics;
+ ::std::unique_ptr<double[]> extrinsics;
int i;
};
- std::vector<Sample> all_extrinsics;
+ ::std::vector<Sample> all_extrinsics;
double intrinsics[IntrinsicParams::kNumParams];
IntrinsicParams().set(&intrinsics[0]);
double geometry[CameraGeometry::kNumParams];
- CameraGeometry().set(&geometry[0]);
+ {
+ // Assume the camera is near the center of the robot, and start by pointing
+ // it from the center of the robot to the location of the first target.
+ CameraGeometry camera_geometry;
+ const double x =
+ info.to_tape_measure_start[0] +
+ info.beginning_tape_measure_reading * info.tape_measure_direction[0];
+ const double y =
+ info.to_tape_measure_start[1] +
+ info.beginning_tape_measure_reading * info.tape_measure_direction[1];
+ camera_geometry.heading = ::std::atan2(y, x);
+ printf("Inital heading is %f\n", camera_geometry.heading);
+ camera_geometry.set(&geometry[0]);
+ }
Solver::Options options;
options.minimizer_progress_to_stdout = false;
Solver::Summary summary;
- std::cout << summary.BriefReport() << "\n";
+ ::std::cout << summary.BriefReport() << "\n";
IntrinsicParams intrinsics_ = IntrinsicParams::get(&intrinsics[0]);
- std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
- std::cout << "fl = " << intrinsics_.focal_length << ";\n";
- std::cout << "error = " << summary.final_cost << ";\n";
+ ::std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
+ ::std::cout << "fl = " << intrinsics_.focal_length << ";\n";
+ ::std::cout << "error = " << summary.final_cost << ";\n";
for (int i = 0; i < info.num_images; ++i) {
- auto frame = aos::vision::LoadFile(std::string(base_directory) +
- info.filename_prefix +
- std::to_string(i) + ".yuyv");
+ ::aos::vision::DatasetFrame frame =
+ aos::vision::LoadFile(FLAGS_prefix + std::to_string(i) + ".yuyv");
- aos::vision::ImageFormat fmt{640, 480};
- aos::vision::BlobList imgs =
- aos::vision::FindBlobs(aos::vision::DoThresholdYUYV(
+ const ::aos::vision::ImageFormat fmt{640, 480};
+ ::aos::vision::BlobList imgs =
+ ::aos::vision::FindBlobs(aos::vision::DoThresholdYUYV(
fmt, frame.data.data(), TargetFinder::GetThresholdValue()));
- finder_.PreFilter(&imgs);
+ target_finder.PreFilter(&imgs);
- bool verbose = false;
- std::vector<std::vector<Segment<2>>> raw_polys;
+ constexpr bool verbose = false;
+ ::std::vector<std::vector<Segment<2>>> raw_polys;
for (const RangeImage &blob : imgs) {
// Convert blobs to contours in the corrected space.
- ContourNode* contour = finder_.GetContour(blob);
- finder_.UnWarpContour(contour);
- std::vector<Segment<2>> polygon = finder_.FillPolygon(contour, verbose);
- if (polygon.empty()) {
- } else {
+ ContourNode *contour = target_finder.GetContour(blob);
+ target_finder.UnWarpContour(contour);
+ const ::std::vector<Segment<2>> polygon =
+ target_finder.FillPolygon(contour, verbose);
+ if (!polygon.empty()) {
raw_polys.push_back(polygon);
}
}
// Calculate each component side of a possible target.
- std::vector<TargetComponent> target_component_list =
- finder_.FillTargetComponentList(raw_polys);
+ const ::std::vector<TargetComponent> target_component_list =
+ target_finder.FillTargetComponentList(raw_polys);
// Put the compenents together into targets.
- std::vector<Target> target_list =
- finder_.FindTargetsFromComponents(target_component_list, verbose);
+ const ::std::vector<Target> target_list =
+ target_finder.FindTargetsFromComponents(target_component_list, verbose);
- // Use the solver to generate an intermediate version of our results.
- std::vector<IntermediateResult> results;
+ // Now, iterate over the targets (in pixel space). Generate a residual
+ // block for each valid target which compares the actual pixel locations
+ // with the expected pixel locations given the intrinsics and extrinsics.
for (const Target &target : target_list) {
- ::std::array<aos::vision::Vector<2>, 8> target_value =
+ const ::std::array<::aos::vision::Vector<2>, 8> target_value =
target.ToPointList();
- ::std::array<aos::vision::Vector<2>, 8> template_value =
- finder_.GetTemplateTarget().ToPointList();
+ const ::std::array<::aos::vision::Vector<2>, 8> template_value =
+ target_finder.GetTemplateTarget().ToPointList();
- // TODO(austin): Memory leak below, fix.
- double *extrinsics = new double[ExtrinsicParams::kNumParams];
+ all_extrinsics.push_back(
+ {::std::unique_ptr<double[]>(new double[ExtrinsicParams::kNumParams]),
+ i});
+ double *extrinsics = all_extrinsics.back().extrinsics.get();
ExtrinsicParams().set(extrinsics);
- all_extrinsics.push_back({std::unique_ptr<double[]>(extrinsics), i});
for (size_t j = 0; j < 8; ++j) {
- aos::vision::Vector<2> temp = template_value[j];
- aos::vision::Vector<2> targ = target_value[j];
+ // Template target in target space as documented by GetTemplateTarget()
+ const ::aos::vision::Vector<2> template_point = template_value[j];
+ // Target in pixel space.
+ const ::aos::vision::Vector<2> target_point = target_value[j];
- auto ftor = [temp, targ, i](const double *const intrinsics,
- const double *const extrinsics,
- double *residual) {
- auto in = IntrinsicParams::get(intrinsics);
- auto extrinsic_params = ExtrinsicParams::get(extrinsics);
- auto pt = targ - Project(temp, in, extrinsic_params);
- residual[0] = pt.x();
- residual[1] = pt.y();
+ // Now build up the residual block.
+ auto ftor = [template_point, target_point, i](
+ const double *const intrinsics, const double *const extrinsics,
+ double *residual) {
+ const IntrinsicParams intrinsic_params =
+ IntrinsicParams::get(intrinsics);
+ const ExtrinsicParams extrinsic_params =
+ ExtrinsicParams::get(extrinsics);
+ // Project the target location into pixel coordinates.
+ const ::aos::vision::Vector<2> projected_point =
+ Project(template_point, intrinsic_params, extrinsic_params);
+ const ::aos::vision::Vector<2> residual_point =
+ target_point - projected_point;
+ residual[0] = residual_point.x();
+ residual[1] = residual_point.y();
return true;
};
problem.AddResidualBlock(
new NumericDiffCostFunction<decltype(ftor), CENTRAL, 2,
IntrinsicParams::kNumParams,
ExtrinsicParams::kNumParams>(
- new decltype(ftor)(std::move(ftor))),
+ new decltype(ftor)(::std::move(ftor))),
NULL, &intrinsics[0], extrinsics);
}
+ // Now, compare the estimated location of the target that we just solved
+ // for with the extrinsic params above with the known location of the
+ // target.
auto ftor = [&info, i](const double *const extrinsics,
const double *const geometry, double *residual) {
- std::array<double, 3> err = GetError(info, extrinsics, geometry, i);
- residual[0] = 32.0 * err[0];
- residual[1] = 32.0 * err[1];
- residual[2] = 32.0 * err[2];
+ const ::std::array<double, 3> err =
+ GetError(info, extrinsics, geometry, i);
+ // We care a lot more about geometry than pixels. Especially since
+ // pixels are small and meters are big, so there's a small penalty to
+ // being off by meters.
+ residual[0] = err[0] * 1259.0;
+ residual[1] = err[1] * 1259.0;
+ residual[2] = err[2] * 1259.0;
return true;
};
@@ -199,7 +244,7 @@
new NumericDiffCostFunction<decltype(ftor), CENTRAL, 3,
ExtrinsicParams::kNumParams,
CameraGeometry::kNumParams>(
- new decltype(ftor)(std::move(ftor))),
+ new decltype(ftor)(::std::move(ftor))),
NULL, extrinsics, &geometry[0]);
}
}
@@ -209,41 +254,41 @@
Solve(options, &problem, &summary);
{
- std::cout << summary.BriefReport() << "\n";
+ ::std::cout << summary.BriefReport() << ::std::endl;
IntrinsicParams intrinsics_ = IntrinsicParams::get(&intrinsics[0]);
CameraGeometry geometry_ = CameraGeometry::get(&geometry[0]);
- std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
- std::cout << "fl = " << intrinsics_.focal_length << ";\n";
- std::cout << "error = " << summary.final_cost << ";\n";
+ ::std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";"
+ << ::std::endl;
+ ::std::cout << "fl = " << intrinsics_.focal_length << ";" << ::std::endl;
+ ::std::cout << "error = " << summary.final_cost << ";" << ::std::endl;
- std::cout << "camera_angle = " << geometry_.heading * 180 / M_PI << "\n";
- std::cout << "camera_x = " << geometry_.location[0] / kInchesToMeters
- << "\n";
- std::cout << "camera_y = " << geometry_.location[1] / kInchesToMeters
- << "\n";
- std::cout << "camera_z = " << geometry_.location[2] / kInchesToMeters
- << "\n";
- std::cout << "camera_barrel = " << intrinsics_.barrel_mount * 180.0 / M_PI
- << "\n";
+ ::std::cout << "camera_angle = " << geometry_.heading * 180 / M_PI
+ << ::std::endl;
+ ::std::cout << "camera_x = " << geometry_.location[0] << ::std::endl;
+ ::std::cout << "camera_y = " << geometry_.location[1] << ::std::endl;
+ ::std::cout << "camera_z = " << geometry_.location[2] << ::std::endl;
+ ::std::cout << "camera_barrel = " << intrinsics_.barrel_mount * 180.0 / M_PI
+ << ::std::endl;
for (const Sample &sample : all_extrinsics) {
- int i = sample.i;
+ const int i = sample.i;
double *data = sample.extrinsics.get();
- ExtrinsicParams extn = ExtrinsicParams::get(data);
+ const ExtrinsicParams extrinsic_params = ExtrinsicParams::get(data);
- auto err = GetError(info, data, &geometry[0], i);
+ const ::std::array<double, 3> error =
+ GetError(info, data, &geometry[0], i);
- std::cout << i << ", ";
- std::cout << extn.z / kInchesToMeters << ", ";
- std::cout << extn.y / kInchesToMeters << ", ";
- std::cout << extn.r1 * 180 / M_PI << ", ";
- std::cout << extn.r2 * 180 / M_PI << ", ";
+ ::std::cout << i << ", ";
+ ::std::cout << extrinsic_params.z << "m, ";
+ ::std::cout << extrinsic_params.y << "m, ";
+ ::std::cout << extrinsic_params.r1 * 180 / M_PI << ", ";
+ ::std::cout << extrinsic_params.r2 * 180 / M_PI << ", ";
// TODO: Methodology problem: This should really have a separate solve for
// extrinsics...
- std::cout << err[0] << ", ";
- std::cout << err[1] << ", ";
- std::cout << err[2] << "\n";
+ ::std::cout << error[0] << "m, ";
+ ::std::cout << error[1] << "m, ";
+ ::std::cout << error[2] << "m" << ::std::endl;
}
}
@@ -251,7 +296,7 @@
results.dataset = info;
results.intrinsics = IntrinsicParams::get(&intrinsics[0]);
results.geometry = CameraGeometry::get(&geometry[0]);
- DumpCameraConstants("y2019/vision/constants.cc", info.camera_id, results);
+ DumpCameraConstants(FLAGS_constants.c_str(), info.camera_id, results);
}
} // namespace y2019
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index d39764a..45678eb 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -7,7 +7,7 @@
namespace y2019 {
namespace vision {
-TargetFinder::TargetFinder() { target_template_ = Target::MakeTemplate(); }
+TargetFinder::TargetFinder() : target_template_(Target::MakeTemplate()) {}
aos::vision::RangeImage TargetFinder::Threshold(aos::vision::ImagePtr image) {
const uint8_t threshold_value = GetThresholdValue();
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index 666ae2f..1ba79ba 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -73,7 +73,7 @@
aos::vision::AnalysisAllocator alloc_;
// The template for the default target in the standard space.
- Target target_template_;
+ const Target target_template_;
IntrinsicParams intrinsics_;
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index 1dd6a1a..d4b8a54 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -124,20 +124,18 @@
Target Project(const Target &target, const IntrinsicParams &intrinsics,
const ExtrinsicParams &extrinsics) {
- auto project = [&](Vector<2> pt) {
- return Project(pt, intrinsics, extrinsics);
- };
Target new_targ;
new_targ.right.is_right = true;
- new_targ.right.top = project(target.right.top);
- new_targ.right.inside = project(target.right.inside);
- new_targ.right.bottom = project(target.right.bottom);
- new_targ.right.outside = project(target.right.outside);
+ new_targ.right.top = Project(target.right.top, intrinsics, extrinsics);
+ new_targ.right.inside = Project(target.right.inside, intrinsics, extrinsics);
+ new_targ.right.bottom = Project(target.right.bottom, intrinsics, extrinsics);
+ new_targ.right.outside =
+ Project(target.right.outside, intrinsics, extrinsics);
- new_targ.left.top = project(target.left.top);
- new_targ.left.inside = project(target.left.inside);
- new_targ.left.bottom = project(target.left.bottom);
- new_targ.left.outside = project(target.left.outside);
+ new_targ.left.top = Project(target.left.top, intrinsics, extrinsics);
+ new_targ.left.inside = Project(target.left.inside, intrinsics, extrinsics);
+ new_targ.left.bottom = Project(target.left.bottom, intrinsics, extrinsics);
+ new_targ.left.outside = Project(target.left.outside, intrinsics, extrinsics);
return new_targ;
}
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
index d662316..3174b90 100644
--- a/y2019/vision/target_types.h
+++ b/y2019/vision/target_types.h
@@ -38,6 +38,9 @@
TargetComponent left;
TargetComponent right;
+ // Returns a target. The resulting target is in meters with 0, 0 centered
+ // between the upper inner corners of the two pieces of tape, y being up and x
+ // being to the right.
static Target MakeTemplate();
// Get the points in some order (will match against the template).
std::array<aos::vision::Vector<2>, 8> ToPointList() const;
@@ -72,6 +75,7 @@
return out;
}
};
+
// Projects a point from idealized template space to camera space.
aos::vision::Vector<2> Project(aos::vision::Vector<2> pt,
const IntrinsicParams &intrinsics,
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 2b21eb8..2a21274 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -16,6 +16,7 @@
#include "frc971/wpilib/ahal/DriverStation.h"
#include "frc971/wpilib/ahal/Encoder.h"
#include "frc971/wpilib/ahal/VictorSP.h"
+#include "ctre/phoenix/CANifier.h"
#undef ERROR
#include "aos/commonmath.h"
@@ -47,6 +48,7 @@
#include "y2019/control_loops/drivetrain/camera.q.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
#include "y2019/jevois/spi.h"
+#include "y2019/status_light.q.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -523,6 +525,59 @@
to_log.read_solenoids = pcm_.GetAll();
LOG_STRUCT(DEBUG, "pneumatics info", to_log);
}
+
+ status_light.FetchLatest();
+ // If we don't have a light request (or it's an old one), we are borked.
+ // Flash the red light slowly.
+ if (!status_light.get() ||
+ status_light.Age() > chrono::milliseconds(100)) {
+ StatusLight color;
+ color.red = 0.0;
+ color.green = 0.0;
+ color.blue = 0.0;
+
+ ++light_flash_;
+ if (light_flash_ > 10) {
+ color.red = 0.5;
+ }
+
+ if (light_flash_ > 20) {
+ light_flash_ = 0;
+ }
+
+ LOG_STRUCT(DEBUG, "color", color);
+ SetColor(color);
+ } else {
+ LOG_STRUCT(DEBUG, "color", *status_light);
+ SetColor(*status_light);
+ }
+ }
+ }
+
+ void SetColor(const StatusLight &status_light) {
+ // Save CAN bandwidth and CPU at the cost of RT. Only change the light when
+ // it actually changes. This is pretty low priority anyways.
+ static int time_since_last_send = 0;
+ ++time_since_last_send;
+ if (time_since_last_send > 10) {
+ time_since_last_send = 0;
+ }
+ if (status_light.green != last_green_ || time_since_last_send == 0) {
+ canifier_.SetLEDOutput(1.0 - status_light.green,
+ ::ctre::phoenix::CANifier::LEDChannelB);
+ last_green_ = status_light.green;
+ }
+
+ if (status_light.blue != last_blue_ || time_since_last_send == 0) {
+ canifier_.SetLEDOutput(1.0 - status_light.blue,
+ ::ctre::phoenix::CANifier::LEDChannelA);
+ last_blue_ = status_light.blue;
+ }
+
+ if (status_light.red != last_red_ || time_since_last_send == 0) {
+ canifier_.SetLEDOutput(1.0 - status_light.red,
+ ::ctre::phoenix::CANifier::LEDChannelC);
+ last_red_ = status_light.red;
}
}
@@ -541,7 +596,15 @@
::y2019::control_loops::superstructure::SuperstructureQueue::Output>
superstructure_;
+ ::ctre::phoenix::CANifier canifier_{0};
+
::std::atomic<bool> run_{true};
+
+ double last_red_ = -1.0;
+ double last_green_ = -1.0;
+ double last_blue_ = -1.0;
+
+ int light_flash_ = 0;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {