diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h
index a71436d..7be41f9 100755
--- a/include/spinnaker_sdk_camera_driver/capture.h
+++ b/include/spinnaker_sdk_camera_driver/capture.h
@@ -131,6 +131,7 @@ namespace acquisition {
         bool TIME_BENCHMARK_;
         bool MASTER_TIMESTAMP_FOR_ALL_;
         bool EXTERNAL_TRIGGER_;
+        bool USE_IMU_TS_EXTERNAL_TRIGGER_;
         bool SAVE_;
         bool SAVE_BIN_;
         bool MANUAL_TRIGGER_;
diff --git a/launch/acquisition.launch b/launch/acquisition.launch
index 33a5093..1d7a29b 100644
--- a/launch/acquisition.launch
+++ b/launch/acquisition.launch
@@ -6,7 +6,9 @@
   <arg name="binning"           default="1"      doc="Binning for cameras, when changing from 2 to 1 cameras need to be unplugged and replugged"/>
   <arg name="color"             default="false"  doc="Should color images be used (only works on models that support color images)"/>
   <arg name="exposure_time"     default="0"      doc="Exposure_time setting for cameras"/>
-  <arg name="external_trigger"  default="false"  doc="External trigger (No camera is master)"/>
+  <arg name="external_trigger"  default="true"  doc="External trigger (No camera is master)"/>
+  <arg name="utstamps"          default="false"  doc="Flag whether each image should have Unique timestamps vs the master cams time stamp for all" />
+  <arg name="use_imu_ts"        default="false"   doc="If Trigger_msgs are present and if you dont have a timestamp info for triggering "/>
   <arg name="gain"              default="0"      doc="Gain setting. Gain should be positive (full range - 0 - 18 dB for blackfly-s camera) 
                                                       or zero (auto gain). if gain > max, it will be set to max allowed value.
                                                       Default is 0, auto gain which is set according to target grey value or autoexposure settings"/>
@@ -21,10 +23,9 @@
   <arg name="save"              default="false"  doc="flag whether images should be saved or not"/>
   <arg name="save_path"         default="~"      doc="location to save the image data"/>
   <arg name="save_type"         default="bmp"    doc="Type of file type to save to when saving images locally: binary, tiff, bmp, jpeg etc." />
-  <arg name="soft_framerate"    default="30"     doc="When hybrid software triggering is used, this controls the FPS, 0=as fast as possible"/>
+  <arg name="soft_framerate"    default="10"     doc="When hybrid software triggering is used, this controls the FPS, 0=as fast as possible"/>
   <arg name="time"              default="false"  doc="Show time/FPS on output"/>
   <arg name="to_ros"            default="true"   doc="Flag whether images should be published to ROS" />
-  <arg name="utstamps"          default="false"  doc="Flag whether each image should have Unique timestamps vs the master cams time stamp for all" />
   <arg name="max_rate_save"     default="false"  doc="Flag for max rate mode which is when the master triggerst the slaves and saves images at maximum rate possible" />
 
 
@@ -35,7 +36,7 @@
 
   <!-- Capture nodelet params-->
   <arg name="tf_prefix" default="" doc="will prefix (arg tf_prefix)/ to existing frame_id in Image msg and camerainfo msg" />
-  <arg name="config_file"          default="$(find spinnaker_sdk_camera_driver)/params/test_params.yaml" doc="File specifying the parameters of the camera_array"/>
+  <arg name="config_file"          default="$(find spinnaker_sdk_camera_driver)/params/calibration_files/stereo_17197554_17197553.yaml" doc="File specifying the parameters of the camera_array"/>
 
    <!-- start the nodelet manager if $(arg start_nodelet_manager) is true-->
   <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen" unless="$(arg external_manager)" />
@@ -50,6 +51,8 @@
     <!-- Load parameters onto server using argument or default values above -->
     <param name="exposure_time"     value="$(arg exposure_time)" />
     <param name="external_trigger"     value="$(arg external_trigger)" />
+    <param name="utstamps"          value="$(arg utstamps)" />
+    <param name="use_imu_ts"        value="$(arg use_imu_ts)" />
     <param name="gain"              value="$(arg gain)"/>
     <param name="target_grey_value" value="$(arg target_grey_value)" />
     <param name="binning"           value="$(arg binning)" />
@@ -62,11 +65,10 @@
     <param name="save_path"         value="$(arg save_path)" />
     <param name="soft_framerate"    value="$(arg soft_framerate)" />
     <param name="time"              value="$(arg time)" />
-    <param name="utstamps"          value="$(arg utstamps)" />
     <param name="to_ros"            value="$(arg to_ros)"/>
     <param name="max_rate_save"     value="$(arg max_rate_save)" />
     <param name="tf_prefix"         value="$(arg tf_prefix)" />
     
   </node>
 
-</launch>
+</launch>   
diff --git a/launch/acquisition_external_trigger.launch b/launch/acquisition_external_trigger.launch
index ae636d3..fc2fc36 100644
--- a/launch/acquisition_external_trigger.launch
+++ b/launch/acquisition_external_trigger.launch
@@ -6,6 +6,8 @@
   <arg name="binning"           default="1"      doc="Binning for cameras, when changing from 2 to 1 cameras need to be unplugged and replugged"/>
   <arg name="color"             default="false"  doc="Should color images be used (only works on models that support color images)"/>
   <arg name="exposure_time"     default="0"      doc="Exposure_time setting for cameras"/>
+  <arg name="utstamps"          default="false"  doc="Flag whether each image should have Unique timestamps vs the master cams time stamp for all"/>
+  <arg name="use_imu_ts"        default="true"   doc="If Trigger_msgs are present and if you dont have a timestamp info for triggering "/>
   <arg name="external_trigger"  default="true"   doc="External trigger (No camera is master)"/>
   <arg name="gain"              default="0"      doc="Gain setting. Gain should be positive (full range - 0 - 18 dB for blackfly-s camera) 
                                                       or zero (auto gain). if gain > max, it will be set to max allowed value.
@@ -24,7 +26,6 @@
   <arg name="soft_framerate"    default="30"     doc="When hybrid software triggering is used, this controls the FPS, 0=as fast as possible"/>
   <arg name="time"              default="false"  doc="Show time/FPS on output"/>
   <arg name="to_ros"            default="true"   doc="Flag whether images should be published to ROS" />
-  <arg name="utstamps"          default="false"  doc="Flag whether each image should have Unique timestamps vs the master cams time stamp for all" />
   <arg name="max_rate_save"     default="false"  doc="Flag for max rate mode which is when the master triggerst the slaves and saves images at maximum rate possible" />
 
 
@@ -50,6 +51,8 @@
     <!-- Load parameters onto server using argument or default values above -->
     <param name="exposure_time"     value="$(arg exposure_time)" />
     <param name="external_trigger"  value="$(arg external_trigger)" />
+    <param name="utstamps"          value="$(arg utstamps)" />
+    <param name="use_imu_ts"        value="$(arg use_imu_ts)" />
     <param name="gain"              value="$(arg gain)"/>
     <param name="target_grey_value" value="$(arg target_grey_value)" />
     <param name="binning"           value="$(arg binning)" />
@@ -62,7 +65,6 @@
     <param name="save_path"         value="$(arg save_path)" />
     <param name="soft_framerate"    value="$(arg soft_framerate)" />
     <param name="time"              value="$(arg time)" />
-    <param name="utstamps"          value="$(arg utstamps)" />
     <param name="to_ros"            value="$(arg to_ros)"/>
     <param name="max_rate_save"     value="$(arg max_rate_save)" />
     <param name="tf_prefix"         value="$(arg tf_prefix)" />
diff --git a/launch/node_acquisition.launch b/launch/node_acquisition.launch
index 83d3aec..4461895 100644
--- a/launch/node_acquisition.launch
+++ b/launch/node_acquisition.launch
@@ -8,6 +8,8 @@
   <arg name="color"		        default="false"	doc="Should color images be used (only works on models that support color images)"/>
   <arg name="exposure_time"     default="0" doc="Exposure_time setting for cameras"/>
   <arg name="external_trigger"  default="false"	doc="External trigger (No camera is master)"/>
+  <arg name="utstamps"          default="false"  doc="Flag whether each image should have Unique timestamps vs the master cams time stamp for all" />
+  <arg name="use_imu_ts"        default="true"   doc="If Trigger_msgs are present and if you dont have a timestamp info for triggering "/>
   <arg name="target_grey_value"   default="0"   doc="AutoExposureTargetGreyValue min: 4 max: 99 if set below 4, AutoExposureTargetGreyValueAuto will be set to continuous(auto) also available as dynamic reconfigurable parameter" />
   <arg name="frames"            default="3400"	doc="Numer of frames to save/view 0=ON"/>
   <arg name="live"              default="false"	doc="Show images on screen GUI"/>
@@ -19,7 +21,6 @@
   <arg name="soft_framerate"    default="20"	doc="When hybrid software triggering is used, this controls the FPS, 0=as fast as possible"/>
   <arg name="time"              default="false"	doc="Show time/FPS on output"/>
   <arg name="to_ros"            default="true"	doc="Flag whether images should be published to ROS" />
-  <arg name="utstamps"          default="false"	doc="Flag whether each image should have Unique timestamps vs the master cams time stamp for all" />
   <arg name="max_rate_save"     default="false"	doc="Flag for max rate mode which is when the master triggerst the slaves and saves images at maximum rate possible" />
   
   <!-- acquisiton node params-->
@@ -37,6 +38,8 @@
     <!-- Load parameters onto server using argument or default values above -->
     <param name="exposure_time"     value="$(arg exposure_time)" />
     <param name="external_trigger"  value="$(arg external_trigger)" />
+    <param name="utstamps"          value="$(arg utstamps)" />
+    <param name="use_imu_ts"        value="$(arg use_imu_ts)" />
     <param name="target_grey_value" value="$(arg target_grey_value)" />
     <param name="binning"           value="$(arg binning)" />
     <param name="color"             value="$(arg color)" />
@@ -48,7 +51,6 @@
     <param name="save_path"         value="$(arg save_path)" />
     <param name="soft_framerate"    value="$(arg soft_framerate)" />
     <param name="time"              value="$(arg time)" />
-    <param name="utstamps"          value="$(arg utstamps)" />
     <param name="to_ros"            value="$(arg to_ros)"/>
     <param name="max_rate_save"     value="$(arg max_rate_save)"/>
     <param name="tf_prefix"         value="$(arg tf_prefix)" />
diff --git a/params/calibration_files/stereo_17197554_17197553.yaml b/params/calibration_files/stereo_17197554_17197553.yaml
new file mode 100644
index 0000000..bd7647d
--- /dev/null
+++ b/params/calibration_files/stereo_17197554_17197553.yaml
@@ -0,0 +1,41 @@
+cam_ids:
+- 17197553
+- 17197554
+cam_aliases:
+- cam0
+- cam1
+
+#master_cam: 17197553
+skip: 20
+delay: 1.0
+
+#Camera info message details
+distortion_model: plumb_bob
+image_height: 1024
+image_width: 1280
+
+distortion_coeffs:
+- [-0.20450164833719758, 0.14743367885666747, 1.3691017237135784e-05, -0.00029960779673081914]
+- [-0.19543806353197976, 0.12250750911745852, -0.0008848056641066484, -0.0021017501699217554]
+
+
+#specified as [fx  0 cx 0 fy cy 0  0  1]
+intrinsic_coeffs:
+- [1275.5155246873633, 0.0, 656.6967116430126, 0.0, 1276.6709737523597, 506.5433446279619, 0.0, 0.0, 1.0]
+- [1252.4067462338985, 0.0, 629.4308621564376, 0.0, 1250.9809134660086, 513.5942922153134, 0.0, 0.0, 1.0]
+
+rectification_coeffs:
+- [0.9665822150177287, 0.0568881655489388, 0.24996511402973415, -0.056811130425504716, 0.9983565526264057, -0.007529228891154418, -0.2499826335392076, -0.006923181955981874, 0.9682255690077631]
+- [0.9627203218217302, 0.03938618350914482, 0.26761597579376367, -0.039468734667831934, 0.9992079277965503, -0.005073067340799848, -0.2676038133793774, -0.005678518918518859, 0.96351230064162]
+
+projection_coeffs:
+- [1445.214023875022, 0.0, 193.84015655517578, 0.0, 0.0, 1445.214023875022, 520.7913436889648, 0.0, 0.0, 0.0, 1.0, 0.0]
+- [1445.214023875022, 0.0, 193.84015655517578, 296.6206626773398, 0.0, 1445.214023875022, 520.7913436889648, 0.0, 0.0, 0.0, 1.0, 0.0]
+
+flip_horizontal:
+- false
+- false
+
+flip_vertical:
+- false
+- false
diff --git a/params/calibration_files/stereo_17197555_17197558.yaml b/params/calibration_files/stereo_17197555_17197558.yaml
new file mode 100644
index 0000000..5b3c226
--- /dev/null
+++ b/params/calibration_files/stereo_17197555_17197558.yaml
@@ -0,0 +1,40 @@
+cam_ids:
+- 17197555
+- 17197558
+cam_aliases:
+- cam0
+- cam1
+
+#master_cam: 17197555
+skip: 20
+delay: 1.0
+
+#Camera info message details
+distortion_model: plumb_bob
+image_height: 1024
+image_width: 1280
+
+distortion_coeffs:
+- [-0.21133156450551877, 0.16554701015049214, 0.0006371203200430035, -0.0010939572806040086]
+- [-0.19951712659266974, 0.14189320394813731, -0.002057441273332286, -0.00279936870291979]
+
+#specified as [fx  0 cx 0 fy cy 0  0  1]
+intrinsic_coeffs:
+- [1255.7661285167007, 0.0, 633.6643372214643, 0.0, 1253.2470339107972, 521.0000311047382, 0.0, 0.0, 1.0]
+- [1257.0217332659643, 0.0, 632.5753725217256, 0.0, 1256.1189388208545, 492.01565714778764, 0.0, 0.0, 1.0]
+
+rectification_coeffs:
+- [0.9957523783024181, -0.0281670698120689, -0.08765738578853412, 0.02769368284386981, 0.9995945878086233, -0.006612106642487861, 0.08780809208507502, 0.004156465073896091, 0.9961287380466786]
+- [0.9956833380506718, -0.06025833095412615, -0.07059478648522698, 0.06063837181798097, 0.9981545070530874, 0.0032508326429390596, 0.07026861455540498, -0.007517552808667829, 0.997499778550468]
+
+projection_coeffs:
+- [1388.2617969436703, 0.0, 759.2249526977539, 0.0, 0.0, 1388.2617969436703, 507.6747169494629, 0.0, 0.0, 0.0, 1.0, 0.0]
+- [1388.2617969436703, 0.0, 759.2249526977539, 278.81369985863324, 0.0, 1388.2617969436703, 507.6747169494629, 0.0, 0.0, 0.0, 1.0, 0.0]
+
+flip_horizontal:
+- true
+- true
+
+flip_vertical:
+- true
+- true
diff --git a/params/calibration_files/stereo_17197556_17197557.yaml b/params/calibration_files/stereo_17197556_17197557.yaml
new file mode 100644
index 0000000..57d6b02
--- /dev/null
+++ b/params/calibration_files/stereo_17197556_17197557.yaml
@@ -0,0 +1,40 @@
+cam_ids:
+- 17197556
+- 17197557
+cam_aliases:
+- cam0
+- cam1
+
+#master_cam: 17197556
+skip: 20
+delay: 1.0
+
+#Camera info message details
+distortion_model: plumb_bob
+image_height: 1024
+image_width: 1280
+
+distortion_coeffs:
+- [-0.20704574734171718, 0.1378606052753204, -0.00016623127312903865, -0.0014678552308168902]
+- [-0.20315604562055115, 0.13925984473430503, 0.0011700633742295359, -0.002631592980875036]
+
+#specified as [fx  0 cx 0 fy cy 0  0  1]
+intrinsic_coeffs:
+- [1253.702985566858, 0.0, 626.0038426699172, 0.0, 1253.5611530315684, 521.6826648653606, 0.0, 0.0, 1.0]
+- [1249.0625547880688, 0.0, 633.4407842588579, 0.0, 1249.0308397534995, 535.9834578202574, 0.0, 0.0, 1.0]
+
+rectification_coeffs:
+- [0.999870166919909, 0.015624103543014101, -0.00394166107506912, -0.015616951626054152, 0.9998763572767314, 0.0018387443878286702, 0.0039699024500641535, -0.001776948927645647, 0.9999905411187876]
+- [0.9999919133838011, 0.0028219174708584696, -0.002865300820562143, -0.0028270930622141884, 0.9999943768034167, -0.001803858742772549, 0.002860194367911154, 0.0018119446277301825, 0.9999942680559943]
+
+projection_coeffs:
+- [1242.8692771645083, 0.0, 630.979866027832, 0.0, 0.0, 1242.8692771645083, 531.3879089355469, 0.0, 0.0, 0.0, 1.0, 0.0]
+- [1242.8692771645083, 0.0, 630.979866027832, 253.03796302310053, 0.0, 1242.8692771645083, 531.3879089355469, 0.0, 0.0, 0.0, 1.0, 0.0]
+
+flip_horizontal:
+- true
+- true
+
+flip_vertical:
+- true
+- true
diff --git a/params/calibration_files/stereo_17197560_17197550.yaml b/params/calibration_files/stereo_17197560_17197550.yaml
new file mode 100644
index 0000000..1437483
--- /dev/null
+++ b/params/calibration_files/stereo_17197560_17197550.yaml
@@ -0,0 +1,40 @@
+cam_ids:
+- 17197560
+- 17197550
+cam_aliases:
+- cam0
+- cam1
+
+#master_cam: 17197560
+skip: 20
+delay: 1.0
+
+#Camera info message details
+distortion_model: plumb_bob
+image_height: 1024
+image_width: 1280
+
+distortion_coeffs:
+- [-0.19646860778761444, 0.11966919829956081, 0.0007669487706857788, -0.0013965801592123956]
+- [-0.1914413046332957, 0.11346023268077468, 0.0019526799897460604, -0.002542153300965656]
+
+#specified as [fx  0 cx 0 fy cy 0  0  1]
+intrinsic_coeffs:
+- [1278.623621111693, 0.0, 634.8138812505675, 0.0, 1276.7310814743978, 535.3629837510302, 0.0, 0.0, 1.0]
+- [1273.2817308511162, 0.0, 629.495280191018, 0.0, 1273.4616558529785, 537.4608865156434, 0.0, 0.0, 1.0]
+
+rectification_coeffs:
+- [0.999951055794313, -0.008506807285265669, 0.0050517566895154475, 0.008499995333857072, 0.999962938127486, 0.001368375229905995, -0.00506320996632779, -0.0013253683475786214, 0.9999863035579938]
+- [0.9995596958074359, -0.029351275291735073, -0.004349385714674004, 0.02934538988112363, 0.9995683356052899, -0.001410867816356254, 0.004388919009400241, 0.001282612185801062, 0.9999895460933128]
+
+projection_coeffs:
+- [1251.7378592347225, 0.0, 627.0195465087891, 0.0, 0.0, 1251.7378592347225, 540.562442779541, 0.0, 0.0, 0.0, 1.0, 0.0]
+- [1251.7378592347225, 0.0, 627.0195465087891, 256.12061596895546, 0.0, 1251.7378592347225, 540.562442779541, 0.0, 0.0, 0.0, 1.0, 0.0]
+
+flip_horizontal:
+- true
+- true
+
+flip_vertical:
+- true
+- true
diff --git a/params/external_trigger_params.yaml b/params/external_trigger_params.yaml
index 232d9cc..a00d48a 100644
--- a/params/external_trigger_params.yaml
+++ b/params/external_trigger_params.yaml
@@ -1,15 +1,19 @@
 cam_ids:
-- 17197559
+- 17197557
+- 17197555
 cam_aliases:
 - cam0
+- cam1
 skip: 20
 delay: 1.0
 
 flip_horizontal:
 - false
+- false
 
 flip_vertical:
 - false
+- false
 
 # Assign all the follwing via launch file to prevent confusion and conflict
 
diff --git a/params/stereo_camera_example.yaml b/params/stereo_camera_example.yaml
index a588b41..b1260dc 100644
--- a/params/stereo_camera_example.yaml
+++ b/params/stereo_camera_example.yaml
@@ -1,11 +1,10 @@
 cam_ids:
-<<<<<<< HEAD
-- 17197554
+- 17197557
 - 17197556
 cam_aliases:
 - cam0
 - cam1
-master_cam: 17197554
+master_cam: 17197557
 skip: 20
 delay: 1.0
 
@@ -42,7 +41,6 @@ rectification_coeffs:
 projection_coeffs:
 - [913.700317, 0.000000, 953.448302, 0.000000, 0.000000, 1063.296631, 777.871993, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
 - [913.700317, 0.000000, 953.448302, 0.000000, 0.000000, 1063.296631, 777.871993, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
-<<<<<<< HEAD
 
 flip_horizontal:
 - false
@@ -51,5 +49,3 @@ flip_horizontal:
 flip_vertical:
 - false
 - false
-=======
->>>>>>> master
diff --git a/src/capture.cpp b/src/capture.cpp
index c3f6e44..eae58f3 100755
--- a/src/capture.cpp
+++ b/src/capture.cpp
@@ -87,6 +87,7 @@ void acquisition::Capture::init_variables_register_to_ros() {
     TIME_BENCHMARK_ = false;
     MASTER_TIMESTAMP_FOR_ALL_ = true;
     EXTERNAL_TRIGGER_ = false;
+    USE_IMU_TS_EXTERNAL_TRIGGER_ = false;
     EXPORT_TO_ROS_ = false;
     PUBLISH_CAM_INFO_ = false;
     SAVE_ = false;
@@ -156,8 +157,8 @@ void acquisition::Capture::init_variables_register_to_ros() {
 
     #ifdef trigger_msgs_FOUND
     // initiliazing the trigger subscriber
-        if (EXTERNAL_TRIGGER_){
-            timeStamp_sub = nh_.subscribe("/imu/sync_trigger", 1000, &acquisition::Capture::assignTimeStampCallback,this);
+        if (EXTERNAL_TRIGGER_ && !USE_IMU_TS_EXTERNAL_TRIGGER_){
+            timeStamp_sub = nh_.subscribe("imu/sync_trigger", 1000, &acquisition::Capture::assignTimeStampCallback,this);
 
             for ( int i=0;i<numCameras_;i++){
                 std::queue<SyncInfo_> sync_message_queue;
@@ -362,6 +363,9 @@ void acquisition::Capture::read_parameters() {
     
     if (nh_pvt_.getParam("external_trigger", EXTERNAL_TRIGGER_)){
         ROS_INFO("  External trigger: %s",EXTERNAL_TRIGGER_?"true":"false");
+        if(nh_pvt_.getParam("use_imu_ts", USE_IMU_TS_EXTERNAL_TRIGGER_)){
+            ROS_INFO_STREAM("   External trigger, use_imu_ts "<<USE_IMU_TS_EXTERNAL_TRIGGER_);
+        }
     }
     else ROS_WARN("  'external_trigger' Parameter not set, using default behavior external_trigger=%s",EXTERNAL_TRIGGER_?"true":"false");
 
@@ -837,6 +841,7 @@ void acquisition::Capture::save_mat_frames(int dump) {
         create_cam_directories();
     
     string timestamp;
+mesg.name.clear();
     for (unsigned int i = 0; i < numCameras_; i++) {
 
         if (dump) {
@@ -852,7 +857,13 @@ void acquisition::Capture::save_mat_frames(int dump) {
                 timestamp = time_stamps_[i];
 
             ostringstream filename;
-            filename<< path_ << cam_names_[i] << "/" << timestamp << ext_;
+	filename.precision(19);
+            bool save_timestamp_hybrid = true;
+            if(save_timestamp_hybrid)
+                filename<< path_ << cam_names_[i] << "/" << mesg.header.stamp.toSec() << "_"<< timestamp << ext_;
+            else
+                filename<< path_ << cam_names_[i] << "/" << timestamp << ext_;            
+//filename<< path_ << cam_names_[i] << "/" << timestamp << ext_;
             ROS_DEBUG_STREAM("Saving image at " << filename.str());
             //ros image names 
             mesg.name.push_back(filename.str());
@@ -872,21 +883,25 @@ void acquisition::Capture::export_to_ROS() {
     
     #ifdef trigger_msgs_FOUND
         if (EXTERNAL_TRIGGER_){
-            if (latest_imu_trigger_count_ - prev_imu_trigger_count_ > 1 ){
-                ROS_WARN("Difference in trigger count more than 1, latest_count = %d and prev_count = %d",latest_imu_trigger_count_,prev_imu_trigger_count_);
-            }
-
-            else if (latest_imu_trigger_count_ - prev_imu_trigger_count_ == 0){
-                double wait_time_start = ros::Time::now().toSec();
-                ROS_WARN("Difference in trigger count zero, latest_count = %d and prev_count = %d",latest_imu_trigger_count_,prev_imu_trigger_count_);
-                while(latest_imu_trigger_count_ - prev_imu_trigger_count_ == 0){	
-                    ros::Duration(0.0001).sleep();
+            if (USE_IMU_TS_EXTERNAL_TRIGGER_){
+                if (latest_imu_trigger_count_ - prev_imu_trigger_count_ > 1 ){
+                    ROS_WARN("Difference in trigger count more than 1, latest_count = %d and prev_count = %d",latest_imu_trigger_count_,prev_imu_trigger_count_);
+                }
+                else if (latest_imu_trigger_count_ - prev_imu_trigger_count_ == 0){
+                    double wait_time_start = ros::Time::now().toSec();
+                    ROS_WARN("Difference in trigger count zero, latest_count = %d and prev_count = %d",latest_imu_trigger_count_,prev_imu_trigger_count_);
+                    while(latest_imu_trigger_count_ - prev_imu_trigger_count_ == 0){	
+                        ros::Duration(0.0001).sleep();
+                    }
+                    ROS_INFO_STREAM("Time gap for sync messages: "<<ros::Time::now().toSec() - wait_time_start);
                 }
-                ROS_INFO_STREAM("Time gap for sync messages: "<<ros::Time::now().toSec() - wait_time_start);
+                img_msg_header.stamp = latest_imu_trigger_time_;
+                prev_imu_trigger_count_ = latest_imu_trigger_count_;
             }
-            img_msg_header.stamp = latest_imu_trigger_time_;
-            prev_imu_trigger_count_ = latest_imu_trigger_count_;
-        }
+            else {
+                img_msg_header.stamp = mesg.header.stamp;
+            }
+        }   
         else {
             img_msg_header.stamp = mesg.header.stamp;
         }
@@ -1005,8 +1020,10 @@ void acquisition::Capture::run_soft_trig() {
 
     // Camera directories created at first save
     
-    if (LIVE_)namedWindow("Acquisition", CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO);
-
+    if (LIVE_){
+        namedWindow("Acquisition", CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO);
+        resizeWindow("Acquisition", 1280, 1080);
+    }
     int count = 0;
     
     if (!EXTERNAL_TRIGGER_) {
@@ -1050,7 +1067,7 @@ void acquisition::Capture::run_soft_trig() {
                     imshow("Acquisition", frames_[CAM_]);
                     char title[50];
                     sprintf(title, "cam # = %d, cam ID = %s, cam name = %s", CAM_, cam_ids_[CAM_].c_str(), cam_names_[CAM_].c_str());
-                    displayOverlay("Acquisition", title);
+                    //displayOverlay("Acquisition", title);
                 }
             }
 
@@ -1133,7 +1150,7 @@ void acquisition::Capture::run_soft_trig() {
             
             achieved_time_=ros::Time::now().toSec();
             
-            if (SOFT_FRAME_RATE_CTRL_) {ros_rate.sleep();}
+            if (!EXTERNAL_TRIGGER_ && SOFT_FRAME_RATE_CTRL_) {ros_rate.sleep();}
 
         }
     }
@@ -1182,7 +1199,6 @@ void acquisition::Capture::update_grid() {
 
     for (int i=0; i<cams.size(); i++)
         frames_[i].copyTo(grid_.colRange(i*frames_[i].cols,i*frames_[i].cols+frames_[i].cols).rowRange(0,grid_.rows));
-    
 }
 
 //*** CODE FOR MULTITHREADED WRITING