@@ -97,7 +97,6 @@ void acquisition::Capture::init_variables_register_to_ros() {
97
97
region_of_interest_set_ = false ;
98
98
skip_num_ = 20 ;
99
99
init_delay_ = 1 ;
100
- master_fps_ = 20.0 ;
101
100
binning_ = 1 ;
102
101
SPINNAKER_GET_NEXT_IMAGE_TIMEOUT_ = 2000 ;
103
102
todays_date_ = todays_date ();
@@ -126,7 +125,6 @@ void acquisition::Capture::init_variables_register_to_ros() {
126
125
127
126
// default flag values
128
127
129
- MANUAL_TRIGGER_ = false ;
130
128
CAM_DIRS_CREATED_ = false ;
131
129
132
130
GRID_CREATED_ = false ;
@@ -385,8 +383,9 @@ void acquisition::Capture::read_parameters() {
385
383
found = true ;
386
384
}
387
385
ROS_ASSERT_MSG (found," Specified master cam is not in the cam_ids list!" );
386
+
388
387
}
389
-
388
+
390
389
if (nh_pvt_.getParam (" utstamps" , MASTER_TIMESTAMP_FOR_ALL_)){
391
390
MASTER_TIMESTAMP_FOR_ALL_ = !MASTER_TIMESTAMP_FOR_ALL_;
392
391
ROS_INFO (" Unique time stamps for each camera: %s" ,!MASTER_TIMESTAMP_FOR_ALL_?" true" :" false" );
@@ -462,15 +461,6 @@ void acquisition::Capture::read_parameters() {
462
461
}
463
462
} else ROS_WARN (" 'delay' Parameter not set, using default behavior: delay=%f" ,init_delay_);
464
463
465
- if (nh_pvt_.getParam (" fps" , master_fps_)){
466
- if (master_fps_>=0 ) ROS_INFO (" Master cam fps set to : %0.2f" ,master_fps_);
467
- else {
468
- master_fps_=20 ;
469
- ROS_WARN (" Provided 'fps' is not valid, using default behavior, fps=%0.2f" ,master_fps_);
470
- }
471
- }
472
- else ROS_WARN (" 'fps' Parameter not set, using default behavior: fps=%0.2f" ,master_fps_);
473
-
474
464
if (nh_pvt_.getParam (" exposure_time" , exposure_time_)){
475
465
if (exposure_time_ >0 ) ROS_INFO (" Exposure set to: %.1f" ,exposure_time_);
476
466
else ROS_INFO (" 'exposure_time'=%0.f, Setting autoexposure" ,exposure_time_);
@@ -489,7 +479,6 @@ void acquisition::Capture::read_parameters() {
489
479
else ROS_INFO (" 'target_grey_value'=%0.f, Setting AutoExposureTargetGreyValueAuto to Continuous/ auto" ,target_grey_value_);}
490
480
else ROS_WARN (" 'target_grey_value' Parameter not set, using default behavior: AutoExposureTargetGreyValueAuto to auto" );
491
481
492
-
493
482
if (nh_pvt_.getParam (" binning" , binning_)){
494
483
if (binning_ >0 ) ROS_INFO (" Binning set to: %d" ,binning_);
495
484
else {
@@ -499,13 +488,13 @@ void acquisition::Capture::read_parameters() {
499
488
} else ROS_WARN (" 'binning' Parameter not set, using default behavior: Binning = %d" ,binning_);
500
489
501
490
if (nh_pvt_.getParam (" soft_framerate" , soft_framerate_)){
502
- if (soft_framerate_ >0 ) {
503
- SOFT_FRAME_RATE_CTRL_=true ;
504
- ROS_INFO (" Using Software rate control, rate set to: %d" ,soft_framerate_);
505
- }
506
- else ROS_INFO (" 'soft_framerate'=%d, software rate control set to off" ,soft_framerate_);
491
+ if (soft_framerate_ >0 ) {
492
+ SOFT_FRAME_RATE_CTRL_=true ;
493
+ ROS_INFO (" Using Software rate control, rate set to: %d" ,soft_framerate_);
494
+ }
495
+ else ROS_INFO (" 'soft_framerate'=%d, software rate control set to off" ,soft_framerate_);
507
496
}
508
- else ROS_WARN (" 'soft_framerate' Parameter not set, using default behavior: No Software Rate Control " );
497
+ else ROS_WARN (" 'soft_framerate' Parameter not set, using default behavior: No Software Rate Control " );
509
498
510
499
if (nh_pvt_.getParam (" save" , SAVE_))
511
500
ROS_INFO (" Saving images set to: %d" ,SAVE_);
@@ -837,6 +826,7 @@ void acquisition::Capture::save_mat_frames(int dump) {
837
826
create_cam_directories ();
838
827
839
828
string timestamp;
829
+ mesg.name .clear ();
840
830
for (unsigned int i = 0 ; i < numCameras_; i++) {
841
831
842
832
if (dump) {
@@ -879,7 +869,8 @@ void acquisition::Capture::export_to_ROS() {
879
869
else if (latest_imu_trigger_count_ - prev_imu_trigger_count_ == 0 ){
880
870
double wait_time_start = ros::Time::now ().toSec ();
881
871
ROS_WARN (" Difference in trigger count zero, latest_count = %d and prev_count = %d" ,latest_imu_trigger_count_,prev_imu_trigger_count_);
882
- while (latest_imu_trigger_count_ - prev_imu_trigger_count_ == 0 ){
872
+ while (latest_imu_trigger_count_ - prev_imu_trigger_count_ == 0 ){
873
+
883
874
ros::Duration (0.0001 ).sleep ();
884
875
}
885
876
ROS_INFO_STREAM (" Time gap for sync messages: " <<ros::Time::now ().toSec () - wait_time_start);
@@ -924,6 +915,7 @@ void acquisition::Capture::save_binary_frames(int dump) {
924
915
create_cam_directories ();
925
916
926
917
string timestamp;
918
+ mesg.name .clear ();
927
919
for (unsigned int i = 0 ; i < numCameras_; i++) {
928
920
929
921
if (dump) {
@@ -1065,20 +1057,17 @@ void acquisition::Capture::run_soft_trig() {
1065
1057
} else if ( (key & 255 )==81 ) { // LEFT ARROW
1066
1058
if (CAM_>0 )
1067
1059
CAM_--;
1068
- } else if ( (key & 255 )==84 && MANUAL_TRIGGER_) { // t
1069
- cams[MASTER_CAM_].trigger ();
1070
- get_mat_images ();
1071
1060
} else if ( (key & 255 )==32 && !SAVE_) { // SPACE
1072
1061
ROS_INFO_STREAM (" Saving frame..." );
1073
1062
if (SAVE_BIN_)
1074
1063
save_binary_frames (0 );
1075
- else {
1076
- save_mat_frames (0 );
1077
- if (!EXPORT_TO_ROS_){
1078
- ROS_INFO_STREAM (" Exporting frames to ROS..." );
1079
- export_to_ROS ();
1080
- }
1064
+ else {
1065
+ save_mat_frames (0 );
1066
+ if (!EXPORT_TO_ROS_){
1067
+ ROS_INFO_STREAM (" Exporting frames to ROS..." );
1068
+ export_to_ROS ();
1081
1069
}
1070
+ }
1082
1071
} else if ( (key & 255 )==27 ) { // ESC
1083
1072
ROS_INFO_STREAM (" Terminating..." );
1084
1073
cvDestroyAllWindows ();
@@ -1091,12 +1080,11 @@ void acquisition::Capture::run_soft_trig() {
1091
1080
double disp_time_ = ros::Time::now ().toSec () - t;
1092
1081
1093
1082
// Call update functions
1094
- if (!MANUAL_TRIGGER_) {
1095
- if (!EXTERNAL_TRIGGER_) {
1096
- cams[MASTER_CAM_].trigger ();
1097
- }
1098
- get_mat_images ();
1083
+
1084
+ if (!EXTERNAL_TRIGGER_) {
1085
+ cams[MASTER_CAM_].trigger ();
1099
1086
}
1087
+ get_mat_images ();
1100
1088
1101
1089
if (SAVE_) {
1102
1090
count++;
@@ -1107,7 +1095,7 @@ void acquisition::Capture::run_soft_trig() {
1107
1095
}
1108
1096
1109
1097
if (FIXED_NUM_FRAMES_) {
1110
- cout<< " Nframes " << nframes_<<endl ;
1098
+ ROS_INFO_STREAM ( " Recorded frames " <<count<< " / " <<nframes_) ;
1111
1099
if (count > nframes_) {
1112
1100
ROS_INFO_STREAM (nframes_ << " frames recorded. Terminating..." );
1113
1101
cvDestroyAllWindows ();
@@ -1133,8 +1121,7 @@ void acquisition::Capture::run_soft_trig() {
1133
1121
1134
1122
achieved_time_=ros::Time::now ().toSec ();
1135
1123
1136
- if (SOFT_FRAME_RATE_CTRL_) {ros_rate.sleep ();}
1137
-
1124
+ if (!EXTERNAL_TRIGGER_ && SOFT_FRAME_RATE_CTRL_) {ros_rate.sleep ();}
1138
1125
}
1139
1126
}
1140
1127
catch (const std::exception &e){
0 commit comments