From 0c7aab664214413e269103216ae90a9ebe4bb237 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Wed, 14 Dec 2022 10:16:59 +0100 Subject: [PATCH 1/8] Added parameter for device throughput limit --- .../include/spinnaker_camera_driver/SpinnakerCamera.h | 3 +++ .../include/spinnaker_camera_driver/camera.h | 1 + spinnaker_camera_driver/src/SpinnakerCamera.cpp | 7 +++++++ spinnaker_camera_driver/src/camera.cpp | 6 ++++++ spinnaker_camera_driver/src/nodelet.cpp | 4 ++++ 5 files changed, 21 insertions(+) diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h index 4cd9251b..eac04f30 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h @@ -159,6 +159,7 @@ class SpinnakerCamera * \param id serial number for the camera. Should be something like 10491081. */ void setDesiredCamera(const uint32_t& id); + void setThroughputLimit(const uint32_t& limit); void setGain(const float& gain); int getHeightMax(); @@ -198,6 +199,8 @@ class SpinnakerCamera unsigned int packet_size_; /// GigE packet delay: unsigned int packet_delay_; + /// GigE throughput limit: + unsigned int throughput_limit_; uint64_t timeout_; diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h index 5454588d..92e449f1 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h @@ -65,6 +65,7 @@ class Camera virtual void setGain(const float& gain); int getHeightMax(); int getWidthMax(); + void setThroughputLimit(const int& limit); Spinnaker::GenApi::CNodePtr readProperty(const Spinnaker::GenICam::gcstring property_name); diff --git a/spinnaker_camera_driver/src/SpinnakerCamera.cpp b/spinnaker_camera_driver/src/SpinnakerCamera.cpp index 3ecaac9f..47f96fa4 100644 --- a/spinnaker_camera_driver/src/SpinnakerCamera.cpp +++ b/spinnaker_camera_driver/src/SpinnakerCamera.cpp @@ -96,6 +96,12 @@ void SpinnakerCamera::setNewConfiguration(const spinnaker_camera_driver::Spinnak } } // end setNewConfiguration +void SpinnakerCamera::setThroughputLimit(const uint32_t& limit) +{ + if (camera_) + camera_->setThroughputLimit(limit); +} + void SpinnakerCamera::setGain(const float& gain) { if (camera_) @@ -377,6 +383,7 @@ void SpinnakerCamera::grabImage(sensor_msgs::Image* image, const std::string& fr Spinnaker::GenICam::gcstring bayer_gb_str = "BayerGB"; Spinnaker::GenICam::gcstring bayer_bg_str = "BayerBG"; + // if(isColor_ && bayer_format != NONE) if (color_filter_ptr->GetCurrentEntry() != color_filter_ptr->GetEntryByName("None")) { diff --git a/spinnaker_camera_driver/src/camera.cpp b/spinnaker_camera_driver/src/camera.cpp index c7b4956f..3f1c9177 100644 --- a/spinnaker_camera_driver/src/camera.cpp +++ b/spinnaker_camera_driver/src/camera.cpp @@ -242,6 +242,12 @@ void Camera::setGain(const float& gain) setProperty(node_map_, "Gain", static_cast(gain)); } +void Camera::setThroughputLimit(const int& limit) +{ + setProperty(node_map_, "DeviceLinkThroughputLimit", limit); +} + + /* void Camera::setGigEParameters(bool auto_packet_size, unsigned int packet_size, unsigned int packet_delay) { diff --git a/spinnaker_camera_driver/src/nodelet.cpp b/spinnaker_camera_driver/src/nodelet.cpp index 1c8588d0..f1ec784b 100644 --- a/spinnaker_camera_driver/src/nodelet.cpp +++ b/spinnaker_camera_driver/src/nodelet.cpp @@ -302,6 +302,7 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet pnh.param("packet_size", packet_size_, 1400); pnh.param("auto_packet_size", auto_packet_size_, true); pnh.param("packet_delay", packet_delay_, 4000); + pnh.param("throughput_limit", throughput_limit_, 125000000); // TODO(mhosmar): Set GigE parameters: // spinnaker_.setGigEParameters(auto_packet_size_, packet_size_, packet_delay_); @@ -518,6 +519,7 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet NODELET_DEBUG("Connected to camera."); // Set last configuration, forcing the reconfigure level to stop + spinnaker_.setThroughputLimit(throughput_limit_); spinnaker_.setNewConfiguration(config_, SpinnakerCamera::LEVEL_RECONFIGURE_STOP); // Set the timeout for grabbing images. @@ -719,6 +721,8 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet int packet_size_; /// GigE packet delay: int packet_delay_; + /// GigE throughput limit: + int throughput_limit_; /// Configuration: spinnaker_camera_driver::SpinnakerConfig config_; From 6cafb1344b1a6aa189c0d01dab9b19c676a8270a Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Mon, 28 Aug 2023 14:57:16 +0200 Subject: [PATCH 2/8] Added framework for trigger management. To be tested on real hardware --- .../spinnaker_camera_driver/SpinnakerCamera.h | 4 + .../src/SpinnakerCamera.cpp | 162 ++++++++++++++++++ spinnaker_camera_driver/src/nodelet.cpp | 58 ++++++- 3 files changed, 217 insertions(+), 7 deletions(-) diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h index eac04f30..1ba966a0 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h @@ -130,6 +130,8 @@ class SpinnakerCamera */ void stop(); + void trigger(); + /*! * \brief Loads the raw data from the cameras buffer. * @@ -167,6 +169,8 @@ class SpinnakerCamera bool readableProperty(const Spinnaker::GenICam::gcstring property_name); Spinnaker::GenApi::CNodePtr readProperty(const Spinnaker::GenICam::gcstring property_name); + void configureTrigger(bool enable, bool software); + uint32_t getSerial() { return serial_; diff --git a/spinnaker_camera_driver/src/SpinnakerCamera.cpp b/spinnaker_camera_driver/src/SpinnakerCamera.cpp index 47f96fa4..eec3a355 100644 --- a/spinnaker_camera_driver/src/SpinnakerCamera.cpp +++ b/spinnaker_camera_driver/src/SpinnakerCamera.cpp @@ -303,6 +303,32 @@ void SpinnakerCamera::disconnect() } } +void SpinnakerCamera::trigger() +{ + try + { + // Check if camera is connected + if (pCam_ && captureRunning_) + { + // Execute software trigger + Spinnaker::GenApi::CCommandPtr ptrSoftwareTriggerCommand = node_map_->GetNode("TriggerSoftware"); + if (!Spinnaker::GenApi::IsAvailable(ptrSoftwareTriggerCommand) || + !Spinnaker::GenApi::IsWritable(ptrSoftwareTriggerCommand)) + { + throw std::runtime_error("[SpinnakerCamera::trigger] Unable to execute trigger "); + } + + ptrSoftwareTriggerCommand->Execute(); + + + } + } + catch (const Spinnaker::Exception& e) + { + throw std::runtime_error("[SpinnakerCamera::trigger] capture is not running "); + } +} + void SpinnakerCamera::start() { try @@ -338,6 +364,138 @@ void SpinnakerCamera::stop() } } +// This function configures the camera to use a trigger. First, trigger mode is +// set to off in order to select the trigger source. Once the trigger source +// has been selected, trigger mode is then enabled, which has the camera +// capture only a single image upon the execution of the chosen trigger. +void SpinnakerCamera::configureTrigger(bool enable, bool software) +{ + try + { + // + // Ensure trigger mode off + // + // *** NOTES *** + // The trigger must be disabled in order to configure whether the source + // is software or hardware. + // + Spinnaker::GenApi::CEnumerationPtr ptrTriggerMode = node_map_->GetNode("TriggerMode"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerMode) || + !Spinnaker::GenApi::IsReadable(ptrTriggerMode)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to disable trigger mode (node retrieval)"); + } + + Spinnaker::GenApi::CEnumEntryPtr ptrTriggerModeOff = ptrTriggerMode->GetEntryByName("Off"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerModeOff) || + !Spinnaker::GenApi::IsReadable(ptrTriggerModeOff)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to disable trigger mode (enum entry retrieval)"); + } + + ptrTriggerMode->SetIntValue(ptrTriggerModeOff->GetValue()); + + if (!enable) { + ROS_INFO("Trigger mode disabled"); + return; + } + ROS_INFO("Trigger mode disabled for configuration..."); + + // + // Set TriggerSelector to FrameStart + // + // *** NOTES *** + // For this example, the trigger selector should be set to frame start. + // This is the default for most cameras. + // + Spinnaker::GenApi::CEnumerationPtr ptrTriggerSelector = node_map_->GetNode("TriggerSelector"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerSelector) || + !Spinnaker::GenApi::IsWritable(ptrTriggerSelector)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to set trigger selector (node retrieval)"); + } + + Spinnaker::GenApi::CEnumEntryPtr ptrTriggerSelectorFrameStart = ptrTriggerSelector->GetEntryByName("FrameStart"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerSelectorFrameStart) || + !Spinnaker::GenApi::IsReadable(ptrTriggerSelectorFrameStart)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to set trigger selector (enum entry retrieval)"); + } + + ptrTriggerSelector->SetIntValue(ptrTriggerSelectorFrameStart->GetValue()); + + ROS_INFO("Trigger selector set to frame start..."); + + // + // Select trigger source + // + // *** NOTES *** + // The trigger source must be set to hardware or software while trigger + // mode is off. + // + Spinnaker::GenApi::CEnumerationPtr ptrTriggerSource = node_map_->GetNode("TriggerSource"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerSource) || + !Spinnaker::GenApi::IsWritable(ptrTriggerSource)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to set trigger mode (node retrieval)"); + } + + if (software) { + // Set trigger mode to software + Spinnaker::GenApi::CEnumEntryPtr ptrTriggerSourceSoftware = ptrTriggerSource->GetEntryByName("Software"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerSourceSoftware) || + !Spinnaker::GenApi::IsReadable(ptrTriggerSourceSoftware)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to set trigger mode (enum entry retrieval)"); + } + + ptrTriggerSource->SetIntValue(ptrTriggerSourceSoftware->GetValue()); + + ROS_INFO("Trigger source set to software."); + } else { + // Set trigger mode to hardware ('Line0') + Spinnaker::GenApi::CEnumEntryPtr ptrTriggerSourceHardware = ptrTriggerSource->GetEntryByName("Line0"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerSourceHardware) || + !Spinnaker::GenApi::IsReadable(ptrTriggerSourceHardware)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to set trigger mode (enum entry retrieval)"); + } + + ptrTriggerSource->SetIntValue(ptrTriggerSourceHardware->GetValue()); + + ROS_INFO("Trigger source set to hardware."); + } + + // + // Turn trigger mode on + // + // *** LATER *** + // Once the appropriate trigger source has been set, turn trigger mode + // on in order to retrieve images using the trigger. + // + + Spinnaker::GenApi::CEnumEntryPtr ptrTriggerModeOn = ptrTriggerMode->GetEntryByName("On"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerModeOn) || + !Spinnaker::GenApi::IsReadable(ptrTriggerModeOn)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to enable trigger mode (enum entry retrieval)"); + } + + ptrTriggerMode->SetIntValue(ptrTriggerModeOn->GetValue()); + + // NOTE: Blackfly and Flea3 GEV cameras need 1 second delay after trigger mode is turned on + + ROS_INFO("Trigger mode turned back on..."); + } + catch (Spinnaker::Exception& e) + { + throw std::runtime_error(std::string("[SpinnakerCamera::configureTrigger] ") + e.what()); + } + +} + + + void SpinnakerCamera::grabImage(sensor_msgs::Image* image, const std::string& frame_id) { std::lock_guard scopedLock(mutex_); @@ -382,6 +540,9 @@ void SpinnakerCamera::grabImage(sensor_msgs::Image* image, const std::string& fr Spinnaker::GenICam::gcstring bayer_gr_str = "BayerGR"; Spinnaker::GenICam::gcstring bayer_gb_str = "BayerGB"; Spinnaker::GenICam::gcstring bayer_bg_str = "BayerBG"; + ROS_WARN_STREAM_ONCE("[SpinnakerCamera::grabImage] camera " + << std::to_string(serial_) + << " pixel color filter is " << color_filter_str); // if(isColor_ && bayer_format != NONE) @@ -481,6 +642,7 @@ void SpinnakerCamera::setTimeout(const double& timeout) { timeout_ = static_cast(std::round(timeout * 1000)); } + void SpinnakerCamera::setDesiredCamera(const uint32_t& id) { serial_ = id; diff --git a/spinnaker_camera_driver/src/nodelet.cpp b/spinnaker_camera_driver/src/nodelet.cpp index f1ec784b..9694bc70 100644 --- a/spinnaker_camera_driver/src/nodelet.cpp +++ b/spinnaker_camera_driver/src/nodelet.cpp @@ -303,6 +303,9 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet pnh.param("auto_packet_size", auto_packet_size_, true); pnh.param("packet_delay", packet_delay_, 4000); pnh.param("throughput_limit", throughput_limit_, 125000000); + pnh.param("trigger", trigger_, false); + pnh.param("sw_trigger", sw_trigger_, false); + pnh.param("max_trigger_delay", max_trigger_delay_, 0.2); // TODO(mhosmar): Set GigE parameters: // spinnaker_.setGigEParameters(auto_packet_size_, packet_size_, packet_delay_); @@ -356,6 +359,13 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet pnh.param("min_acceptable_delay", min_acceptable, 0.0); double max_acceptable; // The maximum publishing delay (in seconds) before warning. pnh.param("max_acceptable_delay", max_acceptable, 0.2); + + if (trigger_) { + trig_sub_ = pnh.subscribe("trigger", 1, + &spinnaker_camera_driver::SpinnakerCameraNodelet::triggerCallback, this); + } + + ros::SubscriberStatusCallback cb2 = boost::bind(&SpinnakerCameraNodelet::connectCb, this); pub_.reset(new diagnostic_updater::DiagnosedPublisher( nh.advertise("image", queue_size, cb2, cb2), @@ -519,6 +529,7 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet NODELET_DEBUG("Connected to camera."); // Set last configuration, forcing the reconfigure level to stop + ROS_INFO("Nodelet: throughput limit to %d",throughput_limit_); spinnaker_.setThroughputLimit(throughput_limit_); spinnaker_.setNewConfiguration(config_, SpinnakerCamera::LEVEL_RECONFIGURE_STOP); @@ -530,6 +541,11 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet NODELET_DEBUG_ONCE("Setting timeout to: %f.", timeout); spinnaker_.setTimeout(timeout); + + NODELET_DEBUG_ONCE("Configuring trigger: %s %s", trigger_?"enabled":"disabled", + sw_trigger_?"software":"hardware"); + spinnaker_.configureTrigger(trigger_,sw_trigger_); + } catch (const std::runtime_error& e) { @@ -539,8 +555,7 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet // Subscribe to gain and white balance changes { std::lock_guard scopedLock(connect_mutex_); - sub_ = - getMTNodeHandle().subscribe("image_exposure_sequence", 10, + sub_ = getMTNodeHandle().subscribe("image_exposure_sequence", 10, &spinnaker_camera_driver::SpinnakerCameraNodelet::gainWBCallback, this); } state = CONNECTED; @@ -586,6 +601,20 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet NODELET_DEBUG_ONCE("Starting a new grab from camera with serial {%d}.", spinnaker_.getSerial()); spinnaker_.grabImage(&wfov_image->image, frame_id_); + ros::Time time = ros::Time::now() + ros::Duration(config_.time_offset); + wfov_image->header.stamp = time; + wfov_image->image.header.stamp = time; + if (trigger_) { + double trigger_delay = (wfov_image->header.stamp - trigger_header_.stamp).toSec(); + if (trigger_delay > max_trigger_delay_) { + NODELET_ERROR("Rejecting image not attached to trigger (age %.3fs max %.3f)",trigger_delay,max_trigger_delay_); + break; + } + // replacing timestamp with trigger stamp + wfov_image->header.stamp = trigger_header_.stamp; + wfov_image->image.header.stamp = trigger_header_.stamp; + } + // Set other values wfov_image->header.frame_id = frame_id_; @@ -595,9 +624,6 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet // wfov_image->temperature = spinnaker_.getCameraTemperature(); - ros::Time time = ros::Time::now() + ros::Duration(config_.time_offset); - wfov_image->header.stamp = time; - wfov_image->image.header.stamp = time; // Set the CameraInfo message ci_.reset(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo())); @@ -614,8 +640,11 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet wfov_image->info = *ci_; - // Publish the full message - pub_->publish(wfov_image); + if (pub_->getPublisher().getNumSubscribers() > 0) + { + // Publish the full message + pub_->publish(wfov_image); + } // Publish the message using standard image transport if (it_pub_.getNumSubscribers() > 0) @@ -667,6 +696,14 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet } } + void triggerCallback(const std_msgs::Header& msg) + { + trigger_header_ = msg; + if (sw_trigger_) { + spinnaker_.trigger(); + } // else we just store the timestamp and assume the camera is hardware triggered + } + /* Class Fields */ std::shared_ptr > srv_; ///< Needed to /// initialize @@ -685,6 +722,7 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet /// constructor /// requirements ros::Subscriber sub_; ///< Subscriber for gain and white balance changes. + ros::Subscriber trig_sub_; ///< Subscriber for gain and white balance changes. std::mutex connect_mutex_; @@ -724,6 +762,12 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet /// GigE throughput limit: int throughput_limit_; + // Data for software trigger management + std_msgs::Header trigger_header_; + bool trigger_; + bool sw_trigger_; + double max_trigger_delay_; + /// Configuration: spinnaker_camera_driver::SpinnakerConfig config_; }; From d94f99acfcf5a2cfd53c4f82eaa79c5ce8f24bc8 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Wed, 14 Dec 2022 10:16:59 +0100 Subject: [PATCH 3/8] Added parameter for device throughput limit --- .../include/spinnaker_camera_driver/SpinnakerCamera.h | 3 +++ .../include/spinnaker_camera_driver/camera.h | 1 + spinnaker_camera_driver/src/SpinnakerCamera.cpp | 7 +++++++ spinnaker_camera_driver/src/camera.cpp | 6 ++++++ spinnaker_camera_driver/src/nodelet.cpp | 4 ++++ 5 files changed, 21 insertions(+) diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h index 4cd9251b..eac04f30 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h @@ -159,6 +159,7 @@ class SpinnakerCamera * \param id serial number for the camera. Should be something like 10491081. */ void setDesiredCamera(const uint32_t& id); + void setThroughputLimit(const uint32_t& limit); void setGain(const float& gain); int getHeightMax(); @@ -198,6 +199,8 @@ class SpinnakerCamera unsigned int packet_size_; /// GigE packet delay: unsigned int packet_delay_; + /// GigE throughput limit: + unsigned int throughput_limit_; uint64_t timeout_; diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h index 5454588d..92e449f1 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h @@ -65,6 +65,7 @@ class Camera virtual void setGain(const float& gain); int getHeightMax(); int getWidthMax(); + void setThroughputLimit(const int& limit); Spinnaker::GenApi::CNodePtr readProperty(const Spinnaker::GenICam::gcstring property_name); diff --git a/spinnaker_camera_driver/src/SpinnakerCamera.cpp b/spinnaker_camera_driver/src/SpinnakerCamera.cpp index 3ecaac9f..47f96fa4 100644 --- a/spinnaker_camera_driver/src/SpinnakerCamera.cpp +++ b/spinnaker_camera_driver/src/SpinnakerCamera.cpp @@ -96,6 +96,12 @@ void SpinnakerCamera::setNewConfiguration(const spinnaker_camera_driver::Spinnak } } // end setNewConfiguration +void SpinnakerCamera::setThroughputLimit(const uint32_t& limit) +{ + if (camera_) + camera_->setThroughputLimit(limit); +} + void SpinnakerCamera::setGain(const float& gain) { if (camera_) @@ -377,6 +383,7 @@ void SpinnakerCamera::grabImage(sensor_msgs::Image* image, const std::string& fr Spinnaker::GenICam::gcstring bayer_gb_str = "BayerGB"; Spinnaker::GenICam::gcstring bayer_bg_str = "BayerBG"; + // if(isColor_ && bayer_format != NONE) if (color_filter_ptr->GetCurrentEntry() != color_filter_ptr->GetEntryByName("None")) { diff --git a/spinnaker_camera_driver/src/camera.cpp b/spinnaker_camera_driver/src/camera.cpp index c7b4956f..3f1c9177 100644 --- a/spinnaker_camera_driver/src/camera.cpp +++ b/spinnaker_camera_driver/src/camera.cpp @@ -242,6 +242,12 @@ void Camera::setGain(const float& gain) setProperty(node_map_, "Gain", static_cast(gain)); } +void Camera::setThroughputLimit(const int& limit) +{ + setProperty(node_map_, "DeviceLinkThroughputLimit", limit); +} + + /* void Camera::setGigEParameters(bool auto_packet_size, unsigned int packet_size, unsigned int packet_delay) { diff --git a/spinnaker_camera_driver/src/nodelet.cpp b/spinnaker_camera_driver/src/nodelet.cpp index 1c8588d0..f1ec784b 100644 --- a/spinnaker_camera_driver/src/nodelet.cpp +++ b/spinnaker_camera_driver/src/nodelet.cpp @@ -302,6 +302,7 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet pnh.param("packet_size", packet_size_, 1400); pnh.param("auto_packet_size", auto_packet_size_, true); pnh.param("packet_delay", packet_delay_, 4000); + pnh.param("throughput_limit", throughput_limit_, 125000000); // TODO(mhosmar): Set GigE parameters: // spinnaker_.setGigEParameters(auto_packet_size_, packet_size_, packet_delay_); @@ -518,6 +519,7 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet NODELET_DEBUG("Connected to camera."); // Set last configuration, forcing the reconfigure level to stop + spinnaker_.setThroughputLimit(throughput_limit_); spinnaker_.setNewConfiguration(config_, SpinnakerCamera::LEVEL_RECONFIGURE_STOP); // Set the timeout for grabbing images. @@ -719,6 +721,8 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet int packet_size_; /// GigE packet delay: int packet_delay_; + /// GigE throughput limit: + int throughput_limit_; /// Configuration: spinnaker_camera_driver::SpinnakerConfig config_; From 6d865143c6af5e02e8753eb5bb3d039fbdd2ed63 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Mon, 28 Aug 2023 14:57:16 +0200 Subject: [PATCH 4/8] Added framework for trigger management. To be tested on real hardware --- .../spinnaker_camera_driver/SpinnakerCamera.h | 4 + .../src/SpinnakerCamera.cpp | 162 ++++++++++++++++++ spinnaker_camera_driver/src/nodelet.cpp | 58 ++++++- 3 files changed, 217 insertions(+), 7 deletions(-) diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h index eac04f30..1ba966a0 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h @@ -130,6 +130,8 @@ class SpinnakerCamera */ void stop(); + void trigger(); + /*! * \brief Loads the raw data from the cameras buffer. * @@ -167,6 +169,8 @@ class SpinnakerCamera bool readableProperty(const Spinnaker::GenICam::gcstring property_name); Spinnaker::GenApi::CNodePtr readProperty(const Spinnaker::GenICam::gcstring property_name); + void configureTrigger(bool enable, bool software); + uint32_t getSerial() { return serial_; diff --git a/spinnaker_camera_driver/src/SpinnakerCamera.cpp b/spinnaker_camera_driver/src/SpinnakerCamera.cpp index 47f96fa4..eec3a355 100644 --- a/spinnaker_camera_driver/src/SpinnakerCamera.cpp +++ b/spinnaker_camera_driver/src/SpinnakerCamera.cpp @@ -303,6 +303,32 @@ void SpinnakerCamera::disconnect() } } +void SpinnakerCamera::trigger() +{ + try + { + // Check if camera is connected + if (pCam_ && captureRunning_) + { + // Execute software trigger + Spinnaker::GenApi::CCommandPtr ptrSoftwareTriggerCommand = node_map_->GetNode("TriggerSoftware"); + if (!Spinnaker::GenApi::IsAvailable(ptrSoftwareTriggerCommand) || + !Spinnaker::GenApi::IsWritable(ptrSoftwareTriggerCommand)) + { + throw std::runtime_error("[SpinnakerCamera::trigger] Unable to execute trigger "); + } + + ptrSoftwareTriggerCommand->Execute(); + + + } + } + catch (const Spinnaker::Exception& e) + { + throw std::runtime_error("[SpinnakerCamera::trigger] capture is not running "); + } +} + void SpinnakerCamera::start() { try @@ -338,6 +364,138 @@ void SpinnakerCamera::stop() } } +// This function configures the camera to use a trigger. First, trigger mode is +// set to off in order to select the trigger source. Once the trigger source +// has been selected, trigger mode is then enabled, which has the camera +// capture only a single image upon the execution of the chosen trigger. +void SpinnakerCamera::configureTrigger(bool enable, bool software) +{ + try + { + // + // Ensure trigger mode off + // + // *** NOTES *** + // The trigger must be disabled in order to configure whether the source + // is software or hardware. + // + Spinnaker::GenApi::CEnumerationPtr ptrTriggerMode = node_map_->GetNode("TriggerMode"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerMode) || + !Spinnaker::GenApi::IsReadable(ptrTriggerMode)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to disable trigger mode (node retrieval)"); + } + + Spinnaker::GenApi::CEnumEntryPtr ptrTriggerModeOff = ptrTriggerMode->GetEntryByName("Off"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerModeOff) || + !Spinnaker::GenApi::IsReadable(ptrTriggerModeOff)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to disable trigger mode (enum entry retrieval)"); + } + + ptrTriggerMode->SetIntValue(ptrTriggerModeOff->GetValue()); + + if (!enable) { + ROS_INFO("Trigger mode disabled"); + return; + } + ROS_INFO("Trigger mode disabled for configuration..."); + + // + // Set TriggerSelector to FrameStart + // + // *** NOTES *** + // For this example, the trigger selector should be set to frame start. + // This is the default for most cameras. + // + Spinnaker::GenApi::CEnumerationPtr ptrTriggerSelector = node_map_->GetNode("TriggerSelector"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerSelector) || + !Spinnaker::GenApi::IsWritable(ptrTriggerSelector)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to set trigger selector (node retrieval)"); + } + + Spinnaker::GenApi::CEnumEntryPtr ptrTriggerSelectorFrameStart = ptrTriggerSelector->GetEntryByName("FrameStart"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerSelectorFrameStart) || + !Spinnaker::GenApi::IsReadable(ptrTriggerSelectorFrameStart)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to set trigger selector (enum entry retrieval)"); + } + + ptrTriggerSelector->SetIntValue(ptrTriggerSelectorFrameStart->GetValue()); + + ROS_INFO("Trigger selector set to frame start..."); + + // + // Select trigger source + // + // *** NOTES *** + // The trigger source must be set to hardware or software while trigger + // mode is off. + // + Spinnaker::GenApi::CEnumerationPtr ptrTriggerSource = node_map_->GetNode("TriggerSource"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerSource) || + !Spinnaker::GenApi::IsWritable(ptrTriggerSource)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to set trigger mode (node retrieval)"); + } + + if (software) { + // Set trigger mode to software + Spinnaker::GenApi::CEnumEntryPtr ptrTriggerSourceSoftware = ptrTriggerSource->GetEntryByName("Software"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerSourceSoftware) || + !Spinnaker::GenApi::IsReadable(ptrTriggerSourceSoftware)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to set trigger mode (enum entry retrieval)"); + } + + ptrTriggerSource->SetIntValue(ptrTriggerSourceSoftware->GetValue()); + + ROS_INFO("Trigger source set to software."); + } else { + // Set trigger mode to hardware ('Line0') + Spinnaker::GenApi::CEnumEntryPtr ptrTriggerSourceHardware = ptrTriggerSource->GetEntryByName("Line0"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerSourceHardware) || + !Spinnaker::GenApi::IsReadable(ptrTriggerSourceHardware)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to set trigger mode (enum entry retrieval)"); + } + + ptrTriggerSource->SetIntValue(ptrTriggerSourceHardware->GetValue()); + + ROS_INFO("Trigger source set to hardware."); + } + + // + // Turn trigger mode on + // + // *** LATER *** + // Once the appropriate trigger source has been set, turn trigger mode + // on in order to retrieve images using the trigger. + // + + Spinnaker::GenApi::CEnumEntryPtr ptrTriggerModeOn = ptrTriggerMode->GetEntryByName("On"); + if (!Spinnaker::GenApi::IsAvailable(ptrTriggerModeOn) || + !Spinnaker::GenApi::IsReadable(ptrTriggerModeOn)) + { + throw std::runtime_error("[SpinnakerCamera::configureTrigger] Unable to enable trigger mode (enum entry retrieval)"); + } + + ptrTriggerMode->SetIntValue(ptrTriggerModeOn->GetValue()); + + // NOTE: Blackfly and Flea3 GEV cameras need 1 second delay after trigger mode is turned on + + ROS_INFO("Trigger mode turned back on..."); + } + catch (Spinnaker::Exception& e) + { + throw std::runtime_error(std::string("[SpinnakerCamera::configureTrigger] ") + e.what()); + } + +} + + + void SpinnakerCamera::grabImage(sensor_msgs::Image* image, const std::string& frame_id) { std::lock_guard scopedLock(mutex_); @@ -382,6 +540,9 @@ void SpinnakerCamera::grabImage(sensor_msgs::Image* image, const std::string& fr Spinnaker::GenICam::gcstring bayer_gr_str = "BayerGR"; Spinnaker::GenICam::gcstring bayer_gb_str = "BayerGB"; Spinnaker::GenICam::gcstring bayer_bg_str = "BayerBG"; + ROS_WARN_STREAM_ONCE("[SpinnakerCamera::grabImage] camera " + << std::to_string(serial_) + << " pixel color filter is " << color_filter_str); // if(isColor_ && bayer_format != NONE) @@ -481,6 +642,7 @@ void SpinnakerCamera::setTimeout(const double& timeout) { timeout_ = static_cast(std::round(timeout * 1000)); } + void SpinnakerCamera::setDesiredCamera(const uint32_t& id) { serial_ = id; diff --git a/spinnaker_camera_driver/src/nodelet.cpp b/spinnaker_camera_driver/src/nodelet.cpp index f1ec784b..9694bc70 100644 --- a/spinnaker_camera_driver/src/nodelet.cpp +++ b/spinnaker_camera_driver/src/nodelet.cpp @@ -303,6 +303,9 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet pnh.param("auto_packet_size", auto_packet_size_, true); pnh.param("packet_delay", packet_delay_, 4000); pnh.param("throughput_limit", throughput_limit_, 125000000); + pnh.param("trigger", trigger_, false); + pnh.param("sw_trigger", sw_trigger_, false); + pnh.param("max_trigger_delay", max_trigger_delay_, 0.2); // TODO(mhosmar): Set GigE parameters: // spinnaker_.setGigEParameters(auto_packet_size_, packet_size_, packet_delay_); @@ -356,6 +359,13 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet pnh.param("min_acceptable_delay", min_acceptable, 0.0); double max_acceptable; // The maximum publishing delay (in seconds) before warning. pnh.param("max_acceptable_delay", max_acceptable, 0.2); + + if (trigger_) { + trig_sub_ = pnh.subscribe("trigger", 1, + &spinnaker_camera_driver::SpinnakerCameraNodelet::triggerCallback, this); + } + + ros::SubscriberStatusCallback cb2 = boost::bind(&SpinnakerCameraNodelet::connectCb, this); pub_.reset(new diagnostic_updater::DiagnosedPublisher( nh.advertise("image", queue_size, cb2, cb2), @@ -519,6 +529,7 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet NODELET_DEBUG("Connected to camera."); // Set last configuration, forcing the reconfigure level to stop + ROS_INFO("Nodelet: throughput limit to %d",throughput_limit_); spinnaker_.setThroughputLimit(throughput_limit_); spinnaker_.setNewConfiguration(config_, SpinnakerCamera::LEVEL_RECONFIGURE_STOP); @@ -530,6 +541,11 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet NODELET_DEBUG_ONCE("Setting timeout to: %f.", timeout); spinnaker_.setTimeout(timeout); + + NODELET_DEBUG_ONCE("Configuring trigger: %s %s", trigger_?"enabled":"disabled", + sw_trigger_?"software":"hardware"); + spinnaker_.configureTrigger(trigger_,sw_trigger_); + } catch (const std::runtime_error& e) { @@ -539,8 +555,7 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet // Subscribe to gain and white balance changes { std::lock_guard scopedLock(connect_mutex_); - sub_ = - getMTNodeHandle().subscribe("image_exposure_sequence", 10, + sub_ = getMTNodeHandle().subscribe("image_exposure_sequence", 10, &spinnaker_camera_driver::SpinnakerCameraNodelet::gainWBCallback, this); } state = CONNECTED; @@ -586,6 +601,20 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet NODELET_DEBUG_ONCE("Starting a new grab from camera with serial {%d}.", spinnaker_.getSerial()); spinnaker_.grabImage(&wfov_image->image, frame_id_); + ros::Time time = ros::Time::now() + ros::Duration(config_.time_offset); + wfov_image->header.stamp = time; + wfov_image->image.header.stamp = time; + if (trigger_) { + double trigger_delay = (wfov_image->header.stamp - trigger_header_.stamp).toSec(); + if (trigger_delay > max_trigger_delay_) { + NODELET_ERROR("Rejecting image not attached to trigger (age %.3fs max %.3f)",trigger_delay,max_trigger_delay_); + break; + } + // replacing timestamp with trigger stamp + wfov_image->header.stamp = trigger_header_.stamp; + wfov_image->image.header.stamp = trigger_header_.stamp; + } + // Set other values wfov_image->header.frame_id = frame_id_; @@ -595,9 +624,6 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet // wfov_image->temperature = spinnaker_.getCameraTemperature(); - ros::Time time = ros::Time::now() + ros::Duration(config_.time_offset); - wfov_image->header.stamp = time; - wfov_image->image.header.stamp = time; // Set the CameraInfo message ci_.reset(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo())); @@ -614,8 +640,11 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet wfov_image->info = *ci_; - // Publish the full message - pub_->publish(wfov_image); + if (pub_->getPublisher().getNumSubscribers() > 0) + { + // Publish the full message + pub_->publish(wfov_image); + } // Publish the message using standard image transport if (it_pub_.getNumSubscribers() > 0) @@ -667,6 +696,14 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet } } + void triggerCallback(const std_msgs::Header& msg) + { + trigger_header_ = msg; + if (sw_trigger_) { + spinnaker_.trigger(); + } // else we just store the timestamp and assume the camera is hardware triggered + } + /* Class Fields */ std::shared_ptr > srv_; ///< Needed to /// initialize @@ -685,6 +722,7 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet /// constructor /// requirements ros::Subscriber sub_; ///< Subscriber for gain and white balance changes. + ros::Subscriber trig_sub_; ///< Subscriber for gain and white balance changes. std::mutex connect_mutex_; @@ -724,6 +762,12 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet /// GigE throughput limit: int throughput_limit_; + // Data for software trigger management + std_msgs::Header trigger_header_; + bool trigger_; + bool sw_trigger_; + double max_trigger_delay_; + /// Configuration: spinnaker_camera_driver::SpinnakerConfig config_; }; From 981a725256280aefd31f9ef281e3a34328b53ec6 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Wed, 14 Dec 2022 10:16:59 +0100 Subject: [PATCH 5/8] Added parameter for device throughput limit --- spinnaker_camera_driver/src/SpinnakerCamera.cpp | 1 + spinnaker_camera_driver/src/nodelet.cpp | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/spinnaker_camera_driver/src/SpinnakerCamera.cpp b/spinnaker_camera_driver/src/SpinnakerCamera.cpp index eec3a355..9f21b3a5 100644 --- a/spinnaker_camera_driver/src/SpinnakerCamera.cpp +++ b/spinnaker_camera_driver/src/SpinnakerCamera.cpp @@ -545,6 +545,7 @@ void SpinnakerCamera::grabImage(sensor_msgs::Image* image, const std::string& fr << " pixel color filter is " << color_filter_str); + // if(isColor_ && bayer_format != NONE) if (color_filter_ptr->GetCurrentEntry() != color_filter_ptr->GetEntryByName("None")) { diff --git a/spinnaker_camera_driver/src/nodelet.cpp b/spinnaker_camera_driver/src/nodelet.cpp index 9694bc70..b483fc04 100644 --- a/spinnaker_camera_driver/src/nodelet.cpp +++ b/spinnaker_camera_driver/src/nodelet.cpp @@ -303,9 +303,12 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet pnh.param("auto_packet_size", auto_packet_size_, true); pnh.param("packet_delay", packet_delay_, 4000); pnh.param("throughput_limit", throughput_limit_, 125000000); +<<<<<<< HEAD pnh.param("trigger", trigger_, false); pnh.param("sw_trigger", sw_trigger_, false); pnh.param("max_trigger_delay", max_trigger_delay_, 0.2); +======= +>>>>>>> Added parameter for device throughput limit // TODO(mhosmar): Set GigE parameters: // spinnaker_.setGigEParameters(auto_packet_size_, packet_size_, packet_delay_); @@ -529,7 +532,10 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet NODELET_DEBUG("Connected to camera."); // Set last configuration, forcing the reconfigure level to stop +<<<<<<< HEAD ROS_INFO("Nodelet: throughput limit to %d",throughput_limit_); +======= +>>>>>>> Added parameter for device throughput limit spinnaker_.setThroughputLimit(throughput_limit_); spinnaker_.setNewConfiguration(config_, SpinnakerCamera::LEVEL_RECONFIGURE_STOP); From 4497fd0f4bc26a2fe11f737c0ba0989086acd07e Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Mon, 30 Oct 2023 16:54:26 +0100 Subject: [PATCH 6/8] Working version after merge --- spinnaker_camera_driver/src/SpinnakerCamera.cpp | 1 - spinnaker_camera_driver/src/nodelet.cpp | 8 +------- 2 files changed, 1 insertion(+), 8 deletions(-) diff --git a/spinnaker_camera_driver/src/SpinnakerCamera.cpp b/spinnaker_camera_driver/src/SpinnakerCamera.cpp index 9f21b3a5..eec3a355 100644 --- a/spinnaker_camera_driver/src/SpinnakerCamera.cpp +++ b/spinnaker_camera_driver/src/SpinnakerCamera.cpp @@ -545,7 +545,6 @@ void SpinnakerCamera::grabImage(sensor_msgs::Image* image, const std::string& fr << " pixel color filter is " << color_filter_str); - // if(isColor_ && bayer_format != NONE) if (color_filter_ptr->GetCurrentEntry() != color_filter_ptr->GetEntryByName("None")) { diff --git a/spinnaker_camera_driver/src/nodelet.cpp b/spinnaker_camera_driver/src/nodelet.cpp index b483fc04..581aa7d0 100644 --- a/spinnaker_camera_driver/src/nodelet.cpp +++ b/spinnaker_camera_driver/src/nodelet.cpp @@ -303,12 +303,9 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet pnh.param("auto_packet_size", auto_packet_size_, true); pnh.param("packet_delay", packet_delay_, 4000); pnh.param("throughput_limit", throughput_limit_, 125000000); -<<<<<<< HEAD - pnh.param("trigger", trigger_, false); + pnh.param("en_trigger", trigger_, false); pnh.param("sw_trigger", sw_trigger_, false); pnh.param("max_trigger_delay", max_trigger_delay_, 0.2); -======= ->>>>>>> Added parameter for device throughput limit // TODO(mhosmar): Set GigE parameters: // spinnaker_.setGigEParameters(auto_packet_size_, packet_size_, packet_delay_); @@ -532,10 +529,7 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet NODELET_DEBUG("Connected to camera."); // Set last configuration, forcing the reconfigure level to stop -<<<<<<< HEAD ROS_INFO("Nodelet: throughput limit to %d",throughput_limit_); -======= ->>>>>>> Added parameter for device throughput limit spinnaker_.setThroughputLimit(throughput_limit_); spinnaker_.setNewConfiguration(config_, SpinnakerCamera::LEVEL_RECONFIGURE_STOP); From 59aa0807667cf66f8aa76201d4d8b3b8c60006c4 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Mon, 30 Oct 2023 16:49:24 +0100 Subject: [PATCH 7/8] Removed trigger configuration. Doing it through dynamic reconfigure now. --- spinnaker_camera_driver/src/nodelet.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/spinnaker_camera_driver/src/nodelet.cpp b/spinnaker_camera_driver/src/nodelet.cpp index 581aa7d0..c520a1ca 100644 --- a/spinnaker_camera_driver/src/nodelet.cpp +++ b/spinnaker_camera_driver/src/nodelet.cpp @@ -542,9 +542,11 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet NODELET_DEBUG_ONCE("Setting timeout to: %f.", timeout); spinnaker_.setTimeout(timeout); +#if 0 NODELET_DEBUG_ONCE("Configuring trigger: %s %s", trigger_?"enabled":"disabled", sw_trigger_?"software":"hardware"); spinnaker_.configureTrigger(trigger_,sw_trigger_); +#endif } catch (const std::runtime_error& e) From 95b9502306fe9ef4ecea8c36bd4b8839917407fb Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Mon, 4 Dec 2023 09:50:47 +0100 Subject: [PATCH 8/8] Added url --- url.txt | 1 + 1 file changed, 1 insertion(+) create mode 100644 url.txt diff --git a/url.txt b/url.txt new file mode 100644 index 00000000..8fdbc1dd --- /dev/null +++ b/url.txt @@ -0,0 +1 @@ +https://www.flir.com/support-center/iis/machine-vision/knowledge-base/lost-ethernet-data-packets-on-linux-systems/