diff --git a/CMakeLists.txt b/CMakeLists.txt index 698d872..d2d9d43 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,6 +46,7 @@ add_service_files( get_float_feature_value.srv get_integer_feature_value.srv get_string_feature_value.srv + one_shot_white_balance.srv set_boolean_feature_value.srv set_float_feature_value.srv set_integer_feature_value.srv diff --git a/README.md b/README.md index 9ad513c..6ad9ffa 100644 --- a/README.md +++ b/README.md @@ -146,6 +146,10 @@ specified in 'wb_ratio_selectors'. This list should be the same size as the rati - Default: ```""``` - Example: ```"1.4,2.5"``` +To trigger an automatic white balance computation and a subsequent setting of ```BalanceWhiteAuto``` to ```Once```, camera_aravis provides a service called ```trigger_one_shot_white_balance```. +Calling this service will trigger a one shot computation of the white balance parameters and return the newly computed balance ratio. +This can be called no matter which mode has been set previously. + ### Activating PTP Timestamp Some cameras support the use of the Precision Time Protocol (PTP) to set the timestamps of the diff --git a/include/camera_aravis/camera_aravis_nodelet.h b/include/camera_aravis/camera_aravis_nodelet.h index bf5a6b2..48c83dc 100644 --- a/include/camera_aravis/camera_aravis_nodelet.h +++ b/include/camera_aravis/camera_aravis_nodelet.h @@ -74,6 +74,7 @@ extern "C" { #include #include #include +#include #include #include @@ -171,6 +172,9 @@ class CameraAravisNodelet : public nodelet::Nodelet ros::ServiceServer set_boolean_service_; bool setBooleanFeatureCallback(camera_aravis::set_boolean_feature_value::Request& request, camera_aravis::set_boolean_feature_value::Response& response); + ros::ServiceServer one_shot_white_balance_service_; + bool oneShotWhiteBalanceCallback(camera_aravis::one_shot_white_balance::Request& request, camera_aravis::one_shot_white_balance::Response& response); + // triggers a shot at regular intervals, sleeps in between void softwareTriggerLoop(); diff --git a/src/camera_aravis_nodelet.cpp b/src/camera_aravis_nodelet.cpp index 1e38a0f..f06b4c7 100644 --- a/src/camera_aravis_nodelet.cpp +++ b/src/camera_aravis_nodelet.cpp @@ -677,6 +677,8 @@ void CameraAravisNodelet::spawnStream() this->set_string_service_ = pnh.advertiseService("set_string_feature_value", &CameraAravisNodelet::setStringFeatureCallback, this); this->set_boolean_service_ = pnh.advertiseService("set_boolean_feature_value", &CameraAravisNodelet::setBooleanFeatureCallback, this); + this->one_shot_white_balance_service_ = pnh.advertiseService("trigger_one_shot_white_balance", &CameraAravisNodelet::oneShotWhiteBalanceCallback, this); + ROS_INFO("Done initializing camera_aravis."); } @@ -772,6 +774,59 @@ bool CameraAravisNodelet::setBooleanFeatureCallback(camera_aravis::set_boolean_f return true; } +bool CameraAravisNodelet::oneShotWhiteBalanceCallback(camera_aravis::one_shot_white_balance::Request& request, camera_aravis::one_shot_white_balance::Response& response) +{ + //--- set return values to default + response.is_successful = false; + response.balance_ratio_red = 0.f; + response.balance_ratio_green = 0.f; + response.balance_ratio_blue = 0.f; + + //--- check for correct state of camera + if(!this->p_device_ || !implemented_features_["BalanceWhiteAuto"]) + return false; + + arv_device_set_string_feature_value(p_device_, "BalanceWhiteAuto", "Once"); + if (arv_device_get_status(this->p_device_) == ARV_DEVICE_STATUS_SUCCESS) + { + const char* tmpCharPtr = arv_device_get_string_feature_value(p_device_, "BalanceWhiteAuto"); + std::string modeStr = "n/a"; + if(tmpCharPtr) + modeStr = std::string(tmpCharPtr); + + if (implemented_features_["BalanceRatio"] && implemented_features_["BalanceRatioSelector"]) + { + arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Red"); + response.balance_ratio_red = static_cast( + arv_device_get_float_feature_value(p_device_, "BalanceRatio")); + + arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Green"); + response.balance_ratio_green = static_cast( + arv_device_get_float_feature_value(p_device_, "BalanceRatio")); + + arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Blue"); + response.balance_ratio_blue = static_cast( + arv_device_get_float_feature_value(p_device_, "BalanceRatio")); + } + + ROS_INFO("Setting Auto White Balance successful!"); + ROS_INFO("> BalanceWhiteAuto Mode: %s", modeStr.c_str()); + ROS_INFO("> BalanceRatio Red: %f", response.balance_ratio_red); + ROS_INFO("> BalanceRatio Green: %f", response.balance_ratio_green); + ROS_INFO("> BalanceRatio Blue: %f", response.balance_ratio_blue); + + + response.is_successful = true; + return true; + } + else + { + ROS_ERROR("Trying to set Auto White Balance once failed!"); + + return false; + } +} + void CameraAravisNodelet::resetPtpClock() { if (ptp_status_feature_.empty() || ptp_enable_feature_.empty()) diff --git a/srv/one_shot_white_balance.srv b/srv/one_shot_white_balance.srv new file mode 100644 index 0000000..93a4c91 --- /dev/null +++ b/srv/one_shot_white_balance.srv @@ -0,0 +1,5 @@ +--- +bool is_successful +float32 balance_ratio_red +float32 balance_ratio_green +float32 balance_ratio_blue