diff --git a/README.md b/README.md index db123cce..737b4453 100644 --- a/README.md +++ b/README.md @@ -72,7 +72,9 @@ This is a list of the supported functionality accesible through ROS services, wh * Acquisition Frame Rate * Resulting Frame Rate * Trigger Timeout - * grabbing Timeout + * Grabbing Timeout + - Grabbing Strategy + - Output Queue Size ### Digital I/O Control * Line Selector @@ -153,6 +155,8 @@ Service Name | Notes /pylon_camera_node/stop_grabbing | - /pylon_camera_node/set_grab_timeout | - /pylon_camera_node/set_trigger_timeout | - +/pylon_camera_node/set_grabbing_strategy | value : 0 = GrabStrategy_OneByOne, 1 = GrabStrategy_LatestImageOnly, 2 = GrabStrategy_LatestImages +/pylon_camera_node/set_output_queue_size | - ## Image pixel encoding diff --git a/camera_control_msgs/srv/SetIntegerValue.srv b/camera_control_msgs/srv/SetIntegerValue.srv index 47b613d4..69b6c00f 100644 --- a/camera_control_msgs/srv/SetIntegerValue.srv +++ b/camera_control_msgs/srv/SetIntegerValue.srv @@ -19,6 +19,8 @@ # - set_gamma_selector (value: 0 = User, 1 = sRGB) # - set_grab_timeout (unit : ms) # - set_trigger_timeout (unit : ms) +# - set_grabbing_strategy : set 0 = GrabStrategy_OneByOne, set 1 = GrabStrategy_LatestImageOnly, set 2 = GrabStrategy_LatestImages + int32 value # value to be setted --- diff --git a/pylon_camera/CHANGELOG.rst b/pylon_camera/CHANGELOG.rst index e148271e..a1e01023 100644 --- a/pylon_camera/CHANGELOG.rst +++ b/pylon_camera/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package pylon_camera ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.16.6 (2020-10-19) +------------------- +* Add below listed new ROS services : + - set_grabbing_strategy. + - set_output_queue_size. +* Make camera grabbing strategy configurable parameters in default.yaml + 0.16.5 (2020-10-15) ------------------- * Add below listed new ROS services : diff --git a/pylon_camera/README.rst b/pylon_camera/README.rst index b1616f9b..ddcacc5e 100644 --- a/pylon_camera/README.rst +++ b/pylon_camera/README.rst @@ -119,6 +119,9 @@ The following settings do **NOT** have to be set. Each camera has default values - **grab_timeout** The camera grab timout in ms +- **grab_strategy** + The camera grabbing strategy. + ****** **Usage** @@ -169,6 +172,8 @@ Some of the ROS service use integer values for as a commands, below are a list o - **set_gamma_selector** ROS Service: 0 = User, 1 = sRGB +- **set_grabbing_strategy** ROS Service: 0 = GrabStrategy_OneByOne, 1 = GrabStrategy_LatestImageOnly, 2 = GrabStrategy_LatestImages + ****** **Questions** ****** diff --git a/pylon_camera/config/default.yaml b/pylon_camera/config/default.yaml index 8a557157..9733d868 100644 --- a/pylon_camera/config/default.yaml +++ b/pylon_camera/config/default.yaml @@ -36,6 +36,12 @@ frame_rate: 5.0 trigger_timeout: 5000 # in ms grab_timeout: 500 # in ms +# Camera grab strategy +# 0 : GrabStrategy_OneByOne +# 1 : GrabStrategy_LatestImageOnly +# 2 : GrabStrategy_LatestImages +grab_strategy : 0 + # Mode of camera's shutter. # The supported modes are "rolling", "global" and "global_reset" diff --git a/pylon_camera/include/pylon_camera/internal/impl/pylon_camera_base.hpp b/pylon_camera/include/pylon_camera/internal/impl/pylon_camera_base.hpp index f4047fa4..2b1ec241 100644 --- a/pylon_camera/include/pylon_camera/internal/impl/pylon_camera_base.hpp +++ b/pylon_camera/include/pylon_camera/internal/impl/pylon_camera_base.hpp @@ -44,8 +44,6 @@ namespace pylon_camera { - int trigger_timeout; - template PylonCameraImpl::PylonCameraImpl(Pylon::IPylonDevice* device) : PylonCamera(), @@ -179,7 +177,8 @@ std::string PylonCameraImpl::currentROSEncoding() const ROS_ERROR_STREAM("No ROS equivalent to GenApi encoding"); cam_->StopGrabbing(); setImageEncoding(gen_api_encoding); - cam_->StartGrabbing(); + //cam_->StartGrabbing(); + grabbingStarting(); //return "NO_ENCODING"; } return ros_encoding; @@ -359,8 +358,9 @@ bool PylonCameraImpl::startGrabbing(const PylonCameraParameter& pa } if (error) return false; } - - cam_->StartGrabbing(); + grab_strategy = parameters.grab_strategy_; + //cam_->StartGrabbing(); + grabbingStarting(); user_output_selector_enums_ = detectAndCountNumUserOutputs(); device_user_id_ = cam_->DeviceUserID.GetValue(); img_rows_ = static_cast(cam_->Height.GetValue()); @@ -641,7 +641,8 @@ std::string PylonCameraImpl::setImageEncoding(const std::string& r if ( GenApi::IsAvailable(cam_->PixelFormat) ) { GenApi::INodeMap& node_map = cam_->GetNodeMap(); - cam_->StartGrabbing(); + //cam_->StartGrabbing(); + grabbingStarting(); cam_->StopGrabbing(); GenApi::CEnumerationPtr(node_map.GetNode("PixelFormat"))->FromString(gen_api_encoding.c_str()); return "done"; @@ -784,7 +785,8 @@ bool PylonCameraImpl::setROI(const sensor_msgs::RegionOfInterest t cam_->OffsetX.SetValue(offset_x_to_set); cam_->OffsetY.SetValue(offset_y_to_set); reached_roi = currentROI(); - cam_->StartGrabbing(); + grabbingStarting(); + //cam_->StartGrabbing(); img_cols_ = static_cast(cam_->Width.GetValue()); img_rows_ = static_cast(cam_->Height.GetValue()); @@ -842,7 +844,8 @@ bool PylonCameraImpl::setBinningX(const size_t& target_binning_x, } cam_->BinningHorizontal.SetValue(binning_x_to_set); reached_binning_x = currentBinningX(); - cam_->StartGrabbing(); + //cam_->StartGrabbing(); + grabbingStarting(); img_cols_ = static_cast(cam_->Width.GetValue()); img_size_byte_ = img_cols_ * img_rows_ * imagePixelDepth(); } @@ -889,7 +892,8 @@ bool PylonCameraImpl::setBinningY(const size_t& target_binning_y, } cam_->BinningVertical.SetValue(binning_y_to_set); reached_binning_y = currentBinningY(); - cam_->StartGrabbing(); + //cam_->StartGrabbing(); + grabbingStarting(); img_rows_ = static_cast(cam_->Height.GetValue()); img_size_byte_ = img_cols_ * img_rows_ * imagePixelDepth(); } @@ -2761,11 +2765,20 @@ std::string PylonCameraImpl::triggerDeviceReset() } template -std::string PylonCameraImpl::grabbingStarting() +std::string PylonCameraImpl::grabbingStarting() const { try { - cam_->StartGrabbing(); + if(grab_strategy == 0) { + cam_->StartGrabbing(Pylon::EGrabStrategy::GrabStrategy_OneByOne); + } else if (grab_strategy == 1) { + cam_->StartGrabbing(Pylon::EGrabStrategy::GrabStrategy_LatestImageOnly); + } else if (grab_strategy == 2) { + cam_->StartGrabbing(Pylon::EGrabStrategy::GrabStrategy_LatestImages); + } else { + cam_->StartGrabbing(Pylon::EGrabStrategy::GrabStrategy_OneByOne); + } + return "done"; } @@ -2803,7 +2816,7 @@ std::string PylonCameraImpl::setMaxTransferSize(const int& maxTran } catch ( const GenICam::GenericException &e ) { - ROS_ERROR_STREAM("An exception while starting the free run mode occurred:" << e.GetDescription()); + ROS_ERROR_STREAM("An exception while setting the max transfer size occurred:" << e.GetDescription()); grabbingStarting(); return e.GetDescription(); } @@ -2814,6 +2827,36 @@ float PylonCameraImpl::getTemperature(){ return 0.0; } + +template +bool PylonCameraImpl::setGrabbingStrategy(const int& strategy) { + if (strategy >= 0 && strategy <= 2){ + grab_strategy = strategy; + return true; + } else { + return false; + } + +} + +template +std::string PylonCameraImpl::setOutputQueueSize(const int& size) { + if (size >= 0 && size <= cam_->MaxNumBuffer.GetValue()){ + try { + cam_->OutputQueueSize.SetValue(size); + return "done"; + } catch ( const GenICam::GenericException &e ){ + ROS_ERROR_STREAM("An exception while setting the output queue size occurred:" << e.GetDescription()); + return e.GetDescription(); + } + } else { + return "requested output queue size is out side the limits of : 0-"+std::to_string(cam_->MaxNumBuffer.GetValue()); + } + +} + } // namespace pylon_camera + + #endif // PYLON_CAMERA_INTERNAL_BASE_HPP_ diff --git a/pylon_camera/include/pylon_camera/internal/impl/pylon_camera_gige.hpp b/pylon_camera/include/pylon_camera/internal/impl/pylon_camera_gige.hpp index 231af28e..58ef7d6a 100644 --- a/pylon_camera/include/pylon_camera/internal/impl/pylon_camera_gige.hpp +++ b/pylon_camera/include/pylon_camera/internal/impl/pylon_camera_gige.hpp @@ -92,7 +92,8 @@ bool PylonGigECamera::setAutoflash(const std::map flash_on_lines) { try { - cam_->StartGrabbing(); + //cam_->StartGrabbing(); + grabbingStarting(); cam_->StopGrabbing(); ROS_INFO("Executing SetAutoFlash: %i -> %i", p.first, p.second); if (p.first == 2) @@ -146,7 +147,8 @@ bool PylonGigECamera::applyCamSpecificStartupSettings(const PylonCameraParameter { try { - cam_->StartGrabbing(); + //cam_->StartGrabbing(); + grabbingStarting(); cam_->StopGrabbing(); if (parameters.startup_user_set_ == "Default") { diff --git a/pylon_camera/include/pylon_camera/internal/impl/pylon_camera_usb.hpp b/pylon_camera/include/pylon_camera/internal/impl/pylon_camera_usb.hpp index 7d609bd9..1e1ba463 100644 --- a/pylon_camera/include/pylon_camera/internal/impl/pylon_camera_usb.hpp +++ b/pylon_camera/include/pylon_camera/internal/impl/pylon_camera_usb.hpp @@ -87,7 +87,8 @@ bool PylonUSBCamera::applyCamSpecificStartupSettings(const PylonCameraParameter& { try { - cam_->StartGrabbing(); + //cam_->StartGrabbing(); + grabbingStarting(); cam_->StopGrabbing(); if (parameters.startup_user_set_ == "Default") { diff --git a/pylon_camera/include/pylon_camera/internal/pylon_camera.h b/pylon_camera/include/pylon_camera/internal/pylon_camera.h index 1864f364..1b24a4c4 100644 --- a/pylon_camera/include/pylon_camera/internal/pylon_camera.h +++ b/pylon_camera/include/pylon_camera/internal/pylon_camera.h @@ -243,7 +243,7 @@ class PylonCameraImpl : public PylonCamera virtual std::string triggerDeviceReset(); - virtual std::string grabbingStarting(); + virtual std::string grabbingStarting() const; virtual std::string grabbingStopping(); @@ -255,6 +255,10 @@ class PylonCameraImpl : public PylonCamera virtual float getTemperature(); + virtual bool setGrabbingStrategy(const int& strategy); + + virtual std::string setOutputQueueSize(const int& size); + protected: typedef typename CameraTraitT::CBaslerInstantCameraT CBaslerInstantCameraT; typedef typename CameraTraitT::ExposureAutoEnums ExposureAutoEnums; diff --git a/pylon_camera/include/pylon_camera/pylon_camera.h b/pylon_camera/include/pylon_camera/pylon_camera.h index e75cd4b8..924c5b36 100644 --- a/pylon_camera/include/pylon_camera/pylon_camera.h +++ b/pylon_camera/include/pylon_camera/pylon_camera.h @@ -763,7 +763,7 @@ class PylonCamera * start camera aqcuisition * @return error message if an error occurred or done message otherwise. */ - virtual std::string grabbingStarting() = 0; + virtual std::string grabbingStarting() const = 0; /** * stop camera aqcuisition @@ -792,12 +792,26 @@ class PylonCamera */ virtual std::string gammaEnable(const bool& enable) = 0; + + /** + * set the camera grapping strategy + * @param strategy : 0 = GrabStrategy_OneByOne, 1 = GrabStrategy_LatestImageOnly, 2 = GrabStrategy_LatestImages + * @return error message if an error occurred or done message otherwise. + */ + virtual bool setGrabbingStrategy(const int& strategy) = 0; + /** * returns the current internal camera temperature * @return 0.0 if error or unknown */ virtual float getTemperature() = 0; + /** + * set The size of the grab result buffer output queue + * @return error message if an error occurred or done message otherwise. + */ + virtual std::string setOutputQueueSize(const int& size) = 0; + virtual ~PylonCamera(); protected: /** @@ -850,6 +864,21 @@ class PylonCamera */ bool is_ready_; + /** + * Camera trigger timeout in ms + */ + int trigger_timeout; + + /** + * Camera grab strategy + * 0 = GrabStrategy_OneByOne + * 1 = GrabStrategy_LatestImageOnly + * 2 = GrabStrategy_LatestImages + */ + int grab_strategy ; + + + /** * True if the extended binary exposure search is running. */ diff --git a/pylon_camera/include/pylon_camera/pylon_camera_node.h b/pylon_camera/include/pylon_camera/pylon_camera_node.h index 3b745521..c9d93a54 100644 --- a/pylon_camera/include/pylon_camera/pylon_camera_node.h +++ b/pylon_camera/include/pylon_camera/pylon_camera_node.h @@ -938,6 +938,22 @@ class PylonCameraNode */ bool setTriggerTimeoutCallback(camera_control_msgs::SetIntegerValue::Request &req, camera_control_msgs::SetIntegerValue::Response &res); + /** + * Service callback for setting the camera grabbing strategy + * @param req request + * @param res response + * @return true on success + */ + bool setGrabbingStrategyCallback(camera_control_msgs::SetIntegerValue::Request &req, camera_control_msgs::SetIntegerValue::Response &res); + + /** + * Service callback for setting the size of the grab result buffer output queue. + * @param req request + * @param res response + * @return true on success + */ + bool setOutputQueueSizeCallback(camera_control_msgs::SetIntegerValue::Request &req, camera_control_msgs::SetIntegerValue::Response &res); + ros::NodeHandle nh_; PylonCameraParameter pylon_camera_parameter_set_; ros::ServiceServer set_binning_srv_; @@ -986,6 +1002,8 @@ class PylonCameraNode ros::ServiceServer gamma_enable_srv; ros::ServiceServer set_grab_timeout_srv; ros::ServiceServer set_trigger_timeout_srv; + ros::ServiceServer set_grabbing_strategy_srv; + ros::ServiceServer set_output_queue_size_srv; std::vector set_user_output_srvs_; diff --git a/pylon_camera/include/pylon_camera/pylon_camera_parameter.h b/pylon_camera/include/pylon_camera/pylon_camera_parameter.h index 8a6b9564..3d64191b 100644 --- a/pylon_camera/include/pylon_camera/pylon_camera_parameter.h +++ b/pylon_camera/include/pylon_camera/pylon_camera_parameter.h @@ -316,6 +316,14 @@ class PylonCameraParameter */ int trigger_timeout_; + /** + * Camera grab strategy + * 0 = GrabStrategy_OneByOne + * 1 = GrabStrategy_LatestImageOnly + * 2 = GrabStrategy_LatestImages + */ + int grab_strategy_; + protected: diff --git a/pylon_camera/src/pylon_camera/pylon_camera_node.cpp b/pylon_camera/src/pylon_camera/pylon_camera_node.cpp index 3a50fe68..502a962b 100644 --- a/pylon_camera/src/pylon_camera/pylon_camera_node.cpp +++ b/pylon_camera/src/pylon_camera/pylon_camera_node.cpp @@ -186,6 +186,12 @@ PylonCameraNode::PylonCameraNode() set_trigger_timeout_srv(nh_.advertiseService("set_trigger_timeout", &PylonCameraNode::setTriggerTimeoutCallback, this)), + set_grabbing_strategy_srv(nh_.advertiseService("set_grabbing_strategy", + &PylonCameraNode::setGrabbingStrategyCallback, + this)), + set_output_queue_size_srv(nh_.advertiseService("set_output_queue_size", + &PylonCameraNode::setOutputQueueSizeCallback, + this)), set_user_output_srvs_(), pylon_camera_(nullptr), it_(new image_transport::ImageTransport(nh_)), @@ -2941,6 +2947,45 @@ bool PylonCameraNode::setTriggerTimeoutCallback(camera_control_msgs::SetIntegerV return true; } +bool PylonCameraNode::setGrabbingStrategyCallback(camera_control_msgs::SetIntegerValue::Request &req, camera_control_msgs::SetIntegerValue::Response &res) +{ // set 0 = GrabStrategy_OneByOne + // set 1 = GrabStrategy_LatestImageOnly + // set 2 = GrabStrategy_LatestImages + + if(req.value >= 0 && req.value <= 2) { + grabbingStopping(); + res.success = pylon_camera_->setGrabbingStrategy(req.value); + if(res.success){ + pylon_camera_parameter_set_.grab_strategy_ = req.value; + } + grabbingStarting(); + + } else { + res.success = false; + res.message = "Unknown grabbing strategy"; + } + + return true; +} + + +bool PylonCameraNode::setOutputQueueSizeCallback(camera_control_msgs::SetIntegerValue::Request &req, camera_control_msgs::SetIntegerValue::Response &res) +{ + + grabbingStopping(); + res.message = pylon_camera_->setOutputQueueSize(req.value); + grabbingStarting(); + if ((res.message.find("done") != std::string::npos) != 0) + { + res.success = true; + } else { + res.success = false; + } + + return true; +} + + void PylonCameraNode::currentParamPub() { boost::lock_guard lock(grab_mutex_); diff --git a/pylon_camera/src/pylon_camera/pylon_camera_parameter.cpp b/pylon_camera/src/pylon_camera/pylon_camera_parameter.cpp index f10215ad..e4f94619 100644 --- a/pylon_camera/src/pylon_camera/pylon_camera_parameter.cpp +++ b/pylon_camera/src/pylon_camera/pylon_camera_parameter.cpp @@ -71,7 +71,8 @@ PylonCameraParameter::PylonCameraParameter() : shutter_mode_(SM_DEFAULT), auto_flash_(false), grab_timeout_(500), - trigger_timeout_(5000) + trigger_timeout_(5000), + grab_strategy_(0) {} PylonCameraParameter::~PylonCameraParameter() @@ -267,6 +268,10 @@ void PylonCameraParameter::readFromRosParameterServer(const ros::NodeHandle& nh) { nh.getParam("trigger_timeout", trigger_timeout_); } + if ( nh.hasParam("grab_strategy") ) + { + nh.getParam("grab_strategy", grab_strategy_); + } nh.param("auto_flash", auto_flash_, false); nh.param("auto_flash_line_2", auto_flash_line_2_, true);