diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1e7bafb..c07e623 100755
--- 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
index 7ed5e09..ea05ccd 100644
--- 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
index 1f12fbc..2bbc15c 100755
--- 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
index 9b1aff5..ea7b228 100755
--- 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 b/msg/SpinnakerImageInfo.msg
similarity index 66%
rename from msg/SpinnakerImageNames.msg
rename to msg/SpinnakerImageInfo.msg
index 2ae1d15..35922ed 100755
--- a/msg/SpinnakerImageNames.msg
+++ b/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
index ed04971..7053924 100755
--- 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
index 12a59f0..1f1d3df 100755
--- 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,7 +900,6 @@ 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_ +"/";
@@ -881,17 +907,26 @@ void acquisition::Capture::export_to_ROS() {
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);