Add 2nd robot ball color rects
Also, switch logger hostname to pi6 so we can actually get the team
number on it.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I90f1329ff1a5922559b2b37c9778eec0dcdd59be
diff --git a/y2022/constants.cc b/y2022/constants.cc
index aaf8cde..c97058e 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -21,14 +21,6 @@
const int Values::kZeroingSampleSize;
-namespace {
-
-const uint16_t kCompTeamNumber = 971;
-const uint16_t kPracticeTeamNumber = 9971;
-const uint16_t kCodingRobotTeamNumber = 7971;
-
-} // namespace
-
Values MakeValues(uint16_t team) {
LOG(INFO) << "creating a Constants for team: " << team;
@@ -182,6 +174,8 @@
});
}
+ Values::BallColorParams *const ball_color = &r.ball_color;
+
switch (team) {
// A set of constants for tests.
case 1:
@@ -211,6 +205,11 @@
catapult_params->zeroing_constants.measured_absolute_position = 0.0;
catapult->potentiometer_offset = 0.0;
+
+ ball_color->reference_red = {0, 0, 1, 1};
+ ball_color->reference_blue = {0, 0, 1, 1};
+ ball_color->ball_location = {0, 0, 1, 1};
+
break;
case kCompTeamNumber:
@@ -249,6 +248,11 @@
catapult_params->zeroing_constants.measured_absolute_position =
1.71723370408082;
catapult->potentiometer_offset = -2.03383240293769;
+
+ ball_color->reference_red = {440, 150, 50, 130};
+ ball_color->reference_blue = {440, 350, 30, 100};
+ ball_color->ball_location = {100, 400, 140, 50};
+
break;
case kPracticeTeamNumber:
@@ -299,6 +303,11 @@
catapult_params->zeroing_constants.measured_absolute_position =
1.62909518684227;
catapult->potentiometer_offset = -1.52951814169821 - 0.0200812009850977;
+
+ ball_color->reference_red = {526, 75, 110, 220};
+ ball_color->reference_blue = {526, 340, 110, 100};
+ ball_color->ball_location = {40, 440, 200, 30};
+
break;
case kCodingRobotTeamNumber:
@@ -328,6 +337,11 @@
catapult_params->zeroing_constants.measured_absolute_position = 0.0;
catapult->potentiometer_offset = 0.0;
+
+ ball_color->reference_red = {0, 0, 1, 1};
+ ball_color->reference_blue = {0, 0, 1, 1};
+ ball_color->ball_location = {0, 0, 1, 1};
+
break;
default:
diff --git a/y2022/constants.h b/y2022/constants.h
index f410014..297f814 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -20,6 +20,10 @@
namespace y2022 {
namespace constants {
+constexpr uint16_t kCompTeamNumber = 971;
+constexpr uint16_t kPracticeTeamNumber = 9971;
+constexpr uint16_t kCodingRobotTeamNumber = 7971;
+
struct Values {
static const int kZeroingSampleSize = 200;
@@ -233,6 +237,15 @@
InterpolationTable<ShotParams> shot_interpolation_table;
InterpolationTable<ShotVelocityParams> shot_velocity_interpolation_table;
+
+ struct BallColorParams {
+ // Rects stored as {x, y, width, height}
+ std::array<int, 4> reference_red;
+ std::array<int, 4> reference_blue;
+ std::array<int, 4> ball_location;
+ };
+
+ BallColorParams ball_color;
};
// Creates and returns a Values instance for the constants.
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index e582d9e..3d997da 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -325,8 +325,7 @@
: ::frc971::testing::ControlLoopTest(
aos::configuration::ReadConfig("y2022/aos_config.json"),
std::chrono::microseconds(5050)),
- values_(std::make_shared<constants::Values>(constants::MakeValues(
- frc971::control_loops::testing::kTeamNumber))),
+ values_(std::make_shared<constants::Values>(constants::MakeValues())),
roborio_(aos::configuration::GetNode(configuration(), "roborio")),
logger_pi_(aos::configuration::GetNode(configuration(), "logger")),
superstructure_event_loop(MakeEventLoop("Superstructure", roborio_)),
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index 5f6d8a8..396a2eb 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -199,10 +199,10 @@
":ball_color_fbs",
"//aos/events:event_loop",
"//aos/events:shm_event_loop",
- "//aos/network:team_number",
"//frc971/input:joystick_state_fbs",
"//frc971/vision:vision_fbs",
"//third_party:opencv",
+ "//y2022:constants",
],
)
diff --git a/y2022/vision/ball_color.cc b/y2022/vision/ball_color.cc
index e896da5..dd1ae6e 100644
--- a/y2022/vision/ball_color.cc
+++ b/y2022/vision/ball_color.cc
@@ -15,8 +15,19 @@
namespace y2022 {
namespace vision {
-BallColorDetector::BallColorDetector(aos::EventLoop *event_loop)
- : ball_color_sender_(event_loop->MakeSender<BallColor>("/superstructure")) {
+namespace {
+cv::Rect ArrayToRect(const std::array<int, 4> &values) {
+ return cv::Rect{values[0], values[1], values[2], values[3]};
+}
+} // namespace
+
+BallColorDetector::BallColorDetector(
+ aos::EventLoop *event_loop, std::shared_ptr<const constants::Values> values)
+ : ball_color_sender_(event_loop->MakeSender<BallColor>("/superstructure")),
+ values_(values),
+ reference_red_(ArrayToRect(values_->ball_color.reference_red)),
+ reference_blue_(ArrayToRect(values_->ball_color.reference_blue)),
+ ball_location_(ArrayToRect(values_->ball_color.ball_location)) {
event_loop->MakeWatcher("/camera", [this](const CameraImage &camera_image) {
this->ProcessImage(camera_image);
});
@@ -42,22 +53,20 @@
cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV);
// Look at 3 chunks of the image
- cv::Mat reference_red =
- BallColorDetector::SubImage(hsv, BallColorDetector::kReferenceRed());
+ cv::Mat reference_red_mat = BallColorDetector::SubImage(hsv, reference_red_);
- cv::Mat reference_blue =
- BallColorDetector::SubImage(hsv, BallColorDetector::kReferenceBlue());
- cv::Mat ball_location =
- BallColorDetector::SubImage(hsv, BallColorDetector::kBallLocation());
+ cv::Mat reference_blue_mat =
+ BallColorDetector::SubImage(hsv, reference_blue_);
+ cv::Mat ball_location_mat = BallColorDetector::SubImage(hsv, ball_location_);
// OpenCV HSV hues go from [0 to 179]
// Average the average color of each patch in both directions
// Rejecting pixels that have too low saturation or to bright or dark value
// And dealing with the wrapping of the red hues by shifting the wrap to be
// around 90 instead of 180. 90 is a color we don't care about.
- double red = BallColorDetector::mean_hue(reference_red);
- double blue = BallColorDetector::mean_hue(reference_blue);
- double ball = BallColorDetector::mean_hue(ball_location);
+ double red = BallColorDetector::mean_hue(reference_red_mat);
+ double blue = BallColorDetector::mean_hue(reference_blue_mat);
+ double ball = BallColorDetector::mean_hue(ball_location_mat);
// Just look at the hue values for distance
const double distance_to_blue = std::abs(ball - blue);
diff --git a/y2022/vision/ball_color.h b/y2022/vision/ball_color.h
index ef3bdd2..695d2e3 100644
--- a/y2022/vision/ball_color.h
+++ b/y2022/vision/ball_color.h
@@ -6,6 +6,7 @@
#include "aos/events/shm_event_loop.h"
#include "frc971/input/joystick_state_generated.h"
#include "frc971/vision/vision_generated.h"
+#include "y2022/constants.h"
#include "y2022/vision/ball_color_generated.h"
namespace y2022 {
@@ -20,10 +21,7 @@
public:
// The size image that the reference rectangles were measure with
// These constants will be scaled if the image sent is not the same size
- static const cv::Size kMeasurementsImageSize() { return {640, 480}; };
- static const cv::Rect kReferenceRed() { return {440, 150, 50, 130}; };
- static const cv::Rect kReferenceBlue() { return {440, 350, 30, 100}; };
- static const cv::Rect kBallLocation() { return {100, 400, 140, 50}; };
+ static const cv::Size kMeasurementsImageSize() { return {640, 480}; }
// Constants used to filter out pixels that don't have good color information
static constexpr double kMinSaturation = 128;
@@ -32,7 +30,8 @@
static constexpr double kMaxHueDistance = 10;
- BallColorDetector(aos::EventLoop *event_loop);
+ BallColorDetector(aos::EventLoop *event_loop,
+ std::shared_ptr<const constants::Values> values);
void ProcessImage(const CameraImage &camera_image);
@@ -42,7 +41,11 @@
// the average hue of each patch but discard pixels that we deem not colorful
// enough. Then we decide whether the ball color looks close enough to either
// of the reference colors. If no good color is detected, outputs kInvalid.
- static aos::Alliance DetectColor(cv::Mat image);
+ aos::Alliance DetectColor(cv::Mat image);
+
+ cv::Rect reference_red() const { return reference_red_; }
+ cv::Rect reference_blue() const { return reference_blue_; }
+ cv::Rect ball_location() const { return ball_location_; }
static cv::Mat SubImage(cv::Mat image, cv::Rect location);
@@ -52,6 +55,12 @@
private:
aos::Sender<BallColor> ball_color_sender_;
+
+ std::shared_ptr<const constants::Values> values_;
+
+ const cv::Rect reference_red_;
+ const cv::Rect reference_blue_;
+ const cv::Rect ball_location_;
};
} // namespace vision
} // namespace y2022
diff --git a/y2022/vision/ball_color_main.cc b/y2022/vision/ball_color_main.cc
index 63f9d06..03e3033 100644
--- a/y2022/vision/ball_color_main.cc
+++ b/y2022/vision/ball_color_main.cc
@@ -1,5 +1,6 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
+#include "y2022/constants.h"
#include "y2022/vision/ball_color.h"
// config used to allow running ball_color_detector independently. E.g.,
@@ -19,8 +20,9 @@
aos::configuration::ReadConfig(FLAGS_config);
aos::ShmEventLoop event_loop(&config.message());
-
- BallColorDetector ball_color_detector(&event_loop);
+ std::shared_ptr<const constants::Values> values =
+ std::make_shared<const constants::Values>(constants::MakeValues());
+ BallColorDetector ball_color_detector(&event_loop, std::move(values));
event_loop.Run();
}
diff --git a/y2022/vision/ball_color_test.cc b/y2022/vision/ball_color_test.cc
index 695791b..b8ad359 100644
--- a/y2022/vision/ball_color_test.cc
+++ b/y2022/vision/ball_color_test.cc
@@ -33,9 +33,10 @@
event_loop_factory_.MakeEventLoop("Superstructure", roborio_)),
ball_color_fetcher_(superstructure_event_loop_->MakeFetcher<BallColor>(
"/superstructure")),
- image_sender_(camera_event_loop_->MakeSender<CameraImage>("/camera"))
-
- {}
+ image_sender_(camera_event_loop_->MakeSender<CameraImage>("/camera")),
+ detector_(color_detector_event_loop_.get(),
+ std::make_shared<const constants::Values>(
+ constants::MakeValues(constants::kCompTeamNumber))) {}
// copied from camera_reader.cc
void SendImage(cv::Mat bgr_image) {
@@ -84,6 +85,8 @@
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
aos::Fetcher<BallColor> ball_color_fetcher_;
aos::Sender<CameraImage> image_sender_;
+
+ BallColorDetector detector_;
};
TEST_F(BallColorTest, DetectColorFromTestImage) {
@@ -92,7 +95,7 @@
ASSERT_TRUE(bgr_image.data != nullptr);
- aos::Alliance detected_color = BallColorDetector::DetectColor(bgr_image);
+ aos::Alliance detected_color = detector_.DetectColor(bgr_image);
EXPECT_EQ(detected_color, aos::Alliance::kRed);
}
@@ -102,8 +105,6 @@
cv::imread("y2022/vision/test_ball_color_image.jpg", cv::IMREAD_COLOR);
ASSERT_TRUE(bgr_image.data != nullptr);
- BallColorDetector detector(color_detector_event_loop_.get());
-
camera_event_loop_->OnRun([this, bgr_image]() { SendImage(bgr_image); });
event_loop_factory_.RunFor(std::chrono::milliseconds(5));
@@ -128,13 +129,13 @@
ASSERT_TRUE(bgr_image.data != nullptr);
cv::Rect reference_red = BallColorDetector::RescaleRect(
- bgr_image, BallColorDetector::kReferenceRed(),
+ bgr_image, detector_.reference_red(),
BallColorDetector::kMeasurementsImageSize());
cv::Rect reference_blue = BallColorDetector::RescaleRect(
- bgr_image, BallColorDetector::kReferenceBlue(),
+ bgr_image, detector_.reference_blue(),
BallColorDetector::kMeasurementsImageSize());
cv::Rect ball_location = BallColorDetector::RescaleRect(
- bgr_image, BallColorDetector::kBallLocation(),
+ bgr_image, detector_.ball_location(),
BallColorDetector::kMeasurementsImageSize());
cv::rectangle(bgr_image, reference_red, cv::Scalar(0, 0, 255));
diff --git a/y2022/y2022_logger.json b/y2022/y2022_logger.json
index f37a4d6..a8a4bbd 100644
--- a/y2022/y2022_logger.json
+++ b/y2022/y2022_logger.json
@@ -549,7 +549,7 @@
"nodes": [
{
"name": "logger",
- "hostname": "logger",
+ "hostname": "pi6",
"hostnames": [
"pi-971-6",
"pi-9971-6",