From f8d97d2217aceeb40dc4fc891de7114a6b2844d8 Mon Sep 17 00:00:00 2001 From: Fabio Falezza Date: Fri, 7 Sep 2018 12:23:38 +0200 Subject: [PATCH 1/3] Implemented decimation features --- config/default.yaml | 7 + .../internal/impl/pylon_camera_base.hpp | 122 ++++++++++++++++++ include/pylon_camera/internal/pylon_camera.h | 10 ++ include/pylon_camera/pylon_camera.h | 23 ++++ include/pylon_camera/pylon_camera_node.h | 20 +++ include/pylon_camera/pylon_camera_parameter.h | 13 ++ src/pylon_camera/pylon_camera_node.cpp | 107 +++++++++++++++ src/pylon_camera/pylon_camera_parameter.cpp | 39 ++++++ 8 files changed, 341 insertions(+) diff --git a/config/default.yaml b/config/default.yaml index 6e517ac6..23e0dd36 100644 --- a/config/default.yaml +++ b/config/default.yaml @@ -28,6 +28,13 @@ camera_info_url: "" # binning_x: 1 # binning_y: 1 +# The Decimation camera feature allows you to reduce the number of sensor pixel +# columns or rows that are transmitted by the camera. +# This procedure is also known as "subsampling". It reduces the amount of data +# to be transferred and may increase the camera's frame rate. +# decimation_x: 1 +# decimation_y: 1 + # The desired publisher frame rate if listening to the topics. # This parameter can only be set once at startup # Calling the GrabImages-Action can result in a higher framerate diff --git a/include/pylon_camera/internal/impl/pylon_camera_base.hpp b/include/pylon_camera/internal/impl/pylon_camera_base.hpp index 24165bb9..279f4298 100644 --- a/include/pylon_camera/internal/impl/pylon_camera_base.hpp +++ b/include/pylon_camera/internal/impl/pylon_camera_base.hpp @@ -124,6 +124,32 @@ size_t PylonCameraImpl::currentBinningY() } } +template +size_t PylonCameraImpl::currentDecimationX() +{ + if (GenApi::IsAvailable(cam_->DecimationHorizontal)) + { + return static_cast(cam_->DecimationHorizontal.GetValue()); + } + else + { + return 1; + } +} + +template +size_t PylonCameraImpl::currentDecimationY() +{ + if (GenApi::IsAvailable(cam_->DecimationVertical)) + { + return static_cast(cam_->DecimationVertical.GetValue()); + } + else + { + return 1; + } +} + template std::string PylonCameraImpl::currentROSEncoding() const { @@ -619,6 +645,102 @@ bool PylonCameraImpl::setBinningY(const size_t& target_binning_y, return true; } +template +bool PylonCameraImpl::setDecimationX(const size_t &target_decimation_x, + size_t &reached_decimation_x) +{ + try + { + if (GenApi::IsAvailable(cam_->DecimationHorizontal)) + { + cam_->StopGrabbing(); + size_t decimation_x_to_set = target_decimation_x; + if (decimation_x_to_set < cam_->DecimationHorizontal.GetMin()) + { + ROS_WARN_STREAM("Desired horizontal decimation_x factor(" + << decimation_x_to_set << ") unreachable! Setting to lower " + << "limit: " << cam_->DecimationHorizontal.GetMin()); + decimation_x_to_set = cam_->DecimationHorizontal.GetMin(); + } + else if (decimation_x_to_set > cam_->DecimationHorizontal.GetMax()) + { + ROS_WARN_STREAM("Desired horizontal decimation_x factor(" + << decimation_x_to_set << ") unreachable! Setting to upper " + << "limit: " << cam_->DecimationHorizontal.GetMax()); + decimation_x_to_set = cam_->DecimationHorizontal.GetMax(); + } + cam_->DecimationHorizontal.SetValue(decimation_x_to_set); + reached_decimation_x = currentDecimationX(); + cam_->StartGrabbing(); + std::cout << "-----------" << cam_->Width.GetValue() << std::endl; + img_cols_ = static_cast(cam_->Width.GetValue()); + img_size_byte_ = img_cols_ * img_rows_ * imagePixelDepth(); + } + else + { + ROS_WARN_STREAM("Camera does not support decimation. Will keep the " + << "current settings"); + reached_decimation_x = currentDecimationX(); + } + } + catch (const GenICam::GenericException &e) + { + ROS_ERROR_STREAM("An exception while setting target horizontal " + << "decimation_x factor to " << target_decimation_x << " occurred: " + << e.GetDescription()); + return false; + } + return true; +} + +template +bool PylonCameraImpl::setDecimationY(const size_t &target_decimation_y, + size_t &reached_decimation_y) +{ + try + { + if (GenApi::IsAvailable(cam_->DecimationVertical)) + { + cam_->StopGrabbing(); + size_t decimation_y_to_set = target_decimation_y; + if (decimation_y_to_set < cam_->DecimationVertical.GetMin()) + { + ROS_WARN_STREAM("Desired vertical decimation_y factor(" + << decimation_y_to_set << ") unreachable! Setting to lower " + << "limit: " << cam_->DecimationVertical.GetMin()); + decimation_y_to_set = cam_->DecimationVertical.GetMin(); + } + else if (decimation_y_to_set > cam_->DecimationVertical.GetMax()) + { + ROS_WARN_STREAM("Desired vertical decimation_y factor(" + << decimation_y_to_set << ") unreachable! Setting to upper " + << "limit: " << cam_->DecimationVertical.GetMax()); + decimation_y_to_set = cam_->DecimationVertical.GetMax(); + } + cam_->DecimationVertical.SetValue(decimation_y_to_set); + reached_decimation_y = currentDecimationY(); + cam_->StartGrabbing(); + std::cout << "-----------" << cam_->Height.GetValue() << std::endl; + img_rows_ = static_cast(cam_->Height.GetValue()); + img_size_byte_ = img_cols_ * img_rows_ * imagePixelDepth(); + } + else + { + ROS_WARN_STREAM("Camera does not support decimation. Will keep the " + << "current settings"); + reached_decimation_y = currentDecimationY(); + } + } + catch (const GenICam::GenericException &e) + { + ROS_ERROR_STREAM("An exception while setting target vertical " + << "decimation_y factor to " << target_decimation_y << " occurred: " + << e.GetDescription()); + return false; + } + return true; +} + template bool PylonCameraImpl::setExposure(const float& target_exposure, float& reached_exposure) diff --git a/include/pylon_camera/internal/pylon_camera.h b/include/pylon_camera/internal/pylon_camera.h index e65de355..2997d357 100644 --- a/include/pylon_camera/internal/pylon_camera.h +++ b/include/pylon_camera/internal/pylon_camera.h @@ -76,6 +76,12 @@ class PylonCameraImpl : public PylonCamera virtual bool setBinningY(const size_t& target_binning_y, size_t& reached_binning_y); + virtual bool setDecimationX(const size_t &target_decimation_x, + size_t &reached_decimation_x); + + virtual bool setDecimationY(const size_t &target_decimation_y, + size_t &reached_decimation_y); + virtual bool setImageEncoding(const std::string& target_ros_encoding); virtual bool setExposure(const float& target_exposure, float& reached_exposure); @@ -97,6 +103,10 @@ class PylonCameraImpl : public PylonCamera virtual size_t currentBinningY(); + virtual size_t currentDecimationX(); + + virtual size_t currentDecimationY(); + virtual std::vector detectAvailableImageEncodings(); virtual std::string currentROSEncoding() const; diff --git a/include/pylon_camera/pylon_camera.h b/include/pylon_camera/pylon_camera.h index 5c5c848b..49099315 100644 --- a/include/pylon_camera/pylon_camera.h +++ b/include/pylon_camera/pylon_camera.h @@ -144,6 +144,25 @@ class PylonCamera virtual bool setBinningY(const size_t& target_binning_y, size_t& reached_binning_y) = 0; + /** + * Sets the target horizontal decimation_x factor + * @param target_decimation_x the target horizontal decimation_x factor. + * @param reached_decimation_x the reached horizontal decimation_x factor. + * @return false if a communication error occured or true otherwise. + */ + virtual bool setDecimationX(const size_t& target_decimation_x, + size_t& reached_decimation_x) = 0; + + /** + * Sets the target vertical decimation_y factor + * @param target_decimation_y the target vertical decimation_y factor. + * @param reached_decimation_y the reached vertical decimation_y factor. + * @return false if a communication error occured or true otherwise. + */ + + virtual bool setDecimationY(const size_t &target_decimation_y, + size_t &reached_decimation_y) = 0; + /** * Detects the supported image pixel encodings of the camera an stores * them in a vector. @@ -237,6 +256,10 @@ class PylonCamera */ virtual size_t currentBinningY() = 0; + virtual size_t currentDecimationX() = 0; + + virtual size_t currentDecimationY() = 0; + /** * Get the camera image encoding according to sensor_msgs::image_encodings * The supported encodings are 'mono8', 'bgr8', 'rgb8', 'bayer_bggr8', diff --git a/include/pylon_camera/pylon_camera_node.h b/include/pylon_camera/pylon_camera_node.h index 01858abd..c2290a37 100644 --- a/include/pylon_camera/pylon_camera_node.h +++ b/include/pylon_camera/pylon_camera_node.h @@ -165,6 +165,26 @@ class PylonCameraNode */ bool setBinningCallback(camera_control_msgs::SetBinning::Request &req, camera_control_msgs::SetBinning::Response &res); + + /** + * Update the horizontal decimation_x factor to get downsampled images + * + * @param target_decimation_x the target horizontal decimation_x factor + * @param reached_decimation_x the horizontal decimation_x factor that could be reached + * @return true if the targeted decimation could be reached + */ + bool setDecimationX(const size_t& target_decimation_x, + size_t& reached_decimation_x); + + /** + * Update the vertical decimation_y factor to get downsampled images + * + * @param target_decimation_y the target vertical decimation_y factor + * @param reached_decimation_y the vertical decimation_y factor that could be reached + * @return true if the targeted decimation could be reached + */ + bool setDecimationY(const size_t &target_decimation_y, + size_t &reached_decimation_y); /** * Update the exposure value on the camera * @param target_exposure the targeted exposure diff --git a/include/pylon_camera/pylon_camera_parameter.h b/include/pylon_camera/pylon_camera_parameter.h index 586030b3..c501b04b 100644 --- a/include/pylon_camera/pylon_camera_parameter.h +++ b/include/pylon_camera/pylon_camera_parameter.h @@ -130,6 +130,19 @@ class PylonCameraParameter bool binning_x_given_; bool binning_y_given_; + /** The Decimation camera feature allows you to reduce the number of sensor + * pixel columns or rows that are transmitted by the camera. + */ + size_t decimation_x_; + size_t decimation_y_; + + /** + * Flags which indicate if the decimation factors are provided and hence + * should be set during startup + */ + bool decimation_x_given_; + bool decimation_y_given_; + /** * Factor that describes the image downsampling to speed up the exposure * search to find the desired brightness. diff --git a/src/pylon_camera/pylon_camera_node.cpp b/src/pylon_camera/pylon_camera_node.cpp index 0af37f3c..4b3296fc 100644 --- a/src/pylon_camera/pylon_camera_node.cpp +++ b/src/pylon_camera/pylon_camera_node.cpp @@ -269,6 +269,22 @@ bool PylonCameraNode::startGrabbing() << "be adapted, so that the binning_y value in this msg remains 1"); } + if ( pylon_camera_parameter_set_.decimation_x_given_ ) + { + size_t reached_decimation_x; + setDecimationX(pylon_camera_parameter_set_.decimation_x_, reached_decimation_x); + ROS_INFO_STREAM("Setting horizontal decimation_x to " + << pylon_camera_parameter_set_.decimation_x_); + } + + if (pylon_camera_parameter_set_.decimation_y_given_) + { + size_t reached_decimation_y; + setDecimationY(pylon_camera_parameter_set_.decimation_y_, reached_decimation_y); + ROS_INFO_STREAM("Setting horizontal decimation_y to " + << pylon_camera_parameter_set_.decimation_y_); + } + if ( pylon_camera_parameter_set_.exposure_given_ ) { float reached_exposure; @@ -1002,6 +1018,97 @@ bool PylonCameraNode::setBinningCallback(camera_control_msgs::SetBinning::Reques return true; } +bool PylonCameraNode::setDecimationX(const size_t &target_decimation_x, + size_t &reached_decimation_x) +{ + boost::lock_guard lock(grab_mutex_); + if (!pylon_camera_->setDecimationX(target_decimation_x, reached_decimation_x)) + { + // retry till timeout + ros::Rate r(10.0); + ros::Time timeout(ros::Time::now() + ros::Duration(2.0)); + while (ros::ok()) + { + if (pylon_camera_->setDecimationX(target_decimation_x, reached_decimation_x)) + { + break; + } + if (ros::Time::now() > timeout) + { + ROS_ERROR_STREAM("Error in setDecimationX(): Unable to set target " + << "decimation_x factor before timeout"); + CameraInfoPtr cam_info(new CameraInfo(camera_info_manager_->getCameraInfo())); + cam_info->width = pylon_camera_->imageCols(); + camera_info_manager_->setCameraInfo(*cam_info); + img_raw_msg_.width = pylon_camera_->imageCols(); + // step = full row length in bytes, img_size = (step * rows), imagePixelDepth + // already contains the number of channels + img_raw_msg_.step = img_raw_msg_.width * pylon_camera_->imagePixelDepth(); + return false; + } + r.sleep(); + } + } + CameraInfoPtr cam_info(new CameraInfo(camera_info_manager_->getCameraInfo())); + cam_info->width = pylon_camera_->imageCols(); + camera_info_manager_->setCameraInfo(*cam_info); + img_raw_msg_.width = pylon_camera_->imageCols(); + // step = full row length in bytes, img_size = (step * rows), imagePixelDepth + // already contains the number of channels + img_raw_msg_.step = img_raw_msg_.width * pylon_camera_->imagePixelDepth(); + setupSamplingIndices(sampling_indices_, + pylon_camera_->imageRows(), + pylon_camera_->imageCols(), + pylon_camera_parameter_set_.downsampling_factor_exp_search_); + return true; +} + +bool PylonCameraNode::setDecimationY(const size_t &target_decimation_y, + size_t &reached_decimation_y) +{ + boost::lock_guard lock(grab_mutex_); + if (!pylon_camera_->setDecimationY(target_decimation_y, reached_decimation_y)) + { + // retry till timeout + ros::Rate r(10.0); + ros::Time timeout(ros::Time::now() + ros::Duration(2.0)); + while (ros::ok()) + { + if (pylon_camera_->setDecimationY(target_decimation_y, reached_decimation_y)) + { + break; + } + if (ros::Time::now() > timeout) + { + ROS_ERROR_STREAM("Error in setDecimationY(): Unable to set target " + << "decimation_x factor before timeout"); + CameraInfoPtr cam_info(new CameraInfo(camera_info_manager_->getCameraInfo())); + cam_info->height = pylon_camera_->imageRows(); + camera_info_manager_->setCameraInfo(*cam_info); + img_raw_msg_.height = pylon_camera_->imageRows(); + // step = full row length in bytes, img_size = (step * rows), imagePixelDepth + // already contains the number of channels + img_raw_msg_.step = img_raw_msg_.width * pylon_camera_->imagePixelDepth(); + return false; + } + r.sleep(); + } + } + CameraInfoPtr cam_info(new CameraInfo(camera_info_manager_->getCameraInfo())); + // cam_info->binning_y = pylon_camera_->currentBinningY(); + cam_info->height = pylon_camera_->imageRows(); + camera_info_manager_->setCameraInfo(*cam_info); + img_raw_msg_.height = pylon_camera_->imageRows(); + // step = full row length in bytes, img_size = (step * rows), imagePixelDepth + // already contains the number of channels + img_raw_msg_.step = img_raw_msg_.width * pylon_camera_->imagePixelDepth(); + setupSamplingIndices(sampling_indices_, + pylon_camera_->imageRows(), + pylon_camera_->imageCols(), + pylon_camera_parameter_set_.downsampling_factor_exp_search_); + return true; +} + bool PylonCameraNode::setExposure(const float& target_exposure, float& reached_exposure) { diff --git a/src/pylon_camera/pylon_camera_parameter.cpp b/src/pylon_camera/pylon_camera_parameter.cpp index fc1c30b8..95c397ac 100644 --- a/src/pylon_camera/pylon_camera_parameter.cpp +++ b/src/pylon_camera/pylon_camera_parameter.cpp @@ -123,6 +123,45 @@ void PylonCameraParameter::readFromRosParameterServer(const ros::NodeHandle& nh) binning_y_ = static_cast(binning_y); } } + + decimation_x_given_ = nh.hasParam("decimation_x"); + if ( decimation_x_given_ ) + { + int decimation_x; + nh.getParam("decimation_x", decimation_x); + std::cout << "decimation x is given and has value " << decimation_x << std::endl; + if (decimation_x > 32 || decimation_x < 0) + { + ROS_WARN_STREAM("Desired horizontal decimation_x factor not in valid " + << "range! Decimation x = " << decimation_x << ". Will reset it to " + << "default value (1)"); + decimation_x_given_ = false; + } + else + { + decimation_x_ = static_cast(decimation_x); + } + } + + decimation_y_given_ = nh.hasParam("decimation_y"); + if (decimation_y_given_) + { + int decimation_y; + nh.getParam("decimation_y", decimation_y); + std::cout << "decimation y is given and has value " << decimation_y << std::endl; + if (decimation_y > 32 || decimation_y < 0) + { + ROS_WARN_STREAM("Desired vertical decimation_y factor not in valid " + << "range! Decimation y = " << decimation_y << ". Will reset it to " + << "default value (1)"); + decimation_y_given_ = false; + } + else + { + decimation_y_ = static_cast(decimation_y); + } + } + nh.param("downsampling_factor_exposure_search", downsampling_factor_exp_search_, 20); From acf7fe2c4c94309942ecb8a6ec3911ae55c73c42 Mon Sep 17 00:00:00 2001 From: Fabio Falezza Date: Fri, 7 Sep 2018 16:40:22 +0200 Subject: [PATCH 2/3] Added light_source feature --- .gitignore | 5 ++ config/default.yaml | 18 ++++ .../internal/impl/pylon_camera_gige.hpp | 56 ++++++++++++ .../internal/impl/pylon_camera_usb.hpp | 12 +++ include/pylon_camera/internal/pylon_camera.h | 4 + include/pylon_camera/pylon_camera.h | 17 ++++ include/pylon_camera/pylon_camera_node.h | 6 ++ include/pylon_camera/pylon_camera_parameter.h | 17 +++- src/pylon_camera/pylon_camera_node.cpp | 86 +++++++++++++++++++ src/pylon_camera/pylon_camera_parameter.cpp | 17 ++++ 10 files changed, 236 insertions(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 23b5dbb1..ee346060 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,11 @@ ## folders ## ############# build +.vscode +left.yaml +right.yaml +left.launch +right.launch ############# ## files ## diff --git a/config/default.yaml b/config/default.yaml index 23e0dd36..19ed503f 100644 --- a/config/default.yaml +++ b/config/default.yaml @@ -100,6 +100,24 @@ frame_rate: 5.0 # exposure_auto: true # gain_auto: true +# The Processed Raw Enable camera feature allows you to use the Color +# Transformation and Color Adjustment features when using a Bayer pixel format. +# The camera's pixel format must be set to a Bayer pixel format, e.g., Bayer 12. +# processed_raw_enable: false + +# The Light Source Preset camera feature allows you to correct color shifts +# caused by certain light sources. +# To select a light source preset, set the LightSourceSelector parameter to one +# of the following values: +# "off": No light source preset is selected +# "daylight": The camera corrects color shifts caused by daylight +# lighting that has a color temperature of about 5 000 K. +# "daylight6500K": The camera corrects color shifts caused by daylight lighting +# that has a color temperature of about 6 500 K. +# "tungsten": The camera corrects color shifts caused by tungsten lighting +# that has a color temperature of about 2 500 to 3 000 K. +# light_source: "off" + ########################################################################## # The timeout while searching the exposure which is connected to the diff --git a/include/pylon_camera/internal/impl/pylon_camera_gige.hpp b/include/pylon_camera/internal/impl/pylon_camera_gige.hpp index a0ac69c6..8ce5c676 100644 --- a/include/pylon_camera/internal/impl/pylon_camera_gige.hpp +++ b/include/pylon_camera/internal/impl/pylon_camera_gige.hpp @@ -396,6 +396,62 @@ std::string PylonGigECamera::typeName() const return "GigE"; } +template <> +bool PylonGigECamera::setProcessedRawEnable(bool &enabled) +{ + if ( GenApi::IsAvailable(cam_->ProcessedRawEnable) ){ + try + { + cam_->ProcessedRawEnable.SetValue(enabled); + } + catch (const GenICam::GenericException &e) + { + ROS_ERROR_STREAM("An exception while setting processed raw enable to " + << enabled << " occurred:" + << e.GetDescription()); + return false; + } + return true; + } + return false; +} + +template <> +bool PylonGigECamera::setLightSourceSelector(std::string selector) +{ + if (GenApi::IsAvailable(cam_->LightSourceSelector)) + { + try + { + if (selector.compare("off") == 0) + { + cam_->LightSourceSelector.SetValue(Basler_GigECamera::LightSourceSelectorEnums::LightSourceSelector_Off); + } + else if (selector.compare("daylight") == 0) + { + cam_->LightSourceSelector.SetValue(Basler_GigECamera::LightSourceSelectorEnums::LightSourceSelector_Daylight); + } + else if (selector.compare("tungsten") == 0) + { + cam_->LightSourceSelector.SetValue(Basler_GigECamera::LightSourceSelectorEnums::LightSourceSelector_Tungsten); + } + else if (selector.compare("daylight6500") == 0) + { + cam_->LightSourceSelector.SetValue(Basler_GigECamera::LightSourceSelectorEnums::LightSourceSelector_Daylight6500K); + } + } + catch (const GenICam::GenericException &e) + { + ROS_ERROR_STREAM("An exception while setting processed raw enable to " + << selector << " occurred:" + << e.GetDescription()); + return false; + } + return true; + } + return false; +} + } // namespace pylon_camera #endif // PYLON_CAMERA_INTERNAL_GIGE_H_ diff --git a/include/pylon_camera/internal/impl/pylon_camera_usb.hpp b/include/pylon_camera/internal/impl/pylon_camera_usb.hpp index 7d309a7d..36da10c0 100644 --- a/include/pylon_camera/internal/impl/pylon_camera_usb.hpp +++ b/include/pylon_camera/internal/impl/pylon_camera_usb.hpp @@ -339,6 +339,18 @@ std::string PylonUSBCamera::typeName() const return "USB"; } +template <> +bool PylonUSBCamera::setProcessedRawEnable(bool &enabled) +{ + return false; +} + +template <> +bool PylonUSBCamera::setLightSourceSelector(std::string selector) +{ + return false; +} + } // namespace pylon_camera #endif // PYLON_CAMERA_INTERNAL_USB_H_ diff --git a/include/pylon_camera/internal/pylon_camera.h b/include/pylon_camera/internal/pylon_camera.h index 2997d357..4f780090 100644 --- a/include/pylon_camera/internal/pylon_camera.h +++ b/include/pylon_camera/internal/pylon_camera.h @@ -99,6 +99,10 @@ class PylonCameraImpl : public PylonCamera virtual bool setUserOutput(const int& output_id, const bool& value); + virtual bool setProcessedRawEnable(bool& enabled); + + virtual bool setLightSourceSelector(std::string lightSource); + virtual size_t currentBinningX(); virtual size_t currentBinningY(); diff --git a/include/pylon_camera/pylon_camera.h b/include/pylon_camera/pylon_camera.h index 49099315..3f77ba35 100644 --- a/include/pylon_camera/pylon_camera.h +++ b/include/pylon_camera/pylon_camera.h @@ -244,6 +244,23 @@ class PylonCamera */ virtual bool setUserOutput(const int& output_id, const bool& value) = 0; + /** + * @brief Set the Processed Raw Enable feature + * + * @param enabled + * @return true if it could be enabled + */ + virtual bool setProcessedRawEnable(bool& enabled) = 0; + + /** + * @brief Set the Light Source Selector object + * + * @param enabled + * @return true + * @return false + */ + virtual bool setLightSourceSelector(std::string enabled) = 0; + /** * Returns the current horizontal binning_x setting. * @return the horizontal binning_x setting. diff --git a/include/pylon_camera/pylon_camera_node.h b/include/pylon_camera/pylon_camera_node.h index c2290a37..9d422a5e 100644 --- a/include/pylon_camera/pylon_camera_node.h +++ b/include/pylon_camera/pylon_camera_node.h @@ -43,6 +43,7 @@ #include #include +#include #include #include @@ -185,6 +186,11 @@ class PylonCameraNode */ bool setDecimationY(const size_t &target_decimation_y, size_t &reached_decimation_y); + + bool setProcessedRawEnable(bool &enabled); + + bool setLightSourceSelector(std::string lightSource); + /** * Update the exposure value on the camera * @param target_exposure the targeted exposure diff --git a/include/pylon_camera/pylon_camera_parameter.h b/include/pylon_camera/pylon_camera_parameter.h index c501b04b..f9199b56 100644 --- a/include/pylon_camera/pylon_camera_parameter.h +++ b/include/pylon_camera/pylon_camera_parameter.h @@ -287,14 +287,27 @@ class PylonCameraParameter */ bool auto_flash_; -protected: /** + * Flag that indicates if the camera need to abilitate processed raw + */ + bool processed_raw_enable_; + bool processed_raw_enable_given_; + + /** + * Flag that indicates if the camera need to abilitate processed raw + */ + std::string light_source_; + bool light_source_given_; + + protected: + /** * Validates the parameter set found on the ros parameter server. * If invalid parameters can be detected, the interface will reset them * to the default values. * @param nh the ros::NodeHandle to use */ - void validateParameterSet(const ros::NodeHandle& nh); + void + validateParameterSet(const ros::NodeHandle &nh); /** * The tf frame under which the images were published diff --git a/src/pylon_camera/pylon_camera_node.cpp b/src/pylon_camera/pylon_camera_node.cpp index 4b3296fc..40fb9e5e 100644 --- a/src/pylon_camera/pylon_camera_node.cpp +++ b/src/pylon_camera/pylon_camera_node.cpp @@ -338,6 +338,20 @@ bool PylonCameraNode::startGrabbing() } } + if ( pylon_camera_parameter_set_.processed_raw_enable_given_ ) + { + setProcessedRawEnable(pylon_camera_parameter_set_.processed_raw_enable_); + ROS_INFO_STREAM("Setting processed_raw_enable_given_ to " + << pylon_camera_parameter_set_.processed_raw_enable_); + } + + if ( pylon_camera_parameter_set_.light_source_given_ ) + { + setLightSourceSelector(pylon_camera_parameter_set_.light_source_); + ROS_INFO_STREAM("Setting processed_raw_enable_given_ to " + << pylon_camera_parameter_set_.light_source_); + } + ROS_INFO_STREAM("Startup settings: " << "encoding = '" << pylon_camera_->currentROSEncoding() << "', " << "binning = [" << pylon_camera_->currentBinningX() << ", " @@ -1200,6 +1214,78 @@ bool PylonCameraNode::setGainCallback(camera_control_msgs::SetGain::Request &req return true; } +bool PylonCameraNode::setProcessedRawEnable(bool& enabled) +{ + boost::lock_guard lock(grab_mutex_); + if (!pylon_camera_->isReady()) + { + ROS_WARN("Error in setProcessedRawEnable(): pylon_camera_ is not ready!"); + return false; + } + if (pylon_camera_->setProcessedRawEnable(enabled)) + { + return true; + } + else // retry till timeout + { + // wait for max 5s till the cam has updated the gamma value + ros::Rate r(10.0); + ros::Time timeout(ros::Time::now() + ros::Duration(5.0)); + while (ros::ok()) + { + if (pylon_camera_->setProcessedRawEnable(enabled)) + { + return true; + } + + if (ros::Time::now() > timeout) + { + break; + } + r.sleep(); + } + ROS_ERROR_STREAM("Error in setProcessedRawEnabled(): Unable to set target " + << "processedRawEnabled before timeout"); + return false; + } +} + +bool PylonCameraNode::setLightSourceSelector(std::string lightSource) +{ + boost::lock_guard lock(grab_mutex_); + if (!pylon_camera_->isReady()) + { + ROS_WARN("Error in setLightSource(): pylon_camera_ is not ready!"); + return false; + } + if (pylon_camera_->setLightSourceSelector(lightSource)) + { + return true; + } + else // retry till timeout + { + // wait for max 5s till the cam has updated the gamma value + ros::Rate r(10.0); + ros::Time timeout(ros::Time::now() + ros::Duration(5.0)); + while (ros::ok()) + { + if (pylon_camera_->setLightSourceSelector(lightSource)) + { + return true; + } + + if (ros::Time::now() > timeout) + { + break; + } + r.sleep(); + } + ROS_ERROR_STREAM("Error in setLightSourceSelector(): Unable to set target " + << "processedRawEnabled before timeout"); + return false; + } +} + bool PylonCameraNode::setGamma(const float& target_gamma, float& reached_gamma) { boost::lock_guard lock(grab_mutex_); diff --git a/src/pylon_camera/pylon_camera_parameter.cpp b/src/pylon_camera/pylon_camera_parameter.cpp index 95c397ac..7f688051 100644 --- a/src/pylon_camera/pylon_camera_parameter.cpp +++ b/src/pylon_camera/pylon_camera_parameter.cpp @@ -244,6 +244,23 @@ void PylonCameraParameter::readFromRosParameterServer(const ros::NodeHandle& nh) } } } + + processed_raw_enable_given_ = nh.hasParam("processed_raw_enable"); + if (processed_raw_enable_given_) + { + nh.getParam("processed_raw_enable", processed_raw_enable_); + std::cout << "processed_raw_enable is given and has value " << processed_raw_enable_ + << std::endl; + } + + light_source_given_ = nh.hasParam("light_source"); + if (light_source_given_) + { + nh.getParam("light_source", light_source_); + std::cout << "light_source_ is given and has value " << light_source_ + << std::endl; + } + // ########################## nh.param("exposure_search_timeout", exposure_search_timeout_, 5.); From 773fac9153ea434f73308c4b9a51fbb9b154365c Mon Sep 17 00:00:00 2001 From: Fabio Falezza Date: Fri, 7 Sep 2018 17:32:52 +0200 Subject: [PATCH 3/3] Added doxy comments, updated readme, and removed debug print --- .gitignore | 5 ----- README.rst | 9 +++++++++ .../pylon_camera/internal/impl/pylon_camera_base.hpp | 2 -- .../pylon_camera/internal/impl/pylon_camera_gige.hpp | 4 ++-- include/pylon_camera/pylon_camera.h | 8 ++++++++ include/pylon_camera/pylon_camera_node.h | 12 ++++++++++++ include/pylon_camera/pylon_camera_parameter.h | 2 +- src/pylon_camera/pylon_camera_node.cpp | 7 ++++--- 8 files changed, 36 insertions(+), 13 deletions(-) diff --git a/.gitignore b/.gitignore index ee346060..23b5dbb1 100644 --- a/.gitignore +++ b/.gitignore @@ -2,11 +2,6 @@ ## folders ## ############# build -.vscode -left.yaml -right.yaml -left.launch -right.launch ############# ## files ## diff --git a/README.rst b/README.rst index 0480d82e..fb676b9a 100644 --- a/README.rst +++ b/README.rst @@ -81,6 +81,9 @@ All parameters are listed in the default config file: ``config/default.yaml`` - **binning_x & binning_y** Binning factor to get downsampled images. It refers here to any camera setting which combines rectangular neighborhoods of pixels into larger "super-pixels." It reduces the resolution of the output image to (width / binning_x) x (height / binning_y). The default values binning_x = binning_y = 0 are considered the same as binning_x = binning_y = 1 (no subsampling). +- **decimation_x & decimation_y** + The Decimation camera feature allows you to reduce the number of sensor pixel columns or rows that are transmitted by the camera. This procedure is also known as "subsampling". It reduces the amount of data to be transferred and may increase the camera's frame rate. + - **downsampling_factor_exposure_search** To speed up the exposure search, the mean brightness is not calculated on the entire image, but on a subset instead. The image is downsampled until a desired window hight is reached. The window hight is calculated out of the image height divided by the downsampling_factor_exposure search @@ -113,6 +116,12 @@ The following settings do **NOT** have to be set. Each camera has default values - **exposure_auto & gain_auto** Only relevant, if '**brightness**' is set: If the camera should try to reach and / or keep the brightness, hence adapting to changing light conditions, at least one of the following flags must be set. If both are set, the interface will use the profile that tries to keep the gain at minimum to reduce white noise. The exposure_auto flag indicates, that the desired brightness will be reached by adapting the exposure time. The gain_auto flag indicates, that the desired brightness will be reached by adapting the gain. +- **processed_raw_enable** + Only relevant, if the camera's pixel format is '**Bayer 12**'. It allows you to use the Color Transformation and Color Adjustment features when using a Bayer pixel format. + +- **light_source** + It allows you to correct color shifts caused by certain light sources. The supported light_source values are: 'off', 'daylight', 'daylight6500k', 'tungsten'. + **Optional and device specific parameter** - **gige/mtu_size** diff --git a/include/pylon_camera/internal/impl/pylon_camera_base.hpp b/include/pylon_camera/internal/impl/pylon_camera_base.hpp index 279f4298..82365d51 100644 --- a/include/pylon_camera/internal/impl/pylon_camera_base.hpp +++ b/include/pylon_camera/internal/impl/pylon_camera_base.hpp @@ -672,7 +672,6 @@ bool PylonCameraImpl::setDecimationX(const size_t &target_decimati cam_->DecimationHorizontal.SetValue(decimation_x_to_set); reached_decimation_x = currentDecimationX(); cam_->StartGrabbing(); - std::cout << "-----------" << cam_->Width.GetValue() << std::endl; img_cols_ = static_cast(cam_->Width.GetValue()); img_size_byte_ = img_cols_ * img_rows_ * imagePixelDepth(); } @@ -720,7 +719,6 @@ bool PylonCameraImpl::setDecimationY(const size_t &target_decimati cam_->DecimationVertical.SetValue(decimation_y_to_set); reached_decimation_y = currentDecimationY(); cam_->StartGrabbing(); - std::cout << "-----------" << cam_->Height.GetValue() << std::endl; img_rows_ = static_cast(cam_->Height.GetValue()); img_size_byte_ = img_cols_ * img_rows_ * imagePixelDepth(); } diff --git a/include/pylon_camera/internal/impl/pylon_camera_gige.hpp b/include/pylon_camera/internal/impl/pylon_camera_gige.hpp index 8ce5c676..edd80091 100644 --- a/include/pylon_camera/internal/impl/pylon_camera_gige.hpp +++ b/include/pylon_camera/internal/impl/pylon_camera_gige.hpp @@ -407,7 +407,7 @@ bool PylonGigECamera::setProcessedRawEnable(bool &enabled) catch (const GenICam::GenericException &e) { ROS_ERROR_STREAM("An exception while setting processed raw enable to " - << enabled << " occurred:" + << (enabled ? "true" : "false" )<< " occurred:" << e.GetDescription()); return false; } @@ -442,7 +442,7 @@ bool PylonGigECamera::setLightSourceSelector(std::string selector) } catch (const GenICam::GenericException &e) { - ROS_ERROR_STREAM("An exception while setting processed raw enable to " + ROS_ERROR_STREAM("An exception while setting processed light source to " << selector << " occurred:" << e.GetDescription()); return false; diff --git a/include/pylon_camera/pylon_camera.h b/include/pylon_camera/pylon_camera.h index 3f77ba35..1936e40a 100644 --- a/include/pylon_camera/pylon_camera.h +++ b/include/pylon_camera/pylon_camera.h @@ -273,8 +273,16 @@ class PylonCamera */ virtual size_t currentBinningY() = 0; + /** + * Returns the current horizontal decimation_x setting. + * @return the horizontal decimation_x setting. + */ virtual size_t currentDecimationX() = 0; + /** + * Returns the current vertical decimation_y setting. + * @return the vertical decimation_y setting. + */ virtual size_t currentDecimationY() = 0; /** diff --git a/include/pylon_camera/pylon_camera_node.h b/include/pylon_camera/pylon_camera_node.h index 9d422a5e..1f6d31ee 100644 --- a/include/pylon_camera/pylon_camera_node.h +++ b/include/pylon_camera/pylon_camera_node.h @@ -187,8 +187,20 @@ class PylonCameraNode bool setDecimationY(const size_t &target_decimation_y, size_t &reached_decimation_y); + /** + * Update the processed_raw_enable flag + * + * @param enabled the value of the flag + * @return true if it could change the value + */ bool setProcessedRawEnable(bool &enabled); + /** + * Set the Light Source Selector color correction + * + * @param lightSource the value of color correction to apply + * @return true if it could change the color correction + */ bool setLightSourceSelector(std::string lightSource); /** diff --git a/include/pylon_camera/pylon_camera_parameter.h b/include/pylon_camera/pylon_camera_parameter.h index f9199b56..baed7938 100644 --- a/include/pylon_camera/pylon_camera_parameter.h +++ b/include/pylon_camera/pylon_camera_parameter.h @@ -294,7 +294,7 @@ class PylonCameraParameter bool processed_raw_enable_given_; /** - * Flag that indicates if the camera need to abilitate processed raw + * String that indicates if the camera need to do light color correction */ std::string light_source_; bool light_source_given_; diff --git a/src/pylon_camera/pylon_camera_node.cpp b/src/pylon_camera/pylon_camera_node.cpp index 40fb9e5e..994b8c71 100644 --- a/src/pylon_camera/pylon_camera_node.cpp +++ b/src/pylon_camera/pylon_camera_node.cpp @@ -341,14 +341,15 @@ bool PylonCameraNode::startGrabbing() if ( pylon_camera_parameter_set_.processed_raw_enable_given_ ) { setProcessedRawEnable(pylon_camera_parameter_set_.processed_raw_enable_); - ROS_INFO_STREAM("Setting processed_raw_enable_given_ to " - << pylon_camera_parameter_set_.processed_raw_enable_); + ROS_INFO_STREAM("Setting Processed raw to " + << (pylon_camera_parameter_set_.processed_raw_enable_ ? + "true" : "false")); } if ( pylon_camera_parameter_set_.light_source_given_ ) { setLightSourceSelector(pylon_camera_parameter_set_.light_source_); - ROS_INFO_STREAM("Setting processed_raw_enable_given_ to " + ROS_INFO_STREAM("Setting light source to " << pylon_camera_parameter_set_.light_source_); }