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 #include -#include "spinnaker_sdk_camera_driver/SpinnakerImageNames.h" +#include "spinnaker_sdk_camera_driver/SpinnakerImageInfo.h" #include #include @@ -70,7 +70,7 @@ namespace acquisition { float mem_usage(); - SystemPtr system_; + SystemPtr system_; CameraList camList_; vector cams; vector cam_ids_; @@ -80,7 +80,8 @@ namespace acquisition { vector pCams_; vector pResultImages_; vector frames_; - vector time_stamps_; + vector time_stamp_strs_; + vector exposure_times_; vector< vector > mem_frames_; vector> intrinsic_coeff_vec_; vector> 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 img_msgs; vector 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(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<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("camera", 1000); + acquisition_pub = nh_.advertise("camera", 1000); //dynamic reconfigure dynamicReCfgServer_ = new dynamic_reconfigure::Server(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("camera", 1000); + acquisition_pub = nh_.advertise("camera", 1000); //dynamic reconfigure dynamicReCfgServer_ = new dynamic_reconfigure::Server(nh_pvt_); @@ -226,7 +226,7 @@ void acquisition::Capture::load_cameras() { for (int i=0; iheader = 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);