Add support for software trigger

mjavault · Jul 18, 2018 · 122965e · 122965e
1 parent 6c3e4cb
commit 122965e
Show file tree

Hide file tree

Showing 4 changed files with 53 additions and 2 deletions.
diff --git a/pointgrey_camera_driver/cfg/PointGrey.cfg b/pointgrey_camera_driver/cfg/PointGrey.cfg
@@ -147,15 +147,17 @@ gen.add("enable_trigger",       bool_t,   SensorLevels.RECONFIGURE_RUNNING,   "E
 trigger_modes = gen.enum([gen.const("Mode0", str_t, "mode0", ""),
                     gen.const("Mode1_bulb_trigger", str_t, "mode1", ""),
                     gen.const("Mode3", str_t, "mode3", ""),
-                    gen.const("Mode14", str_t, "mode14", "")],
+                    gen.const("Mode14", str_t, "mode14", ""),
+                    gen.const("Mode15", str_t, "mode15", "")],
                     "IIDC v1.31 Trigger Modes")
 
 gen.add("trigger_mode", str_t, SensorLevels.RECONFIGURE_RUNNING,              "IIDC v1.31 Trigger Modes",                                                                    "mode0",     edit_method = trigger_modes)
 
 gpio_pins = gen.enum([gen.const("GPIO0", str_t, "gpio0", ""),
                     gen.const("GPIO1", str_t, "gpio1", ""),
                     gen.const("GPIO2", str_t, "gpio2", ""),
-                    gen.const("GPIO3", str_t, "gpio3", "")],
+                    gen.const("GPIO3", str_t, "gpio3", ""),
+                    gen.const("SOFTWARE", str_t, "software", "")],
                     "GPIO Trigger Sources")
 
 gen.add("trigger_source", str_t, SensorLevels.RECONFIGURE_RUNNING, "GPIO Trigger Sources",                                                                                   "gpio0",     edit_method = gpio_pins)

diff --git a/pointgrey_camera_driver/include/pointgrey_camera_driver/PointGreyCamera.h b/pointgrey_camera_driver/include/pointgrey_camera_driver/PointGreyCamera.h
@@ -173,6 +173,8 @@ class PointGreyCamera
 
   uint getROIPosition();
 
+  bool fireSoftwareTrigger();
+
 private:
 
   uint32_t serial_; ///< A variable to hold the serial number of the desired camera.

diff --git a/pointgrey_camera_driver/src/PointGreyCamera.cpp b/pointgrey_camera_driver/src/PointGreyCamera.cpp
@@ -675,6 +675,10 @@ static int sourceNumberFromGpioName(const std::string s)
   {
     return 3;
   }
+  else if(s.compare("software") == 0)
+  {
+    return 7;
+  }
   else
   {
     // Unrecognized pin
@@ -765,6 +769,10 @@ bool PointGreyCamera::setExternalTrigger(bool &enable, std::string &mode, std::s
   {
     triggerMode.mode = 14;
   }
+  else if(tmode.compare("mode15") == 0)
+  {
+    triggerMode.mode = 15;
+  }
   else
   {
     // Unrecognized mode
@@ -774,6 +782,7 @@ bool PointGreyCamera::setExternalTrigger(bool &enable, std::string &mode, std::s
   }
 
   // Parameter is used for mode3 (return one out of every N frames).  So if N is two, it returns every other frame.
+  // It is also used for mode15 (camera will acquire N images and stop)
   triggerMode.parameter = parameter;
 
   // Set trigger source
@@ -819,6 +828,21 @@ bool PointGreyCamera::setExternalTrigger(bool &enable, std::string &mode, std::s
   return retVal;
 }
 
+bool PointGreyCamera::fireSoftwareTrigger() {
+  const unsigned int k_softwareTrigger = 0x62C;
+  const unsigned int k_fireVal = 0x80000000;
+  Error error;
+
+  error = cam_.WriteRegister(k_softwareTrigger, k_fireVal);
+  if (error != PGRERROR_OK)
+  {
+    PointGreyCamera::handleError("PointGreyCamera::fireSoftwareTrigger Could not fire software trigger.", error);
+    return false;
+  }
+
+  return true;
+}
+
 void PointGreyCamera::setGigEParameters(bool auto_packet_size, unsigned int packet_size, unsigned int packet_delay)
 {
   auto_packet_size_ = auto_packet_size;

diff --git a/pointgrey_camera_driver/src/nodelet.cpp b/pointgrey_camera_driver/src/nodelet.cpp
@@ -41,6 +41,7 @@ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 
 #include <wfov_camera_msgs/WFOVImage.h>
 #include <image_exposure_msgs/ExposureSequence.h> // Message type for configuring gain and white balance.
+#include <std_msgs/Empty.h> //Message type for software trigger
 
 #include <diagnostic_updater/diagnostic_updater.h> // Headers for publishing diagnostic messages.
 #include <diagnostic_updater/publisher.h>
@@ -171,6 +172,7 @@ class PointGreyCameraNodelet: public nodelet::Nodelet
         scopedLock.lock();
         pubThread_.reset();
         sub_.shutdown();
+        sub_trigger_.shutdown();
 
         try
         {
@@ -377,6 +379,7 @@ class PointGreyCameraNodelet: public nodelet::Nodelet
           {
             boost::mutex::scoped_lock scopedLock(connect_mutex_);
             sub_.shutdown();
+            sub_trigger_.shutdown();
           }
 
           try
@@ -445,6 +448,12 @@ class PointGreyCameraNodelet: public nodelet::Nodelet
               sub_ = getMTNodeHandle().subscribe("image_exposure_sequence", 10, &pointgrey_camera_driver::PointGreyCameraNodelet::gainWBCallback, this);
             }
 
+            // Subscribe to software triggers (must be enabled and supported by the camera)
+            {
+              boost::mutex::scoped_lock scopedLock(connect_mutex_);
+              sub_trigger_ = getMTNodeHandle().subscribe("trigger", 10, &pointgrey_camera_driver::PointGreyCameraNodelet::triggerCallback, this);
+            }
+
             state = CONNECTED;
           }
           catch(std::runtime_error& e)
@@ -562,12 +571,26 @@ class PointGreyCameraNodelet: public nodelet::Nodelet
     }
   }
 
+  void triggerCallback(const std_msgs::Empty &msg)
+  {
+    try
+    {
+      NODELET_DEBUG("Trigger callback: Firing software trigger");
+      pg_.fireSoftwareTrigger();
+    }
+    catch(std::runtime_error& e)
+    {
+      NODELET_ERROR("triggerCallback failed with error: %s", e.what());
+    }
+  }
+
   boost::shared_ptr<dynamic_reconfigure::Server<pointgrey_camera_driver::PointGreyConfig> > srv_; ///< Needed to initialize and keep the dynamic_reconfigure::Server in scope.
   boost::shared_ptr<image_transport::ImageTransport> it_; ///< Needed to initialize and keep the ImageTransport in scope.
   boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_; ///< Needed to initialize and keep the CameraInfoManager in scope.
   image_transport::CameraPublisher it_pub_; ///< CameraInfoManager ROS publisher
   boost::shared_ptr<diagnostic_updater::DiagnosedPublisher<wfov_camera_msgs::WFOVImage> > pub_; ///< Diagnosed publisher, has to be a pointer because of constructor requirements
   ros::Subscriber sub_; ///< Subscriber for gain and white balance changes.
+  ros::Subscriber sub_trigger_; ///< Subscriber for software trigger
 
   boost::mutex connect_mutex_;