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/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));