Added framework for trigger management. To be tested on real hardware

Author: cedricpradalier

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_;

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<std::mutex> 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<uint64_t>(std::round(timeout * 1000));
 }
+
 void SpinnakerCamera::setDesiredCamera(const uint32_t& id)
 {
   serial_ = id;

spinnaker_camera_driver/src/nodelet.cpp

@@ -303,6 +303,9 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet
     pnh.param<bool>("auto_packet_size", auto_packet_size_, true);
     pnh.param<int>("packet_delay", packet_delay_, 4000);
     pnh.param<int>("throughput_limit", throughput_limit_, 125000000);
+    pnh.param<bool>("trigger", trigger_, false);
+    pnh.param<bool>("sw_trigger", sw_trigger_, false);
+    pnh.param<double>("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<double>("min_acceptable_delay", min_acceptable, 0.0);
     double max_acceptable;  // The maximum publishing delay (in seconds) before warning.
     pnh.param<double>("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<wfov_camera_msgs::WFOVImage>(
         nh.advertise<wfov_camera_msgs::WFOVImage>("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<std::mutex> 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<dynamic_reconfigure::Server<spinnaker_camera_driver::SpinnakerConfig> > 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_;
 };