Run clang-format on the entire repo
This patch clang-formats the entire repo. Third-party code is
excluded.
I needed to fix up the .clang-format file so that all the header
includes are ordered properly. I could have sworn that it used to work
without the extra modification, but I guess not.
Signed-off-by: Philipp Schrader <philipp.schrader@gmail.com>
Change-Id: I64bb9f2c795401393f9dfe2fefc4f04cb36b52f6
diff --git a/y2019/constants.cc b/y2019/constants.cc
index cc81f53..10582ed 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -8,9 +8,10 @@
#endif
#include "absl/base/call_once.h"
+#include "glog/logging.h"
+
#include "aos/network/team_number.h"
#include "aos/stl_mutex/stl_mutex.h"
-#include "glog/logging.h"
#include "y2019/control_loops/superstructure/elevator/integral_elevator_plant.h"
#include "y2019/control_loops/superstructure/intake/integral_intake_plant.h"
#include "y2019/control_loops/superstructure/stilts/integral_stilts_plant.h"
diff --git a/y2019/control_loops/drivetrain/drivetrain_replay.cc b/y2019/control_loops/drivetrain/drivetrain_replay.cc
index 26c87e8..d9d928c 100644
--- a/y2019/control_loops/drivetrain/drivetrain_replay.cc
+++ b/y2019/control_loops/drivetrain/drivetrain_replay.cc
@@ -1,14 +1,15 @@
#include <iostream>
+#include "gflags/gflags.h"
+
#include "aos/configuration.h"
-#include "aos/events/logging/log_writer.h"
#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
#include "aos/events/simulated_event_loop.h"
#include "aos/init.h"
#include "aos/json_to_flatbuffer.h"
#include "aos/network/team_number.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
-#include "gflags/gflags.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
#include "y2019/control_loops/drivetrain/event_loop_localizer.h"
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index a78f3a2..0847479 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -66,9 +66,7 @@
Localizer::State Xhat() const override { return localizer_.X_hat(); }
- TargetSelector *target_selector() override {
- return &target_selector_;
- }
+ TargetSelector *target_selector() override { return &target_selector_; }
private:
void HandleFrame(const CameraFrame *frame);
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index 4d6f84a..016e3b7 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -1,11 +1,12 @@
#include <queue>
+#include "gtest/gtest.h"
+
#include "aos/network/team_number.h"
#include "frc971/control_loops/control_loop_test.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "frc971/control_loops/team_number_test_environment.h"
-#include "gtest/gtest.h"
#include "y2019/control_loops/drivetrain/camera_generated.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
#include "y2019/control_loops/drivetrain/event_loop_localizer.h"
diff --git a/y2019/control_loops/drivetrain/localizer.h b/y2019/control_loops/drivetrain/localizer.h
index 818e1b9..2e7c3a5 100644
--- a/y2019/control_loops/drivetrain/localizer.h
+++ b/y2019/control_loops/drivetrain/localizer.h
@@ -95,15 +95,13 @@
dhdx;
make_h_queue_.CorrectKnownHBuilder(
z, nullptr,
- ExpectedObservationBuilder(this, camera, targets, &h_functions,
- &dhdx),
+ ExpectedObservationBuilder(this, camera, targets, &h_functions, &dhdx),
R, t);
// Fetch cache:
for (size_t ii = 1; ii < targets.size(); ++ii) {
TargetViewToMatrices(targets[ii], &z, &R);
h_queue_.CorrectKnownH(
- z, nullptr,
- ExpectedObservationFunctor(h_functions[ii], dhdx[ii]), R,
+ z, nullptr, ExpectedObservationFunctor(h_functions[ii], dhdx[ii]), R,
t);
}
}
@@ -189,8 +187,8 @@
const State &state, const StateSquare &P) {
HFunction h;
Eigen::Matrix<Scalar, kNOutputs, kNStates> dhdx;
- localizer_->MakeH(camera_, target_views_, h_functions_, dhdx_,
- state, P, &h, &dhdx);
+ localizer_->MakeH(camera_, target_views_, h_functions_, dhdx_, state, P,
+ &h, &dhdx);
functor_.emplace(h, dhdx);
return &functor_.value();
}
@@ -410,8 +408,7 @@
if (view_idx >= camera_views.size()) {
AOS_LOG(ERROR, "Somehow, the view scorer failed.\n");
h_functions->emplace_back();
- dhdx->push_back(
- Eigen::Matrix<Scalar, kNOutputs, kNStates>::Zero());
+ dhdx->push_back(Eigen::Matrix<Scalar, kNOutputs, kNStates>::Zero());
continue;
}
const Eigen::Matrix<Scalar, kNOutputs, kNStates> best_H =
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index 1c9485a..28f0235 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -3,15 +3,17 @@
#include <queue>
#include <random>
+#include "gflags/gflags.h"
+
#include "aos/testing/random_seed.h"
#include "aos/testing/test_shm.h"
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
#include "frc971/control_loops/drivetrain/trajectory.h"
-#include "gflags/gflags.h"
#if defined(SUPPORT_PLOT)
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
#endif
#include "gtest/gtest.h"
+
#include "y2019/constants.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
@@ -490,9 +492,9 @@
const double left_enc = state(StateIdx::kLeftEncoder, 0);
const double right_enc = state(StateIdx::kRightEncoder, 0);
- const double gyro = (state(StateIdx::kRightVelocity) -
- state(StateIdx::kLeftVelocity)) /
- dt_config_.robot_radius / 2.0;
+ const double gyro =
+ (state(StateIdx::kRightVelocity) - state(StateIdx::kLeftVelocity)) /
+ dt_config_.robot_radius / 2.0;
const TestLocalizer::State xdot = DiffEq(state, U);
const Eigen::Vector3d accel(
localizer_.CalcLongitudinalVelocity(xdot) -
diff --git a/y2019/control_loops/drivetrain/replay_localizer.cc b/y2019/control_loops/drivetrain/replay_localizer.cc
index 3687e3d..b7d269b 100644
--- a/y2019/control_loops/drivetrain/replay_localizer.cc
+++ b/y2019/control_loops/drivetrain/replay_localizer.cc
@@ -1,5 +1,7 @@
#include <fcntl.h>
+#include "gflags/gflags.h"
+
#include "aos/init.h"
#include "aos/logging/implementations.h"
#include "aos/logging/replay.h"
@@ -8,7 +10,6 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/localizer.q.h"
#include "frc971/wpilib/imu.q.h"
-#include "gflags/gflags.h"
#include "y2019/constants.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
#include "y2019/control_loops/drivetrain/event_loop_localizer.h"
@@ -29,9 +30,10 @@
namespace drivetrain {
using ::y2019::constants::Field;
-typedef TypedLocalizer<
- constants::Values::kNumCameras, Field::kNumTargets, Field::kNumObstacles,
- EventLoopLocalizer::kMaxTargetsPerFrame, double> TestLocalizer;
+typedef TypedLocalizer<constants::Values::kNumCameras, Field::kNumTargets,
+ Field::kNumObstacles,
+ EventLoopLocalizer::kMaxTargetsPerFrame, double>
+ TestLocalizer;
typedef typename TestLocalizer::Camera TestCamera;
typedef typename TestCamera::Pose Pose;
typedef typename TestCamera::LineSegment Obstacle;
@@ -42,7 +44,7 @@
const ::std::map<::std::string, ::std::string> &kwargs) {
::std::vector<double> x;
::std::vector<double> y;
- for (const Pose & p : poses) {
+ for (const Pose &p : poses) {
x.push_back(p.abs_pos().x());
y.push_back(p.abs_pos().y());
}
@@ -336,8 +338,8 @@
// Whether the robot has been enabled yet.
bool has_been_enabled_ = false;
// Cache of last gyro value to forward to the localizer.
- double latest_gyro_ = 0.0; // rad/sec
- double battery_voltage_ = 12.0; // volts
+ double latest_gyro_ = 0.0; // rad/sec
+ double battery_voltage_ = 12.0; // volts
::Eigen::Matrix<double, 2, 1> last_U_{0, 0};
::Eigen::Matrix<double, 2, 1> last_last_U_{0, 0};
diff --git a/y2019/control_loops/drivetrain/target_selector_test.cc b/y2019/control_loops/drivetrain/target_selector_test.cc
index b60085c..d4ef6da 100644
--- a/y2019/control_loops/drivetrain/target_selector_test.cc
+++ b/y2019/control_loops/drivetrain/target_selector_test.cc
@@ -1,7 +1,8 @@
#include "y2019/control_loops/drivetrain/target_selector.h"
-#include "aos/events/simulated_event_loop.h"
#include "gtest/gtest.h"
+
+#include "aos/events/simulated_event_loop.h"
#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
namespace y2019 {
@@ -81,8 +82,7 @@
builder.MakeBuilder<superstructure::Goal>();
goal_builder.add_suction(suction_offset);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
{
auto builder = target_selector_hint_sender_.MakeBuilder();
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
index 338864e..ac88cec 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.h
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -74,7 +74,8 @@
// elevator is below kElevatorClearHeight.
static constexpr double kWristElevatorCollisionMinAngle = -M_PI / 4.0;
static constexpr double kWristElevatorCollisionMaxAngle = M_PI / 4.0;
- static constexpr double kWristElevatorCollisionMaxAngleWithoutObject = M_PI / 6.0;
+ static constexpr double kWristElevatorCollisionMaxAngleWithoutObject =
+ M_PI / 6.0;
// Tolerance for the elevator.
static constexpr double kEps = 0.02;
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
index 9ea373e..68449d6 100644
--- a/y2019/control_loops/superstructure/collision_avoidance_tests.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -1,8 +1,8 @@
-#include "y2019/control_loops/superstructure/collision_avoidance.h"
+#include "gtest/gtest.h"
#include "aos/commonmath.h"
#include "aos/flatbuffers.h"
-#include "gtest/gtest.h"
+#include "y2019/control_loops/superstructure/collision_avoidance.h"
#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
@@ -81,8 +81,7 @@
wrist_offset = wrist_builder.Finish();
}
- flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
- elevator_offset;
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus> elevator_offset;
{
PotAndAbsoluteEncoderProfiledJointStatus::Builder elevator_builder(fbb);
@@ -90,8 +89,7 @@
elevator_offset = elevator_builder.Finish();
}
- flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus>
- intake_offset;
+ flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> intake_offset;
{
AbsoluteEncoderProfiledJointStatus::Builder intake_builder(fbb);
@@ -462,15 +460,15 @@
// Fix Collision Wrist Above Elevator
TEST_P(CollisionAvoidanceTests, FixWristElevatorCollision) {
// changes the goals
- unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal ( 0.0);
- unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal ( 0.0);
- unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal (
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes
- status_.mutable_message()->mutable_wrist()->mutate_position ( 0.0);
- status_.mutable_message()->mutable_elevator()->mutate_position ( 0.0);
- status_.mutable_message()->mutable_intake()->mutate_position (
+ status_.mutable_message()->mutable_wrist()->mutate_position(0.0);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
@@ -484,7 +482,7 @@
}
INSTANTIATE_TEST_SUITE_P(CollisionAvoidancePieceTest, CollisionAvoidanceTests,
- ::testing::Bool());
+ ::testing::Bool());
} // namespace testing
} // namespace superstructure
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index f86d64c..f8ad0fd 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -1,8 +1,8 @@
#ifndef Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
-#include "frc971/control_loops/control_loop.h"
#include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2019/constants.h"
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index 4e51086..a09913b 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -3,12 +3,13 @@
#include <chrono>
#include <memory>
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
#include "frc971/control_loops/capped_test_plant.h"
#include "frc971/control_loops/control_loop_test.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
-#include "glog/logging.h"
-#include "gtest/gtest.h"
#include "y2019/constants.h"
#include "y2019/control_loops/superstructure/elevator/elevator_plant.h"
#include "y2019/control_loops/superstructure/intake/intake_plant.h"
@@ -503,8 +504,7 @@
goal_builder.add_intake(intake_offset);
goal_builder.add_stilts(stilts_offset);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(10));
VerifyNearGoal();
@@ -553,8 +553,7 @@
goal_builder.add_intake(intake_offset);
goal_builder.add_stilts(stilts_offset);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
// Give it a lot of time to get there.
@@ -597,8 +596,7 @@
goal_builder.add_intake(intake_offset);
goal_builder.add_stilts(stilts_offset);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(8));
VerifyNearGoal();
@@ -635,8 +633,7 @@
goal_builder.add_intake(intake_offset);
goal_builder.add_stilts(stilts_offset);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
superstructure_plant_.set_peak_elevator_velocity(23.0);
superstructure_plant_.set_peak_elevator_acceleration(0.2);
@@ -688,8 +685,7 @@
goal_builder.add_intake(intake_offset);
goal_builder.add_stilts(stilts_offset);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
WaitUntilZeroed();
VerifyNearGoal();
@@ -750,8 +746,7 @@
goal_builder.add_intake(intake_offset);
goal_builder.add_stilts(stilts_offset);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
// Give it a lot of time to get there.
@@ -791,8 +786,7 @@
goal_builder.add_stilts(stilts_offset);
goal_builder.add_roller_voltage(6.0);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(5));
@@ -827,8 +821,7 @@
goal_builder.add_stilts(stilts_offset);
goal_builder.add_roller_voltage(6.0);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(5));
@@ -870,8 +863,7 @@
goal_builder.add_stilts(stilts_offset);
goal_builder.add_suction(suction_offset);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10));
@@ -915,8 +907,7 @@
goal_builder.add_stilts(stilts_offset);
goal_builder.add_suction(suction_offset);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
// Verify that at 0 pressure after short time voltage is still high
@@ -966,8 +957,7 @@
goal_builder.add_stilts(stilts_offset);
goal_builder.add_suction(suction_offset);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
// Get a Gamepiece
@@ -1004,8 +994,7 @@
goal_builder.add_stilts(stilts_offset);
goal_builder.add_suction(suction_offset);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
superstructure_plant_.set_simulated_pressure(1.0);
diff --git a/y2019/control_loops/superstructure/superstructure_main.cc b/y2019/control_loops/superstructure/superstructure_main.cc
index 5dc1542..35c140c 100644
--- a/y2019/control_loops/superstructure/superstructure_main.cc
+++ b/y2019/control_loops/superstructure/superstructure_main.cc
@@ -1,7 +1,6 @@
-#include "y2019/control_loops/superstructure/superstructure.h"
-
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
+#include "y2019/control_loops/superstructure/superstructure.h"
int main(int argc, char **argv) {
::aos::InitGoogle(&argc, &argv);
diff --git a/y2019/control_loops/superstructure/vacuum.cc b/y2019/control_loops/superstructure/vacuum.cc
index 6d4696b..fda123e 100644
--- a/y2019/control_loops/superstructure/vacuum.cc
+++ b/y2019/control_loops/superstructure/vacuum.cc
@@ -44,7 +44,6 @@
last_disable_has_piece_time_ = monotonic_now;
}
-
// If we've had the piece for enough time, go to lower pump_voltage
low_pump_voltage = *has_piece && filtered_pressure_ < kVacuumOnThreshold;
@@ -64,7 +63,8 @@
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) {
+ } else if (unsafe_goal->grab_piece() &&
+ unsafe_goal->gamepiece_mode() == 1) {
output->intake_suction_top = true;
output->intake_suction_bottom = true;
} else {
diff --git a/y2019/image_streamer/flip_image.cc b/y2019/image_streamer/flip_image.cc
index a882223..3781d8c 100644
--- a/y2019/image_streamer/flip_image.cc
+++ b/y2019/image_streamer/flip_image.cc
@@ -1,4 +1,4 @@
-#include "flip_image.h"
+#include "y2019/image_streamer/flip_image.h"
#ifdef __clang__
// CImg has undefined behavior that Clang warns about. Just suppress the
diff --git a/y2019/image_streamer/image_streamer.cc b/y2019/image_streamer/image_streamer.cc
index 6640df0..cbb4788 100644
--- a/y2019/image_streamer/image_streamer.cc
+++ b/y2019/image_streamer/image_streamer.cc
@@ -1,26 +1,27 @@
-#include "aos/vision/image/image_stream.h"
-
#include <sys/stat.h>
+
#include <deque>
#include <fstream>
#include <string>
+#include "gflags/gflags.h"
+
#include "aos/logging/implementations.h"
#include "aos/logging/logging.h"
#include "aos/vision/blob/codec.h"
#include "aos/vision/events/socket_types.h"
#include "aos/vision/events/udp.h"
+#include "aos/vision/image/image_stream.h"
#include "aos/vision/image/reader.h"
-#include "gflags/gflags.h"
#include "y2019/image_streamer/flip_image.h"
#include "y2019/vision.pb.h"
+using ::aos::monotonic_clock;
using ::aos::events::DataSocket;
using ::aos::events::RXUdpSocket;
using ::aos::events::TCPServer;
using ::aos::vision::DataRef;
using ::aos::vision::Int32Codec;
-using ::aos::monotonic_clock;
using ::y2019::VisionControl;
DEFINE_string(roborio_ip, "10.9.71.2", "RoboRIO IP Address");
diff --git a/y2019/jevois/camera/image_stream.h b/y2019/jevois/camera/image_stream.h
index dc52bd5..f1ab0af 100644
--- a/y2019/jevois/camera/image_stream.h
+++ b/y2019/jevois/camera/image_stream.h
@@ -1,12 +1,12 @@
#ifndef Y2019_JEVOIS_CAMERA_IMAGE_STREAM_H_
#define Y2019_JEVOIS_CAMERA_IMAGE_STREAM_H_
+#include <memory>
+
#include "aos/vision/events/epoll_events.h"
#include "aos/vision/image/camera_params.pb.h"
#include "y2019/jevois/camera/reader.h"
-#include <memory>
-
namespace y2019 {
namespace camera {
diff --git a/y2019/jevois/camera/reader.cc b/y2019/jevois/camera/reader.cc
index 259e650..84bf7fe 100644
--- a/y2019/jevois/camera/reader.cc
+++ b/y2019/jevois/camera/reader.cc
@@ -13,9 +13,10 @@
#include <cstdlib>
#include <cstring>
-#include "aos/time/time.h"
#include "glog/logging.h"
+#include "aos/time/time.h"
+
#define CLEAR(x) memset(&(x), 0, sizeof(x))
namespace y2019 {
diff --git a/y2019/jevois/cobs_test.cc b/y2019/jevois/cobs_test.cc
index fa4742f..303c4d3 100644
--- a/y2019/jevois/cobs_test.cc
+++ b/y2019/jevois/cobs_test.cc
@@ -1,9 +1,10 @@
#include "y2019/jevois/cobs.h"
#include "absl/types/span.h"
-#include "aos/testing/test_logging.h"
#include "gtest/gtest.h"
+#include "aos/testing/test_logging.h"
+
namespace frc971 {
namespace jevois {
diff --git a/y2019/jevois/spi.h b/y2019/jevois/spi.h
index cdb0dfc..62e5ea7 100644
--- a/y2019/jevois/spi.h
+++ b/y2019/jevois/spi.h
@@ -6,6 +6,7 @@
#include <optional>
#include "absl/types/span.h"
+
#include "y2019/jevois/structures.h"
// This file manages serializing and deserializing the various structures for
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index 73705b1..85cca21 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -7,6 +7,7 @@
#include <cstdint>
#include "Eigen/Dense"
+
#include "aos/containers/sized_array.h"
#include "aos/time/time.h"
diff --git a/y2019/jevois/teensy.cc b/y2019/jevois/teensy.cc
index 95fe186..7c71ae8 100644
--- a/y2019/jevois/teensy.cc
+++ b/y2019/jevois/teensy.cc
@@ -3,6 +3,7 @@
#include <optional>
#include "absl/types/span.h"
+
#include "aos/time/time.h"
#include "motors/core/kinetis.h"
#include "motors/core/time.h"
diff --git a/y2019/jevois/uart.cc b/y2019/jevois/uart.cc
index 9cded62..0fc25c9 100644
--- a/y2019/jevois/uart.cc
+++ b/y2019/jevois/uart.cc
@@ -3,6 +3,7 @@
#include <array>
#include "absl/types/span.h"
+
#include "aos/util/bitpacking.h"
#include "y2019/jevois/jevois_crc.h"
#ifdef __linux__
diff --git a/y2019/jevois/uart.h b/y2019/jevois/uart.h
index 7a7f251..d3eebee 100644
--- a/y2019/jevois/uart.h
+++ b/y2019/jevois/uart.h
@@ -4,6 +4,7 @@
#include <optional>
#include "absl/types/span.h"
+
#include "aos/containers/sized_array.h"
#include "y2019/jevois/cobs.h"
#include "y2019/jevois/structures.h"
diff --git a/y2019/joystick_angle.cc b/y2019/joystick_angle.cc
index 90baf32..986a3fa 100644
--- a/y2019/joystick_angle.cc
+++ b/y2019/joystick_angle.cc
@@ -1,7 +1,8 @@
+#include "y2019/joystick_angle.h"
+
#include <cmath>
#include "frc971/zeroing/wrap.h"
-#include "y2019/joystick_angle.h"
namespace y2019 {
namespace input {
diff --git a/y2019/joystick_angle_test.cc b/y2019/joystick_angle_test.cc
index 7173b80..4d53ca0 100644
--- a/y2019/joystick_angle_test.cc
+++ b/y2019/joystick_angle_test.cc
@@ -2,9 +2,10 @@
#include <iostream>
-#include "frc971/input/driver_station_data.h"
#include "gtest/gtest.h"
+#include "frc971/input/driver_station_data.h"
+
using y2019::input::joysticks::GetJoystickPosition;
using y2019::input::joysticks::JoystickAngle;
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index ed55dfc..4e73f9e 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -5,13 +5,14 @@
#include <cstdio>
#include <cstring>
+#include "external/com_google_protobuf/src/google/protobuf/stubs/stringprintf.h"
+
#include "aos/actions/actions.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/network/team_number.h"
#include "aos/util/log_interval.h"
#include "aos/vision/events/udp.h"
-#include "external/com_google_protobuf/src/google/protobuf/stubs/stringprintf.h"
#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/input/action_joystick_input.h"
diff --git a/y2019/vision/constants.cc b/y2019/vision/constants.cc
index fabe75c..3fa99a7 100644
--- a/y2019/vision/constants.cc
+++ b/y2019/vision/constants.cc
@@ -7,7 +7,9 @@
CameraCalibration camera_1 = {
{
- -1.01208 / 180.0 * M_PI, 342.679, 1.79649 / 180.0 * M_PI,
+ -1.01208 / 180.0 * M_PI,
+ 342.679,
+ 1.79649 / 180.0 * M_PI,
},
{
{{-5.08996 * kInchesToMeters, 1.82468 * kInchesToMeters,
@@ -25,7 +27,9 @@
CameraCalibration camera_4 = {
{
- 3.73623 / 180.0 * M_PI, 588.1, 0.269291 / 180.0 * M_PI,
+ 3.73623 / 180.0 * M_PI,
+ 588.1,
+ 0.269291 / 180.0 * M_PI,
},
{
{{6.02674 * kInchesToMeters, 4.57805 * kInchesToMeters,
@@ -43,7 +47,9 @@
CameraCalibration camera_5 = {
{
- 1.00774 / 180.0 * M_PI, 658.554, 2.43864 / 180.0 * M_PI,
+ 1.00774 / 180.0 * M_PI,
+ 658.554,
+ 2.43864 / 180.0 * M_PI,
},
{
{{5.51248 * kInchesToMeters, 2.04087 * kInchesToMeters,
@@ -61,7 +67,9 @@
CameraCalibration camera_6 = {
{
- -1.15844 / 180.0 * M_PI, 348.161, 1.16894 / 180.0 * M_PI,
+ -1.15844 / 180.0 * M_PI,
+ 348.161,
+ 1.16894 / 180.0 * M_PI,
},
{
{{4.73183 * kInchesToMeters, 2.0984 * kInchesToMeters,
@@ -79,7 +87,9 @@
CameraCalibration camera_7 = {
{
- -2.24098 / 180.0 * M_PI, 339.231, 1.15487 / 180.0 * M_PI,
+ -2.24098 / 180.0 * M_PI,
+ 339.231,
+ 1.15487 / 180.0 * M_PI,
},
{
{{3.50224 * kInchesToMeters, 3.95441 * kInchesToMeters,
@@ -97,7 +107,9 @@
CameraCalibration camera_8 = {
{
- 37.1859 / 180.0 * M_PI, 339.517, 0.0405714 / 180.0 * M_PI,
+ 37.1859 / 180.0 * M_PI,
+ 339.517,
+ 0.0405714 / 180.0 * M_PI,
},
{
{{3.57002 * kInchesToMeters, 5.26966 * kInchesToMeters,
@@ -115,7 +127,9 @@
CameraCalibration camera_9 = {
{
- 35.4154 / 180.0 * M_PI, 337.471, 3.30546 / 180.0 * M_PI,
+ 35.4154 / 180.0 * M_PI,
+ 337.471,
+ 3.30546 / 180.0 * M_PI,
},
{
{{4.25679 * kInchesToMeters, -2.93066 * kInchesToMeters,
@@ -133,7 +147,9 @@
CameraCalibration camera_10 = {
{
- -0.305107 / 180.0 * M_PI, 336.952, -0.0804389 / 180.0 * M_PI,
+ -0.305107 / 180.0 * M_PI,
+ 336.952,
+ -0.0804389 / 180.0 * M_PI,
},
{
{{-5.64467 * kInchesToMeters, 2.41348 * kInchesToMeters,
@@ -151,7 +167,9 @@
CameraCalibration camera_14 = {
{
- 0.108434 / 180.0 * M_PI, 338.756, 0.606249 / 180.0 * M_PI,
+ 0.108434 / 180.0 * M_PI,
+ 338.756,
+ 0.606249 / 180.0 * M_PI,
},
{
{{5.90372 * kInchesToMeters, 2.08009 * kInchesToMeters,
@@ -169,7 +187,9 @@
CameraCalibration camera_15 = {
{
- -0.855459 / 180.0 * M_PI, 348.799, 1.4559 / 180.0 * M_PI,
+ -0.855459 / 180.0 * M_PI,
+ 348.799,
+ 1.4559 / 180.0 * M_PI,
},
{
{{3.15291 * kInchesToMeters, 4.16556 * kInchesToMeters,
@@ -187,7 +207,9 @@
CameraCalibration camera_16 = {
{
- -1.30906 / 180.0 * M_PI, 347.372, 2.18486 / 180.0 * M_PI,
+ -1.30906 / 180.0 * M_PI,
+ 347.372,
+ 2.18486 / 180.0 * M_PI,
},
{
{{4.98126 * kInchesToMeters, 1.96988 * kInchesToMeters,
@@ -205,7 +227,9 @@
CameraCalibration camera_17 = {
{
- 34.8231 / 180.0 * M_PI, 338.051, 2.43035 / 180.0 * M_PI,
+ 34.8231 / 180.0 * M_PI,
+ 338.051,
+ 2.43035 / 180.0 * M_PI,
},
{
{{3.17222 * kInchesToMeters, -2.49752 * kInchesToMeters,
@@ -223,7 +247,9 @@
CameraCalibration camera_18 = {
{
- 33.9761 / 180.0 * M_PI, 338.017, -2.32243 / 180.0 * M_PI,
+ 33.9761 / 180.0 * M_PI,
+ 338.017,
+ -2.32243 / 180.0 * M_PI,
},
{
{{3.95182 * kInchesToMeters, 5.50479 * kInchesToMeters,
@@ -241,7 +267,9 @@
CameraCalibration camera_19 = {
{
- -0.341036 / 180.0 * M_PI, 324.626, 1.2545 / 180.0 * M_PI,
+ -0.341036 / 180.0 * M_PI,
+ 324.626,
+ 1.2545 / 180.0 * M_PI,
},
{
{{-6.93309 * kInchesToMeters, 2.64735 * kInchesToMeters,
diff --git a/y2019/vision/constants_formatting.cc b/y2019/vision/constants_formatting.cc
index a2922a0..ebda53f 100644
--- a/y2019/vision/constants_formatting.cc
+++ b/y2019/vision/constants_formatting.cc
@@ -1,8 +1,8 @@
-#include "y2019/vision/constants.h"
-
#include <fstream>
#include <sstream>
+#include "y2019/vision/constants.h"
+
namespace y2019 {
namespace vision {
@@ -31,8 +31,9 @@
}
void IntrinsicParams::Dump(std::basic_ostream<char> *o) const {
- *o << " {\n " << fmt_rad(mount_angle) << ", " << focal_length;
- *o << ", " << fmt_rad(barrel_mount) << ",\n },\n";
+ *o << " {\n " << fmt_rad(mount_angle) << ",\n "
+ << focal_length;
+ *o << ",\n " << fmt_rad(barrel_mount) << ",\n },\n";
}
void CameraGeometry::Dump(std::basic_ostream<char> *o) const {
diff --git a/y2019/vision/constants_formatting_main.cc b/y2019/vision/constants_formatting_main.cc
index fd0bb22..b8f4503 100644
--- a/y2019/vision/constants_formatting_main.cc
+++ b/y2019/vision/constants_formatting_main.cc
@@ -1,6 +1,7 @@
-#include "y2019/vision/constants.h"
#include <iostream>
+#include "y2019/vision/constants.h"
+
int main(int argc, char *argv[]) {
if (argc != 2) {
::std::cout << "Expected a command line argument specifying the file name "
diff --git a/y2019/vision/debug_serial.cc b/y2019/vision/debug_serial.cc
index a658bd7..9b88260 100644
--- a/y2019/vision/debug_serial.cc
+++ b/y2019/vision/debug_serial.cc
@@ -1,4 +1,10 @@
-#include "y2019/jevois/serial.h"
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <chrono>
+#include <fstream>
+#include <iostream>
+#include <thread>
#include "aos/logging/implementations.h"
#include "aos/logging/logging.h"
@@ -7,13 +13,6 @@
#include "y2019/jevois/structures.h"
#include "y2019/jevois/uart.h"
-#include <fcntl.h>
-#include <unistd.h>
-#include <chrono>
-#include <fstream>
-#include <iostream>
-#include <thread>
-
namespace y2019 {
namespace vision {
@@ -86,7 +85,7 @@
}
}
-} // namespace y2019
} // namespace vision
+} // namespace y2019
int main(int argc, char **argv) { y2019::vision::main(argc, argv); }
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index e98f018..b2ad1be 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -1,23 +1,23 @@
-#include <Eigen/Dense>
#include <iostream>
-#include "y2019/vision/target_finder.h"
+#include "gflags/gflags.h"
+#include <Eigen/Dense>
#include "aos/vision/blob/move_scale.h"
#include "aos/vision/blob/stream_view.h"
#include "aos/vision/blob/transpose.h"
#include "aos/vision/debug/debug_framework.h"
#include "aos/vision/math/vector.h"
-#include "gflags/gflags.h"
+#include "y2019/vision/target_finder.h"
-using aos::vision::ImageRange;
-using aos::vision::ImageFormat;
-using aos::vision::RangeImage;
using aos::vision::AnalysisAllocator;
using aos::vision::BlobList;
-using aos::vision::Vector;
-using aos::vision::Segment;
+using aos::vision::ImageFormat;
+using aos::vision::ImageRange;
using aos::vision::PixelRef;
+using aos::vision::RangeImage;
+using aos::vision::Segment;
+using aos::vision::Vector;
DEFINE_int32(camera, 10, "The camera to use the intrinsics for");
@@ -56,9 +56,10 @@
class FilterHarness : public aos::vision::FilterHarness {
public:
- FilterHarness() {
- *(target_finder_.mutable_intrinsics()) = GetCamera(FLAGS_camera)->intrinsics;
- }
+ FilterHarness() {
+ *(target_finder_.mutable_intrinsics()) =
+ GetCamera(FLAGS_camera)->intrinsics;
+ }
aos::vision::RangeImage Threshold(aos::vision::ImagePtr image) override {
return target_finder_.Threshold(image);
}
@@ -228,12 +229,16 @@
} else if (key == 'h') {
printf("Key Mappings:\n");
printf(" z: Toggle drawing final target pose.\n");
- printf(" x: Toggle drawing re-projected targets and print solver results.\n");
+ printf(
+ " x: Toggle drawing re-projected targets and print solver "
+ "results.\n");
printf(" c: Toggle drawing proposed target groupings.\n");
printf(" v: Toggle drawing ordered target components.\n");
printf(" b: Toggle drawing proposed target components.\n");
printf(" n: Toggle drawing countours before and after warping.\n");
- printf(" m: Toggle drawing raw blob data (may need to change image to toggle a redraw).\n");
+ printf(
+ " m: Toggle drawing raw blob data (may need to change image to "
+ "toggle a redraw).\n");
printf(" h: Print this message.\n");
printf(" a: May log camera image to /tmp/debug_viewer_jpeg_<#>.yuyv\n");
printf(" q: Exit the application.\n");
@@ -354,7 +359,7 @@
};
} // namespace vision
-} // namespace y2017
+} // namespace y2019
int main(int argc, char **argv) {
::gflags::ParseCommandLineFlags(&argc, &argv, true);
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index e1e5ca8..b8f5b3c 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -21,14 +21,14 @@
"Path to the constants file to update");
DEFINE_double(beginning_tape_measure_reading, 11,
- "The tape measure measurement (in inches) of the first image.");
+ "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_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,
@@ -37,20 +37,20 @@
tape_direction_y, 0.0,
"The y component of \"1\" inch along the tape measure in meters.");
+using ::aos::monotonic_clock;
using ::aos::events::DataSocket;
using ::aos::events::RXUdpSocket;
using ::aos::events::TCPServer;
using ::aos::vision::DataRef;
using ::aos::vision::Int32Codec;
-using ::aos::monotonic_clock;
using aos::vision::Segment;
-using ceres::NumericDiffCostFunction;
using ceres::CENTRAL;
using ceres::CostFunction;
+using ceres::NumericDiffCostFunction;
using ceres::Problem;
-using ceres::Solver;
using ceres::Solve;
+using ceres::Solver;
namespace y2019 {
namespace vision {
@@ -195,8 +195,8 @@
// Now build up the residual block.
auto ftor = [template_point, target_point](
- const double *const intrinsics, const double *const extrinsics,
- double *residual) {
+ const double *const intrinsics,
+ const double *const extrinsics, double *residual) {
const IntrinsicParams intrinsic_params =
IntrinsicParams::get(intrinsics);
const ExtrinsicParams extrinsic_params =
@@ -293,7 +293,7 @@
DumpCameraConstants(FLAGS_constants.c_str(), info.camera_id, results);
}
-} // namespace y2019
} // namespace vision
+} // namespace y2019
int main(int argc, char **argv) { y2019::vision::main(argc, argv); }
diff --git a/y2019/vision/image_writer.cc b/y2019/vision/image_writer.cc
index 51e2c60..4ca9b96 100644
--- a/y2019/vision/image_writer.cc
+++ b/y2019/vision/image_writer.cc
@@ -1,14 +1,16 @@
-#include <fstream>
+#include "y2019/vision/image_writer.h"
+
#include <sys/stat.h>
+#include <fstream>
+
#include "glog/logging.h"
-#include "y2019/vision/image_writer.h"
namespace y2019 {
namespace vision {
ImageWriter::ImageWriter() {
- LOG(INFO) << "Initializing image writer";
+ LOG(INFO) << "Initializing image writer";
SetDirPath();
}
diff --git a/y2019/vision/image_writer.h b/y2019/vision/image_writer.h
index f33cca7..1192368 100644
--- a/y2019/vision/image_writer.h
+++ b/y2019/vision/image_writer.h
@@ -24,6 +24,6 @@
};
} // namespace vision
-} // namespace y2017
+} // namespace y2019
#endif // Y2019_VISION_IMAGE_WRITER_H_
diff --git a/y2019/vision/serial_waiter.cc b/y2019/vision/serial_waiter.cc
index 551e179..7bffa0d 100644
--- a/y2019/vision/serial_waiter.cc
+++ b/y2019/vision/serial_waiter.cc
@@ -2,8 +2,8 @@
#include <chrono>
-#include "y2019/jevois/serial.h"
#include "aos/time/time.h"
+#include "y2019/jevois/serial.h"
using ::aos::monotonic_clock;
using ::y2019::jevois::open_via_terminos;
diff --git a/y2019/vision/server/server.cc b/y2019/vision/server/server.cc
index 817da17..8378f77 100644
--- a/y2019/vision/server/server.cc
+++ b/y2019/vision/server/server.cc
@@ -1,12 +1,17 @@
+#include "seasocks/Server.h"
+
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
+
#include <array>
#include <cmath>
#include <memory>
#include <set>
#include <sstream>
+#include "google/protobuf/util/json_util.h"
+
#include "aos/containers/ring_buffer.h"
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
@@ -15,9 +20,7 @@
#include "aos/time/time.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/pose.h"
-#include "google/protobuf/util/json_util.h"
#include "internal/Embedded.h"
-#include "seasocks/Server.h"
#include "seasocks/StringUtil.h"
#include "seasocks/WebSocket.h"
#include "y2019/constants.h"
@@ -157,123 +160,122 @@
DebugData debug_data;
::aos::RingBuffer<DrivetrainPosition, 200> drivetrain_log;
- event_loop.MakeWatcher(
- "/camera", [websocket_handler, server, &latest_frames, &last_target_time,
- &drivetrain_status_fetcher, &superstructure_status_fetcher,
- &last_send_time, &drivetrain_log, &debug_data](
- const ::y2019::control_loops::drivetrain::CameraFrame
- &camera_frames) {
- while (drivetrain_status_fetcher.FetchNext()) {
- DrivetrainPosition drivetrain_position{
- drivetrain_status_fetcher.context().monotonic_event_time,
- drivetrain_status_fetcher->x(), drivetrain_status_fetcher->y(),
- drivetrain_status_fetcher->theta()};
+ event_loop.MakeWatcher("/camera", [websocket_handler, server, &latest_frames,
+ &last_target_time,
+ &drivetrain_status_fetcher,
+ &superstructure_status_fetcher,
+ &last_send_time, &drivetrain_log,
+ &debug_data](const ::y2019::control_loops::
+ drivetrain::CameraFrame
+ &camera_frames) {
+ while (drivetrain_status_fetcher.FetchNext()) {
+ DrivetrainPosition drivetrain_position{
+ drivetrain_status_fetcher.context().monotonic_event_time,
+ drivetrain_status_fetcher->x(), drivetrain_status_fetcher->y(),
+ drivetrain_status_fetcher->theta()};
- drivetrain_log.Push(drivetrain_position);
+ drivetrain_log.Push(drivetrain_position);
+ }
+ superstructure_status_fetcher.Fetch();
+ if (!drivetrain_status_fetcher.get() ||
+ !superstructure_status_fetcher.get()) {
+ // Try again if we don't have any drivetrain statuses.
+ return;
+ }
+ const auto now = aos::monotonic_clock::now();
+
+ {
+ const auto &new_frame = camera_frames;
+ // TODO(james): Maybe we only want to fill out a new frame if it has
+ // targets or the saved target is > 0.1 sec old? Not sure, but for now
+ if (new_frame.camera() < latest_frames.size()) {
+ latest_frames[new_frame.camera()].capture_time =
+ aos::monotonic_clock::time_point(
+ chrono::nanoseconds(new_frame.timestamp()));
+ latest_frames[new_frame.camera()].targets.clear();
+ if (new_frame.has_targets() && new_frame.targets()->size() > 0) {
+ last_target_time[new_frame.camera()] =
+ latest_frames[new_frame.camera()].capture_time;
}
- superstructure_status_fetcher.Fetch();
- if (!drivetrain_status_fetcher.get() ||
- !superstructure_status_fetcher.get()) {
- // Try again if we don't have any drivetrain statuses.
- return;
+ for (const control_loops::drivetrain::CameraTarget *target :
+ *new_frame.targets()) {
+ latest_frames[new_frame.camera()].targets.emplace_back();
+ const float heading = target->heading();
+ const float distance = target->distance();
+ latest_frames[new_frame.camera()].targets.back().x =
+ ::std::cos(heading) * distance;
+ latest_frames[new_frame.camera()].targets.back().y =
+ ::std::sin(heading) * distance;
+ latest_frames[new_frame.camera()].targets.back().theta =
+ target->skew();
}
- const auto now = aos::monotonic_clock::now();
+ }
+ }
- {
- const auto &new_frame = camera_frames;
- // TODO(james): Maybe we only want to fill out a new frame if it has
- // targets or the saved target is > 0.1 sec old? Not sure, but for now
- if (new_frame.camera() < latest_frames.size()) {
- latest_frames[new_frame.camera()].capture_time =
- aos::monotonic_clock::time_point(
- chrono::nanoseconds(new_frame.timestamp()));
- latest_frames[new_frame.camera()].targets.clear();
- if (new_frame.has_targets() && new_frame.targets()->size() > 0) {
- last_target_time[new_frame.camera()] =
- latest_frames[new_frame.camera()].capture_time;
- }
- for (const control_loops::drivetrain::CameraTarget *target :
- *new_frame.targets()) {
- latest_frames[new_frame.camera()].targets.emplace_back();
- const float heading = target->heading();
- const float distance = target->distance();
- latest_frames[new_frame.camera()].targets.back().x =
- ::std::cos(heading) * distance;
- latest_frames[new_frame.camera()].targets.back().y =
- ::std::sin(heading) * distance;
- latest_frames[new_frame.camera()].targets.back().theta =
- target->skew();
- }
- }
- }
+ for (size_t ii = 0; ii < latest_frames.size(); ++ii) {
+ CameraDebug *camera_debug = debug_data.add_camera_debug();
+ LocalCameraFrame cur_frame = latest_frames[ii];
+ constants::Values::CameraCalibration camera_info =
+ constants::GetValues().cameras[ii];
+ frc971::control_loops::TypedPose<double> camera_pose = camera_info.pose;
- for (size_t ii = 0; ii < latest_frames.size(); ++ii) {
- CameraDebug *camera_debug = debug_data.add_camera_debug();
- LocalCameraFrame cur_frame = latest_frames[ii];
- constants::Values::CameraCalibration camera_info =
- constants::GetValues().cameras[ii];
- frc971::control_loops::TypedPose<double> camera_pose =
- camera_info.pose;
+ const DrivetrainPosition robot_position =
+ ComputePosition(drivetrain_log, cur_frame.capture_time);
+ const ::frc971::control_loops::TypedPose<double> robot_pose(
+ {robot_position.x, robot_position.y, 0}, robot_position.theta);
- const DrivetrainPosition robot_position =
- ComputePosition(drivetrain_log, cur_frame.capture_time);
- const ::frc971::control_loops::TypedPose<double> robot_pose(
- {robot_position.x, robot_position.y, 0}, robot_position.theta);
+ camera_pose.set_base(&robot_pose);
- camera_pose.set_base(&robot_pose);
+ camera_debug->set_current_frame_age(
+ ::aos::time::DurationInSeconds(now - cur_frame.capture_time));
+ camera_debug->set_time_since_last_target(
+ ::aos::time::DurationInSeconds(now - last_target_time[ii]));
+ for (const auto &target : cur_frame.targets) {
+ frc971::control_loops::TypedPose<double> target_pose(
+ &camera_pose, {target.x, target.y, 0}, target.theta);
+ Pose *pose = camera_debug->add_targets();
+ pose->set_x(target_pose.abs_pos().x());
+ pose->set_y(target_pose.abs_pos().y());
+ pose->set_theta(target_pose.abs_theta());
+ }
+ }
- camera_debug->set_current_frame_age(
- ::aos::time::DurationInSeconds(now - cur_frame.capture_time));
- camera_debug->set_time_since_last_target(
- ::aos::time::DurationInSeconds(now - last_target_time[ii]));
- for (const auto &target : cur_frame.targets) {
- frc971::control_loops::TypedPose<double> target_pose(
- &camera_pose, {target.x, target.y, 0}, target.theta);
- Pose *pose = camera_debug->add_targets();
- pose->set_x(target_pose.abs_pos().x());
- pose->set_y(target_pose.abs_pos().y());
- pose->set_theta(target_pose.abs_theta());
- }
- }
+ if (now > last_send_time + chrono::milliseconds(100)) {
+ last_send_time = now;
+ debug_data.mutable_robot_pose()->set_x(drivetrain_status_fetcher->x());
+ debug_data.mutable_robot_pose()->set_y(drivetrain_status_fetcher->y());
+ debug_data.mutable_robot_pose()->set_theta(
+ drivetrain_status_fetcher->theta());
+ {
+ LineFollowDebug *line_debug = debug_data.mutable_line_follow_debug();
+ line_debug->set_frozen(
+ drivetrain_status_fetcher->line_follow_logging()->frozen());
+ line_debug->set_have_target(
+ drivetrain_status_fetcher->line_follow_logging()->have_target());
+ line_debug->mutable_goal_target()->set_x(
+ drivetrain_status_fetcher->line_follow_logging()->x());
+ line_debug->mutable_goal_target()->set_y(
+ drivetrain_status_fetcher->line_follow_logging()->y());
+ line_debug->mutable_goal_target()->set_theta(
+ drivetrain_status_fetcher->line_follow_logging()->theta());
+ }
- if (now > last_send_time + chrono::milliseconds(100)) {
- last_send_time = now;
- debug_data.mutable_robot_pose()->set_x(drivetrain_status_fetcher->x());
- debug_data.mutable_robot_pose()->set_y(drivetrain_status_fetcher->y());
- debug_data.mutable_robot_pose()->set_theta(
- drivetrain_status_fetcher->theta());
- {
- LineFollowDebug *line_debug =
- debug_data.mutable_line_follow_debug();
- line_debug->set_frozen(
- drivetrain_status_fetcher->line_follow_logging()->frozen());
- line_debug->set_have_target(
- drivetrain_status_fetcher->line_follow_logging()->have_target());
- line_debug->mutable_goal_target()->set_x(
- drivetrain_status_fetcher->line_follow_logging()->x());
- line_debug->mutable_goal_target()->set_y(
- drivetrain_status_fetcher->line_follow_logging()->y());
- line_debug->mutable_goal_target()->set_theta(
- drivetrain_status_fetcher->line_follow_logging()->theta());
- }
+ Sensors *sensors = debug_data.mutable_sensors();
+ sensors->set_wrist(superstructure_status_fetcher->wrist()->position());
+ sensors->set_elevator(
+ superstructure_status_fetcher->elevator()->position());
+ sensors->set_intake(superstructure_status_fetcher->intake()->position());
+ sensors->set_stilts(superstructure_status_fetcher->stilts()->position());
+ sensors->set_has_piece(superstructure_status_fetcher->has_piece());
- Sensors *sensors = debug_data.mutable_sensors();
- sensors->set_wrist(
- superstructure_status_fetcher->wrist()->position());
- sensors->set_elevator(
- superstructure_status_fetcher->elevator()->position());
- sensors->set_intake(superstructure_status_fetcher->intake()->position());
- sensors->set_stilts(superstructure_status_fetcher->stilts()->position());
- sensors->set_has_piece(superstructure_status_fetcher->has_piece());
+ ::std::string json;
+ google::protobuf::util::MessageToJsonString(debug_data, &json);
+ server->execute(
+ std::make_shared<UpdateData>(websocket_handler, ::std::move(json)));
- ::std::string json;
- google::protobuf::util::MessageToJsonString(debug_data, &json);
- server->execute(std::make_shared<UpdateData>(websocket_handler,
- ::std::move(json)));
-
- debug_data.Clear();
- }
- });
+ debug_data.Clear();
+ }
+ });
event_loop.Run();
}
@@ -305,9 +307,8 @@
}
}
- server.serve(
- serve_www ? "/home/admin/bin/www" : "y2019/vision/server/www",
- 1180);
+ server.serve(serve_www ? "/home/admin/bin/www" : "y2019/vision/server/www",
+ 1180);
return 0;
}
diff --git a/y2019/vision/server/www/generate_camera.cc b/y2019/vision/server/www/generate_camera.cc
index 757ce69..455b0a9 100644
--- a/y2019/vision/server/www/generate_camera.cc
+++ b/y2019/vision/server/www/generate_camera.cc
@@ -1,14 +1,14 @@
-#include "y2019/constants.h"
-#include "y2019/vision/constants.h"
-
#include <fstream>
#include <iostream>
+#include "y2019/constants.h"
+#include "y2019/vision/constants.h"
+
namespace y2019 {
namespace vision {
void DumpPose(std::basic_ostream<char> *o, const vision::CameraGeometry &pose) {
*o << "{x: " << pose.location[0] << ", y: " << pose.location[1]
- << ", theta: " << pose.heading << "}";
+ << ", theta: " << pose.heading << "}";
}
void DumpTypescriptConstants(const char *fname) {
::std::ofstream out_file(fname);
@@ -23,7 +23,7 @@
}
out_file << "];\n";
}
-} // namespace constants
+} // namespace vision
} // namespace y2019
int main(int argc, char *argv[]) {
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index b16fba1..b59792c 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -1,8 +1,9 @@
#include "y2019/vision/target_finder.h"
-#include "aos/vision/blob/hierarchical_contour_merge.h"
#include "ceres/ceres.h"
+#include "aos/vision/blob/hierarchical_contour_merge.h"
+
using namespace aos::vision;
namespace y2019 {
@@ -75,7 +76,7 @@
constexpr int iterations = 7;
-}
+} // namespace
::Eigen::Vector2f UnWarpPoint(const Point point) {
const double x0 = ((double)point.x - c_x) / f_x;
@@ -566,8 +567,9 @@
// Sort the target list so that the widest (ie closest) target is first.
sort(filtered.begin(), filtered.end(),
- [](const IntermediateResult &a, const IntermediateResult &b)
- -> bool { return a.target_width > b.target_width; });
+ [](const IntermediateResult &a, const IntermediateResult &b) -> bool {
+ return a.target_width > b.target_width;
+ });
frame_count_++;
if (!filtered.empty()) {
@@ -588,8 +590,8 @@
int pixel_count, int *desired_exposure) {
// TODO(ben): Add these values to config file.
constexpr double low_dist = 0.8;
- constexpr int low_exposure = 60;
- constexpr int mid_exposure = 200;
+ constexpr int low_exposure = 60;
+ constexpr int mid_exposure = 200;
bool needs_update = false;
if (results.size() > 0) {
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index d54dc2d..fdcfbde 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -19,11 +19,11 @@
namespace y2019 {
namespace vision {
+using aos::vision::BlobList;
+using aos::vision::ContourNode;
using aos::vision::ImageRange;
using aos::vision::RangeImage;
-using aos::vision::BlobList;
using aos::vision::Vector;
-using aos::vision::ContourNode;
struct Polygon {
::std::vector<aos::vision::Segment<2>> segments;
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index 86ead34..e562f49 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -1,7 +1,8 @@
#include <cmath>
-#include "aos/util/math.h"
#include "ceres/ceres.h"
+
+#include "aos/util/math.h"
#include "y2019/vision/target_finder.h"
using ceres::CENTRAL;
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
index 990debc..5019646 100644
--- a/y2019/vision/target_types.h
+++ b/y2019/vision/target_types.h
@@ -94,13 +94,13 @@
// Projects a point from idealized template space to camera space.
template <typename Extrinsics>
aos::vision::Vector<2> Project(aos::vision::Vector<2> pt,
- const IntrinsicParams &intrinsics,
- const Extrinsics &extrinsics);
+ const IntrinsicParams &intrinsics,
+ const Extrinsics &extrinsics);
template <typename T, typename Extrinsics>
::Eigen::Matrix<T, 2, 1> Project(::Eigen::Matrix<T, 2, 1> pt,
- const IntrinsicParams &intrinsics,
- const Extrinsics &extrinsics);
+ const IntrinsicParams &intrinsics,
+ const Extrinsics &extrinsics);
Target Project(const Target &target, const IntrinsicParams &intrinsics,
const ExtrinsicParams &extrinsics);
@@ -146,7 +146,7 @@
float skew;
};
-template<typename T>
+template <typename T>
::Eigen::Matrix<T, 2, 1> ToEigenMatrix(aos::vision::Vector<2> pt) {
return (::Eigen::Matrix<T, 2, 1>() << T(pt.x()), T(pt.y())).finished();
}
@@ -163,8 +163,8 @@
template <typename T, typename Extrinsics>
::Eigen::Matrix<T, 2, 1> Project(::Eigen::Matrix<T, 2, 1> pt,
- const IntrinsicParams &intrinsics,
- const Extrinsics &extrinsics) {
+ const IntrinsicParams &intrinsics,
+ const Extrinsics &extrinsics) {
const T y = extrinsics.y; // height
const T z = extrinsics.z; // distance
const T r1 = extrinsics.r1; // skew
@@ -184,7 +184,8 @@
const T s = sin(theta);
const T c = cos(theta);
pts = (::Eigen::Matrix<T, 3, 3>() << c, T(0), -s, T(0), T(1), T(0), s, T(0),
- c).finished() *
+ c)
+ .finished() *
pts;
}
@@ -199,7 +200,8 @@
const T s = sin(theta);
const T c = cos(theta);
pts = (::Eigen::Matrix<T, 3, 3>() << c, T(0), -s, T(0), T(1), T(0), s, T(0),
- c).finished() *
+ c)
+ .finished() *
pts;
}
@@ -212,7 +214,8 @@
const T c = T(cos(theta));
pts = (::Eigen::Matrix<T, 3, 3>() << T(1), T(0), T(0), T(0), c, -s, T(0), s,
- c).finished() *
+ c)
+ .finished() *
pts;
}
@@ -233,7 +236,7 @@
// pixel-space.
const T scale = fl / res.z();
return ::Eigen::Matrix<T, 2, 1>(res.x() * scale + 320.0,
- 240.0 - res.y() * scale);
+ 240.0 - res.y() * scale);
}
} // namespace vision
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index ace17fb..e918aaf 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -12,6 +12,7 @@
#include <thread>
#include "ctre/phoenix/CANifier.h"
+
#include "frc971/wpilib/ahal/AnalogInput.h"
#include "frc971/wpilib/ahal/Counter.h"
#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
@@ -20,6 +21,8 @@
#include "frc971/wpilib/ahal/VictorSP.h"
#undef ERROR
+#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
+
#include "aos/commonmath.h"
#include "aos/events/event_loop.h"
#include "aos/events/shm_event_loop.h"
@@ -30,7 +33,6 @@
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
-#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
#include "frc971/autonomous/auto_mode_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/input/robot_state_generated.h"