diff --git a/src/bitbots_misc/bitbots_bringup/launch/vision.launch b/src/bitbots_misc/bitbots_bringup/launch/vision.launch index 5e78ca588..538c2c92e 100644 --- a/src/bitbots_misc/bitbots_bringup/launch/vision.launch +++ b/src/bitbots_misc/bitbots_bringup/launch/vision.launch @@ -4,6 +4,8 @@ + + @@ -18,6 +20,14 @@ + + + + diff --git a/src/bitbots_misc/bitbots_bringup/launch/vision_standalone.launch b/src/bitbots_misc/bitbots_bringup/launch/vision_standalone.launch index 2cdc0ecb7..75a2209e1 100644 --- a/src/bitbots_misc/bitbots_bringup/launch/vision_standalone.launch +++ b/src/bitbots_misc/bitbots_bringup/launch/vision_standalone.launch @@ -4,6 +4,8 @@ + + @@ -21,5 +23,7 @@ + + diff --git a/src/bitbots_vision/CMakeLists.txt b/src/bitbots_vision/CMakeLists.txt index b71cd26a7..aa1fa4c45 100644 --- a/src/bitbots_vision/CMakeLists.txt +++ b/src/bitbots_vision/CMakeLists.txt @@ -65,8 +65,10 @@ generate_parameter_library(vision_parameters config/vision_parameters.yaml) # --------------------------------------------------------------------------- # Processing library (no ONNX dependency) – used by tests and the handler # --------------------------------------------------------------------------- -add_library(bitbots_vision_processing SHARED - src/yoeo_processing.cpp src/debug_image.cpp src/model_config.cpp) +add_library( + bitbots_vision_processing SHARED + src/yoeo_processing.cpp src/debug_image.cpp src/model_config.cpp + src/auto_gain_controller.cpp) target_include_directories( bitbots_vision_processing @@ -110,6 +112,12 @@ ament_target_dependencies( std_msgs vision_msgs) +add_executable(fixed_exposure_auto_gain src/fixed_exposure_auto_gain_node.cpp) + +target_link_libraries(fixed_exposure_auto_gain bitbots_vision_processing) + +ament_target_dependencies(fixed_exposure_auto_gain cv_bridge rclcpp sensor_msgs) + # --------------------------------------------------------------------------- # Tests # --------------------------------------------------------------------------- @@ -128,12 +136,15 @@ if(BUILD_TESTING) ament_add_gtest(test_processing test/test_processing.cpp) target_link_libraries(test_processing bitbots_vision_processing) + ament_add_gtest(test_auto_gain_controller test/test_auto_gain_controller.cpp) + target_link_libraries(test_auto_gain_controller bitbots_vision_processing) + ament_add_gtest(test_yoeo_handler test/test_yoeo_handler.cpp) target_link_libraries(test_yoeo_handler bitbots_vision_lib) endif() -install(TARGETS vision bitbots_vision_lib bitbots_vision_processing - DESTINATION lib/${PROJECT_NAME}) +install(TARGETS vision fixed_exposure_auto_gain bitbots_vision_lib + bitbots_vision_processing DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ DESTINATION include) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) diff --git a/src/bitbots_vision/README.md b/src/bitbots_vision/README.md index b92b55190..52203f867 100644 --- a/src/bitbots_vision/README.md +++ b/src/bitbots_vision/README.md @@ -1,5 +1,4 @@ -Bit-Bots Vision -=============== +# Bit-Bots Vision [![CodeFactor](https://www.codefactor.io/repository/github/bit-bots/bitbots_vision/badge)](https://www.codefactor.io/repository/github/bit-bots/bitbots_vision)    @@ -10,11 +9,27 @@ This is the vision ROS package of the Hamburg Bit-Bots. The vision is able to detect balls, lines, the field itself, the field boundary, goal posts, teammates, enemies and other obstacles. +## Fixed exposure with automatic gain + +The ZED Mini couples its hardware auto-exposure and auto-gain controls. The +bringup package therefore provides an optional software gain controller that +keeps exposure fixed and adjusts `video.gain` from the mean image brightness. + +```bash +pixi run -e default ros2 launch bitbots_bringup vision_standalone.launch \ + fixed_exposure_auto_gain:=true camera_exposure:=20 +``` + +The controller defaults are in +`config/fixed_exposure_auto_gain.yaml`. In particular, +`target_brightness`, `brightness_deadband`, and `gain_kp` control its response. + For further information and getting started, see the documentation of this package at our website: [docs.bit-bots.de](https://docs.bit-bots.de/package/bitbots_vision/latest/index.html) An earlier version of this pipeline is presented in our paper [An Open Source Vision Pipeline Approach for RoboCup Humanoid Soccer](https://robocup.informatik.uni-hamburg.de/wp-content/uploads/2019/06/vision_paper.pdf). When you use this pipeline or parts of it, please cite it. -``` + +```bibtex @inproceedings{vision2019, author={Fiedler, Niklas and Brandt, Hendrik and Gutsche, Jan and Vahl, Florian and Hagge, Jonas and Bestmann, Marc}, year={2019}, diff --git a/src/bitbots_vision/config/fixed_exposure_auto_gain.yaml b/src/bitbots_vision/config/fixed_exposure_auto_gain.yaml new file mode 100644 index 000000000..e98b0f90e --- /dev/null +++ b/src/bitbots_vision/config/fixed_exposure_auto_gain.yaml @@ -0,0 +1,12 @@ +fixed_exposure_auto_gain: + ros__parameters: + image_topic: "zed/zed_node/rgb/image_rect_color" + camera_node: "/zed/zed_node" + initial_gain: 40 + update_rate: 2.0 + target_brightness: 110.0 + brightness_deadband: 8.0 + gain_kp: 0.08 + max_gain_step: 4 + min_gain: 0 + max_gain: 100 diff --git a/src/bitbots_vision/include/bitbots_vision/auto_gain_controller.hpp b/src/bitbots_vision/include/bitbots_vision/auto_gain_controller.hpp new file mode 100644 index 000000000..e840caeaf --- /dev/null +++ b/src/bitbots_vision/include/bitbots_vision/auto_gain_controller.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include + +namespace bitbots_vision { + +struct AutoGainConfig { + double target_brightness{110.0}; + double brightness_deadband{8.0}; + double gain_kp{0.08}; + int max_gain_step{4}; + int min_gain{0}; + int max_gain{100}; +}; + +class AutoGainController { + public: + explicit AutoGainController(AutoGainConfig config); + + /// Returns a new gain when the brightness error requires an adjustment. + std::optional update(double brightness, int current_gain) const; + + private: + AutoGainConfig config_; +}; + +} // namespace bitbots_vision diff --git a/src/bitbots_vision/src/auto_gain_controller.cpp b/src/bitbots_vision/src/auto_gain_controller.cpp new file mode 100644 index 000000000..071f5dabb --- /dev/null +++ b/src/bitbots_vision/src/auto_gain_controller.cpp @@ -0,0 +1,37 @@ +#include +#include +#include +#include + +namespace bitbots_vision { + +AutoGainController::AutoGainController(AutoGainConfig config) : config_(config) { + if (config_.target_brightness < 0.0 || config_.target_brightness > 255.0) { + throw std::invalid_argument("target brightness must be between 0 and 255"); + } + if (config_.brightness_deadband < 0.0 || config_.gain_kp <= 0.0 || config_.max_gain_step <= 0 || + config_.min_gain < 0 || config_.max_gain > 100 || config_.min_gain > config_.max_gain) { + throw std::invalid_argument("invalid auto gain configuration"); + } +} + +std::optional AutoGainController::update(double brightness, int current_gain) const { + const double error = config_.target_brightness - brightness; + if (std::abs(error) <= config_.brightness_deadband) { + return std::nullopt; + } + + int step = static_cast(std::round(config_.gain_kp * error)); + if (step == 0) { + step = error > 0.0 ? 1 : -1; + } + step = std::clamp(step, -config_.max_gain_step, config_.max_gain_step); + + const int new_gain = std::clamp(current_gain + step, config_.min_gain, config_.max_gain); + if (new_gain == current_gain) { + return std::nullopt; + } + return new_gain; +} + +} // namespace bitbots_vision diff --git a/src/bitbots_vision/src/fixed_exposure_auto_gain_node.cpp b/src/bitbots_vision/src/fixed_exposure_auto_gain_node.cpp new file mode 100644 index 000000000..365e2f672 --- /dev/null +++ b/src/bitbots_vision/src/fixed_exposure_auto_gain_node.cpp @@ -0,0 +1,141 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace bitbots_vision { + +class FixedExposureAutoGainNode : public rclcpp::Node { + public: + FixedExposureAutoGainNode() : Node("fixed_exposure_auto_gain") { + const auto image_topic = declare_parameter("image_topic", "zed/zed_node/rgb/image_rect_color"); + const auto camera_node = declare_parameter("camera_node", "/zed/zed_node"); + exposure_ = static_cast(declare_parameter("exposure", 20)); + current_gain_ = static_cast(declare_parameter("initial_gain", 40)); + const double update_rate = declare_parameter("update_rate", 2.0); + if (update_rate <= 0.0) { + throw std::invalid_argument("update rate must be positive"); + } + update_period_ = rclcpp::Duration::from_seconds(1.0 / update_rate); + + AutoGainConfig config; + config.target_brightness = declare_parameter("target_brightness", 110.0); + config.brightness_deadband = declare_parameter("brightness_deadband", 8.0); + config.gain_kp = declare_parameter("gain_kp", 0.08); + config.max_gain_step = static_cast(declare_parameter("max_gain_step", 4)); + config.min_gain = static_cast(declare_parameter("min_gain", 0)); + config.max_gain = static_cast(declare_parameter("max_gain", 100)); + controller_ = std::make_unique(config); + + if (exposure_ < 0 || exposure_ > 100 || current_gain_ < config.min_gain || current_gain_ > config.max_gain) { + throw std::invalid_argument("exposure or initial gain is outside the configured range"); + } + + camera_parameters_ = std::make_shared(this, camera_node); + setup_timer_ = + create_wall_timer(std::chrono::seconds(1), std::bind(&FixedExposureAutoGainNode::configure_camera, this)); + + image_sub_ = create_subscription( + image_topic, rclcpp::SensorDataQoS(), + std::bind(&FixedExposureAutoGainNode::image_callback, this, std::placeholders::_1)); + } + + private: + void configure_camera() { + if (!camera_parameters_->service_is_ready() || parameter_update_pending_) { + return; + } + + parameter_update_pending_ = true; + const std::vector parameters{ + rclcpp::Parameter("video.auto_exposure_gain", false), + rclcpp::Parameter("video.exposure", exposure_), + rclcpp::Parameter("video.gain", current_gain_), + }; + camera_parameters_->set_parameters(parameters, [this](const auto future) { + parameter_update_pending_ = false; + const auto results = future.get(); + if (results.size() != 3 || !results[0].successful || !results[1].successful || !results[2].successful) { + RCLCPP_ERROR(get_logger(), "Failed to configure fixed exposure and manual gain"); + return; + } + + camera_configured_ = true; + setup_timer_->cancel(); + RCLCPP_INFO(get_logger(), "Fixed exposure at %d; software auto-gain started at %d", exposure_, current_gain_); + }); + } + + void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& image_msg) { + const auto now = get_clock()->now(); + if (!camera_configured_ || parameter_update_pending_ || now - last_update_ < update_period_) { + return; + } + last_update_ = now; + + cv::Mat gray; + try { + gray = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::MONO8)->image; + } catch (const cv_bridge::Exception& error) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "Cannot measure image brightness: %s", error.what()); + return; + } + + cv::Mat sampled; + cv::resize(gray, sampled, cv::Size(), 0.125, 0.125, cv::INTER_AREA); + const double brightness = cv::mean(sampled)[0]; + const auto requested_gain = controller_->update(brightness, current_gain_); + if (!requested_gain.has_value()) { + return; + } + + parameter_update_pending_ = true; + const int new_gain = requested_gain.value(); + camera_parameters_->set_parameters( + {rclcpp::Parameter("video.gain", new_gain)}, [this, new_gain, brightness](const auto future) { + parameter_update_pending_ = false; + const auto results = future.get(); + if (results.size() != 1 || !results[0].successful) { + RCLCPP_WARN(get_logger(), "Failed to set camera gain to %d", new_gain); + return; + } + + current_gain_ = new_gain; + RCLCPP_DEBUG(get_logger(), "Brightness %.1f, camera gain %d", brightness, current_gain_); + }); + } + + int exposure_; + int current_gain_; + rclcpp::Duration update_period_{0, 0}; + rclcpp::Time last_update_{0, 0, RCL_ROS_TIME}; + bool camera_configured_{false}; + bool parameter_update_pending_{false}; + std::unique_ptr controller_; + std::shared_ptr camera_parameters_; + rclcpp::TimerBase::SharedPtr setup_timer_; + rclcpp::Subscription::SharedPtr image_sub_; +}; + +} // namespace bitbots_vision + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::experimental::executors::EventsExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/src/bitbots_vision/test/test_auto_gain_controller.cpp b/src/bitbots_vision/test/test_auto_gain_controller.cpp new file mode 100644 index 000000000..9cd4dbf30 --- /dev/null +++ b/src/bitbots_vision/test/test_auto_gain_controller.cpp @@ -0,0 +1,46 @@ +#include + +#include +#include + +using bitbots_vision::AutoGainConfig; +using bitbots_vision::AutoGainController; + +TEST(AutoGainController, HoldsGainInsideDeadband) { + AutoGainController controller(AutoGainConfig{}); + EXPECT_FALSE(controller.update(105.0, 40).has_value()); + EXPECT_FALSE(controller.update(118.0, 40).has_value()); +} + +TEST(AutoGainController, RaisesGainForDarkImage) { + AutoGainController controller(AutoGainConfig{}); + EXPECT_EQ(controller.update(50.0, 40), 44); +} + +TEST(AutoGainController, LowersGainForBrightImage) { + AutoGainController controller(AutoGainConfig{}); + EXPECT_EQ(controller.update(180.0, 40), 36); +} + +TEST(AutoGainController, UsesAtLeastOneGainStepOutsideDeadband) { + AutoGainConfig config; + config.brightness_deadband = 0.0; + config.gain_kp = 0.001; + AutoGainController controller(config); + + EXPECT_EQ(controller.update(109.0, 40), 41); + EXPECT_EQ(controller.update(111.0, 40), 39); +} + +TEST(AutoGainController, RespectsGainLimits) { + AutoGainController controller(AutoGainConfig{}); + EXPECT_FALSE(controller.update(0.0, 100).has_value()); + EXPECT_FALSE(controller.update(255.0, 0).has_value()); +} + +TEST(AutoGainController, RejectsInvalidConfiguration) { + AutoGainConfig config; + config.min_gain = 80; + config.max_gain = 20; + EXPECT_THROW(AutoGainController controller(config), std::invalid_argument); +}