Merge "Tune autonav splines"
diff --git a/aos/events/logging/log_stats.cc b/aos/events/logging/log_stats.cc
index f408e27..130a11f 100644
--- a/aos/events/logging/log_stats.cc
+++ b/aos/events/logging/log_stats.cc
@@ -1,6 +1,7 @@
#include <iomanip>
#include <iostream>
+#include "absl/strings/str_format.h"
#include "aos/events/logging/log_reader.h"
#include "aos/events/simulated_event_loop.h"
#include "aos/init.h"
@@ -18,23 +19,153 @@
"Only print channels that have a set max message size that is more "
"than double of the max message size.");
-// define struct to hold all information
-struct ChannelStats {
+// This class implements a histogram for tracking message period percentiles.
+class Histogram {
+ public:
+ Histogram(size_t buckets = 1024)
+ : max_value_bucket_(0.01), values_(buckets, 0.0), counts_(buckets, 0) {}
+
+ // Adds a new sample to the histogram, potentially downsampling the existing
+ // data.
+ void Add(double value) {
+ if (value < max_value_bucket_) {
+ const ssize_t bucket = static_cast<size_t>(
+ std::floor(value * values_.size() / max_value_bucket_));
+ CHECK_GE(bucket, 0);
+ CHECK_LT(bucket, static_cast<ssize_t>(values_.size()));
+ values_[bucket] += value;
+ if (all_counts_ == 0 || value > max_value_) {
+ max_value_ = value;
+ }
+ if (all_counts_ == 0 || value < min_value_) {
+ min_value_ = value;
+ }
+ ++counts_[bucket];
+ ++all_counts_;
+ } else {
+ // Double all the bucket sizes by merging adjacent buckets and doubling
+ // the max value. If this isn't enough, we'll recurse inside Add and
+ // do it again until it fits.
+ max_value_bucket_ *= 2.0;
+ for (size_t bucket = 0; bucket < values_.size() / 2; ++bucket) {
+ values_[bucket] = values_[bucket * 2] + values_[bucket * 2 + 1];
+ counts_[bucket] = counts_[bucket * 2] + counts_[bucket * 2 + 1];
+ }
+ for (size_t bucket = values_.size() / 2; bucket < values_.size();
+ ++bucket) {
+ values_[bucket] = 0.0;
+ counts_[bucket] = 0;
+ }
+ Add(value);
+ }
+ }
+
+ // Prints out the percentiles for a couple of critical numbers.
+ std::string Percentile() const {
+ const size_t percentile5 = all_counts_ / 20;
+ double percentile5_value = 0.0;
+ const size_t percentile50 = all_counts_ / 2;
+ double percentile50_value = 0.0;
+ const size_t percentile95 = all_counts_ - percentile5;
+ double percentile95_value = 0.0;
+
+ size_t count = 0;
+ for (size_t i = 0; i < values_.size(); ++i) {
+ if (count < percentile5 && count + counts_[i] >= percentile5) {
+ percentile5_value = values_[i] / counts_[i];
+ }
+ if (count < percentile50 && count + counts_[i] >= percentile50) {
+ percentile50_value = values_[i] / counts_[i];
+ }
+ if (count < percentile95 && count + counts_[i] >= percentile95) {
+ percentile95_value = values_[i] / counts_[i];
+ }
+ count += counts_[i];
+ }
+
+ // Assume here that these are periods in seconds. Convert to ms for
+ // readability. This isn't super generic, but that's fine for now.
+ return absl::StrFormat(
+ "[max %.3fms 95%%:%.3fms 50%%:%.3fms 5%%:%.3fms min %.3fms]",
+ max_value_ * 1000., percentile95_value * 1000.,
+ percentile50_value * 1000., percentile5_value * 1000.,
+ min_value_ * 1000.);
+ }
+
+ private:
+ // The size of the largest bucket. Used to figure out which bucket something
+ // goes into.
+ double max_value_bucket_;
+ // Max and min values overall we have seen.
+ double max_value_ = 0;
+ double min_value_ = 0;
+ // A list of the sum of values and counts for those per bucket.
+ std::vector<double> values_;
+ std::vector<size_t> counts_;
+ // Total number of samples.
+ size_t all_counts_ = 0;
+};
+
+class ChannelStats {
+ public:
+ ChannelStats(const aos::Channel *channel) : channel_(channel) {}
+
+ // Adds a sample to the statistics.
+ void Add(const aos::Context &context) {
+ max_message_size_ = std::max(max_message_size_, context.size);
+ total_message_size_ += context.size;
+ total_num_messages_++;
+ channel_end_time_ = context.realtime_event_time;
+ first_message_time_ =
+ std::min(first_message_time_, context.monotonic_event_time);
+ if (current_message_time_ != aos::monotonic_clock::min_time) {
+ histogram_.Add(std::chrono::duration<double>(
+ context.monotonic_event_time - current_message_time_)
+ .count());
+ }
+ current_message_time_ = context.monotonic_event_time;
+ }
+
+ std::string Percentile() const { return histogram_.Percentile(); }
+
+ double SecondsActive() const {
+ return aos::time::DurationInSeconds(current_message_time_ -
+ first_message_time_);
+ }
+
+ size_t max_message_size() const { return max_message_size_; }
+ size_t total_num_messages() const { return total_num_messages_; }
+
+ double avg_messages_per_sec() const {
+ return total_num_messages_ / SecondsActive();
+ }
+ size_t avg_message_size() const {
+ return total_message_size_ / total_num_messages_;
+ }
+
+ aos::realtime_clock::time_point channel_end_time() const {
+ return channel_end_time_;
+ }
+
+ const aos::Channel *channel() const { return channel_; }
+
+ private:
// pointer to the channel for which stats are collected
- const aos::Channel *channel;
- aos::realtime_clock::time_point channel_end_time =
+ const aos::Channel *channel_;
+ aos::realtime_clock::time_point channel_end_time_ =
aos::realtime_clock::min_time;
- aos::monotonic_clock::time_point first_message_time =
+ aos::monotonic_clock::time_point first_message_time_ =
// needs to be higher than time in the logfile!
aos::monotonic_clock::max_time;
- aos::monotonic_clock::time_point current_message_time =
+ aos::monotonic_clock::time_point current_message_time_ =
aos::monotonic_clock::min_time;
+
// channel stats to collect per channel
- int total_num_messages = 0;
- size_t max_message_size = 0;
- size_t total_message_size = 0;
- double avg_messages_sec = 0.0; // TODO in Lambda, now in stats overview.
- double max_messages_sec = 0.0; // TODO in Lambda
+ int total_num_messages_ = 0;
+ size_t max_message_size_ = 0;
+ size_t total_message_size_ = 0;
+
+ Histogram histogram_;
};
struct LogfileStats {
@@ -126,17 +257,9 @@
stats_event_loop->MakeRawNoArgWatcher(
channel,
[&logfile_stats, &channel_stats, it](const aos::Context &context) {
- channel_stats[it].max_message_size =
- std::max(channel_stats[it].max_message_size, context.size);
- channel_stats[it].total_message_size += context.size;
- channel_stats[it].total_num_messages++;
- // asume messages are send in sequence per channel
- channel_stats[it].channel_end_time = context.realtime_event_time;
- channel_stats[it].first_message_time =
- std::min(channel_stats[it].first_message_time,
- context.monotonic_event_time);
- channel_stats[it].current_message_time = context.monotonic_event_time;
- // update the overall logfile statistics
+ channel_stats[it].Add(context);
+
+ // Update the overall logfile statistics
logfile_stats.logfile_length += context.size;
});
it++;
@@ -152,58 +275,34 @@
log_reader_factory.Run();
std::cout << std::endl;
+
// Print out the stats per channel and for the logfile
for (size_t i = 0; i != channel_stats.size(); i++) {
- if (channel_stats[i].total_num_messages > 0) {
- double sec_active =
- aos::time::DurationInSeconds(channel_stats[i].current_message_time -
- channel_stats[i].first_message_time);
- channel_stats[i].avg_messages_sec =
- (channel_stats[i].total_num_messages / sec_active);
- logfile_stats.total_log_messages += channel_stats[i].total_num_messages;
- logfile_stats.logfile_end_time = std::max(
- logfile_stats.logfile_end_time, channel_stats[i].channel_end_time);
+ if (!FLAGS_excessive_size_only ||
+ (channel_stats[i].max_message_size() * 2) <
+ static_cast<size_t>(channel_stats[i].channel()->max_size())) {
+ if (channel_stats[i].total_num_messages() > 0) {
+ std::cout << channel_stats[i].channel()->name()->string_view() << " "
+ << channel_stats[i].channel()->type()->string_view() << "\n";
- if (!FLAGS_excessive_size_only ||
- (channel_stats[i].max_message_size * 2) <
- static_cast<unsigned long>(
- channel_stats[i].channel->max_size())) {
- std::cout << "Channel name: "
- << channel_stats[i].channel->name()->string_view()
- << "\tMsg type: "
- << channel_stats[i].channel->type()->string_view() << "\n";
+ logfile_stats.total_log_messages +=
+ channel_stats[i].total_num_messages();
+ logfile_stats.logfile_end_time =
+ std::max(logfile_stats.logfile_end_time,
+ channel_stats[i].channel_end_time());
+
if (!FLAGS_excessive_size_only) {
- std::cout << "Number of msg: " << channel_stats[i].total_num_messages
- << std::setprecision(3) << std::fixed
- << "\tAvg msg per sec: "
- << channel_stats[i].avg_messages_sec
- << "\tSet max msg frequency: "
- << channel_stats[i].channel->frequency() << "\n";
+ std::cout << " " << channel_stats[i].total_num_messages()
+ << " msgs, " << channel_stats[i].avg_messages_per_sec()
+ << "hz avg, " << channel_stats[i].channel()->frequency()
+ << "hz max";
}
- std::cout << "Avg msg size: "
- << (channel_stats[i].total_message_size /
- channel_stats[i].total_num_messages)
- << "\tMax msg size: " << channel_stats[i].max_message_size
- << "\tSet max msg size: "
- << channel_stats[i].channel->max_size() << "\n";
- if (!FLAGS_excessive_size_only) {
- std::cout << "First msg time: " << channel_stats[i].first_message_time
- << "\tLast msg time: "
- << channel_stats[i].current_message_time
- << "\tSeconds active: " << sec_active << "sec\n";
- }
+ std::cout << " " << channel_stats[i].avg_message_size()
+ << " bytes avg, " << channel_stats[i].max_message_size()
+ << " bytes max / " << channel_stats[i].channel()->max_size()
+ << "bytes " << channel_stats[i].Percentile();
std::cout << std::endl;
}
- } else {
- std::cout << "Channel name: "
- << channel_stats[i].channel->name()->string_view() << "\t"
- << "Msg type: "
- << channel_stats[i].channel->type()->string_view() << "\n"
- << "Set max msg frequency: "
- << channel_stats[i].channel->frequency() << "\t"
- << "Set max msg size: " << channel_stats[i].channel->max_size()
- << "\n--- No messages in channel ---"
- << "\n";
}
}
std::cout << std::setfill('-') << std::setw(80) << "-"
diff --git a/tools/ci/buildkite_gerrit_trigger.go b/tools/ci/buildkite_gerrit_trigger.go
index eb31a92..0dc9b13 100644
--- a/tools/ci/buildkite_gerrit_trigger.go
+++ b/tools/ci/buildkite_gerrit_trigger.go
@@ -14,6 +14,7 @@
"regexp"
"strings"
"sync"
+ "time"
)
type Commit struct {
@@ -87,7 +88,13 @@
type EventInfo struct {
Author *User `json:"author"`
Uploader *User `json:"uploader"`
+ Reviewer *User `json:"reviewer"`
+ Adder *User `json:"adder"`
+ Remover *User `json:"remover"`
Submitter User `json:"submitter,omitempty"`
+ NewRev string `json:"newRev,omitempty"`
+ Ref string `json:"ref,omitempty"`
+ TargetNode string `json:"targetNode,omitempty"`
Approvals []Approval `json:"approvals,omitempty"`
Comment string `json:"comment,omitempty"`
PatchSet *PatchSet `json:"patchSet"`
@@ -124,95 +131,111 @@
log.Printf("Got a matching change of %s %s %d,%d\n",
eventInfo.Change.ID, eventInfo.PatchSet.Revision, eventInfo.Change.Number, eventInfo.PatchSet.Number)
- // Triggering a build creates a UUID, and we can see events back from the webhook before the command returns. Lock across the command so nothing access Commits while the new UUID is being added.
- s.mu.Lock()
+ for {
- var user *User
- if eventInfo.Author != nil {
- user = eventInfo.Author
- } else if eventInfo.Uploader != nil {
- user = eventInfo.Uploader
- } else {
- log.Fatalf("Failed to find Author or Uploader")
- }
+ // Triggering a build creates a UUID, and we can see events back from the webhook before the command returns. Lock across the command so nothing access Commits while the new UUID is being added.
+ s.mu.Lock()
- // Trigger the build.
- if build, _, err := client.Builds.Create(
- "spartan-robotics", "971-robot-code", &buildkite.CreateBuild{
- Commit: eventInfo.PatchSet.Revision,
- Branch: eventInfo.Change.ID,
- Author: buildkite.Author{
- Name: user.Name,
- Email: user.Email,
- },
- Env: map[string]string{
- "GERRIT_CHANGE_NUMBER": fmt.Sprintf("%d", eventInfo.Change.Number),
- "GERRIT_PATCH_NUMBER": fmt.Sprintf("%d", eventInfo.PatchSet.Number),
- },
- }); err == nil {
-
- if build.ID != nil {
- log.Printf("Scheduled build %s\n", *build.ID)
- s.Commits[*build.ID] = Commit{
- Sha1: eventInfo.PatchSet.Revision,
- ChangeId: eventInfo.Change.ID,
- ChangeNumber: eventInfo.Change.Number,
- Patchset: eventInfo.PatchSet.Number,
- }
- }
- s.mu.Unlock()
-
- if data, err := json.MarshalIndent(build, "", "\t"); err != nil {
- log.Fatalf("json encode failed: %s", err)
+ var user *User
+ if eventInfo.Author != nil {
+ user = eventInfo.Author
+ } else if eventInfo.Uploader != nil {
+ user = eventInfo.Uploader
} else {
- log.Printf("%s\n", string(data))
+ log.Fatalf("Failed to find Author or Uploader")
}
- // Now remove the verified from Gerrit and post the link.
- cmd := exec.Command("ssh",
- "-p",
- "29418",
- "-i",
- s.Key,
- s.User+"@software.frc971.org",
- "gerrit",
- "review",
- "-m",
- fmt.Sprintf("\"Build Started: %s\"", *build.WebURL),
- "--verified",
- "0",
- fmt.Sprintf("%d,%d", eventInfo.Change.Number, eventInfo.PatchSet.Number))
+ // Trigger the build.
+ if build, _, err := client.Builds.Create(
+ "spartan-robotics", "971-robot-code", &buildkite.CreateBuild{
+ Commit: eventInfo.PatchSet.Revision,
+ Branch: eventInfo.Change.ID,
+ Author: buildkite.Author{
+ Name: user.Name,
+ Email: user.Email,
+ },
+ Env: map[string]string{
+ "GERRIT_CHANGE_NUMBER": fmt.Sprintf("%d", eventInfo.Change.Number),
+ "GERRIT_PATCH_NUMBER": fmt.Sprintf("%d", eventInfo.PatchSet.Number),
+ },
+ }); err == nil {
- log.Printf("Running 'ssh -p 29418 -i %s %s@software.frc971.org gerrit review -m '\"Build Started: %s\"' --verified 0 %d,%d' and waiting for it to finish...",
- s.Key, s.User,
- *build.WebURL, eventInfo.Change.Number, eventInfo.PatchSet.Number)
- if err := cmd.Run(); err != nil {
- log.Printf("Command failed with error: %v", err)
+ if build.ID != nil {
+ log.Printf("Scheduled build %s\n", *build.ID)
+ s.Commits[*build.ID] = Commit{
+ Sha1: eventInfo.PatchSet.Revision,
+ ChangeId: eventInfo.Change.ID,
+ ChangeNumber: eventInfo.Change.Number,
+ Patchset: eventInfo.PatchSet.Number,
+ }
+ }
+ s.mu.Unlock()
+
+ if data, err := json.MarshalIndent(build, "", "\t"); err != nil {
+ log.Fatalf("json encode failed: %s", err)
+ } else {
+ log.Printf("%s\n", string(data))
+ }
+
+ // Now remove the verified from Gerrit and post the link.
+ cmd := exec.Command("ssh",
+ "-p",
+ "29418",
+ "-i",
+ s.Key,
+ s.User+"@software.frc971.org",
+ "gerrit",
+ "review",
+ "-m",
+ fmt.Sprintf("\"Build Started: %s\"", *build.WebURL),
+ // Don't email out the initial link to lower the spam.
+ "-n",
+ "NONE",
+ "--verified",
+ "0",
+ fmt.Sprintf("%d,%d", eventInfo.Change.Number, eventInfo.PatchSet.Number))
+
+ log.Printf("Running 'ssh -p 29418 -i %s %s@software.frc971.org gerrit review -m '\"Build Started: %s\"' -n NONE --verified 0 %d,%d' and waiting for it to finish...",
+ s.Key, s.User,
+ *build.WebURL, eventInfo.Change.Number, eventInfo.PatchSet.Number)
+ if err := cmd.Run(); err != nil {
+ log.Printf("Command failed with error: %v", err)
+ }
+ return
+ } else {
+ s.mu.Unlock()
+ log.Printf("Failed to trigger build: %s", err)
+ log.Printf("Trying again in 30 seconds")
+ time.Sleep(30 * time.Second)
}
- } else {
- s.mu.Unlock()
- log.Fatalf("Failed to trigger build: %s", err)
}
}
+type BuildkiteChange struct {
+ ID string `json:"id,omitempty"`
+ Number int `json:"number,omitempty"`
+ URL string `json:"url,omitempty"`
+}
+
type Build struct {
- ID string `json:"id,omitempty"`
- GraphqlId string `json:"graphql_id,omitempty"`
- URL string `json:"url,omitempty"`
- WebURL string `json:"web_url,omitempty"`
- Number int `json:"number,omitempty"`
- State string `json:"state,omitempty"`
- Blocked bool `json:"blocked,omitempty"`
- BlockedState string `json:"blocked_state,omitempty"`
- Message string `json:"message,omitempty"`
- Commit string `json:"commit"`
- Branch string `json:"branch"`
- Source string `json:"source,omitempty"`
- CreatedAt string `json:"created_at,omitempty"`
- ScheduledAt string `json:"scheduled_at,omitempty"`
- StartedAt string `json:"started_at,omitempty"`
- FinishedAt string `json:"finished_at,omitempty"`
+ ID string `json:"id,omitempty"`
+ GraphqlId string `json:"graphql_id,omitempty"`
+ URL string `json:"url,omitempty"`
+ WebURL string `json:"web_url,omitempty"`
+ Number int `json:"number,omitempty"`
+ State string `json:"state,omitempty"`
+ Blocked bool `json:"blocked,omitempty"`
+ BlockedState string `json:"blocked_state,omitempty"`
+ Message string `json:"message,omitempty"`
+ Commit string `json:"commit"`
+ Branch string `json:"branch"`
+ Source string `json:"source,omitempty"`
+ CreatedAt string `json:"created_at,omitempty"`
+ ScheduledAt string `json:"scheduled_at,omitempty"`
+ StartedAt string `json:"started_at,omitempty"`
+ FinishedAt string `json:"finished_at,omitempty"`
+ RebuiltFrom *BuildkiteChange `json:"rebuilt_from,omitempty"`
}
type BuildkiteWebhook struct {
@@ -252,13 +275,48 @@
// We've successfully received the webhook. Spawn a goroutine in case the mutex is blocked so we don't block this thread.
f := func() {
- if webhook.Event == "build.finished" {
+ if webhook.Event == "build.running" {
+ if webhook.Build.RebuiltFrom != nil {
+ s.mu.Lock()
+ if c, ok := s.Commits[webhook.Build.RebuiltFrom.ID]; ok {
+ log.Printf("Detected a rebuild of %s for build %s", webhook.Build.RebuiltFrom.ID, webhook.Build.ID)
+ s.Commits[webhook.Build.ID] = c
+
+ // And now remove the vote since the rebuild started.
+ cmd := exec.Command("ssh",
+ "-p",
+ "29418",
+ "-i",
+ s.Key,
+ s.User+"@software.frc971.org",
+ "gerrit",
+ "review",
+ "-m",
+ fmt.Sprintf("\"Build Started: %s\"", webhook.Build.WebURL),
+ // Don't email out the initial link to lower the spam.
+ "-n",
+ "NONE",
+ "--verified",
+ "0",
+ fmt.Sprintf("%d,%d", c.ChangeNumber, c.Patchset))
+
+ log.Printf("Running 'ssh -p 29418 -i %s %s@software.frc971.org gerrit review -m '\"Build Started: %s\"' -n NONE --verified 0 %d,%d' and waiting for it to finish...",
+ s.Key, s.User,
+ webhook.Build.WebURL, c.ChangeNumber, c.Patchset)
+ if err := cmd.Run(); err != nil {
+ log.Printf("Command failed with error: %v", err)
+ }
+ }
+ s.mu.Unlock()
+ }
+ } else if webhook.Event == "build.finished" {
var commit *Commit
{
s.mu.Lock()
if c, ok := s.Commits[webhook.Build.ID]; ok {
commit = &c
- delete(s.Commits, webhook.Build.ID)
+ // While we *should* delete this now from the map, that will prevent rebuilds from being mapped correctly.
+ // Instead, leave it in the map indefinately. For the number of builds we do, it should take quite a while to use enough ram to matter. If that becomes an issue, we can either clean the list when a commit is submitted, or keep a fixed number of builds in the list and expire the oldest ones when it is time.
}
s.mu.Unlock()
}
@@ -390,7 +448,22 @@
case "change-abandoned":
case "change-deleted":
case "change-merged":
- // TODO(austin): Trigger a master build?
+ if eventInfo.RefName == "refs/heads/master" && eventInfo.Change.Status == "MERGED" {
+ if build, _, err := client.Builds.Create(
+ "spartan-robotics", "971-robot-code", &buildkite.CreateBuild{
+ Commit: eventInfo.NewRev,
+ Branch: "master",
+ Author: buildkite.Author{
+ Name: eventInfo.Submitter.Name,
+ Email: eventInfo.Submitter.Email,
+ },
+ }); err == nil {
+ log.Printf("Scheduled master build %s\n", *build.ID)
+ } else {
+ log.Printf("Failed to schedule master build %v", err)
+ // TODO(austin): Notify failure to build. Stephan should be able to pick this up in nagios.
+ }
+ }
case "change-restored":
case "comment-added":
if matched, _ := regexp.MatchString(`(?m)^retest$`, eventInfo.Comment); !matched {
diff --git a/y2020/BUILD b/y2020/BUILD
index 90d976b..964def4 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -65,6 +65,7 @@
"//aos/stl_mutex",
"//frc971:constants",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+ "//frc971/shooter_interpolation:interpolation",
"//y2020/control_loops/drivetrain:polydrivetrain_plants",
"//y2020/control_loops/superstructure/accelerator:accelerator_plants",
"//y2020/control_loops/superstructure/control_panel:control_panel_plants",
diff --git a/y2020/constants.cc b/y2020/constants.cc
index b3551aa..07d10ee 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -34,6 +34,20 @@
::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator>
*const hood = &r->hood;
+ constexpr double kFeetToMeters = 0.0254 * 12.0;
+ // Approximate robot length, for converting estimates from the doc below.
+ // Rounded up from exact estimate, since I'm not sure if the original estimate
+ // includes bumpers.
+ constexpr double kRobotLength = 0.9;
+ // { distance_to_target, { hood_angle, accelerator_power, finisher_power }}
+ // Current settings based on
+ // https://docs.google.com/document/d/1NR9F-ntlSoqZ9LqDzLjn-c14t8ZrppawCCG7wQy47RU/edit
+ r->shot_interpolation_table = InterpolationTable<Values::ShotParams>(
+ {{7.6 * kFeetToMeters - kRobotLength, {0.115, 197.0, 175.0}},
+ {7.6 * kFeetToMeters + kRobotLength, {0.31, 265.0, 235.0}},
+ {12.6 * kFeetToMeters + kRobotLength, {0.4, 292.0, 260.0}},
+ {17.6 * kFeetToMeters + kRobotLength, {0.52, 365.0, 325.0}}});
+
// Hood constants.
hood->zeroing_voltage = 2.0;
hood->operating_voltage = 12.0;
diff --git a/y2020/constants.h b/y2020/constants.h
index ee42245..85f9768 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -8,6 +8,7 @@
#include "frc971/constants.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/shooter_interpolation/interpolation.h"
#include "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
#include "y2020/control_loops/superstructure/control_panel/control_panel_plant.h"
@@ -16,6 +17,8 @@
#include "y2020/control_loops/superstructure/intake/intake_plant.h"
#include "y2020/control_loops/superstructure/turret/turret_plant.h"
+using ::frc971::shooter_interpolation::InterpolationTable;
+
namespace y2020 {
namespace constants {
@@ -184,6 +187,28 @@
// Climber
static constexpr double kClimberSupplyCurrentLimit() { return 60.0; }
+
+ struct ShotParams {
+ // Measured in radians
+ double hood_angle;
+ // Angular velocity in radians per second of the slowest (lowest) wheel in the kicker.
+ // Positive is shooting the ball.
+ double accelerator_power;
+ // Angular velocity in radians per seconds of the flywheel. Positive is shooting.
+ double finisher_power;
+
+ static ShotParams BlendY(double coefficient, ShotParams a1, ShotParams a2) {
+ using ::frc971::shooter_interpolation::Blend;
+ return ShotParams{
+ Blend(coefficient, a1.hood_angle, a2.hood_angle),
+ Blend(coefficient, a1.accelerator_power, a2.accelerator_power),
+ Blend(coefficient, a1.finisher_power, a2.finisher_power)};
+ }
+ };
+
+ // { distance_to_target, { hood_angle, accelerator_power, finisher_power }}
+ InterpolationTable<ShotParams>
+ shot_interpolation_table;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index c408f67..91e782e 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -64,11 +64,40 @@
const flatbuffers::Offset<AimerStatus> aimer_status_offset =
aimer_.PopulateStatus(status->fbb());
+ const double distance_to_goal = aimer_.DistanceToGoal();
+
+ aos::FlatbufferFixedAllocatorArray<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 64>
+ hood_goal;
+ aos::FlatbufferFixedAllocatorArray<ShooterGoal, 64> shooter_goal;
+
+ constants::Values::ShotParams shot_params;
+ if (constants::GetValues().shot_interpolation_table.GetInRange(
+ distance_to_goal, &shot_params)) {
+ hood_goal.Finish(frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *hood_goal.fbb(), shot_params.hood_angle));
+
+ shooter_goal.Finish(CreateShooterGoal(*shooter_goal.fbb(),
+ shot_params.accelerator_power,
+ shot_params.finisher_power));
+ } else {
+ hood_goal.Finish(
+ frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *hood_goal.fbb(), constants::GetValues().hood.range.upper));
+
+ shooter_goal.Finish(CreateShooterGoal(*shooter_goal.fbb(), 0.0, 0.0));
+ }
+
OutputT output_struct;
flatbuffers::Offset<AbsoluteAndAbsoluteEncoderProfiledJointStatus>
hood_status_offset = hood_.Iterate(
- unsafe_goal != nullptr ? unsafe_goal->hood() : nullptr,
+ unsafe_goal != nullptr
+ ? (unsafe_goal->hood_tracking() ? &hood_goal.message()
+ : unsafe_goal->hood())
+ : nullptr,
position->hood(),
output != nullptr ? &(output_struct.hood_voltage) : nullptr,
status->fbb());
@@ -109,6 +138,7 @@
? aimer_.TurretGoal()
: unsafe_goal->turret())
: nullptr;
+
flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
turret_status_offset = turret_.Iterate(
turret_goal, position->turret(),
@@ -117,7 +147,10 @@
flatbuffers::Offset<ShooterStatus> shooter_status_offset =
shooter_.RunIteration(
- unsafe_goal != nullptr ? unsafe_goal->shooter() : nullptr,
+ unsafe_goal != nullptr
+ ? (unsafe_goal->shooter_tracking() ? &shooter_goal.message()
+ : unsafe_goal->shooter())
+ : nullptr,
position->shooter(), status->fbb(),
output != nullptr ? &(output_struct) : nullptr, position_timestamp);
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 7b2b8ad..46cf18a 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -909,7 +909,19 @@
class SuperstructureAllianceTest
: public SuperstructureTest,
- public ::testing::WithParamInterface<aos::Alliance> {};
+ public ::testing::WithParamInterface<aos::Alliance> {
+ protected:
+ void SendAlliancePosition() {
+ auto builder = joystick_state_sender_.MakeBuilder();
+
+ aos::JoystickState::Builder joystick_builder =
+ builder.MakeBuilder<aos::JoystickState>();
+
+ joystick_builder.add_alliance(GetParam());
+
+ ASSERT_TRUE(builder.Send(joystick_builder.Finish()));
+ }
+};
// Tests that the turret switches to auto-aiming when we set turret_tracking to
// true.
@@ -921,16 +933,7 @@
WaitUntilZeroed();
constexpr double kShotAngle = 1.0;
- {
- auto builder = joystick_state_sender_.MakeBuilder();
-
- aos::JoystickState::Builder joystick_builder =
- builder.MakeBuilder<aos::JoystickState>();
-
- joystick_builder.add_alliance(GetParam());
-
- ASSERT_TRUE(builder.Send(joystick_builder.Finish()));
- }
+ SendAlliancePosition();
{
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -975,6 +978,187 @@
superstructure_status_fetcher_->aimer()->turret_velocity());
}
+
+
+// Test a manual goal
+TEST_P(SuperstructureAllianceTest, ShooterInterpolationManualGoal) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ auto shooter_goal = CreateShooterGoal(*builder.fbb(), 400.0, 500.0);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kHoodRange().lower);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_shooter(shooter_goal);
+ goal_builder.add_hood(hood_offset);
+
+ builder.Send(goal_builder.Finish());
+ }
+
+ RunFor(chrono::seconds(10));
+
+ superstructure_status_fetcher_.Fetch();
+
+ EXPECT_DOUBLE_EQ(superstructure_status_fetcher_->shooter()
+ ->accelerator_left()
+ ->angular_velocity_goal(),
+ 400.0);
+ EXPECT_DOUBLE_EQ(superstructure_status_fetcher_->shooter()
+ ->accelerator_right()
+ ->angular_velocity_goal(),
+ 400.0);
+ EXPECT_DOUBLE_EQ(superstructure_status_fetcher_->shooter()
+ ->finisher()
+ ->angular_velocity_goal(),
+ 500.0);
+ EXPECT_NEAR(superstructure_status_fetcher_->hood()->position(),
+ constants::Values::kHoodRange().lower, 0.001);
+}
+
+
+// Test an out of range value with auto tracking
+TEST_P(SuperstructureAllianceTest, ShooterInterpolationOutOfRange) {
+ SetEnabled(true);
+ const frc971::control_loops::Pose target = turret::OuterPortPose(GetParam());
+ WaitUntilZeroed();
+ constexpr double kShotAngle = 1.0;
+ {
+ auto builder = drivetrain_status_sender_.MakeBuilder();
+
+ frc971::control_loops::drivetrain::LocalizerState::Builder
+ localizer_builder = builder.MakeBuilder<
+ frc971::control_loops::drivetrain::LocalizerState>();
+ localizer_builder.add_left_velocity(0.0);
+ localizer_builder.add_right_velocity(0.0);
+ const auto localizer_offset = localizer_builder.Finish();
+
+ DrivetrainStatus::Builder status_builder =
+ builder.MakeBuilder<DrivetrainStatus>();
+
+ // Set the robot up at kShotAngle off from the target, 100m away.
+ status_builder.add_x(target.abs_pos().x() + std::cos(kShotAngle) * 100);
+ status_builder.add_y(target.abs_pos().y() + std::sin(kShotAngle) * 100);
+ status_builder.add_theta(0.0);
+ status_builder.add_localizer(localizer_offset);
+
+ ASSERT_TRUE(builder.Send(status_builder.Finish()));
+ }
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ // Add a goal, this should be ignored with auto tracking
+ auto shooter_goal = CreateShooterGoal(*builder.fbb(), 400.0, 500.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kHoodRange().lower);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_shooter(shooter_goal);
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_shooter_tracking(true);
+ goal_builder.add_hood_tracking(true);
+
+ builder.Send(goal_builder.Finish());
+ }
+ RunFor(chrono::seconds(10));
+
+ superstructure_status_fetcher_.Fetch();
+
+ EXPECT_DOUBLE_EQ(superstructure_status_fetcher_->shooter()
+ ->accelerator_left()
+ ->angular_velocity_goal(),
+ 0.0);
+ EXPECT_DOUBLE_EQ(superstructure_status_fetcher_->shooter()
+ ->accelerator_right()
+ ->angular_velocity_goal(),
+ 0.0);
+ EXPECT_DOUBLE_EQ(superstructure_status_fetcher_->shooter()
+ ->finisher()
+ ->angular_velocity_goal(),
+ 0.0);
+ EXPECT_NEAR(superstructure_status_fetcher_->hood()->position(),
+ constants::Values::kHoodRange().upper, 0.001);
+
+}
+
+
+
+TEST_P(SuperstructureAllianceTest, ShooterInterpolationInRange) {
+ SetEnabled(true);
+ const frc971::control_loops::Pose target = turret::OuterPortPose(GetParam());
+ WaitUntilZeroed();
+ constexpr double kShotAngle = 1.0;
+
+ SendAlliancePosition();
+
+ // Test an in range value returns a reasonable result
+ {
+ auto builder = drivetrain_status_sender_.MakeBuilder();
+
+ frc971::control_loops::drivetrain::LocalizerState::Builder
+ localizer_builder = builder.MakeBuilder<
+ frc971::control_loops::drivetrain::LocalizerState>();
+ localizer_builder.add_left_velocity(0.0);
+ localizer_builder.add_right_velocity(0.0);
+ const auto localizer_offset = localizer_builder.Finish();
+
+ DrivetrainStatus::Builder status_builder =
+ builder.MakeBuilder<DrivetrainStatus>();
+
+ // Set the robot up at kShotAngle off from the target, 2.5m away.
+ status_builder.add_x(target.abs_pos().x() + std::cos(kShotAngle) * 2.5);
+ status_builder.add_y(target.abs_pos().y() + std::sin(kShotAngle) * 2.5);
+ status_builder.add_theta(0.0);
+ status_builder.add_localizer(localizer_offset);
+
+ ASSERT_TRUE(builder.Send(status_builder.Finish()));
+ }
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ // Add a goal, this should be ignored with auto tracking
+ auto shooter_goal = CreateShooterGoal(*builder.fbb(), 400.0, 500.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kHoodRange().lower);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_shooter(shooter_goal);
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_shooter_tracking(true);
+ goal_builder.add_hood_tracking(true);
+
+ builder.Send(goal_builder.Finish());
+ }
+ RunFor(chrono::seconds(10));
+
+ superstructure_status_fetcher_.Fetch();
+
+ EXPECT_GE(superstructure_status_fetcher_->shooter()
+ ->accelerator_left()
+ ->angular_velocity_goal(),
+ 100.0);
+ EXPECT_GE(superstructure_status_fetcher_->shooter()
+ ->accelerator_right()
+ ->angular_velocity_goal(),
+ 100.0);
+ EXPECT_GE(superstructure_status_fetcher_->shooter()
+ ->finisher()
+ ->angular_velocity_goal(),
+ 100.0);
+ EXPECT_GE(superstructure_status_fetcher_->hood()->position(),
+ constants::Values::kHoodRange().lower);
+}
+
INSTANTIATE_TEST_CASE_P(ShootAnyAlliance, SuperstructureAllianceTest,
::testing::Values(aos::Alliance::kRed,
aos::Alliance::kBlue,
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 0a37df6..8905315 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -38,7 +38,7 @@
// TODO(sabina): fix button locations.
const ButtonLocation kShootFast(3, 16);
-const ButtonLocation kTurret(3, 15);
+const ButtonLocation kAutoTrack(3, 15);
const ButtonLocation kHood(3, 3);
const ButtonLocation kShootSlow(4, 2);
const ButtonLocation kFeed(4, 1);
@@ -77,16 +77,13 @@
double hood_pos = constants::Values::kHoodRange().middle();
double intake_pos = -0.89;
- double turret_pos = 0.0;
float roller_speed = 0.0f;
float roller_speed_compensation = 0.0f;
double accelerator_speed = 0.0;
double finisher_speed = 0.0;
double climber_speed = 0.0;
- if (data.IsPressed(kTurret)) {
- turret_pos = 3.5;
- }
+ const bool auto_track = data.IsPressed(kAutoTrack);
if (data.IsPressed(kHood)) {
hood_pos = 0.45;
@@ -150,7 +147,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), turret_pos,
+ *builder.fbb(), 0.0,
CreateProfileParameters(*builder.fbb(), 6.0, 20.0));
flatbuffers::Offset<superstructure::ShooterGoal> shooter_offset =
@@ -170,6 +167,10 @@
superstructure_goal_builder.add_shooting(data.IsPressed(kFeed));
superstructure_goal_builder.add_climber_voltage(climber_speed);
+ superstructure_goal_builder.add_turret_tracking(auto_track);
+ superstructure_goal_builder.add_hood_tracking(auto_track);
+ superstructure_goal_builder.add_shooter_tracking(auto_track);
+
if (!builder.Send(superstructure_goal_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
diff --git a/y2020/vision/ball_detection.py b/y2020/vision/ball_detection.py
deleted file mode 100644
index 7f2dd6f..0000000
--- a/y2020/vision/ball_detection.py
+++ /dev/null
@@ -1,29 +0,0 @@
-#!/usr/bin/python3
-
-from rect import Rect
-
-import cv2 as cv
-import numpy as np
-# This function finds the percentage of yellow pixels in the rectangles
-# given that are regions of the given image. This allows us to determine
-# whether there is a ball in those rectangles
-def pct_yellow(img, rects):
- hsv = cv.cvtColor(img, cv.COLOR_BGR2HSV)
- lower_yellow = np.array([23, 100, 75], dtype = np.uint8)
- higher_yellow = np.array([40, 255, 255], dtype = np.uint8)
- mask = cv.inRange(hsv, lower_yellow, higher_yellow)
-
- pcts = np.zeros(len(rects))
- for i in range(len(rects)):
- rect = rects[i]
- slice = mask[rect.y1 : rect.y2, rect.x1 : rect.x2]
- yellow_px = np.count_nonzero(slice)
- pcts[i] = 100 * (yellow_px / (slice.shape[0] * slice.shape[1]))
-
- return pcts
-
-def capture_img():
- video_stream = cv.VideoCapture(0)
- frame = video_stream.read()[1]
- video_stream.release()
- return frame
diff --git a/y2020/vision/galactic_search_config.py b/y2020/vision/galactic_search_config.py
new file mode 100755
index 0000000..86bb693
--- /dev/null
+++ b/y2020/vision/galactic_search_config.py
@@ -0,0 +1,155 @@
+#!/usr/bin/python3
+
+# Creates a UI for a user to select the regions in a camera image where the balls could be placed
+# for each field layout.
+# After the balls have been placed on the field and they submit the regions,
+# galactic_search_path.py will take another picture and based on the yellow regions
+# in that picture it will determine where the balls are.
+# This tells us which path the current field is. It then sends the Alliance and Letter of the path
+# with aos_send to the /camera channel for the robot to excecute the spline for that path.
+
+from galactic_search_path import *
+
+import getopt
+import glog
+import json
+import matplotlib.patches as patches
+import matplotlib.pyplot as plt
+from matplotlib.widgets import Button
+import numpy as np
+import os
+import sys
+
+_num_rects = 3 # can be 3 or 2, can be specified in commang line arg
+
+setup_if_pi()
+
+_path = Path(Letter.kA, Alliance.kRed, [Rect(None, None, None, None)])
+
+# current index in rects list
+_rect_index = 0
+
+_fig, _img_ax = plt.subplots()
+
+_txt = _img_ax.text(0, 0, "", size = 10, backgroundcolor = "white")
+
+_confirm = Button(plt.axes([0.7, 0.05, 0.1, 0.075]), "Confirm")
+_cancel = Button(plt.axes([0.81, 0.05, 0.1, 0.075]), "Cancel")
+_submit = Button(plt.axes([0.4, 0.4, 0.1, 0.1]), "Submit")
+
+def draw_txt(txt):
+ txt.set_text("Click on top left point and bottom right point for rect #%u" % (_rect_index + 1))
+ txt.set_color(_path.alliance.value)
+
+
+def on_confirm(event):
+ global _rect_index
+ if _path.rects[_rect_index].x1 != None and _path.rects[_rect_index].x2 != None:
+ _confirm.ax.set_visible(False)
+ _cancel.ax.set_visible(False)
+ _rect_index += 1
+ clear_rect()
+ if _rect_index == _num_rects:
+ _submit.ax.set_visible(True)
+ else:
+ draw_txt(_txt)
+ _path.rects.append(Rect(None, None, None, None))
+ plt.show()
+
+def on_cancel(event):
+ global _rect_index
+ if _rect_index < _num_rects:
+ _confirm.ax.set_visible(False)
+ _cancel.ax.set_visible(False)
+ clear_rect()
+ _path.rects[_rect_index].x1 = None
+ _path.rects[_rect_index].y1 = None
+ _path.rects[_rect_index].x2 = None
+ _path.rects[_rect_index].y2 = None
+ plt.show()
+
+def on_submit(event):
+ plt.close("all")
+ dict = None
+ with open(RECTS_JSON_PATH, 'r') as rects_json:
+ dict = json.load(rects_json)
+ if _path.letter.name not in dict:
+ dict[_path.letter.name] = {}
+ if _path.alliance.name not in dict[_path.letter.name]:
+ dict[_path.letter.name][_path.alliance.name] = []
+ dict[_path.letter.name][_path.alliance.name] = [rect.to_list() for rect in _path.rects]
+ with open(RECTS_JSON_PATH, 'w') as rects_json:
+ json.dump(dict, rects_json, indent = 2)
+
+# Clears rect on screen
+def clear_rect():
+ if len(_img_ax.patches) == 0:
+ glog.error("There were no patches found in _img_ax")
+ else:
+ _img_ax.patches[-1].remove()
+
+def on_click(event):
+ # This gets called for each click of the rectangle corners,
+ # but also gets called when the user clicks on the Submit button.
+ # At that time _rect_index will equal the length of rects, and so we'll ignore that click.
+ # If it checked the points of the rect at _rect_index, a list out of bounds exception would be thrown.
+ # Additionally, the event xdata or ydata will be None if the user clicks out of
+ # the bounds of the axis
+ if _rect_index < _num_rects and event.xdata != None and event.ydata != None:
+ if _path.rects[_rect_index].x1 == None:
+ _path.rects[_rect_index].x1, _path.rects[_rect_index].y1 = int(event.xdata), int(event.ydata)
+ elif _path.rects[_rect_index].x2 == None:
+ _path.rects[_rect_index].x2, _path.rects[_rect_index].y2 = int(event.xdata), int(event.ydata)
+ if _path.rects[_rect_index].x2 < _path.rects[_rect_index].x1:
+ tmp = _path.rects[_rect_index].x1
+ _path.rects[_rect_index].x1 = _path.rects[_rect_index].x2
+ _path.rects[_rect_index].x2 = tmp
+ if _path.rects[_rect_index].y2 < _path.rects[_rect_index].y1:
+ tmp = _path.rects[_rect_index].y1
+ _path.rects[_rect_index].y1 = _path.rects[_rect_index].y2
+ _path.rects[_rect_index].y2 = tmp
+
+ _img_ax.add_patch(patches.Rectangle((_path.rects[_rect_index].x1, _path.rects[_rect_index].y1),
+ _path.rects[_rect_index].x2 - _path.rects[_rect_index].x1,
+ _path.rects[_rect_index].y2 - _path.rects[_rect_index].y1,
+ edgecolor = 'r', linewidth = 1, facecolor="none"))
+ _confirm.ax.set_visible(True)
+ _cancel.ax.set_visible(True)
+ plt.show()
+ else:
+ glog.info("Either submitted or user pressed out of the bounds of the axis")
+
+def setup_button(button, on_clicked):
+ button.on_clicked(on_clicked)
+ button.ax.set_visible(False)
+
+def setup_ui():
+ _img_ax.imshow(capture_img())
+ release_stream()
+
+ _fig.canvas.mpl_connect("button_press_event", on_click)
+ setup_button(_confirm, on_confirm)
+ setup_button(_cancel, on_cancel)
+ setup_button(_submit, on_submit)
+ draw_txt(_txt)
+ plt.show()
+
+def main(argv):
+ global _num_rects
+
+ glog.setLevel("INFO")
+ opts = getopt.getopt(argv[1 : ], "a:l:n:",
+ ["alliance = ", "letter = ", "_num_rects = "])[0]
+ for opt, arg in opts:
+ if opt in ["-a", "--alliance"]:
+ _path.alliance = Alliance.from_value(arg)
+ elif opt in ["-l", "--letter"]:
+ _path.letter = Letter.from_value(arg.upper())
+ elif opt in ["-n", "--_num_rects"] and arg.isdigit():
+ _num_rects = int(arg)
+
+ setup_ui()
+
+
+if __name__ == "__main__":
+ main(sys.argv)
diff --git a/y2020/vision/galactic_search_path.py b/y2020/vision/galactic_search_path.py
index 242253f..d212b0b 100644
--- a/y2020/vision/galactic_search_path.py
+++ b/y2020/vision/galactic_search_path.py
@@ -1,180 +1,184 @@
#!/usr/bin/python3
-# Creates a UI for a user to select the regions in a camera image where the balls could be placed.
-# After the balls have been placed on the field and they submit the regions,
-# it will take another picture and based on the yellow regions in that picture it will determine where the
-# balls are. This tells us which path the current field is. It then sends the Alliance and Letter of the path
-# with aos_send to the /camera channel for the robot to excecute the spline for that path.
-
-from rect import Rect
-import ball_detection
-
import cv2 as cv
from enum import Enum
import glog
import json
-import matplotlib.patches as patches
import matplotlib.pyplot as plt
-from matplotlib.widgets import Button
import numpy as np
import os
+class Rect:
+
+ # x1 and y1 are top left corner, x2 and y2 are bottom right
+ def __init__(self, x1, y1, x2, y2):
+ self.x1 = x1
+ self.y1 = y1
+ self.x2 = x2
+ self.y2 = y2
+
+ def __str__(self):
+ return "({}, {}), ({}, {})".format(self.x1, self.y1, self.x2, self.y2)
+
+ def to_list(self):
+ return [self.x1, self.y1, self.x2, self.y2]
+
+ @classmethod
+ def from_list(cls, list):
+ rect = None
+ if len(list) == 4:
+ rect = cls(list[0], list[1], list[2], list[3])
+ else:
+ glog.error("Expected list len to be 4 but it was %u", len(list))
+ rect = cls(None, None, None, None)
+ return rect
+
+
class Alliance(Enum):
kRed = "red"
kBlue = "blue"
kUnknown = None
+ @staticmethod
+ def from_value(value):
+ return (Alliance.kRed if value == Alliance.kRed.value else Alliance.kBlue)
+
+ @staticmethod
+ def from_name(name):
+ return (Alliance.kRed if name == Alliance.kRed.name else Alliance.kBlue)
+
class Letter(Enum):
- kA = "A"
- kB = "B"
+ kA = 'A'
+ kB = 'B'
+ @staticmethod
+ def from_value(value):
+ return (Letter.kA if value == Letter.kA.value else Letter.kB)
-NUM_RECTS = 4
+ @staticmethod
+ def from_name(name):
+ return (Letter.kA if name == Letter.kA.name else Letter.kB)
+
+class Path:
+
+ def __init__(self, letter, alliance, rects):
+ self.letter = letter
+ self.alliance = alliance
+ self.rects = rects
+
+ def __str__(self):
+ return "%s %s: " % (self.alliance.value, self.letter.value)
+
+ def to_dict(self):
+ return {"alliance": self.alliance.name, "letter": self.letter.name}
+
+RECTS_JSON_PATH = "rects.json"
+
AOS_SEND_PATH = "bazel-bin/aos/aos_send"
-if os.path.isdir("/home/pi/robot_code"):
- AOS_SEND_PATH = "/home/pi/robot_code/aos_send.stripped"
- os.system("./starter_cmd stop camera_reader")
+def setup_if_pi():
+ if os.path.isdir("/home/pi/robot_code"):
+ AOS_SEND_PATH = "/home/pi/robot_code/aos_send.stripped"
+ os.system("./starter_cmd stop camera_reader")
+
+setup_if_pi()
# The minimum percentage of yellow for a region of a image to
# be considered to have a ball
-BALL_PCT_THRESHOLD = 10
+BALL_PCT_THRESHOLD = 0.1
-rects = [Rect(None, None, None, None)]
+_paths = []
-# current index in rects list
-rect_index = 0
+def _run_detection_loop():
+ global img_fig, rects_dict
-fig, img_ax = plt.subplots()
+ with open(RECTS_JSON_PATH, 'r') as rects_json:
+ rects_dict = json.load(rects_json)
+ for letter in rects_dict:
+ for alliance in rects_dict[letter]:
+ rects = []
+ for rect_list in rects_dict[letter][alliance]:
+ rects.append(Rect.from_list(rect_list))
+ _paths.append(Path(Letter.from_name(letter), Alliance.from_name(alliance), rects))
-txt = img_ax.text(0, 0, "", size = 10, backgroundcolor = "white")
-
-confirm = Button(plt.axes([0.7, 0.05, 0.1, 0.075]), "Confirm")
-cancel = Button(plt.axes([0.81, 0.05, 0.1, 0.075]), "Cancel")
-submit = Button(plt.axes([0.4, 0.4, 0.1, 0.1]), "Submit")
-
-def draw_txt():
- alliance = (Alliance.kRed if rect_index % 2 == 0 else Alliance.kBlue)
- letter = (Letter.kA if rect_index < (NUM_RECTS / 2) else Letter.kB)
- txt.set_text("Click on top left point and bottom right point for " +
- alliance.value + ", path " + letter.value)
- txt.set_color(alliance.value)
-
-
-def on_confirm(event):
- global rect_index
- if rects[rect_index].x1 != None and rects[rect_index].x2 != None:
- confirm.ax.set_visible(False)
- cancel.ax.set_visible(False)
- rect_index += 1
- clear_rect()
- if rect_index == NUM_RECTS:
- submit.ax.set_visible(True)
- else:
- draw_txt()
- rects.append(Rect(None, None, None, None))
- plt.show()
-
-def on_cancel(event):
- global rect_index
- if rect_index < NUM_RECTS:
- confirm.ax.set_visible(False)
- cancel.ax.set_visible(False)
- clear_rect()
- rects[rect_index].x1 = None
- rects[rect_index].y1 = None
- rects[rect_index].x2 = None
- rects[rect_index].y2 = None
- plt.show()
-
-SLEEP = 100
-img_fig = None
-
-def on_submit(event):
- global img_fig
- plt.close("all")
plt.ion()
img_fig = plt.figure()
+
running = True
while running:
- detect_path()
- cv.waitKey(SLEEP)
+ _detect_path()
-def detect_path():
- img = ball_detection.capture_img()
+def _detect_path():
+ img = capture_img()
img_fig.figimage(img)
plt.show()
plt.pause(0.001)
- pcts = ball_detection.pct_yellow(img, rects)
- if len(pcts) == len(rects):
- paths = []
- for i in range(len(pcts)):
- alliance = (Alliance.kRed if i % 2 == 0 else Alliance.kBlue)
- letter = (Letter.kA if i < NUM_RECTS / 2 else Letter.kB)
- paths.append({"alliance" : alliance.name, "letter" : letter.name})
- max_index = np.argmax(pcts)
- path = paths[max_index]
- # Make sure that exactly one percentage is >= the threshold
- rects_with_balls = np.where(pcts >= BALL_PCT_THRESHOLD)[0].size
- glog.info("rects_with_balls: %s" % rects_with_balls)
- if rects_with_balls != 1:
- path["alliance"] = Alliance.kUnknown.name
- glog.warn("More than one ball found, path is unknown" if rects_with_balls > 1 else
- "No balls found")
- glog.info("Path is %s" % path)
- os.system(AOS_SEND_PATH +
- " /pi2/camera y2020.vision.GalacticSearchPath '" + json.dumps(path) + "'")
- for j in range(len(pcts)):
- glog.info("%s: %s%% yellow" % (rects[j], pcts[j]))
- else:
- glog.error("Error: len of pcts (%u) != len of rects: (%u)" % (len(pcts), len(rects)))
+ mask = _create_mask(img)
-# Clears rect on screen
-def clear_rect():
- if len(img_ax.patches) == 0:
- glog.error("There were no patches found in img_ax")
- else:
- img_ax.patches[-1].remove()
+ current_path = None
+ num_current_paths = 0
+ for path in _paths:
+ pcts = _pct_yellow(mask, path.rects)
+ if len(pcts) == len(path.rects):
+ glog.info(path)
+ for i in range(len(pcts)):
+ glog.info("Percent yellow of %s: %f", path.rects[i], pcts[i])
+ glog.info("")
-def on_click(event):
- # This will get called when user clicks on Submit button, don't want to override the points on
- # the last rect. Additionally, the event xdata or ydata will be None if the user clicks out of
- # the bounds of the axis
- if rect_index < NUM_RECTS and event.xdata != None and event.ydata != None:
- if rects[rect_index].x1 == None:
- rects[rect_index].x1, rects[rect_index].y1 = int(event.xdata), int(event.ydata)
- elif rects[rect_index].x2 == None:
- rects[rect_index].x2, rects[rect_index].y2 = int(event.xdata), int(event.ydata)
- if rects[rect_index].x2 < rects[rect_index].x1:
- rects[rect_index].x2 = rects[rect_index].x1 + (rects[rect_index].x1 - rects[rect_index].x2)
- if rects[rect_index].y2 < rects[rect_index].y1:
- rects[rect_index].y2 = rects[rect_index].y1 + (rects[rect_index].y1 - rects[rect_index].y2)
+ # If all the balls in a path were detected then that path is present
+ rects_with_balls = np.where(pcts >= BALL_PCT_THRESHOLD)[0].size
+ if rects_with_balls == len(path.rects):
+ current_path = path
+ num_current_paths += 1
+ else:
+ glog.error("Error: len of pcts (%u) != len of rects: (%u)", len(pcts), len(rects))
- img_ax.add_patch(patches.Rectangle((rects[rect_index].x1, rects[rect_index].y1),
- rects[rect_index].x2 - rects[rect_index].x1, rects[rect_index].y2 - rects[rect_index].y1,
- edgecolor = 'r', linewidth = 1, facecolor="none"))
- confirm.ax.set_visible(True)
- cancel.ax.set_visible(True)
- plt.show()
- else:
- glog.info("Either submitted or user pressed out of the bounds of the axis")
+ if num_current_paths != 1:
+ if num_current_paths == 0:
+ current_path = Path(Letter.kA, None, None)
+ current_path.alliance = Alliance.kUnknown
+ glog.warn("Expected 1 path but detected %u", num_current_paths)
-def setup_button(button, on_clicked):
- button.on_clicked(on_clicked)
- button.ax.set_visible(False)
+
+ path_dict = current_path.to_dict()
+ glog.info("Path is %s", path_dict)
+ os.system(AOS_SEND_PATH +
+ " /pi2/camera y2020.vision.GalacticSearchPath '" + json.dumps(path_dict) + "'")
+
+def _create_mask(img):
+ hsv = cv.cvtColor(img, cv.COLOR_BGR2HSV)
+ lower_yellow = np.array([23, 100, 75], dtype = np.uint8)
+ higher_yellow = np.array([40, 255, 255], dtype = np.uint8)
+ mask = cv.inRange(hsv, lower_yellow, higher_yellow)
+ return mask
+
+# This function finds the percentage of yellow pixels in the rectangles
+# given that are regions of the given image. This allows us to determine
+# whether there is a ball in those rectangles
+def _pct_yellow(mask, rects):
+ pcts = np.zeros(len(rects))
+ for i in range(len(rects)):
+ rect = rects[i]
+ slice = mask[rect.y1 : rect.y2, rect.x1 : rect.x2]
+ yellow_px = np.count_nonzero(slice)
+ pcts[i] = yellow_px / (slice.shape[0] * slice.shape[1])
+
+ return pcts
+
+_video_stream = cv.VideoCapture(0)
+
+def capture_img():
+ global _video_stream
+ return _video_stream.read()[1]
+
+def release_stream():
+ global _video_stream
+ _video_stream.release()
def main():
- glog.setLevel("INFO")
-
- img_ax.imshow(ball_detection.capture_img())
-
- fig.canvas.mpl_connect("button_press_event", on_click)
- setup_button(confirm, on_confirm)
- setup_button(cancel, on_cancel)
- setup_button(submit, on_submit)
- draw_txt()
- plt.show()
+ _run_detection_loop()
+ release_stream()
if __name__ == "__main__":
main()
diff --git a/y2020/vision/rect.py b/y2020/vision/rect.py
deleted file mode 100644
index d57a005..0000000
--- a/y2020/vision/rect.py
+++ /dev/null
@@ -1,13 +0,0 @@
-#!/usr/bin/python3
-
-class Rect:
-
- # x1 and y1 are top left corner, x2 and y2 are bottom right
- def __init__(self, x1, y1, x2, y2):
- self.x1 = x1
- self.y1 = y1
- self.x2 = x2
- self.y2 = y2
-
- def __str__(self):
- return "({}, {}), ({}, {})".format(self.x1, self.y1, self.x2, self.y2)
diff --git a/y2020/vision/rects.json b/y2020/vision/rects.json
new file mode 100755
index 0000000..eb6a17d
--- /dev/null
+++ b/y2020/vision/rects.json
@@ -0,0 +1,80 @@
+{
+ "kA": {
+ "kRed": [
+ [
+ 233,
+ 296,
+ 281,
+ 340
+ ],
+ [
+ 192,
+ 188,
+ 211,
+ 213
+ ]
+ ],
+ "kBlue": [
+ [
+ 270,
+ 176,
+ 289,
+ 193
+ ],
+ [
+ 175,
+ 151,
+ 183,
+ 163
+ ],
+ [
+ 50,
+ 171,
+ 71,
+ 185
+ ]
+ ]
+ },
+ "kB": {
+ "kRed": [
+ [
+ 250,
+ 301,
+ 302,
+ 340
+ ],
+ [
+ 292,
+ 180,
+ 314,
+ 202
+ ],
+ [
+ 44,
+ 190,
+ 63,
+ 206
+ ]
+ ],
+ "kBlue": [
+ [
+ 236,
+ 142,
+ 246,
+ 154
+ ],
+ [
+ 181,
+ 181,
+ 200,
+ 197
+ ],
+ [
+ 94,
+ 158,
+ 106,
+ 172
+ ]
+ ]
+ }
+}
\ No newline at end of file