@@ -97,7 +97,6 @@ void acquisition::Capture::init_variables_register_to_ros() {
9797 region_of_interest_set_ = false ;
9898 skip_num_ = 20 ;
9999 init_delay_ = 1 ;
100- master_fps_ = 20.0 ;
101100 binning_ = 1 ;
102101 SPINNAKER_GET_NEXT_IMAGE_TIMEOUT_ = 2000 ;
103102 todays_date_ = todays_date ();
@@ -126,7 +125,6 @@ void acquisition::Capture::init_variables_register_to_ros() {
126125
127126 // default flag values
128127
129- MANUAL_TRIGGER_ = false ;
130128 CAM_DIRS_CREATED_ = false ;
131129
132130 GRID_CREATED_ = false ;
@@ -385,8 +383,9 @@ void acquisition::Capture::read_parameters() {
385383 found = true ;
386384 }
387385 ROS_ASSERT_MSG (found," Specified master cam is not in the cam_ids list!" );
386+
388387 }
389-
388+
390389 if (nh_pvt_.getParam (" utstamps" , MASTER_TIMESTAMP_FOR_ALL_)){
391390 MASTER_TIMESTAMP_FOR_ALL_ = !MASTER_TIMESTAMP_FOR_ALL_;
392391 ROS_INFO (" Unique time stamps for each camera: %s" ,!MASTER_TIMESTAMP_FOR_ALL_?" true" :" false" );
@@ -462,15 +461,6 @@ void acquisition::Capture::read_parameters() {
462461 }
463462 } else ROS_WARN (" 'delay' Parameter not set, using default behavior: delay=%f" ,init_delay_);
464463
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-
474464 if (nh_pvt_.getParam (" exposure_time" , exposure_time_)){
475465 if (exposure_time_ >0 ) ROS_INFO (" Exposure set to: %.1f" ,exposure_time_);
476466 else ROS_INFO (" 'exposure_time'=%0.f, Setting autoexposure" ,exposure_time_);
@@ -489,7 +479,6 @@ void acquisition::Capture::read_parameters() {
489479 else ROS_INFO (" 'target_grey_value'=%0.f, Setting AutoExposureTargetGreyValueAuto to Continuous/ auto" ,target_grey_value_);}
490480 else ROS_WARN (" 'target_grey_value' Parameter not set, using default behavior: AutoExposureTargetGreyValueAuto to auto" );
491481
492-
493482 if (nh_pvt_.getParam (" binning" , binning_)){
494483 if (binning_ >0 ) ROS_INFO (" Binning set to: %d" ,binning_);
495484 else {
@@ -499,13 +488,13 @@ void acquisition::Capture::read_parameters() {
499488 } else ROS_WARN (" 'binning' Parameter not set, using default behavior: Binning = %d" ,binning_);
500489
501490 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_);
507496 }
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 " );
509498
510499 if (nh_pvt_.getParam (" save" , SAVE_))
511500 ROS_INFO (" Saving images set to: %d" ,SAVE_);
@@ -837,6 +826,7 @@ void acquisition::Capture::save_mat_frames(int dump) {
837826 create_cam_directories ();
838827
839828 string timestamp;
829+ mesg.name .clear ();
840830 for (unsigned int i = 0 ; i < numCameras_; i++) {
841831
842832 if (dump) {
@@ -879,7 +869,8 @@ void acquisition::Capture::export_to_ROS() {
879869 else if (latest_imu_trigger_count_ - prev_imu_trigger_count_ == 0 ){
880870 double wait_time_start = ros::Time::now ().toSec ();
881871 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+
883874 ros::Duration (0.0001 ).sleep ();
884875 }
885876 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) {
924915 create_cam_directories ();
925916
926917 string timestamp;
918+ mesg.name .clear ();
927919 for (unsigned int i = 0 ; i < numCameras_; i++) {
928920
929921 if (dump) {
@@ -1065,20 +1057,17 @@ void acquisition::Capture::run_soft_trig() {
10651057 } else if ( (key & 255 )==81 ) { // LEFT ARROW
10661058 if (CAM_>0 )
10671059 CAM_--;
1068- } else if ( (key & 255 )==84 && MANUAL_TRIGGER_) { // t
1069- cams[MASTER_CAM_].trigger ();
1070- get_mat_images ();
10711060 } else if ( (key & 255 )==32 && !SAVE_) { // SPACE
10721061 ROS_INFO_STREAM (" Saving frame..." );
10731062 if (SAVE_BIN_)
10741063 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 ();
10811069 }
1070+ }
10821071 } else if ( (key & 255 )==27 ) { // ESC
10831072 ROS_INFO_STREAM (" Terminating..." );
10841073 cvDestroyAllWindows ();
@@ -1091,12 +1080,11 @@ void acquisition::Capture::run_soft_trig() {
10911080 double disp_time_ = ros::Time::now ().toSec () - t;
10921081
10931082 // 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 ();
10991086 }
1087+ get_mat_images ();
11001088
11011089 if (SAVE_) {
11021090 count++;
@@ -1107,7 +1095,7 @@ void acquisition::Capture::run_soft_trig() {
11071095 }
11081096
11091097 if (FIXED_NUM_FRAMES_) {
1110- cout<< " Nframes " << nframes_<<endl ;
1098+ ROS_INFO_STREAM ( " Recorded frames " <<count<< " / " <<nframes_) ;
11111099 if (count > nframes_) {
11121100 ROS_INFO_STREAM (nframes_ << " frames recorded. Terminating..." );
11131101 cvDestroyAllWindows ();
@@ -1133,8 +1121,7 @@ void acquisition::Capture::run_soft_trig() {
11331121
11341122 achieved_time_=ros::Time::now ().toSec ();
11351123
1136- if (SOFT_FRAME_RATE_CTRL_) {ros_rate.sleep ();}
1137-
1124+ if (!EXTERNAL_TRIGGER_ && SOFT_FRAME_RATE_CTRL_) {ros_rate.sleep ();}
11381125 }
11391126 }
11401127 catch (const std::exception &e){
0 commit comments