Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/add warnings for fps #97

Open
wants to merge 6 commits into
base: dev
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 7 additions & 1 deletion download_and_install_spinnaker.sh
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
23 changes: 19 additions & 4 deletions src/capture.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,6 @@ acquisition::Capture::~Capture(){

end_acquisition();
deinit_cameras();

// pCam = nullptr;

ROS_INFO_STREAM("Clearing camList...");
camList_.Clear();
Expand Down Expand Up @@ -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();
Expand All @@ -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
Expand All @@ -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() ) {

Expand Down Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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 <soft_framerate_,"Requested FPS %d Average FPS: %.1f",soft_framerate_,average_fps);
// reset
loop_time_per_window = 0.0;
count_per_window = 0;
}

ROS_INFO_COND(TIME_BENCHMARK_,
"total time (ms): %.1f \tPossible FPS: %.1f\tActual FPS: %.1f",
Expand Down