neufieldrobotics · Spozder · Jun 24, 2019
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -53,7 +53,7 @@ endif()
 
 add_message_files(
   FILES
-  SpinnakerImageNames.msg
+  SpinnakerImageInfo.msg
 )
 
 generate_dynamic_reconfigure_options(

diff --git a/README.md b/README.md
@@ -67,6 +67,10 @@ All the parameters can be set via the launch file or via the yaml config_file.
   Exposure setting for cameras, also available as dynamic reconfiguarble parameter.
   * ~external_trigger (bool, default: false)  
   Camera triggering setting when using an external trigger.  In this mode, none of the cameras would be set as a master camera. All cameras are setup to use external trigger.  In this mode the main loop runs at rate set by soft_framerate, so if the external trigger rate is higher than the soft_framerate, the buffer will get filled and images will have a lag. Also in this mode, the getnextimage timeout is set to infinite so that the node dosen't die if a trigger is not received for a while.
+* ~publish_exposure_times (bool, default: false)
+  Turn on publishing of image exposure times to the "camera/exposure_times" topic, using the SpinnakerExposureTimes message type.
+* ~account_for_exposure_time (bool, default: false)
+  Change time stamps for published images to be at the center of their exposure window (starting time stamp + 1/2 exposure time).
 * ~target_grey_value (double, default: 0 , 0:Continous/auto)
   Setting target_grey_value > 4 (min:4 , max:99) will turn AutoExposureTargetGreyValueAuto to 'off' and set AutoExposureTargetGreyValue to target_grey_value. Also available as dynamic reconfigurable parameter. see below in Dynamic reconfigurable parameter section.
 * ~frames (int, default: 50)  

diff --git a/include/spinnaker_sdk_camera_driver/camera.h b/include/spinnaker_sdk_camera_driver/camera.h
@@ -28,9 +28,14 @@ namespace acquisition {
 
         ImagePtr grab_frame();
         Mat grab_mat_frame();
-        string get_time_stamp();
+        string get_time_stamp_str();
+        double get_exposure_time();
         int get_frame_id();
 
+        void enableTimestampCorrection();
+        void enableChunkData();
+        void enableChunkDataType(string);
+
         void setEnumValue(string, string);
         void setIntValue(string, int);
         void setFloatValue(string, float);
@@ -73,12 +78,14 @@ namespace acquisition {
 
         CameraPtr pCam_;
         int64_t timestamp_;
+        double exposure_time_;
         int frameID_;
         int lastFrameID_;
 
         bool COLOR_;
         bool MASTER_;
         uint64_t GET_NEXT_IMAGE_TIMEOUT_;
+        bool EXPOSURE_TIME_ADJUST_;
 
     };
 

diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h
@@ -14,7 +14,7 @@
 #include <dynamic_reconfigure/server.h>
 #include <spinnaker_sdk_camera_driver/spinnaker_camConfig.h>
 
-#include "spinnaker_sdk_camera_driver/SpinnakerImageNames.h"
+#include "spinnaker_sdk_camera_driver/SpinnakerImageInfo.h"
 
 #include <sstream>
 #include <image_transport/image_transport.h>
@@ -70,7 +70,7 @@ namespace acquisition {
 
         float mem_usage();
 
-        SystemPtr system_;    
+        SystemPtr system_;
         CameraList camList_;
         vector<acquisition::Camera> cams;
         vector<string> cam_ids_;
@@ -80,7 +80,8 @@ namespace acquisition {
         vector<CameraPtr> pCams_;
         vector<ImagePtr> pResultImages_;
         vector<Mat> frames_;
-        vector<string> time_stamps_;
+        vector<string> time_stamp_strs_;
+        vector<double> exposure_times_;
         vector< vector<Mat> > mem_frames_;
         vector<vector<double>> intrinsic_coeff_vec_;
         vector<vector<double>> distortion_coeff_vec_;
@@ -132,6 +133,8 @@ namespace acquisition {
         bool PUBLISH_CAM_INFO_;
         bool VERIFY_BINNING_;
         uint64_t SPINNAKER_GET_NEXT_IMAGE_TIMEOUT_;
+        bool PUBLISH_EXPOSURE_TIMES_;
+        bool CORRECT_TIMESTAMPS_;
 
         bool region_of_interest_set_;
         int region_of_interest_width_;
@@ -158,8 +161,8 @@ namespace acquisition {
 
         vector<sensor_msgs::ImagePtr> img_msgs;
         vector<sensor_msgs::CameraInfoPtr> cam_info_msgs;
-        spinnaker_sdk_camera_driver::SpinnakerImageNames mesg;
-        boost::mutex queue_mutex_;  
+        spinnaker_sdk_camera_driver::SpinnakerImageInfo mesg;
+        boost::mutex queue_mutex_;
     };
 
 }

diff --git a/msg/SpinnakerImageNames.msg → msg/SpinnakerImageInfo.msg b/msg/SpinnakerImageNames.msg → msg/SpinnakerImageInfo.msg
@@ -1,3 +1,4 @@
 Header      header
 string[]    name
 time        time
+duration[]  exposure_times
diff --git a/src/camera.cpp b/src/camera.cpp
@@ -21,7 +21,11 @@ acquisition::Camera::Camera(CameraPtr pCam) {
     frameID_ = -1;
     MASTER_ = false;
     timestamp_ = 0;
+    exposure_time_ = 0;
     GET_NEXT_IMAGE_TIMEOUT_ = EVENT_TIMEOUT_INFINITE;
+
+    // Flag set by enableExposureTimeAdjust
+    EXPOSURE_TIME_ADJUST_ = false;
 }
 
 void acquisition::Camera::init() {
@@ -46,7 +50,9 @@ ImagePtr acquisition::Camera::grab_frame() {
 
     } else {
 
-        timestamp_ = pResultImage->GetTimeStamp();
+        // Take time stamp from chunk data - timestamp is right before the image is captured
+        timestamp_ = pResultImage->GetChunkData().GetTimestamp();
+        exposure_time_ = static_cast<double>(pResultImage->GetChunkData().GetExposureTime());
 
         if (frameID_ >= 0) {
             lastFrameID_ = frameID_;
@@ -60,11 +66,16 @@ ImagePtr acquisition::Camera::grab_frame() {
     }
 
     ROS_DEBUG_STREAM("Grabbed frame from camera " << get_id() << " with timestamp " << timestamp_*1000);
-    return pResultImage;    
+    return pResultImage;
+}
+
+// Returns the latest exposure time in nanoseconds
+double acquisition::Camera::get_exposure_time() {
+    return 1000 * exposure_time_;
 }
 
-// Returns last timestamp
-string acquisition::Camera::get_time_stamp() {
+// Returns last timestamp as a string
+string acquisition::Camera::get_time_stamp_str() {
 
     stringstream ss;
     ss<<timestamp_*1000;
@@ -124,6 +135,62 @@ void acquisition::Camera::end_acquisition() {
 
 }
 
+void acquisition::Camera::enableTimestampCorrection() {
+    EXPOSURE_TIME_ADJUST_ = true;
+}
+
+void acquisition::Camera::enableChunkData() {
+
+    INodeMap & nodeMap = pCam_->GetNodeMap();
+
+    // Retrieve chunk data node from node map
+    CBooleanPtr ptrChunkModeActive = nodeMap.GetNode("ChunkModeActive");
+    if (!IsAvailable(ptrChunkModeActive) || !IsWritable(ptrChunkModeActive))
+    {
+        ROS_FATAL_STREAM("Unable to activate chunk mode. Aborting...");
+    }
+    ptrChunkModeActive->SetValue(true);
+
+    ROS_DEBUG_STREAM("Chunk mode activated...");
+}
+
+void acquisition::Camera::enableChunkDataType(string type) {
+
+    INodeMap & nodeMap = pCam_->GetNodeMap();
+
+    CBooleanPtr ptrChunkModeActive = nodeMap.GetNode("ChunkModeActive");
+    if (!IsAvailable(ptrChunkModeActive)) {
+        ROS_FATAL_STREAM("Unable to confirm chunk mode is active. Aborting...");
+    }
+
+    if (!ptrChunkModeActive->GetValue()) {
+        ROS_FATAL_STREAM("Chunk Data must be enabled prior to enabling type: " << type << ". Aborting...");
+    }
+
+    CEnumerationPtr ptrChunkSelector = nodeMap.GetNode("ChunkSelector");
+    if (!IsAvailable(ptrChunkSelector) || !IsReadable(ptrChunkSelector))
+    {
+        ROS_FATAL_STREAM("Unable to retrieve chunk selector. Aborting...");
+    }
+
+    CEnumEntryPtr ptrChunkSelectorEntry = ptrChunkSelector->GetEntryByName(type.c_str());
+    if (!IsAvailable(ptrChunkSelectorEntry) || !IsReadable(ptrChunkSelectorEntry))
+    {
+        ROS_FATAL_STREAM("Unable to select chunk data type for enabling: " << type);
+    }
+
+    // Select the chunk data type to enable by its entry value
+    ptrChunkSelector->SetIntValue(ptrChunkSelectorEntry->GetValue());
+
+    // Retrieve boolean node for cooresponding chunk data type and enable
+    CBooleanPtr ptrChunkEnable = nodeMap.GetNode("ChunkEnable");
+    if (!IsAvailable(ptrChunkEnable) || !IsWritable(ptrChunkEnable))
+    {
+        ROS_FATAL_STREAM("Unable to enable chunk data type: " << type);
+    }
+    ptrChunkEnable->SetValue(true);
+}
+
 void acquisition::Camera::setEnumValue(string setting, string value) {
 
     INodeMap & nodeMap = pCam_->GetNodeMap();

diff --git a/src/capture.cpp b/src/capture.cpp
@@ -118,7 +118,7 @@ acquisition::Capture::Capture(): it_(nh_), nh_pvt_ ("~") {
     load_cameras();
 
     //initializing the ros publisher
-    acquisition_pub = nh_.advertise<spinnaker_sdk_camera_driver::SpinnakerImageNames>("camera", 1000);
+    acquisition_pub = nh_.advertise<spinnaker_sdk_camera_driver::SpinnakerImageInfo>("camera", 1000);
     //dynamic reconfigure
     dynamicReCfgServer_ = new dynamic_reconfigure::Server<spinnaker_sdk_camera_driver::spinnaker_camConfig>(nh_pvt_);
 
@@ -201,7 +201,7 @@ acquisition::Capture::Capture(ros::NodeHandle nodehandl, ros::NodeHandle private
     load_cameras();
 
     //initializing the ros publisher
-    acquisition_pub = nh_.advertise<spinnaker_sdk_camera_driver::SpinnakerImageNames>("camera", 1000);
+    acquisition_pub = nh_.advertise<spinnaker_sdk_camera_driver::SpinnakerImageInfo>("camera", 1000);
     //dynamic reconfigure
     dynamicReCfgServer_ = new dynamic_reconfigure::Server<spinnaker_sdk_camera_driver::spinnaker_camConfig>(nh_pvt_);
 
@@ -226,7 +226,7 @@ void acquisition::Capture::load_cameras() {
     for (int i=0; i<numCameras_; i++) {
         acquisition::Camera cam(camList_.GetByIndex(i));
         ROS_INFO_STREAM("  -"<<cam.get_id());
-       }
+    }
 
     bool master_set = false;
     int cam_counter = 0;
@@ -253,7 +253,10 @@ void acquisition::Capture::load_cameras() {
 
                 Mat img;
                 frames_.push_back(img);
-                time_stamps_.push_back("");
+                time_stamp_strs_.push_back("");
+                exposure_times_.push_back(0);
+
+                mesg.exposure_times.push_back(ros::Duration());
 
                 cams.push_back(cam);
 
@@ -454,6 +457,24 @@ void acquisition::Capture::read_parameters() {
         }
     }
 
+    if (nh_pvt_.getParam("publish_exposure_times", PUBLISH_EXPOSURE_TIMES_)) {
+        ROS_INFO("  Publishing Exposure Times: %s", PUBLISH_EXPOSURE_TIMES_ ? "true" : "false");
+    } else {
+        ROS_WARN(
+            "  'publish_exposure_times' Parameter not set, using default behavior publish_exposure_times = %s",
+            PUBLISH_EXPOSURE_TIMES_ ? "true" : "false"
+        );
+    }
+
+    if (nh_pvt_.getParam("account_for_exposure_time", CORRECT_TIMESTAMPS_)) {
+        ROS_INFO("  Correcting timestamps with exposure times: %s", CORRECT_TIMESTAMPS_ ? "true" : "false");
+    } else {
+        ROS_WARN(
+            "  'account_for_exposure_time' Parameter not set, using default behavior account_for_exposure_time = %s",
+            CORRECT_TIMESTAMPS_ ? "true" : "false"
+        );
+    }
+
     if (nh_pvt_.getParam("to_ros", EXPORT_TO_ROS_)) 
         ROS_INFO("  Exporting images to ROS: %s",EXPORT_TO_ROS_?"true":"false");
         else ROS_WARN("  'to_ros' Parameter not set, using default behavior to_ros=%s",EXPORT_TO_ROS_?"true":"false");
@@ -709,6 +730,12 @@ void acquisition::Capture::init_cameras(bool soft = false) {
                 cams[i].setEnumValue("ExposureMode", "Timed");
                 cams[i].setBoolValue("ReverseX", flip_horizontal_vec_[i]);
                 cams[i].setBoolValue("ReverseY", flip_vertical_vec_[i]);
+                cams[i].enableChunkData();
+                cams[i].enableChunkDataType("Timestamp");
+                cams[i].enableChunkDataType("ExposureTime");
+                if (CORRECT_TIMESTAMPS_) {
+                    cams[i].enableTimestampCorrection();
+                }
 
                 if (region_of_interest_set_){
                     if (region_of_interest_width_ != 0)
@@ -851,9 +878,9 @@ void acquisition::Capture::save_mat_frames(int dump) {
         } else {
 
             if (MASTER_TIMESTAMP_FOR_ALL_)
-                timestamp = time_stamps_[MASTER_CAM_];
+                timestamp = time_stamp_strs_[MASTER_CAM_];
             else
-                timestamp = time_stamps_[i];
+                timestamp = time_stamp_strs_[i];
 
             ostringstream filename;
             filename<< path_ << cam_names_[i] << "/" << timestamp << ext_;
@@ -873,25 +900,33 @@ void acquisition::Capture::save_mat_frames(int dump) {
 void acquisition::Capture::export_to_ROS() {
     double t = ros::Time::now().toSec();
     std_msgs::Header img_msg_header;
-    img_msg_header.stamp = mesg.header.stamp;
     string frame_id_prefix;
     if (tf_prefix_.compare("") != 0)
         frame_id_prefix = tf_prefix_ +"/";
     else frame_id_prefix="";
 
     for (unsigned int i = 0; i < numCameras_; i++) {
         img_msg_header.frame_id = frame_id_prefix + "cam_"+to_string(i)+"_optical_frame";
+        if (CORRECT_TIMESTAMPS_) {
+            img_msg_header.stamp = mesg.time - ros::Duration(0, exposure_times_[i] / 2);
+        } else {
+            img_msg_header.stamp = mesg.time;
+        }
+
         cam_info_msgs[i]->header = img_msg_header;
 
+        if (PUBLISH_EXPOSURE_TIMES_) {
+            mesg.exposure_times[i] = ros::Duration(0, exposure_times_[i]);
+        }
+
         if(color_)
             img_msgs[i]=cv_bridge::CvImage(img_msg_header, "bgr8", frames_[i]).toImageMsg();
         else
             img_msgs[i]=cv_bridge::CvImage(img_msg_header, "mono8", frames_[i]).toImageMsg();
 
         camera_image_pubs[i].publish(img_msgs[i],cam_info_msgs[i]);
-
     }
-    export_to_ROS_time_ = ros::Time::now().toSec()-t;;
+    export_to_ROS_time_ = ros::Time::now().toSec()-t;
 }
 
 void acquisition::Capture::save_binary_frames(int dump) {
@@ -910,9 +945,9 @@ void acquisition::Capture::save_binary_frames(int dump) {
         } else {
 
             if (MASTER_TIMESTAMP_FOR_ALL_)
-                timestamp = time_stamps_[MASTER_CAM_];
+                timestamp = time_stamp_strs_[MASTER_CAM_];
             else
-                timestamp = time_stamps_[i];
+                timestamp = time_stamp_strs_[i];
 
             ostringstream filename;
             filename<< path_ << cam_names_[i] << "/" << timestamp << ".bin";
@@ -948,7 +983,10 @@ void acquisition::Capture::get_mat_images() {
         //ROS_INFO_STREAM("CAM ID IS "<< i);
         frames_[i] = cams[i].grab_mat_frame();
         //ROS_INFO("sucess");
-        time_stamps_[i] = cams[i].get_time_stamp();
+        time_stamp_strs_[i] = cams[i].get_time_stamp_str();
+        if (PUBLISH_EXPOSURE_TIMES_) {
+            exposure_times_[i] = cams[i].get_exposure_time();
+        }
 
 
         if (i==0)
@@ -964,7 +1002,7 @@ void acquisition::Capture::get_mat_images() {
 
     }
     mesg.header.stamp = ros::Time::now();
-    mesg.time = ros::Time::now();
+    mesg.time = mesg.header.stamp;
     string message = ss.str();
     ROS_DEBUG_STREAM(message);