From a593d2abe7c1f4bdff4645b0abca3d63d26071c6 Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Tue, 30 Jun 2020 23:56:49 -0400 Subject: [PATCH 1/5] Print camera model and version for connected cameras --- .gitignore | 1 + include/spinnaker_sdk_camera_driver/camera.h | 1 + src/camera.cpp | 12 ++++++++---- src/capture.cpp | 7 +++++-- 4 files changed, 15 insertions(+), 6 deletions(-) diff --git a/.gitignore b/.gitignore index 0fe44ed..a61dac1 100644 --- a/.gitignore +++ b/.gitignore @@ -17,6 +17,7 @@ data/ devel ~devel/lib/datacollection_neu/*.xml .catkin_workspace +include/spinnaker_sdk_camera_driver/spinnaker_configure.h # ignore doc/notes.txt, but not doc/server/arch.txt #doc/*.txt diff --git a/include/spinnaker_sdk_camera_driver/camera.h b/include/spinnaker_sdk_camera_driver/camera.h index c3bba14..750b9f8 100755 --- a/include/spinnaker_sdk_camera_driver/camera.h +++ b/include/spinnaker_sdk_camera_driver/camera.h @@ -59,6 +59,7 @@ namespace acquisition { // void setTrigMode(); // void setTriggerOverlapOff(); + string get_node_value(string node_string); string get_id(); void make_master() { MASTER_ = true; ROS_DEBUG_STREAM( "camera " << get_id() << " set as master"); } bool is_master() { return MASTER_; } diff --git a/src/camera.cpp b/src/camera.cpp index a97fd4f..234c891 100755 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -292,15 +292,19 @@ void acquisition::Camera::trigger() { } -string acquisition::Camera::get_id() { +string acquisition::Camera::get_node_value(string node_string) { INodeMap& nodeMap = pCam_->GetTLDeviceNodeMap(); - CStringPtr ptrDeviceSerialNumber = nodeMap.GetNode("DeviceSerialNumber"); - if (IsReadable(ptrDeviceSerialNumber)){ - return string(ptrDeviceSerialNumber->GetValue()); + CStringPtr ptrNodeValue = nodeMap.GetNode(node_string.c_str()); + if (IsReadable(ptrNodeValue)){ + return string(ptrNodeValue->GetValue()); } return ""; } +string acquisition::Camera::get_id() { + return get_node_value("DeviceSerialNumber"); +} + void acquisition::Camera::targetGreyValueTest() { CFloatPtr ptrExpTest =pCam_->GetNodeMap().GetNode("AutoExposureTargetGreyValue"); //CFloatPtr ptrExpTest=pCam_->GetNodeMap().GetNode("ExposureTime"); diff --git a/src/capture.cpp b/src/capture.cpp index ca66bf3..a579cde 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -130,6 +130,7 @@ void acquisition::Capture::init_variables_register_to_ros() { read_parameters(); // Retrieve singleton reference to system object + ROS_INFO_STREAM("*** SYSTEM INFORMATION ***"); ROS_INFO_STREAM("Creating system instance..."); system_ = System::GetInstance(); @@ -166,8 +167,10 @@ void acquisition::Capture::load_cameras() { for (int i=0; i Date: Wed, 1 Jul 2020 17:41:57 -0400 Subject: [PATCH 2/5] Fixing the stack trace on exit by clearing camera vector --- src/camera.cpp | 2 +- src/capture.cpp | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/camera.cpp b/src/camera.cpp index 234c891..b1b3d82 100755 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -2,7 +2,7 @@ acquisition::Camera::~Camera() { - pCam_ = NULL; + pCam_ = nullptr; timestamp_ = 0; } diff --git a/src/capture.cpp b/src/capture.cpp index a579cde..b7dda06 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -22,8 +22,7 @@ acquisition::Capture::~Capture(){ camList_.Clear(); ROS_INFO_STREAM("Releasing camera pointers..."); - for (int i=0; iReleaseInstance(); From fea9dbbad6f70d5bbaec2d0fc608ba5edf834c8b Mon Sep 17 00:00:00 2001 From: mithun Date: Wed, 1 Jul 2020 21:26:33 -0400 Subject: [PATCH 3/5] will warn if avg fps is below requested fps vis soft_trig --- params/camera.yaml | 167 +++++++++++++++++++++++++++++++++++++++++++++ src/capture.cpp | 21 +++++- src/validator.cpp | 24 +++++++ 3 files changed, 210 insertions(+), 2 deletions(-) create mode 100644 params/camera.yaml create mode 100644 src/validator.cpp diff --git a/params/camera.yaml b/params/camera.yaml new file mode 100644 index 0000000..120061d --- /dev/null +++ b/params/camera.yaml @@ -0,0 +1,167 @@ +# header/frame_id will be tf_prefix/_optical_frame +# example: header/frame_id: robot_name/cam0_optical_frame +tf_prefix: '' # can be '' or robot_name +# serial number of master camera, Transformations in P,R are wrt master_cam +master_cam: 17011862 +# hardware sync trigger info +#trigger_source: master_cam # can be 'master_cam', 'external' +# common config for all cameras +save_type: .bmp # binary or .tiff or .bmp +# binning equal in both x, y +binning: 2 # going from 2 to 1 requires cameras to be unplugged and replugged +color: true +soft_framerate: 10 # software frame rate is set using ros::rate +exposure_time: 0 # microseconds, 0:auto +skip: 5 +#frames: 50 + +# list calibration info of all cameras below +cam_array: + # cam0 + cam0: + cam_id: 17011867 # serial number of camera + # Operational Parameters + flip_horizontal: false + flip_vertical: false + # calibration parameters + camera_name: cam0 + image_width: 2048 + image_height: 1536 + camera_matrix: + rows: 3 + cols: 3 + data: [767.942728, 0.000000, 317.244746, 0.000000, 767.068789, 231.644928, 0.000000, 0.000000, 1.000000] + distortion_model: plumb_bob + distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.504042, 0.268940, 0.003454, -0.000748, 0.000000] + # all transformations should be wrt to master camera + rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] + projection_matrix: + rows: 3 + cols: 4 + data: [694.958557, 0.000000, 316.182977, 0.000000, 0.000000, 720.968750, 229.582940, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] + + # cam1 + cam1: + cam_id: 17011857 # serial number of camera + # Operational Parameters + flip_horizontal: false + flip_vertical: false + binning: 1 # equal in both x, y + # calibration parameters + camera_name: cam1 + image_width: 2048 + image_height: 1536 + camera_matrix: + rows: 3 + cols: 3 + data: [767.942728, 0.000000, 317.244746, 0.000000, 767.068789, 231.644928, 0.000000, 0.000000, 1.000000] + distortion_model: plumb_bob + distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.504042, 0.268940, 0.003454, -0.000748, 0.000000] + # all transformations should be wrt to master camera + rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] + projection_matrix: + rows: 3 + cols: 4 + data: [694.958557, 0.000000, 316.182977, 0.000000, 0.000000, 720.968750, 229.582940, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] + + # cam2 + cam2: + cam_id: 17011862 # serial number of camera + # Operational Parameters + flip_horizontal: false + flip_vertical: false + binning: 1 # equal in both x, y + # calibration parameters + camera_name: cam2 + image_width: 2048 + image_height: 1536 + camera_matrix: + rows: 3 + cols: 3 + data: [767.942728, 0.000000, 317.244746, 0.000000, 767.068789, 231.644928, 0.000000, 0.000000, 1.000000] + distortion_model: plumb_bob + distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.504042, 0.268940, 0.003454, -0.000748, 0.000000] + # all transformations should be wrt to master camera + rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] + projection_matrix: + rows: 3 + cols: 4 + data: [694.958557, 0.000000, 316.182977, 0.000000, 0.000000, 720.968750, 229.582940, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] + + # cam3 + cam3: + cam_id: 17011871 # serial number of camera + # Operational Parameters + flip_horizontal: false + flip_vertical: false + binning: 1 # equal in both x, y + # calibration parameters + camera_name: cam3 + image_width: 2048 + image_height: 1536 + camera_matrix: + rows: 3 + cols: 3 + data: [767.942728, 0.000000, 317.244746, 0.000000, 767.068789, 231.644928, 0.000000, 0.000000, 1.000000] + distortion_model: plumb_bob + distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.504042, 0.268940, 0.003454, -0.000748, 0.000000] + # all transformations should be wrt to master camera + rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] + projection_matrix: + rows: 3 + cols: 4 + data: [694.958557, 0.000000, 316.182977, 0.000000, 0.000000, 720.968750, 229.582940, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] + + # cam4 + cam4: + cam_id: 17011869 # serial number of camera + # Operational Parameters + flip_horizontal: false + flip_vertical: false + binning: 1 # equal in both x, y + # calibration parameters + camera_name: cam4 + image_width: 2048 + image_height: 1536 + camera_matrix: + rows: 3 + cols: 3 + data: [767.942728, 0.000000, 317.244746, 0.000000, 767.068789, 231.644928, 0.000000, 0.000000, 1.000000] + distortion_model: plumb_bob + distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.504042, 0.268940, 0.003454, -0.000748, 0.000000] + # all transformations should be wrt to master camera + rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] + projection_matrix: + rows: 3 + cols: 4 + data: [694.958557, 0.000000, 316.182977, 0.000000, 0.000000, 720.968750, 229.582940, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/src/capture.cpp b/src/capture.cpp index b7dda06..5f405f0 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -929,6 +929,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(); @@ -937,6 +938,7 @@ void acquisition::Capture::run_soft_trig() { get_mat_images(); if (SAVE_) { count++; + count_per_window++; if (SAVE_BIN_) save_binary_frames(0); else @@ -958,6 +960,7 @@ void acquisition::Capture::run_soft_trig() { ros::Rate ros_rate(soft_framerate_); + double loop_time_per_window = 0.0; try{ while( ros::ok() ) { @@ -998,6 +1001,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 @@ -1018,9 +1022,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 @@ -1044,6 +1049,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 camera_info; + + +camera_info = std::shared_ptr( + new camera_info_manager::CameraInfoManager(pnh)); + + + + if (camera_info->validateURL(camera_info_url)) + { + camera_info->loadCameraInfo(camera_info_url); + } + else + { + ROS_INFO("flir_boson_usb - camera_info_url could not be validated. Publishing with unconfigured camera."); + } + + sensor_msgs::CameraInfoPtr + ci(new sensor_msgs::CameraInfo(camera_info->getCameraInfo())); + + ci->header.frame_id = frame_id; +} \ No newline at end of file From 03b4c55a9ba87007b3f2fbfdaf1220a4911314a2 Mon Sep 17 00:00:00 2001 From: mithun Date: Wed, 1 Jul 2020 21:28:05 -0400 Subject: [PATCH 4/5] removed files from local branch --- params/camera.yaml | 167 --------------------------------------------- src/validator.cpp | 24 ------- 2 files changed, 191 deletions(-) delete mode 100644 params/camera.yaml delete mode 100644 src/validator.cpp diff --git a/params/camera.yaml b/params/camera.yaml deleted file mode 100644 index 120061d..0000000 --- a/params/camera.yaml +++ /dev/null @@ -1,167 +0,0 @@ -# header/frame_id will be tf_prefix/_optical_frame -# example: header/frame_id: robot_name/cam0_optical_frame -tf_prefix: '' # can be '' or robot_name -# serial number of master camera, Transformations in P,R are wrt master_cam -master_cam: 17011862 -# hardware sync trigger info -#trigger_source: master_cam # can be 'master_cam', 'external' -# common config for all cameras -save_type: .bmp # binary or .tiff or .bmp -# binning equal in both x, y -binning: 2 # going from 2 to 1 requires cameras to be unplugged and replugged -color: true -soft_framerate: 10 # software frame rate is set using ros::rate -exposure_time: 0 # microseconds, 0:auto -skip: 5 -#frames: 50 - -# list calibration info of all cameras below -cam_array: - # cam0 - cam0: - cam_id: 17011867 # serial number of camera - # Operational Parameters - flip_horizontal: false - flip_vertical: false - # calibration parameters - camera_name: cam0 - image_width: 2048 - image_height: 1536 - camera_matrix: - rows: 3 - cols: 3 - data: [767.942728, 0.000000, 317.244746, 0.000000, 767.068789, 231.644928, 0.000000, 0.000000, 1.000000] - distortion_model: plumb_bob - distortion_coefficients: - rows: 1 - cols: 5 - data: [-0.504042, 0.268940, 0.003454, -0.000748, 0.000000] - # all transformations should be wrt to master camera - rectification_matrix: - rows: 3 - cols: 3 - data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] - projection_matrix: - rows: 3 - cols: 4 - data: [694.958557, 0.000000, 316.182977, 0.000000, 0.000000, 720.968750, 229.582940, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] - - # cam1 - cam1: - cam_id: 17011857 # serial number of camera - # Operational Parameters - flip_horizontal: false - flip_vertical: false - binning: 1 # equal in both x, y - # calibration parameters - camera_name: cam1 - image_width: 2048 - image_height: 1536 - camera_matrix: - rows: 3 - cols: 3 - data: [767.942728, 0.000000, 317.244746, 0.000000, 767.068789, 231.644928, 0.000000, 0.000000, 1.000000] - distortion_model: plumb_bob - distortion_coefficients: - rows: 1 - cols: 5 - data: [-0.504042, 0.268940, 0.003454, -0.000748, 0.000000] - # all transformations should be wrt to master camera - rectification_matrix: - rows: 3 - cols: 3 - data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] - projection_matrix: - rows: 3 - cols: 4 - data: [694.958557, 0.000000, 316.182977, 0.000000, 0.000000, 720.968750, 229.582940, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] - - # cam2 - cam2: - cam_id: 17011862 # serial number of camera - # Operational Parameters - flip_horizontal: false - flip_vertical: false - binning: 1 # equal in both x, y - # calibration parameters - camera_name: cam2 - image_width: 2048 - image_height: 1536 - camera_matrix: - rows: 3 - cols: 3 - data: [767.942728, 0.000000, 317.244746, 0.000000, 767.068789, 231.644928, 0.000000, 0.000000, 1.000000] - distortion_model: plumb_bob - distortion_coefficients: - rows: 1 - cols: 5 - data: [-0.504042, 0.268940, 0.003454, -0.000748, 0.000000] - # all transformations should be wrt to master camera - rectification_matrix: - rows: 3 - cols: 3 - data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] - projection_matrix: - rows: 3 - cols: 4 - data: [694.958557, 0.000000, 316.182977, 0.000000, 0.000000, 720.968750, 229.582940, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] - - # cam3 - cam3: - cam_id: 17011871 # serial number of camera - # Operational Parameters - flip_horizontal: false - flip_vertical: false - binning: 1 # equal in both x, y - # calibration parameters - camera_name: cam3 - image_width: 2048 - image_height: 1536 - camera_matrix: - rows: 3 - cols: 3 - data: [767.942728, 0.000000, 317.244746, 0.000000, 767.068789, 231.644928, 0.000000, 0.000000, 1.000000] - distortion_model: plumb_bob - distortion_coefficients: - rows: 1 - cols: 5 - data: [-0.504042, 0.268940, 0.003454, -0.000748, 0.000000] - # all transformations should be wrt to master camera - rectification_matrix: - rows: 3 - cols: 3 - data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] - projection_matrix: - rows: 3 - cols: 4 - data: [694.958557, 0.000000, 316.182977, 0.000000, 0.000000, 720.968750, 229.582940, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] - - # cam4 - cam4: - cam_id: 17011869 # serial number of camera - # Operational Parameters - flip_horizontal: false - flip_vertical: false - binning: 1 # equal in both x, y - # calibration parameters - camera_name: cam4 - image_width: 2048 - image_height: 1536 - camera_matrix: - rows: 3 - cols: 3 - data: [767.942728, 0.000000, 317.244746, 0.000000, 767.068789, 231.644928, 0.000000, 0.000000, 1.000000] - distortion_model: plumb_bob - distortion_coefficients: - rows: 1 - cols: 5 - data: [-0.504042, 0.268940, 0.003454, -0.000748, 0.000000] - # all transformations should be wrt to master camera - rectification_matrix: - rows: 3 - cols: 3 - data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] - projection_matrix: - rows: 3 - cols: 4 - data: [694.958557, 0.000000, 316.182977, 0.000000, 0.000000, 720.968750, 229.582940, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/src/validator.cpp b/src/validator.cpp deleted file mode 100644 index 0256ba1..0000000 --- a/src/validator.cpp +++ /dev/null @@ -1,24 +0,0 @@ -void validator() -{ -std::shared_ptr camera_info; - - -camera_info = std::shared_ptr( - new camera_info_manager::CameraInfoManager(pnh)); - - - - if (camera_info->validateURL(camera_info_url)) - { - camera_info->loadCameraInfo(camera_info_url); - } - else - { - ROS_INFO("flir_boson_usb - camera_info_url could not be validated. Publishing with unconfigured camera."); - } - - sensor_msgs::CameraInfoPtr - ci(new sensor_msgs::CameraInfo(camera_info->getCameraInfo())); - - ci->header.frame_id = frame_id; -} \ No newline at end of file From 711893e735eb692055c8d7fb5695c66422f7743e Mon Sep 17 00:00:00 2001 From: mithun Date: Wed, 1 Jul 2020 21:56:03 -0400 Subject: [PATCH 5/5] will install libunwind-dev only for amd64 --- download_and_install_spinnaker.sh | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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