diff --git a/download_and_install_spinnaker.sh b/download_and_install_spinnaker.sh index e6fca5f..6232eea 100755 --- a/download_and_install_spinnaker.sh +++ b/download_and_install_spinnaker.sh @@ -8,7 +8,13 @@ export DEBIAN_FRONTEND=noninteractive # install basic packages apt-get update apt-get install -q -y --no-install-recommends \ - build-essential tree wget dirmngr gnupg2 vim nano git debconf-utils libunwind-dev + build-essential tree wget dirmngr gnupg2 vim nano git debconf-utils +# install libunwind-dev only in amd64 +if [[ $SPINNAKER_LINUX_ARCH == "amd64" ]] +then +apt-get update +apt-get install -q -y --no-install-recommends libunwind-dev +fi wget https://coe.northeastern.edu/fieldrobotics/spinnaker_sdk_archive/spinnaker-$SPINNAKER_VERSION-$SPINNAKER_LINUX_ARCH-pkg.tar.gz -nv diff --git a/src/capture.cpp b/src/capture.cpp index 4de4a76..1f14c31 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -15,8 +15,6 @@ acquisition::Capture::~Capture(){ end_acquisition(); deinit_cameras(); - - // pCam = nullptr; ROS_INFO_STREAM("Clearing camList..."); camList_.Clear(); @@ -927,6 +925,7 @@ void acquisition::Capture::run_soft_trig() { if (LIVE_)namedWindow("Acquisition", CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO); int count = 0; + int count_per_window = 0; if (!EXTERNAL_TRIGGER_) { cams[MASTER_CAM_].trigger(); @@ -935,6 +934,7 @@ void acquisition::Capture::run_soft_trig() { get_mat_images(); if (SAVE_) { count++; + count_per_window++; if (SAVE_BIN_) save_binary_frames(0); else @@ -956,6 +956,7 @@ void acquisition::Capture::run_soft_trig() { ros::Rate ros_rate(soft_framerate_); + double loop_time_per_window = 0.0; try{ while( ros::ok() ) { @@ -996,6 +997,7 @@ void acquisition::Capture::run_soft_trig() { if (!EXPORT_TO_ROS_){ ROS_INFO_STREAM("Exporting frames to ROS..."); export_to_ROS(); + count_per_window++; } } } else if( (key & 255)==27 ) { // ESC @@ -1016,9 +1018,10 @@ void acquisition::Capture::run_soft_trig() { } get_mat_images(); } - + count++; + count_per_window++; if (SAVE_) { - count++; + //count++; if (SAVE_BIN_) save_binary_frames(0); else @@ -1042,6 +1045,18 @@ void acquisition::Capture::run_soft_trig() { // double total_time = grab_time_ + toMat_time_ + disp_time_ + save_mat_time_; double total_time = toMat_time_ + disp_time_ + save_mat_time_+export_to_ROS_time_; achieved_time_ = ros::Time::now().toSec() - achieved_time_; + // averaging loop time in a window to get average fps + loop_time_per_window +=achieved_time_; + // refresh the avg every window_length_in_time_sec seconds + float window_length_in_time_sec = 60.0; + if (SOFT_FRAME_RATE_CTRL_ && (loop_time_per_window >= window_length_in_time_sec)) { + double average_fps = count_per_window/loop_time_per_window; + // warns if average_fps+1 is less than requested fps + ROS_WARN_COND(average_fps+1