Merge "Use automatic differentiation instead of numeric"
diff --git a/aos/input/action_joystick_input.cc b/aos/input/action_joystick_input.cc
index 81e277d..061f4dc 100644
--- a/aos/input/action_joystick_input.cc
+++ b/aos/input/action_joystick_input.cc
@@ -43,7 +43,8 @@
void ActionJoystickInput::StartAuto() {
LOG(INFO, "Starting auto mode\n");
- action_queue_.EnqueueAction(::frc971::autonomous::MakeAutonomousAction(0));
+ action_queue_.EnqueueAction(
+ ::frc971::autonomous::MakeAutonomousAction(GetAutonomousMode()));
}
void ActionJoystickInput::StopAuto() {
diff --git a/aos/input/action_joystick_input.h b/aos/input/action_joystick_input.h
index 87b980e..dca4d39 100644
--- a/aos/input/action_joystick_input.h
+++ b/aos/input/action_joystick_input.h
@@ -40,6 +40,10 @@
void StartAuto();
void StopAuto();
+ // Returns the current autonomous mode which has been selected by robot
+ // inputs.
+ virtual uint32_t GetAutonomousMode() { return 0; }
+
// True if the internal state machine thinks auto is running right now.
bool auto_running_ = false;
// True if an action was running last cycle.
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index d4c765f..05889e4 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -106,6 +106,13 @@
drivetrain_motor_plant_.GetRightPosition(), 1e-3);
}
+ void VerifyNearPosition(double x, double y) {
+ my_drivetrain_queue_.status.FetchLatest();
+ auto actual = drivetrain_motor_plant_.GetPosition();
+ EXPECT_NEAR(actual(0), x, 1e-2);
+ EXPECT_NEAR(actual(1), y, 1e-2);
+ }
+
void VerifyNearSplineGoal() {
my_drivetrain_queue_.status.FetchLatest();
double expected_x = my_drivetrain_queue_.status->trajectory_logging.x;
@@ -720,6 +727,29 @@
VerifyNearSplineGoal();
}
+//Tests that a trajectory can be executed after it is planned.
+TEST_F(DrivetrainTest, SplineExecuteAfterPlan) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ goal.Send();
+ WaitForTrajectoryPlan();
+ RunForTime(chrono::milliseconds(2000));
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ start_goal = my_drivetrain_queue_.goal.MakeMessage();
+ start_goal->controller_type = 2;
+ start_goal->spline_handle = 1;
+ start_goal.Send();
+ RunForTime(chrono::milliseconds(2000));
+
+ VerifyNearPosition(1.0, 1.0);
+}
+
// The LineFollowDrivetrain logic is tested in line_follow_drivetrain_test. This
// tests that the integration with drivetrain_lib worked properly.
TEST_F(DrivetrainTest, BasicLineFollow) {
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 0b6026d..a37ca0e 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -93,7 +93,7 @@
const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
current_spline_handle_ = goal.spline_handle;
// If told to stop, set the executing spline to an invalid index.
- if (current_spline_handle_ == 0) {
+ if (current_spline_handle_ == 0 && has_started_execution_) {
current_spline_idx_ = -1;
}
@@ -119,6 +119,7 @@
// Reset internal state to a trajectory start position.
current_xva_ = current_trajectory_->FFAcceleration(0);
current_xva_(1) = 0.0;
+ has_started_execution_ = false;
}
mutex_.Unlock();
}
@@ -133,6 +134,7 @@
::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
if (!IsAtEnd() &&
current_spline_handle_ == current_spline_idx_) {
+ has_started_execution_ = true;
// TODO(alex): It takes about a cycle for the outputs to propagate to the
// motors. Consider delaying the output by a cycle.
U_ff = current_trajectory_->FFVoltage(current_xva_(0));
@@ -187,7 +189,7 @@
status->trajectory_logging.right_velocity = goal_state(4);
}
status->trajectory_logging.planning_state = static_cast<int8_t>(plan_state_.load());
- status->trajectory_logging.is_executing = !IsAtEnd();
+ status->trajectory_logging.is_executing = !IsAtEnd() && has_started_execution_;
status->trajectory_logging.goal_spline_handle = current_spline_handle_;
status->trajectory_logging.current_spline_idx = current_spline_idx_;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 3e47f5a..d70cb6d 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -70,6 +70,7 @@
int32_t current_spline_handle_ = 0; // Current spline told to excecute.
int32_t current_spline_idx_ = 0; // Current executing spline.
+ bool has_started_execution_ = false;
::std::unique_ptr<DistanceSpline> current_distance_spline_;
::std::unique_ptr<Trajectory> current_trajectory_;
diff --git a/frc971/wpilib/ADIS16448.h b/frc971/wpilib/ADIS16448.h
index 1846723..49c4fb2 100644
--- a/frc971/wpilib/ADIS16448.h
+++ b/frc971/wpilib/ADIS16448.h
@@ -45,6 +45,8 @@
// readings.
void operator()();
+ // Sets a function to be called immediately after each time this class uses
+ // the SPI bus. This is a good place to do other things on the bus.
void set_spi_idle_callback(std::function<void()> spi_idle_callback) {
spi_idle_callback_ = std::move(spi_idle_callback);
}
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index 1647f04..41a70fd 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -27,10 +27,6 @@
.count();
}
-constexpr bool is_left = false;
-
-constexpr double turn_scalar = is_left ? 1.0 : -1.0;
-
} // namespace
AutonomousActor::AutonomousActor(
@@ -38,7 +34,8 @@
: frc971::autonomous::BaseAutonomousActor(
s, control_loops::drivetrain::GetDrivetrainConfig()) {}
-void AutonomousActor::Reset() {
+void AutonomousActor::Reset(bool is_left) {
+ const double turn_scalar = is_left ? 1.0 : -1.0;
elevator_goal_ = 0.01;
wrist_goal_ = -M_PI / 2.0;
intake_goal_ = -1.2;
@@ -86,8 +83,15 @@
bool AutonomousActor::RunAction(
const ::frc971::autonomous::AutonomousActionParams ¶ms) {
monotonic_clock::time_point start_time = monotonic_clock::now();
- LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
- Reset();
+ const bool is_left = params.mode == 0;
+
+ {
+ LOG(INFO, "Starting autonomous action with mode %" PRId32 " %s\n",
+ params.mode, is_left ? "left" : "right");
+ }
+ const double turn_scalar = is_left ? 1.0 : -1.0;
+
+ Reset(is_left);
// Grab the disk, wait until we have vacuum, then jump
set_elevator_goal(0.01);
diff --git a/y2019/actors/autonomous_actor.h b/y2019/actors/autonomous_actor.h
index 82e1aeb..38db070 100644
--- a/y2019/actors/autonomous_actor.h
+++ b/y2019/actors/autonomous_actor.h
@@ -26,7 +26,7 @@
const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
private:
- void Reset();
+ void Reset(bool is_left);
double elevator_goal_ = 0.0;
double wrist_goal_ = 0.0;
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 7406488..89fc009 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -178,10 +178,10 @@
break;
case kCompTeamNumber:
- elevator_params->zeroing_constants.measured_absolute_position = 0.160736;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.152217;
elevator->potentiometer_offset =
-0.075017 + 0.015837 + 0.009793 - 0.012017 + 0.019915 + 0.010126 +
- 0.005541 + 0.006088 - 0.039687 + 0.005843;
+ 0.005541 + 0.006088 - 0.039687 + 0.005843 + 0.009007;
intake->zeroing_constants.measured_absolute_position = 1.860016;
diff --git a/y2019/image_streamer/flip_image.cc b/y2019/image_streamer/flip_image.cc
index 2db20b2..48a18c5 100644
--- a/y2019/image_streamer/flip_image.cc
+++ b/y2019/image_streamer/flip_image.cc
@@ -10,7 +10,9 @@
::cimg_library::CImg<unsigned char> image;
image.load_jpeg_buffer((JOCTET *)(input), input_size);
if (flip) {
- image.mirror("xy");
+ image.rotate(90);
+ } else {
+ image.rotate(270);
}
image.save_jpeg_buffer(buffer, *buffer_size, 80);
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index 9c74b11..fcf1ebe 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -144,6 +144,8 @@
// Send As, which triggers the bootstrap script to drop directly into USB
// mode.
kAs,
+ // Log camera images
+ kLog,
};
// This is all the information sent from the Teensy to each camera.
diff --git a/y2019/jevois/teensy.cc b/y2019/jevois/teensy.cc
index 570892f..58d1570 100644
--- a/y2019/jevois/teensy.cc
+++ b/y2019/jevois/teensy.cc
@@ -308,6 +308,8 @@
last_frames_.clear();
}
+ bool HaveLatestFrames() const { return !last_frames_.empty(); }
+
private:
struct FrameData {
aos::SizedArray<Target, 3> targets;
@@ -695,19 +697,26 @@
}
{
bool made_transfer = false;
- if (!first) {
+ const bool have_old_frames = frame_queue.HaveLatestFrames();
+ {
+ const auto new_transfer = frame_queue.MakeTransfer();
DisableInterrupts disable_interrupts;
- made_transfer =
- !SpiQueue::global_instance->HaveTransfer(disable_interrupts);
+ if (!first) {
+ made_transfer =
+ !SpiQueue::global_instance->HaveTransfer(disable_interrupts);
+ }
+ // If we made a transfer just now, then new_transfer might contain
+ // duplicate targets, in which case don't use it.
+ if (!have_old_frames || !made_transfer) {
+ SpiQueue::global_instance->UpdateTransfer(new_transfer,
+ disable_interrupts);
+ }
}
+ // If we made a transfer, then make sure we aren't remembering any
+ // in-flight frames.
if (made_transfer) {
frame_queue.RemoveLatestFrames();
}
- const auto transfer = frame_queue.MakeTransfer();
- {
- DisableInterrupts disable_interrupts;
- SpiQueue::global_instance->UpdateTransfer(transfer, disable_interrupts);
- }
}
{
const auto now = aos::monotonic_clock::now();
@@ -765,6 +774,10 @@
printf("Sending USB mode\n");
stdin_camera_command = CameraCommand::kUsb;
break;
+ case 'l':
+ printf("Log mode\n");
+ stdin_camera_command = CameraCommand::kLog;
+ break;
case 'n':
printf("Sending normal mode\n");
stdin_camera_command = CameraCommand::kNormal;
@@ -789,6 +802,7 @@
printf("UART board commands:\n");
printf(" p: Send passthrough mode\n");
printf(" u: Send USB mode\n");
+ printf(" l: Send Log mode\n");
printf(" n: Send normal mode\n");
printf(" a: Send all-'a' mode\n");
printf(" c: Dump camera configuration\n");
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 9d7117b..665714e 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -88,6 +88,8 @@
const ButtonLocation kMidCargoHint(3, 16);
const ButtonLocation kFarCargoHint(4, 2);
+const ButtonLocation kCameraLog(3, 7);
+
const ElevatorWristPosition kStowPos{0.36, 0.0};
const ElevatorWristPosition kClimbPos{0.0, M_PI / 4.0};
@@ -397,9 +399,25 @@
video_tx_->Send(vision_control_);
last_vision_control_ = monotonic_now;
}
+
+ {
+ auto camera_log_message = camera_log.MakeMessage();
+ camera_log_message->log = data.IsPressed(kCameraLog);
+ LOG_STRUCT(DEBUG, "camera_log", *camera_log_message);
+ camera_log_message.Send();
+ }
}
private:
+ uint32_t GetAutonomousMode() override {
+ ::frc971::autonomous::auto_mode.FetchLatest();
+ if (::frc971::autonomous::auto_mode.get() == nullptr) {
+ LOG(WARNING, "no auto mode values\n");
+ return 0;
+ }
+ return ::frc971::autonomous::auto_mode->mode;
+ }
+
// Current goals here.
ElevatorWristPosition elevator_wrist_pos_ = kStowPos;
bool grab_piece_ = false;
diff --git a/y2019/status_light.q b/y2019/status_light.q
index f84ed28..c26de88 100644
--- a/y2019/status_light.q
+++ b/y2019/status_light.q
@@ -8,3 +8,9 @@
};
queue StatusLight status_light;
+
+message CameraLog {
+ bool log;
+};
+
+queue CameraLog camera_log;
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index bf71116..b87e2ce 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -34,6 +34,16 @@
tools = [":constants_formatting"],
)
+cc_library(
+ name = "image_writer",
+ srcs = ["image_writer.cc"],
+ hdrs = ["image_writer.h"],
+ deps = [
+ "//aos/logging",
+ "//aos/vision/image:image_types",
+ ],
+)
+
sh_test(
name = "constants_formatting_test",
srcs = ["constants_formatting_test.sh"],
@@ -92,6 +102,7 @@
srcs = ["target_sender.cc"],
restricted_to = VISION_TARGETS,
deps = [
+ ":image_writer",
":target_finder",
"//aos/vision/blob:codec",
"//aos/vision/blob:find_blob",
diff --git a/y2019/vision/image_writer.cc b/y2019/vision/image_writer.cc
new file mode 100644
index 0000000..ac35114
--- /dev/null
+++ b/y2019/vision/image_writer.cc
@@ -0,0 +1,40 @@
+#include <fstream>
+#include <sys/stat.h>
+
+#include "aos/logging/logging.h"
+#include "y2019/vision/image_writer.h"
+
+namespace y2019 {
+namespace vision {
+
+ImageWriter::ImageWriter() {
+ LOG(INFO, "Initializing image writer\n");
+ SetDirPath();
+}
+
+void ImageWriter::WriteImage(::aos::vision::DataRef data) {
+ LOG(INFO, "Writing image %d", image_count_);
+ std::ofstream ofs(
+ dir_path_ + file_prefix_ + std::to_string(image_count_) + ".yuyv",
+ std::ofstream::out);
+ ofs << data;
+ ofs.close();
+ ++image_count_;
+}
+
+void ImageWriter::SetDirPath() {
+ ::std::string base_path = "/jevois/data/run_";
+ for (int i = 0;; ++i) {
+ struct stat st;
+ std::string option = base_path + std::to_string(i);
+ if (stat(option.c_str(), &st) != 0) {
+ file_prefix_ = option + "/";
+ LOG(INFO, "Writing to %s\n", file_prefix_.c_str());
+ mkdir(file_prefix_.c_str(), 0777);
+ break;
+ }
+ }
+}
+
+} // namespace vision
+} // namespace y2019
diff --git a/y2019/vision/image_writer.h b/y2019/vision/image_writer.h
new file mode 100644
index 0000000..f33cca7
--- /dev/null
+++ b/y2019/vision/image_writer.h
@@ -0,0 +1,29 @@
+#ifndef Y2019_VISION_IMAGE_WRITER_H_
+#define Y2019_VISION_IMAGE_WRITER_H_
+
+#include <string>
+
+#include "aos/vision/image/image_types.h"
+
+namespace y2019 {
+namespace vision {
+
+class ImageWriter {
+ public:
+ ImageWriter();
+
+ void WriteImage(::aos::vision::DataRef data);
+
+ private:
+ void SetDirPath();
+
+ ::std::string file_prefix_ = std::string("debug_viewer_jpeg_");
+ ::std::string dir_path_;
+
+ unsigned int image_count_ = 0;
+};
+
+} // namespace vision
+} // namespace y2017
+
+#endif // Y2019_VISION_IMAGE_WRITER_H_
diff --git a/y2019/vision/server/BUILD b/y2019/vision/server/BUILD
index 180b41e..420731a 100644
--- a/y2019/vision/server/BUILD
+++ b/y2019/vision/server/BUILD
@@ -1,4 +1,5 @@
load("//aos/seasocks:gen_embedded.bzl", "gen_embedded")
+load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
load("//aos/downloader:downloader.bzl", "aos_downloader_dir")
load("@build_bazel_rules_typescript//:defs.bzl", "ts_library")
load("@build_bazel_rules_nodejs//:defs.bzl", "rollup_bundle")
@@ -21,25 +22,31 @@
aos_downloader_dir(
name = "www_files",
srcs = [
- "//y2019/vision/server/www:visualizer_bundle",
"//y2019/vision/server/www:files",
+ "//y2019/vision/server/www:visualizer_bundle",
],
dir = "www",
visibility = ["//visibility:public"],
)
+cc_proto_library(
+ name = "server_data_proto",
+ srcs = ["server_data.proto"],
+)
+
cc_binary(
name = "server",
srcs = [
"server.cc",
],
data = [
- "//y2019/vision/server/www:visualizer_bundle",
"//y2019/vision/server/www:files",
+ "//y2019/vision/server/www:visualizer_bundle",
],
visibility = ["//visibility:public"],
deps = [
":gen_embedded",
+ ":server_data_proto",
"//aos:init",
"//aos/logging",
"//aos/time",
diff --git a/y2019/vision/server/server.cc b/y2019/vision/server/server.cc
index 54eb399..95ecd8d 100644
--- a/y2019/vision/server/server.cc
+++ b/y2019/vision/server/server.cc
@@ -11,11 +11,13 @@
#include "aos/time/time.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "internal/Embedded.h"
+#include "google/protobuf/util/json_util.h"
#include "seasocks/PrintfLogger.h"
#include "seasocks/Server.h"
#include "seasocks/StringUtil.h"
#include "seasocks/WebSocket.h"
#include "y2019/control_loops/drivetrain/camera.q.h"
+#include "y2019/vision/server/server_data.pb.h"
namespace y2019 {
namespace vision {
@@ -125,6 +127,8 @@
auto &drivetrain_status = frc971::control_loops::drivetrain_queue.status;
std::array<LocalCameraFrame, 5> latest_frames;
+ std::array<aos::monotonic_clock::time_point, 5> last_target_time;
+ last_target_time.fill(aos::monotonic_clock::epoch());
aos::monotonic_clock::time_point last_send_time = aos::monotonic_clock::now();
while (true) {
@@ -134,6 +138,7 @@
// Try again if we don't have any drivetrain statuses.
continue;
}
+ const auto now = aos::monotonic_clock::now();
{
const auto &new_frame = *camera_frames;
@@ -142,6 +147,10 @@
aos::monotonic_clock::time_point(
std::chrono::nanoseconds(new_frame.timestamp));
latest_frames[new_frame.camera].targets.clear();
+ if (new_frame.num_targets > 0) {
+ last_target_time[new_frame.camera] =
+ latest_frames[new_frame.camera].capture_time;
+ }
for (int target = 0; target < new_frame.num_targets; ++target) {
latest_frames[new_frame.camera].targets.emplace_back();
// TODO: Do something useful.
@@ -149,31 +158,47 @@
}
}
- const auto now = aos::monotonic_clock::now();
if (now > last_send_time + std::chrono::milliseconds(100)) {
+ // TODO(james): Use protobuf or the such to generate JSON rather than
+ // doing so manually.
last_send_time = now;
- std::ostringstream stream;
- stream << "{\n";
+ DebugData debug_data;
+ debug_data.mutable_robot_pose()->set_x(drivetrain_status->x);
+ debug_data.mutable_robot_pose()->set_y(drivetrain_status->y);
+ debug_data.mutable_robot_pose()->set_theta(drivetrain_status->theta);
+ {
+ LineFollowDebug *line_debug = debug_data.mutable_line_follow_debug();
+ line_debug->set_frozen(drivetrain_status->line_follow_logging.frozen);
+ line_debug->set_have_target(
+ drivetrain_status->line_follow_logging.have_target);
+ line_debug->mutable_goal_target()->set_x(
+ drivetrain_status->line_follow_logging.x);
+ line_debug->mutable_goal_target()->set_y(
+ drivetrain_status->line_follow_logging.y);
+ line_debug->mutable_goal_target()->set_theta(
+ drivetrain_status->line_follow_logging.theta);
+ }
+ for (size_t ii = 0; ii < latest_frames.size(); ++ii) {
+ CameraDebug *camera_debug = debug_data.add_camera_debug();
+ LocalCameraFrame cur_frame = latest_frames[ii];
- stream << "\"robot\": {";
- stream << "\"x\": " << drivetrain_status->x << ",";
- stream << "\"y\": " << drivetrain_status->y << ",";
- stream << "\"theta\": " << drivetrain_status->theta;
- stream << "}\n";
- stream << ",\"target\": {";
- stream << "\"x\": " << drivetrain_status->line_follow_logging.x << ",";
- stream << "\"y\": " << drivetrain_status->line_follow_logging.y << ",";
- stream << "\"theta\": " << drivetrain_status->line_follow_logging.theta
- << ",";
- stream << "\"frozen\": " << drivetrain_status->line_follow_logging.frozen
- << ",";
- stream << "\"have_target\": "
- << drivetrain_status->line_follow_logging.have_target;
- stream << "} ";
-
- stream << "}";
+ camera_debug->set_current_frame_age(
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ now - cur_frame.capture_time).count());
+ camera_debug->set_time_since_last_target(
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ now - last_target_time[ii]).count());
+ for (const auto &target : cur_frame.targets) {
+ Pose *pose = camera_debug->add_targets();
+ pose->set_x(target.x);
+ pose->set_y(target.y);
+ pose->set_theta(target.theta);
+ }
+ }
+ ::std::string json;
+ google::protobuf::util::MessageToJsonString(debug_data, &json);
server->execute(
- std::make_shared<UpdateData>(websocket_handler, stream.str()));
+ std::make_shared<UpdateData>(websocket_handler, ::std::move(json)));
}
}
}
diff --git a/y2019/vision/server/server_data.proto b/y2019/vision/server/server_data.proto
new file mode 100644
index 0000000..d3e5665
--- /dev/null
+++ b/y2019/vision/server/server_data.proto
@@ -0,0 +1,48 @@
+syntax = "proto2";
+
+package y2019.vision;
+
+// Various proto definitions for use with the websocket server for debugging the
+// vision code.
+
+// Representation of a 2D pose on the field, generally referenced off of the
+// center of the driver's station wall with positive X out towards the field and
+// positive Y to the left from the driver's perspective. theta is zero when
+// pointed straight along the positive X axis and increases towards positive Y
+// (counter-clockwise).
+message Pose {
+ optional float x = 1; // meters
+ optional float y = 2; // meters
+ optional float theta = 3; // radians
+}
+
+// Debugging information for the line following.
+message LineFollowDebug {
+ // Pose of the target that we are currently latched onto.
+ optional Pose goal_target = 1;
+ // Whether we are currently in line following mode and so will freeze the
+ // target shortly.
+ optional bool frozen = 2;
+ // Whether we have chosen a target (otherwise, goal_target may be arbitrary).
+ optional bool have_target = 3;
+}
+
+// Data for a single camera at a given instance in time (corresponding to a
+// camera frame).
+message CameraDebug {
+ // The time that has passed, in seconds, since we last got a frame with a
+ // target in it.
+ optional float time_since_last_target = 1;
+ // The age of the most recent frame and the one that contains the targets included below.
+ optional float current_frame_age = 2;
+ // Target Pose is relative to the camera, *not* the field, so (0, 0) is at the
+ // camera.
+ repeated Pose targets = 3;
+}
+
+// The overall package of data that we send to the webpage.
+message DebugData {
+ optional Pose robot_pose = 1;
+ optional LineFollowDebug line_follow_debug = 2;
+ repeated CameraDebug camera_debug = 3;
+}
diff --git a/y2019/vision/server/www/BUILD b/y2019/vision/server/www/BUILD
index 937b495..428dd41 100644
--- a/y2019/vision/server/www/BUILD
+++ b/y2019/vision/server/www/BUILD
@@ -6,7 +6,7 @@
filegroup(
name = "files",
srcs = glob([
- "**/*",
+ "**/*.html",
]),
)
@@ -14,7 +14,20 @@
name = "visualizer",
srcs = glob([
"*.ts",
- ]),
+ ]) + ["camera_constants.ts"],
+)
+
+cc_binary(
+ name = "generate_camera",
+ srcs = ["generate_camera.cc"],
+ deps = ["//y2019:constants"],
+)
+
+genrule(
+ name = "gen_cam_ts",
+ outs = ["camera_constants.ts"],
+ cmd = "$(location :generate_camera) $@",
+ tools = [":generate_camera"],
)
rollup_bundle(
diff --git a/y2019/vision/server/www/generate_camera.cc b/y2019/vision/server/www/generate_camera.cc
new file mode 100644
index 0000000..757ce69
--- /dev/null
+++ b/y2019/vision/server/www/generate_camera.cc
@@ -0,0 +1,35 @@
+#include "y2019/constants.h"
+#include "y2019/vision/constants.h"
+
+#include <fstream>
+#include <iostream>
+
+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 << "}";
+}
+void DumpTypescriptConstants(const char *fname) {
+ ::std::ofstream out_file(fname);
+ out_file << "export const CAMERA_POSES = [\n";
+ for (size_t ii = 0; ii < constants::Values::kNumCameras; ++ii) {
+ out_file << " ";
+ // TODO(james): Decide how to manage visualization for practice and code
+ // bots.
+ DumpPose(&out_file,
+ GetCamera(CameraSerialNumbers(CompBotTeensyId())[ii])->geometry);
+ out_file << ",\n";
+ }
+ out_file << "];\n";
+}
+} // namespace constants
+} // namespace y2019
+
+int main(int argc, char *argv[]) {
+ if (argc != 2) {
+ ::std::cout << "Must provide a filename for output as an argument\n";
+ return 1;
+ }
+ ::y2019::vision::DumpTypescriptConstants(argv[1]);
+}
diff --git a/y2019/vision/server/www/main.ts b/y2019/vision/server/www/main.ts
index 17b7139..beb2734 100644
--- a/y2019/vision/server/www/main.ts
+++ b/y2019/vision/server/www/main.ts
@@ -16,6 +16,7 @@
private targetX = 0;
private targetY = 0;
private targetTheta = 0;
+ private cameraColors = ['red', 'red', 'red', 'red', 'red'];
constructor() {
const canvas = <HTMLCanvasElement>document.getElementById('field');
@@ -27,15 +28,22 @@
reader.addEventListener('loadend', (e) => {
const text = e.srcElement.result;
const j = JSON.parse(text);
- this.x = j.robot.x;
- this.y = j.robot.y;
- this.theta = j.robot.theta;
+ this.x = j.robotPose.x;
+ this.y = j.robotPose.y;
+ this.theta = j.robotPose.theta;
- if(j.target) {
- this.targetLocked = j.target.frozen && j.target.have_target;
- this.targetX = j.target.x;
- this.targetY = j.target.y;
- this.targetTheta = j.target.theta;
+ if(j.lineFollowDebug) {
+ this.targetLocked = j.lineFollowDebug.frozen && j.lineFollowDebug.haveTarget;
+ this.targetX = j.lineFollowDebug.goalTarget.x;
+ this.targetY = j.lineFollowDebug.goalTarget.y;
+ this.targetTheta = j.lineFollowDebug.goalTarget.theta;
+ }
+ for (let ii of [0, 1, 2, 3, 4]) {
+ if (j.cameraDebug[ii].timeSinceLastTarget > 0.25) {
+ this.cameraColors[ii] = 'red';
+ } else {
+ this.cameraColors[ii] = 'green';
+ }
}
});
socket.addEventListener('message', (event) => {
@@ -63,7 +71,7 @@
this.reset(ctx);
drawField(ctx);
- drawRobot(ctx, this.x, this.y, this.theta);
+ drawRobot(ctx, this.x, this.y, this.theta, this.cameraColors);
ctx.save();
ctx.lineWidth = 2.0 * ctx.lineWidth;
if (this.targetLocked) {
diff --git a/y2019/vision/server/www/robot.ts b/y2019/vision/server/www/robot.ts
index bc1f47f..489112c 100644
--- a/y2019/vision/server/www/robot.ts
+++ b/y2019/vision/server/www/robot.ts
@@ -1,9 +1,22 @@
import {IN_TO_M, FT_TO_M} from './constants';
+import {CAMERA_POSES} from './camera_constants';
const ROBOT_WIDTH = 25 * IN_TO_M;
const ROBOT_LENGTH = 31 * IN_TO_M;
+const CAMERA_SCALE = 0.3;
-export function drawRobot(ctx : CanvasRenderingContext2D, x : number, y : number, theta : number) : void {
+function drawCamera(ctx : canvasRenderingContext2d, pose : {x : number, y : number, theta : number}) : void {
+ ctx.beginPath();
+ ctx.moveTo(pose.x, pose.y);
+ ctx.lineTo(pose.x + CAMERA_SCALE * Math.cos(pose.theta + Math.PI / 4.0),
+ pose.y + CAMERA_SCALE * Math.sin(pose.theta + Math.PI / 4.0));
+ ctx.lineTo(pose.x + CAMERA_SCALE * Math.cos(pose.theta - Math.PI / 4.0),
+ pose.y + CAMERA_SCALE * Math.sin(pose.theta - Math.PI / 4.0));
+ ctx.closePath();
+ ctx.stroke();
+}
+
+export function drawRobot(ctx : CanvasRenderingContext2D, x : number, y : number, theta : number, camera_colors : string[]) : void {
ctx.save();
ctx.translate(x, y);
ctx.rotate(theta);
@@ -11,11 +24,18 @@
ctx.fillStyle = 'blue';
ctx.fillRect(-ROBOT_LENGTH / 2, -ROBOT_WIDTH / 2, ROBOT_LENGTH, ROBOT_WIDTH);
+ ctx.beginPath();
+ ctx.strokeStyle = 'black';
ctx.moveTo(ROBOT_LENGTH / 2, -ROBOT_WIDTH/2);
ctx.lineTo(ROBOT_LENGTH / 2 + 0.1, 0);
ctx.lineTo(ROBOT_LENGTH / 2, ROBOT_WIDTH/2);
ctx.closePath();
ctx.stroke();
+ ctx.lineWidth = 3.0 * ctx.lineWidth;
+ for (let ii of [0, 1, 2, 3, 4]) {
+ ctx.strokeStyle = camera_colors[ii];
+ drawCamera(ctx, CAMERA_POSES[ii]);
+ }
ctx.restore();
}
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 12e0e09..4344cc9 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -8,10 +8,10 @@
#include "aos/vision/events/udp.h"
#include "y2019/jevois/camera/image_stream.h"
#include "y2019/jevois/camera/reader.h"
-
#include "y2019/jevois/serial.h"
#include "y2019/jevois/structures.h"
#include "y2019/jevois/uart.h"
+#include "y2019/vision/image_writer.h"
// This has to be last to preserve compatibility with other headers using AOS
// logging.
@@ -83,6 +83,9 @@
// dup2(itsDev, 2);
TargetFinder finder_;
+ ImageWriter writer;
+ uint32_t image_count = 0;
+ bool log_images = false;
aos::vision::CameraParams params0;
params0.set_exposure(50);
@@ -167,6 +170,13 @@
LOG(INFO) << "Some problem happened";
}
}
+
+ if (log_images) {
+ if ((image_count % 5) == 0) {
+ writer.WriteImage(data);
+ }
+ ++image_count;
+ }
});
aos::events::EpollLoop loop;
@@ -195,6 +205,10 @@
switch (calibration.camera_command) {
case CameraCommand::kNormal:
case CameraCommand::kAs:
+ log_images = false;
+ break;
+ case CameraCommand::kLog:
+ log_images = true;
break;
case CameraCommand::kUsb:
return 0;
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index fd9df73..95ed28e 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -203,6 +203,11 @@
vacuum_sensor_ = make_unique<frc::AnalogInput>(port);
}
+ // Auto mode switches.
+ void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
+ autonomous_modes_.at(i) = ::std::move(sensor);
+ }
+
void RunIteration() override {
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
@@ -254,6 +259,18 @@
superstructure_message.Send();
}
+
+ {
+ auto auto_mode_message = ::frc971::autonomous::auto_mode.MakeMessage();
+ auto_mode_message->mode = 0;
+ for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
+ if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
+ auto_mode_message->mode |= 1 << i;
+ }
+ }
+ LOG_STRUCT(DEBUG, "auto mode", *auto_mode_message);
+ auto_mode_message.Send();
+ }
}
private:
@@ -262,6 +279,8 @@
::std::unique_ptr<frc::AnalogInput> vacuum_sensor_;
+ ::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
+
::frc971::wpilib::AbsoluteEncoder intake_encoder_;
// TODO(sabina): Add wrist and elevator hall effects.
};
@@ -295,10 +314,13 @@
using namespace frc971::jevois;
RoborioToTeensy to_teensy{};
to_teensy.realtime_now = aos::realtime_clock::now();
+ camera_log.FetchLatest();
if (activate_usb_ && !activate_usb_->Get()) {
to_teensy.camera_command = CameraCommand::kUsb;
} else if (activate_passthrough_ && !activate_passthrough_->Get()) {
to_teensy.camera_command = CameraCommand::kCameraPassthrough;
+ } else if (camera_log.get() && camera_log->log) {
+ to_teensy.camera_command = CameraCommand::kLog;
} else {
to_teensy.camera_command = CameraCommand::kNormal;
}
@@ -646,6 +668,9 @@
reader.set_pwm_trigger(true);
reader.set_vacuum_sensor(7);
+ reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(22));
+ reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(23));
+
::std::thread reader_thread(::std::ref(reader));
CameraReader camera_reader;