Use the drivetrain position from when the frame was captured
Change-Id: I30055d13a3dc88497985f484f4e1d08ae1ef592f
diff --git a/y2016/actors/vision_align_actor.cc b/y2016/actors/vision_align_actor.cc
index ad899be..00695f0 100644
--- a/y2016/actors/vision_align_actor.cc
+++ b/y2016/actors/vision_align_actor.cc
@@ -43,38 +43,33 @@
if (!::y2016::vision::vision_status.FetchLatest()) {
continue;
}
+ const auto &vision_status = *::y2016::vision::vision_status;
- if (!::y2016::vision::vision_status->left_image_valid ||
- !::y2016::vision::vision_status->right_image_valid) {
+ if (!vision_status.left_image_valid || !vision_status.right_image_valid) {
continue;
}
const double side_distance_change =
- ::y2016::vision::vision_status->horizontal_angle * robot_radius;
+ vision_status.horizontal_angle * robot_radius;
if (!::std::isfinite(side_distance_change)) {
continue;
}
- drivetrain_queue.status.FetchLatest();
- if (drivetrain_queue.status.get()) {
- const double left_current =
- drivetrain_queue.status->estimated_left_position;
- const double right_current =
- drivetrain_queue.status->estimated_right_position;
+ const double left_current = vision_status.drivetrain_left_position;
+ const double right_current = vision_status.drivetrain_right_position;
- if (!drivetrain_queue.goal.MakeWithBuilder()
- .steering(0.0)
- .throttle(0.0)
- .highgear(false)
- .quickturn(false)
- .control_loop_driving(true)
- .left_goal(left_current + side_distance_change)
- .right_goal(right_current - side_distance_change)
- .left_velocity_goal(0)
- .right_velocity_goal(0)
- .Send()) {
- LOG(WARNING, "sending drivetrain goal failed\n");
- }
+ if (!drivetrain_queue.goal.MakeWithBuilder()
+ .steering(0.0)
+ .throttle(0.0)
+ .highgear(false)
+ .quickturn(false)
+ .control_loop_driving(true)
+ .left_goal(left_current + side_distance_change)
+ .right_goal(right_current - side_distance_change)
+ .left_velocity_goal(0)
+ .right_velocity_goal(0)
+ .Send()) {
+ LOG(WARNING, "sending drivetrain goal failed\n");
}
}
diff --git a/y2016/vision/BUILD b/y2016/vision/BUILD
index 550e605..f41ea84 100644
--- a/y2016/vision/BUILD
+++ b/y2016/vision/BUILD
@@ -92,5 +92,7 @@
':vision_data',
':stereo_geometry',
'//y2016:constants',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//aos/common:mutex',
],
)
diff --git a/y2016/vision/target_receiver.cc b/y2016/vision/target_receiver.cc
index 07c5ea0..80a9f6b 100644
--- a/y2016/vision/target_receiver.cc
+++ b/y2016/vision/target_receiver.cc
@@ -4,12 +4,19 @@
#include <vector>
#include <memory>
+#include <array>
+#include <thread>
+#include <atomic>
+#include <limits>
#include "aos/linux_code/init.h"
#include "aos/common/time.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
#include "aos/vision/events/udp.h"
+#include "aos/common/mutex.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2016/vision/vision.q.h"
#include "y2016/vision/vision_data.pb.h"
@@ -164,11 +171,123 @@
newer_center.y() * age_ratio + (1 - age_ratio) * last_newer_center.y());
}
+// Handles calculating drivetrain offsets.
+class DrivetrainOffsetCalculator {
+ public:
+ // To be called by ::std::thread.
+ void operator()() {
+ auto &status = ::frc971::control_loops::drivetrain_queue.status;
+ while (run_) {
+ status.FetchAnother();
+
+ ::aos::MutexLocker locker(&lock_);
+ data_[data_index_].time = status->sent_time;
+ data_[data_index_].left = status->estimated_left_position;
+ data_[data_index_].right = status->estimated_right_position;
+ ++data_index_;
+ if (data_index_ == data_.size()) data_index_ = 0;
+ if (valid_data_ < data_.size()) ++valid_data_;
+ }
+ }
+
+ // Takes a vision status message with everything except
+ // drivetrain_{left,right}_position set and sets those.
+ // Returns false if it doesn't have enough data to fill them out.
+ bool CompleteVisionStatus(::y2016::vision::VisionStatus *status) {
+ if (valid_data_ == 0) return false;
+
+ const ::aos::time::Time capture_time =
+ ::aos::time::Time::InNS(status->target_time);
+ DrivetrainData before, after;
+ FindBeforeAfter(&before, &after, capture_time);
+
+ if (before.time == after.time) {
+ status->drivetrain_left_position = before.left;
+ status->drivetrain_right_position = before.right;
+ } else {
+ const double age_ratio = (capture_time - before.time).ToSeconds() /
+ (after.time - before.time).ToSeconds();
+ status->drivetrain_left_position =
+ before.left * (1 - age_ratio) + after.left * age_ratio;
+ status->drivetrain_right_position =
+ before.right * (1 - age_ratio) + after.right * age_ratio;
+ }
+
+ return true;
+ }
+
+ void Quit() { run_ = false; }
+
+ private:
+ struct DrivetrainData {
+ ::aos::time::Time time;
+ double left, right;
+ };
+
+ // Fills out before and after with the data surrounding capture_time.
+ // They might be identical if that's the closest approximation.
+ // Do not call this if valid_data_ is 0.
+ void FindBeforeAfter(DrivetrainData *before, DrivetrainData *after,
+ ::aos::time::Time capture_time) {
+ ::aos::MutexLocker locker(&lock_);
+ size_t location = 0;
+ while (true) {
+ // We hit the end of our data. Just fill them both out as the last data
+ // point.
+ if (location >= valid_data_) {
+ *before = *after =
+ data_[previous_index((valid_data_ + data_index_) % data_.size())];
+ return;
+ }
+
+ // The index into data_ corresponding to location positions after
+ // (data_index_ - 1).
+ const size_t index = previous_index(location + data_index_);
+
+ // If we've found the one we want.
+ if (data_[index].time > capture_time) {
+ *after = data_[index];
+ if (location == 0) {
+ // If this is the first one and it's already after, just return the
+ // same thing for both.
+ *before = data_[index];
+ } else {
+ *before = data_[previous_index(index)];
+ }
+ return;
+ }
+
+ ++location;
+ }
+ }
+
+ size_t previous_index(size_t index) const {
+ if (index == 0) {
+ return data_.size() - 1;
+ } else {
+ return index - 1;
+ }
+ }
+
+ ::std::array<DrivetrainData, 200> data_;
+ // The index into data_ the next data point is going at.
+ size_t data_index_ = 0;
+ // How many elemets of data_ are valid.
+ size_t valid_data_ = 0;
+
+ ::aos::Mutex lock_;
+
+ ::std::atomic<bool> run_{true};
+};
+
void Main() {
StereoGeometry stereo(constants::GetValues().vision_name);
LOG(INFO, "calibration: %s\n",
stereo.calibration().ShortDebugString().c_str());
+ DrivetrainOffsetCalculator drivetrain_offset;
+ ::std::thread drivetrain_offset_thread(::std::ref(drivetrain_offset));
+
CameraHandler left;
CameraHandler right;
@@ -242,10 +361,14 @@
new_vision_status->vertical_angle = vertical_angle;
new_vision_status->distance = distance;
}
- LOG_STRUCT(DEBUG, "vision", *new_vision_status);
- if (!new_vision_status.Send()) {
- LOG(ERROR, "Failed to send vision information\n");
+ if (drivetrain_offset.CompleteVisionStatus(new_vision_status.get())) {
+ LOG_STRUCT(DEBUG, "vision", *new_vision_status);
+ if (!new_vision_status.Send()) {
+ LOG(ERROR, "Failed to send vision information\n");
+ }
+ } else {
+ LOG_STRUCT(WARNING, "vision without drivetrain", *new_vision_status);
}
}
@@ -256,6 +379,9 @@
right.target().ShortDebugString().c_str());
}
}
+
+ drivetrain_offset.Quit();
+ drivetrain_offset_thread.join();
}
} // namespace vision
diff --git a/y2016/vision/vision.q b/y2016/vision/vision.q
index d762d17..a0d021c 100644
--- a/y2016/vision/vision.q
+++ b/y2016/vision/vision.q
@@ -23,5 +23,12 @@
// Capture time of the angle using the clock behind Time::Now().
int64_t target_time;
+
+ // The estimated positions of both sides of the drivetrain when the frame
+ // was captured.
+ // These are the estimated_left_position and estimated_right_position members
+ // of the drivetrain queue.
+ double drivetrain_left_position;
+ double drivetrain_right_position;
};
queue VisionStatus vision_status;