diff --git a/.gitignore b/.gitignore index 0fe44ed..d09d6ab 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,8 @@ $ cat .gitignore *.[oa] *~ +include/spinnaker_sdk_camera_driver/spinnaker_configure.h + # no .a files #*.a @@ -17,6 +19,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/.travis.yml b/.travis.yml new file mode 100644 index 0000000..a61cbb2 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,39 @@ +language: minimal + +services: + - docker + +matrix: + include: + - arch: amd64 + env : DOCKER_IMAGE=ros:melodic-perception-bionic SPINNAKER_VERSION=2.0.0.147 + - arch: amd64 + env : DOCKER_IMAGE=ros:kinetic-perception-xenial SPINNAKER_VERSION=2.0.0.147 + - arch: amd64 + env : DOCKER_IMAGE=ros:melodic-perception-bionic SPINNAKER_VERSION=2.2.0.48 + - arch: amd64 + env : DOCKER_IMAGE=ros:kinetic-perception-xenial SPINNAKER_VERSION=1.24.0.60 + - arch: amd64 + env : DOCKER_IMAGE=ros:melodic-perception-bionic SPINNAKER_VERSION=1.24.0.60 + - arch: arm64 + env : DOCKER_IMAGE=arm64v8/ros:kinetic-perception-xenial SPINNAKER_VERSION=2.0.0.147 + - arch: arm64 + env : DOCKER_IMAGE=arm64v8/ros:melodic-perception-bionic SPINNAKER_VERSION=2.0.0.147 + +before_install: +- docker pull $DOCKER_IMAGE + +script: +- echo TRAVIS_BUILD_DIR is $TRAVIS_BUILD_DIR +- > + docker run + -v $TRAVIS_BUILD_DIR:/ros_ws/src/spinnaker_sdk_camera_driver/ + --env SPINNAKER_VERSION=$SPINNAKER_VERSION + --env SPINNAKER_LINUX_ARCH=$TRAVIS_CPU_ARCH + $DOCKER_IMAGE + /bin/bash -c + " + /ros_ws/src/spinnaker_sdk_camera_driver/download_and_install_spinnaker.sh; + cd /ros_ws; + catkin_make + " diff --git a/CMakeLists.txt b/CMakeLists.txt index 14b2b9a..3df184f 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,11 +8,7 @@ set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") ### # camera Acquisition specific ### -#SET("OpenCV_DIR" "~/apps/opencv-2.4.13/") set(PROJECT_INCLUDE_DIR "${PROJECT_SOURCE_DIR}/include" CACHE PATH "Project Include Directory") -#set(SPINNAKER_INCLUDE_DIR "/usr/include/spinnaker" CACHE PATH "Spinnaker Include Directory") -#set(SPINNAKER_LIB_DIR "/usr/lib" CACHE PATH "Spinnaker Libs Directory") -# set(yaml-cpp_DIR "~/apps/yaml-cpp" CACHE PATH "yaml-cpp Directory") set(CUDA_USE_STATIC_CUDA_RUNTIME OFF) find_package(catkin REQUIRED COMPONENTS @@ -25,7 +21,7 @@ find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure nodelet ) - +find_package(trigger_msgs) #find_package(PCL REQUIRED) ### @@ -53,6 +49,13 @@ elseif(NOT Boost_FOUND) message("Boost not found!") endif() +# configure a header file to pass some of the CMake settings +# to the source code +configure_file ( + "${PROJECT_SOURCE_DIR}/include/spinnaker_sdk_camera_driver/spinnaker_configure.h.in" + "${PROJECT_SOURCE_DIR}/include/spinnaker_sdk_camera_driver/spinnaker_configure.h" +) + add_message_files( FILES SpinnakerImageNames.msg @@ -110,31 +113,27 @@ if(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm) endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm) +# Make package version available in CPP as a defination +add_definitions("-D${PROJECT_NAME}_VERSION=\"${${PROJECT_NAME}_VERSION}\"") + add_library (acquilib SHARED src/capture.cpp src/camera.cpp ) - add_dependencies(acquilib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) target_link_libraries(acquilib ${LIBS} ${catkin_LIBRARIES}) -add_library(capture_nodelet src/capture_nodelet.cpp src/subscriber_nodelet.cpp) -target_link_libraries(capture_nodelet acquilib ${catkin_LIBRARIES}) - add_executable (acquisition_node src/acquisition_node.cpp) add_dependencies(acquisition_node acquilib ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) target_link_libraries (acquisition_node acquilib ${LIBS} ${catkin_LIBRARIES}) +## subscriber_example for subscribing as nodelet +add_library (subscriber_example examples/subscriber_nodelet.cpp) +add_dependencies(subscriber_example ${catkin_EXPORTED_TARGETS}) +target_link_libraries(subscriber_example ${catkin_LIBRARIES}) -add_executable (acquisition_nodelet src/acquisition_nodelet.cpp) -add_dependencies(acquisition_nodelet acquilib capture_nodelet ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) -target_link_libraries (acquisition_nodelet acquilib capture_nodelet ${LIBS} ${catkin_LIBRARIES}) - -add_executable (nodelet_test src/nodelet_test.cpp) -add_dependencies(nodelet_test ${catkin_EXPORTED_TARGETS}) -target_link_libraries (nodelet_test capture_nodelet ${LIBS} ${catkin_LIBRARIES}) -install(TARGETS acquilib acquisition_node capture_nodelet acquisition_nodelet nodelet_test +install(TARGETS acquilib acquisition_node subscriber_example ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} @@ -144,3 +143,7 @@ install(DIRECTORY include DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" ) + +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/README.md b/README.md index a7fd4c1..38caf5a 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,29 @@ -# spinnaker_sdk_camera_driver +master: +[![Build Status](https://travis-ci.org/neufieldrobotics/spinnaker_sdk_camera_driver.svg?branch=master)](https://travis-ci.org/neufieldrobotics/spinnaker_sdk_camera_driver) +dev: [![Build Status](https://travis-ci.org/neufieldrobotics/spinnaker_sdk_camera_driver.svg?branch=dev)](https://travis-ci.org/neufieldrobotics/spinnaker_sdk_camera_driver) +# spinnaker_sdk_camera_driver These are the ros drivers for running the Pt Grey (FLIR) cameras that use the Spinnaker SDK. This code has been tested with various Point Grey Blackfly S (BFS) cameras. +## Compatibility Matrix +| Spinnaker | Architecture | Ubnuntu 16.04 Xenial +
ROS Kinetic | Ubnuntu 18.04 Bionic +
ROS Melodic | Ubnuntu 20.04 Focal +
ROS Noetic | +|-----------|:------------:|:--------------------------------------:|:-------------------------------------:|:-----------------------------------:| +| 1.17.0.23 | AMD64 | :heavy_check_mark: |:heavy_minus_sign: |:heavy_minus_sign: | +| 1.17.0.23 | ARM64 | :heavy_check_mark: |:heavy_minus_sign: |:heavy_minus_sign: | +| 1.24.0.60 | AMD64 | :heavy_check_mark: |:heavy_minus_sign: |:heavy_minus_sign: | +| 1.24.0.60 | ARM64 | :heavy_check_mark: |:heavy_minus_sign: |:heavy_minus_sign: | +| 2.0.0.147 | AMD64 | :heavy_check_mark: |:white_check_mark: |:heavy_minus_sign: | +| 2.0.0.147 | ARM64 | :heavy_check_mark: | |:heavy_minus_sign: | +| 2.2.0.48 | AMD64 | :heavy_minus_sign: |:heavy_check_mark: | | +| 2.2.0.48 | ARM64 | :heavy_minus_sign: | | | +| 2.3.0.77 | AMD64 | :heavy_minus_sign: | | | +| 2.3.0.77 | ARM64 | :heavy_minus_sign: | | | + +:heavy_check_mark: Tested +:heavy_minus_sign: Not Applicable +:white_check_mark: Reported to work +:x: Known compatibility Issue + ## Getting Started These instructions will get you a copy of the project up and running on your local machine for development and testing purposes. @@ -48,15 +70,12 @@ source ~/spinnaker_ws/devel/setup.bash Modify the `params/test_params.yaml` file replacing the cam-ids and master cam serial number to match your camera's serial number. Then run the code as: ```bash # To launch nodelet verison of driver, use # - roslaunch spinnaker_sdk_camera_driver acquisition.launch # To launch node version of driver, use # - -roslaunch spinnaker_sdk_camera_driver acquisition_node.launch +roslaunch spinnaker_sdk_camera_driver node_acquisition.launch # Test that the images are being published by running - rqt_image_view ``` ## Parmeters @@ -69,6 +88,8 @@ All the parameters can be set via the launch file or via the yaml config_file. Should color images be used (only works on models that support color images) * ~exposure_time (int, default: 0, 0:auto) Exposure setting for cameras, also available as dynamic reconfiguarble parameter. +* ~external_trigger (bool, default: false) + Camera triggering setting when using an external trigger. In this mode, none of the cameras would be set as a master camera. All cameras are setup to use external trigger. In this mode the main loop runs at rate set by soft_framerate, so if the external trigger rate is higher than the soft_framerate, the buffer will get filled and images will have a lag. Also in this mode, the getnextimage timeout is set to infinite so that the node dosen't die if a trigger is not received for a while. * ~target_grey_value (double, default: 0 , 0:Continous/auto) Setting target_grey_value > 4 (min:4 , max:99) will turn AutoExposureTargetGreyValueAuto to 'off' and set AutoExposureTargetGreyValue to target_grey_value. Also available as dynamic reconfigurable parameter. see below in Dynamic reconfigurable parameter section. * ~frames (int, default: 50) @@ -81,7 +102,7 @@ All the parameters can be set via the launch file or via the yaml config_file. Flag whether images should be saved or not (via opencv mat objects to disk) * ~save_path (string, default: "\~/projects/data") Location to save the image data -* \~save_type (string, default: "bmp") +* ~save_type (string, default: "bmp") Type of file type to save to when saving images locally: binary, tiff, bmp, jpeg etc. * ~soft_framerate (int, default: 20) When hybrid software triggering is used, this controls the FPS, 0=as fast as possible @@ -93,6 +114,18 @@ All the parameters can be set via the launch file or via the yaml config_file. Flag whether each image should have Unique timestamps vs the master cams time stamp for all * ~max_rate_save (bool, default: false) Flag for max rate mode which is when the master triggers the slaves and saves images at maximum rate possible. This is the multithreaded mode +* ~flip_horizontal (bool, default: false) + Flag to flip image horizontally on camera itself, this is not a rotate only a mirror image. This setting does enumeration: "reverseX". It should be specified for all cameras or can be left unspecified for all cameras for default behaviour. + * ~flip_vertical (bool, default: false) + Flag to flip image vertically on camera itself, this is not a rotate only a mirror image. This setting does enumeration: "reverseY". It should be specified for all cameras or can be left unspecified for all cameras for default behaviour. + If both horizontal and vertical flags are true, then it is equivalent to rotating 180deg. + * ~region_of_interest (dict, default = { width: 0, height: 0, x_offset: 0, y_offset: 0 } + Specify the region of interest in the camera image as dict with width, height, x_offset and y_offset. Width and height specify size of the final image (should be smaller than sensor size). X and Y offsets specify the image origin. The offset plus image size should be smaller than the sensor size. + +### node/nodelet Specific Parameters +* ~tf_prefix (string, default: "") + tf_prefix will be pre-fixed to (existing cam_*_optical_frame) frame id in ros Image msg and cameraInfo msg. default option doesn't add any prefix to frame id. eg: uas1/cam_0_optical_frame if tf_prefix:= uas1 + ### System configuration parameters * ~cam_ids (yaml sequence or array) @@ -120,11 +153,11 @@ This is the names that would be given to the cameras for filenames and rostopics When exposure_time is set to 0(zero), the ExposureAuto is set to 'Continuous', enabling auto exposure. ### nodelet details -* ~nodelet_manager_name (string, default: vision_nodelet_manager) +* ~manager (string, default: vision_nodelet_manager) Specify the name of the nodelet_manager under which nodelet to be launched. -* ~start_nodelet_manager (bool, default: false) - If set True, nodelet_manager of name $(arg nodelet_manager_name) will be launched. - If set False(default), the acquisition/capture_nodelet waits for the nodelet_manager name $(arg nodelet_manager_name). +* ~external_manager (bool, default: false) + If set False(default), nodelet_manager of name $(arg manager) will be launched. + If set True, the acquisition/Capture waits for the nodelet_manager name $(arg manager). ### Camera info message details * ~image_width (int) diff --git a/cfg/debug_console.conf b/cfg/debug_console.conf index 74b73ca..06128d1 100644 --- a/cfg/debug_console.conf +++ b/cfg/debug_console.conf @@ -1,5 +1,5 @@ # Set the default ros output to warning and higher log4j.logger.ros=INFO -log4j.logger.ros.datacollection_neu=DEBUG +log4j.logger.ros.spinnaker_sdk_camera_driver=DEBUG diff --git a/download_and_install_spinnaker.sh b/download_and_install_spinnaker.sh new file mode 100755 index 0000000..411a967 --- /dev/null +++ b/download_and_install_spinnaker.sh @@ -0,0 +1,30 @@ +#!/usr/bin/env bash +set -x + +CWD=`pwd` + +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 iputils-ping dialog apt-utils + +wget https://coe.northeastern.edu/fieldrobotics/spinnaker_sdk_archive/spinnaker-$SPINNAKER_VERSION-$SPINNAKER_LINUX_ARCH-pkg.tar.gz -nv + +tar -zxvf spinnaker-$SPINNAKER_VERSION-$SPINNAKER_LINUX_ARCH-pkg.tar.gz +cd spinnaker-$SPINNAKER_VERSION-$SPINNAKER_LINUX_ARCH + +# fix issue with 'logname' command on docker, required by spinnaker 2.2.0.48 +mv /usr/bin/logname /usr/bin/logname_old +echo "echo root" > /usr/bin/logname +chmod +x /usr/bin/logname + +# auto accept spinnaker license agreements +echo libspinnaker libspinnaker/present-flir-eula note '' | debconf-set-selections +echo libspinnaker libspinnaker/accepted-flir-eula boolean true | debconf-set-selections + +dpkg -i *.deb + +cd .. + diff --git a/examples/subscriber_nodelet.cpp b/examples/subscriber_nodelet.cpp new file mode 100644 index 0000000..5d16833 --- /dev/null +++ b/examples/subscriber_nodelet.cpp @@ -0,0 +1,57 @@ +// +// Created by pushyami on 1/15/19. +// +//cpp +#include +// ROS +#include +#include +//#include +// msgs +#include "sensor_msgs/Image.h" +//#include "sensor_msgs/CameraInfo.h" + +// nodelets +#include +#include + + + +namespace subscriber_nodelet_ns +{ + class subscriber_nodelet: public nodelet::Nodelet + { + //This is a test nodelet for measuring nodelet performance + public: + subscriber_nodelet(){} + ~subscriber_nodelet() + { + it_.reset(); + } + + std::shared_ptr it_; + image_transport::Subscriber image_sub_; + + virtual void onInit() + { + NODELET_INFO("Initializing Subscriber nodelet"); + ros::NodeHandle& node = getNodeHandle(); + ros::NodeHandle& private_nh = getPrivateNodeHandle(); + + it_.reset(new image_transport::ImageTransport(node)); + image_sub_ = it_->subscribe("/camera_array/cam0/image_raw",1, &subscriber_nodelet::imgCallback, this); + NODELET_INFO("onInit for Subscriber nodelet Initialized"); + } + + void imgCallback(const sensor_msgs::Image::ConstPtr& msg) + { + // dont modify the input msg in any way + sensor_msgs::Image tmp_img = *msg; + NODELET_INFO_STREAM("diff time is "<< ros::Time::now().toSec() - tmp_img.header.stamp.toSec()); + // copy input img to cv::mat and do any cv stuff + // dont do time intense stuff in callbacks + } + }; +} + +PLUGINLIB_EXPORT_CLASS(subscriber_nodelet_ns::subscriber_nodelet, nodelet::Nodelet) \ No newline at end of file diff --git a/include/spinnaker_sdk_camera_driver/camera.h b/include/spinnaker_sdk_camera_driver/camera.h index a3b37f6..13af81b 100755 --- a/include/spinnaker_sdk_camera_driver/camera.h +++ b/include/spinnaker_sdk_camera_driver/camera.h @@ -59,10 +59,15 @@ namespace acquisition { // void setTrigMode(); // void setTriggerOverlapOff(); - string get_id() { return string(pCam_->GetUniqueID()); } + string getTLNodeStringValue(string node_string); + double getFloatValueMax(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_; } void set_color(bool flag) { COLOR_ = flag; } + void setGetNextImageTimeout(uint64_t get_next_image_timeout) { GET_NEXT_IMAGE_TIMEOUT_ = get_next_image_timeout; } + bool verifyBinning(int binningDesired); + void calibrationParamsTest(int calibrationWidth, int calibrationHeight); private: diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h index 3c73b7a..a71436d 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -4,7 +4,7 @@ #include "std_include.h" #include "serialization.h" #include "camera.h" - +#include "spinnaker_configure.h" #include #include //ROS @@ -18,6 +18,14 @@ #include #include +// nodelets +#include +#include +#include "pluginlib/class_list_macros.h" + +#ifdef trigger_msgs_FOUND + #include +#endif using namespace Spinnaker; using namespace Spinnaker::GenApi; @@ -27,16 +35,18 @@ using namespace std; namespace acquisition { - class Capture { + class Capture : public nodelet::Nodelet { public: ~Capture(); Capture(); - Capture( ros::NodeHandle node,ros::NodeHandle private_nh); + virtual void onInit(); + + std::shared_ptr pubThread_; void load_cameras(); - + void init_variables_register_to_ros(); void init_array(); void init_cameras(bool); void start_acquisition(); @@ -44,6 +54,7 @@ namespace acquisition { void deinit_cameras(); void acquire_mat_images(int); void run(); + void run_external_trig(); void run_soft_trig(); void run_mt(); void publish_to_ros(int, char**, float); @@ -66,7 +77,7 @@ namespace acquisition { void update_grid(); void export_to_ROS(); void dynamicReconfigureCallback(spinnaker_sdk_camera_driver::spinnaker_camConfig &config, uint32_t level); - + float mem_usage(); SystemPtr system_; @@ -86,6 +97,8 @@ namespace acquisition { vector> rect_coeff_vec_; vector> proj_coeff_vec_; vector imageNames; + vector flip_horizontal_vec_; + vector flip_vertical_vec_; string path_; string todays_date_; @@ -102,17 +115,22 @@ namespace acquisition { string dump_img_; string ext_; float exposure_time_; + float gain_; double target_grey_value_; + bool first_image_received; // int decimation_; - + string tf_prefix_; int soft_framerate_; // Software (ROS) frame rate int MASTER_CAM_; int CAM_; // active cam during live + int image_width_; + int image_height_; bool FIXED_NUM_FRAMES_; bool TIME_BENCHMARK_; bool MASTER_TIMESTAMP_FOR_ALL_; + bool EXTERNAL_TRIGGER_; bool SAVE_; bool SAVE_BIN_; bool MANUAL_TRIGGER_; @@ -124,16 +142,39 @@ namespace acquisition { bool EXPORT_TO_ROS_; bool MAX_RATE_SAVE_; bool PUBLISH_CAM_INFO_; + bool VERIFY_BINNING_; + uint64_t SPINNAKER_GET_NEXT_IMAGE_TIMEOUT_; + + #ifdef trigger_msgs_FOUND + ros::Time latest_imu_trigger_time_; + uint32_t prev_imu_trigger_count_ = 0; + uint32_t latest_imu_trigger_count_; + + void assignTimeStampCallback(const trigger_msgs::sync_trigger::ConstPtr& msg); + struct SyncInfo_{ + uint32_t latest_imu_trigger_count_; + ros::Time latest_imu_trigger_time_; + }; + std::vector> sync_message_queue_vector_; + ros::Subscriber timeStamp_sub; + #endif + + + bool region_of_interest_set_; + int region_of_interest_width_; + int region_of_interest_height_; + int region_of_interest_x_offset_; + int region_of_interest_y_offset_; // grid view related variables bool GRID_CREATED_; Mat grid_; - + // ros variables ros::NodeHandle nh_; ros::NodeHandle nh_pvt_; - //image_transport::ImageTransport it_; - image_transport::ImageTransport it_; + std::shared_ptr it_; + dynamic_reconfigure::Server* dynamicReCfgServer_; ros::Publisher acquisition_pub; diff --git a/include/spinnaker_sdk_camera_driver/capture_nodelet.h b/include/spinnaker_sdk_camera_driver/capture_nodelet.h deleted file mode 100644 index 0955c99..0000000 --- a/include/spinnaker_sdk_camera_driver/capture_nodelet.h +++ /dev/null @@ -1,32 +0,0 @@ -// -// Created by pushyami on 1/10/19. -// - -#ifndef SPINNAKER_SDK_CAMERA_DRIVER_CAPTURE_NODELET_H -#define SPINNAKER_SDK_CAMERA_DRIVER_CAPTURE_NODELET_H - -#endif //SPINNAKER_SDK_CAMERA_DRIVER_CAPTURE_NODELET_H - -#include -#include "capture.h" -namespace acquisition { - class capture_nodelet: public nodelet::Nodelet - { - - public: - capture_nodelet(){} - ~capture_nodelet(){ - if (pubThread_) { - pubThread_->interrupt(); - pubThread_->join(); - } - } - virtual void onInit(); - - boost::shared_ptr inst_; - std::shared_ptr pubThread_; - - }; - -} - diff --git a/include/spinnaker_sdk_camera_driver/spinnaker_configure.h.in b/include/spinnaker_sdk_camera_driver/spinnaker_configure.h.in new file mode 100644 index 0000000..3589f3b --- /dev/null +++ b/include/spinnaker_sdk_camera_driver/spinnaker_configure.h.in @@ -0,0 +1 @@ +#cmakedefine trigger_msgs_FOUND diff --git a/include/spinnaker_sdk_camera_driver/subscriber_nodelet.h b/include/spinnaker_sdk_camera_driver/subscriber_nodelet.h deleted file mode 100644 index a94a44a..0000000 --- a/include/spinnaker_sdk_camera_driver/subscriber_nodelet.h +++ /dev/null @@ -1,26 +0,0 @@ -// -// Created by pushyami on 1/15/19. -// - -#ifndef SPINNAKER_SDK_CAMERA_DRIVER_SUBSCRIBER_NODELET_H -#define SPINNAKER_SDK_CAMERA_DRIVER_SUBSCRIBER_NODELET_H - -#endif //SPINNAKER_SDK_CAMERA_DRIVER_SUBSCRIBER_NODELET_H -#include "spinnaker_sdk_camera_driver/std_include.h" -#include -#include "capture.h" -namespace acquisition { - class subscriber_nodelet: public nodelet::Nodelet - { - - public: - subscriber_nodelet(){} - ~subscriber_nodelet(){} - virtual void onInit(); - void chatterCallback(const sensor_msgs::Image::ConstPtr& msg); - std::shared_ptr it_; - image_transport::Subscriber image_sub_; - - }; - -} \ No newline at end of file diff --git a/include/spinnaker_sdk_camera_driver/utils.h b/include/spinnaker_sdk_camera_driver/utils.h deleted file mode 100755 index 0ac8d67..0000000 --- a/include/spinnaker_sdk_camera_driver/utils.h +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef UTILS_HEADER -#define UTILS_HEADER - -#include "std_include.h" - -using namespace Spinnaker; -using namespace Spinnaker::GenApi; -using namespace Spinnaker::GenICam; -using namespace std; - -// Serial number of camera in order 1 through 5 with 1 being master -string CAMERA_SNs[] = {"17011867", "17011857", "17011862", "17011871", "17011869"}; - -int AcquireImages(CameraList camList, int numCameras, int numImgsToCapture); - -int PrintDeviceInfo(INodeMap & nodeMap); - -int initializeCameras(CameraList camList, int numCameras, int numImgsToCapture); - -CameraList sort_cam_list(CameraList camList); - -std::string today_date() - - -#endif diff --git a/launch/acquisition.launch b/launch/acquisition.launch index eca0a22..33a5093 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -3,41 +3,54 @@ - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + args="load acquisition/Capture $(arg manager)" > + override what is in the yaml file. Thus use it to set parameters camer_array configuration params --> + + @@ -51,8 +64,9 @@ - - + + + diff --git a/launch/acquisition_external_trigger.launch b/launch/acquisition_external_trigger.launch new file mode 100644 index 0000000..ae636d3 --- /dev/null +++ b/launch/acquisition_external_trigger.launch @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/multiple_nodelet_example.launch b/launch/multiple_nodelet_example.launch index ba96a9e..8180b67 100644 --- a/launch/multiple_nodelet_example.launch +++ b/launch/multiple_nodelet_example.launch @@ -1,20 +1,27 @@ - - + + + + - - + + + - + + + + diff --git a/launch/acquisition_node.launch b/launch/node_acquisition.launch similarity index 88% rename from launch/acquisition_node.launch rename to launch/node_acquisition.launch index e799c17..83d3aec 100644 --- a/launch/acquisition_node.launch +++ b/launch/node_acquisition.launch @@ -7,6 +7,7 @@ + @@ -20,9 +21,13 @@ - + + + + + + @@ -45,6 +51,7 @@ + diff --git a/launch/nodelet_subscriber_example.launch b/launch/nodelet_subscriber_example.launch deleted file mode 100644 index f26c2bd..0000000 --- a/launch/nodelet_subscriber_example.launch +++ /dev/null @@ -1,62 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index 2e4c3db..f4ebede 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -1,12 +1,15 @@ - - - - Nodelet for image acquisition using spinnaker sdk - + + + + Nodelet for image acquisition using spinnaker sdk + - + + + + - Example Subscriber nodelet demonstrates subscribing to acquisition/capture_nodelet and display delay time w.r.t imagemsg.header.stamp + Nodelet for image acquisition using spinnaker sdk diff --git a/package.xml b/package.xml index 06f3ed1..bf1e8c4 100755 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ spinnaker_sdk_camera_driver - 0.0.0 + 1.1.0 Point Grey (FLIR) Spinnaker based camera driver (Blackfly S etc.) Vikrant Shah @@ -24,11 +24,13 @@ cv_bridge image_transport sensor_msgs - nodelet - + nodelet + nodelet + message_runtime + - message_runtime + diff --git a/params/external_trigger_params.yaml b/params/external_trigger_params.yaml new file mode 100644 index 0000000..232d9cc --- /dev/null +++ b/params/external_trigger_params.yaml @@ -0,0 +1,23 @@ +cam_ids: +- 17197559 +cam_aliases: +- cam0 +skip: 20 +delay: 1.0 + +flip_horizontal: +- false + +flip_vertical: +- false + +# Assign all the follwing via launch file to prevent confusion and conflict + +#save_path: ~/projects/data +#save_type: .bmp #binary or .tiff or .bmp +#binning: 1 # going from 2 to 1 requires cameras to be unplugged and replugged +#color: false +#frames: 50 +#soft_framerate: 4 # this frame rate reflects to the software frame rate set using ros::rate +#exp: 997 +#to_ros: true #When to_ros is not selected, but live is selected, pressing 'space' exports single image to ROS diff --git a/params/multi-cam_example.yaml b/params/multi-cam_example.yaml index d665c7c..1a15d7e 100644 --- a/params/multi-cam_example.yaml +++ b/params/multi-cam_example.yaml @@ -52,3 +52,16 @@ projection_coeffs: - [923.700443, 0.000000, 973.847232, 0.000000, 0.000000, 1100.763631, 832.341983, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] - [800.784635, 0.000000, 983.78252, 0.000000, 0.000000, 903.736234, 798.561973, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] +flip_horizontal: +- false +- false +- false +- false +- false + +flip_vertical: +- false +- false +- false +- false +- false \ No newline at end of file diff --git a/params/stereo_camera_example.yaml b/params/stereo_camera_example.yaml index 49cc1d6..a588b41 100644 --- a/params/stereo_camera_example.yaml +++ b/params/stereo_camera_example.yaml @@ -1,10 +1,11 @@ cam_ids: -- 17197559 -- 17197547 +<<<<<<< HEAD +- 17197554 +- 17197556 cam_aliases: - cam0 - cam1 -master_cam: 17197559 +master_cam: 17197554 skip: 20 delay: 1.0 @@ -21,8 +22,9 @@ delay: 1.0 #Camera info message details distortion_model: plumb_bob -image_height: 1536 -image_width: 2048 +image_height: 1024 +image_width: 1280 + distortion_coeffs: - [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725] - [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725] @@ -40,3 +42,14 @@ rectification_coeffs: projection_coeffs: - [913.700317, 0.000000, 953.448302, 0.000000, 0.000000, 1063.296631, 777.871993, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] - [913.700317, 0.000000, 953.448302, 0.000000, 0.000000, 1063.296631, 777.871993, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] +<<<<<<< HEAD + +flip_horizontal: +- false +- false + +flip_vertical: +- false +- false +======= +>>>>>>> master diff --git a/params/test_params.yaml b/params/test_params.yaml index 55d7fc3..67cd6e6 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -19,8 +19,8 @@ delay: 1.0 #Camera info message details distortion_model: plumb_bob -image_height: 1536 -image_width: 2048 +image_height: 1080 +image_width: 1440 distortion_coeffs: - [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725] @@ -34,3 +34,15 @@ rectification_coeffs: projection_coeffs: - [913.700317, 0.000000, 953.448302, 0.000000, 0.000000, 1063.296631, 777.871993, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] + +flip_horizontal: +- false + +flip_vertical: +- false + +region_of_interest: + width: 0 + height: 0 + x_offset: 0 + y_offset: 0 diff --git a/src/acquisition_node.cpp b/src/acquisition_node.cpp index 098ed45..8aa550d 100755 --- a/src/acquisition_node.cpp +++ b/src/acquisition_node.cpp @@ -1,4 +1,5 @@ #include "spinnaker_sdk_camera_driver/capture.h" +#include using namespace Spinnaker; using namespace Spinnaker::GenApi; @@ -7,18 +8,16 @@ using namespace std; int main(int argc, char** argv) { - int result = 0; - // Initializing the ros node ros::init(argc, argv, "acquisition_node"); - //spinners - ros::AsyncSpinner spinner(0); // Use max cores possible for mt - spinner.start(); - - acquisition::Capture cobj; - cobj.init_array(); - cobj.run(); - return 0; - + nodelet::Loader nodelet; + nodelet::M_string remap(ros::names::getRemappings()); + nodelet::V_string nargv; + std::string nodelet_name = ros::this_node::getName(); + nodelet.load(nodelet_name, "acquisition/Capture", remap, nargv); + + ros::waitForShutdown(); + + return 0; } diff --git a/src/acquisition_nodelet.cpp b/src/acquisition_nodelet.cpp deleted file mode 100644 index 6caecbd..0000000 --- a/src/acquisition_nodelet.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include "spinnaker_sdk_camera_driver/capture.h" -#include - -using namespace Spinnaker; -using namespace Spinnaker::GenApi; -using namespace Spinnaker::GenICam; -using namespace std; - -int main(int argc, char** argv) { - - int result = 0; - - // Initializing the ros node - ros::init(argc, argv, "acquisition_node"); - - // This is code based nodelet loading, the preferred nodelet launching is done through roslaunch - nodelet::Loader nodelet; - nodelet::M_string remap(ros::names::getRemappings()); - nodelet::V_string nargv; - std::string nodelet_name = ros::this_node::getName(); - nodelet.load(nodelet_name, "acquisition/capture_nodelet", remap, nargv); - - return 0; - -} -// -// Created by auv on 1/10/19. -// - diff --git a/src/camera.cpp b/src/camera.cpp index de4c37d..9b7010c 100755 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -2,7 +2,7 @@ acquisition::Camera::~Camera() { - pCam_ = NULL; + pCam_ = nullptr; timestamp_ = 0; } @@ -21,8 +21,7 @@ acquisition::Camera::Camera(CameraPtr pCam) { frameID_ = -1; MASTER_ = false; timestamp_ = 0; - GET_NEXT_IMAGE_TIMEOUT_ = 2000; - + GET_NEXT_IMAGE_TIMEOUT_ = EVENT_TIMEOUT_INFINITE; } void acquisition::Camera::init() { @@ -38,31 +37,38 @@ void acquisition::Camera::deinit() { } ImagePtr acquisition::Camera::grab_frame() { + ImagePtr pResultImage; + try{ + pResultImage = pCam_->GetNextImage(GET_NEXT_IMAGE_TIMEOUT_); + // Check if the Image is complete - ImagePtr pResultImage = pCam_->GetNextImage(GET_NEXT_IMAGE_TIMEOUT_); - // Check if the Image is complete - - if (pResultImage->IsIncomplete()) { - - ROS_WARN_STREAM("Image incomplete with image status " << pResultImage->GetImageStatus() << "!"); + if (pResultImage->IsIncomplete()) { - } else { + ROS_WARN_STREAM("Image incomplete with image status " << pResultImage->GetImageStatus() << "!"); - timestamp_ = pResultImage->GetTimeStamp(); - - if (frameID_ >= 0) { - lastFrameID_ = frameID_; - frameID_ = pResultImage->GetFrameID(); - ROS_WARN_STREAM_COND(frameID_ > lastFrameID_ + 1,"Frames are being skipped!"); } else { - frameID_ = pResultImage->GetFrameID(); - ROS_ASSERT_MSG(frameID_ == 0 ,"First frame ID was not zero! Might cause sync issues later..."); + + timestamp_ = pResultImage->GetTimeStamp(); + + if (frameID_ >= 0) { + lastFrameID_ = frameID_; + frameID_ = pResultImage->GetFrameID(); + ROS_WARN_STREAM_COND(frameID_ > lastFrameID_ + 1,"Frames are being skipped!"); + } else { + frameID_ = pResultImage->GetFrameID(); + ROS_ASSERT_MSG(frameID_ == 0 ,"First frame ID was not zero! Might cause sync issues later..."); + } + } + ROS_DEBUG_STREAM("Grabbed frame from camera " << get_id() << " with timestamp " << timestamp_*1000); + return pResultImage; + } + catch(Spinnaker::Exception &e){ + ROS_FATAL_STREAM(e.what()<<"\n Likely reason is that slaves are not triggered. Check GPIO cables\n"); } - ROS_DEBUG_STREAM("Grabbed frame from camera " << get_id() << " with timestamp " << timestamp_*1000); - return pResultImage; + return pResultImage; } // Returns last timestamp @@ -82,8 +88,14 @@ int acquisition::Camera::get_frame_id() { Mat acquisition::Camera::grab_mat_frame() { - ImagePtr pResultImage = grab_frame(); - return convert_to_mat(pResultImage); + try{ + ImagePtr pResultImage = grab_frame(); + return convert_to_mat(pResultImage); + } + catch(Spinnaker::Exception &e){ + ros::shutdown(); + } + } @@ -186,8 +198,7 @@ void acquisition::Camera::setBoolValue(string setting, bool val) { if (!IsAvailable(ptr) || !IsWritable(ptr)) { ROS_FATAL_STREAM("Unable to set " << setting << " to " << val << " (ptr retrieval). Aborting..."); } - if (val) ptr->SetValue("True"); - else ptr->SetValue("False"); + ptr->SetValue(val); ROS_DEBUG_STREAM(setting << " set to " << val); @@ -235,7 +246,6 @@ void acquisition::Camera::adcBitDepth(gcstring bitDep) { } void acquisition::Camera::setBufferSize(int numBuf) { - INodeMap & sNodeMap = pCam_->GetTLStreamNodeMap(); CIntegerPtr StreamNode = sNodeMap.GetNode("StreamDefaultBufferCount"); int64_t bufferCount = StreamNode->GetValue(); @@ -296,6 +306,35 @@ void acquisition::Camera::trigger() { } +double acquisition::Camera::getFloatValueMax(string node_string) { + INodeMap& nodeMap = pCam_->GetNodeMap(); + + CFloatPtr ptrNodeValue = nodeMap.GetNode(node_string.c_str()); + + if (IsAvailable(ptrNodeValue)){ + //cout << "\tMax " << ptrNodeValue->GetValue() << "..." << endl; + return ptrNodeValue->GetMax(); + } else { + ROS_FATAL_STREAM("Node " << node_string << " not available" << endl); + return -1; + } +} + + +string acquisition::Camera::getTLNodeStringValue(string node_string) { + INodeMap& nodeMap = pCam_->GetTLDeviceNodeMap(); + CStringPtr ptrNodeValue = nodeMap.GetNode(node_string.c_str()); + if (IsReadable(ptrNodeValue)){ + return string(ptrNodeValue->GetValue()); + } else{ + ROS_FATAL_STREAM("Node " << node_string << " not readable" << endl); + return ""; + } +} + +string acquisition::Camera::get_id() { + return getTLNodeStringValue("DeviceSerialNumber"); +} void acquisition::Camera::targetGreyValueTest() { CFloatPtr ptrExpTest =pCam_->GetNodeMap().GetNode("AutoExposureTargetGreyValue"); @@ -318,4 +357,17 @@ void acquisition::Camera::exposureTest() { float expTime=ptrExpTest->GetValue(); ROS_DEBUG_STREAM("Exposure Time: "<SensorWidth())/(pCam_ ->Width()); + int actualBinningY = (pCam_ ->SensorHeight())/(pCam_ ->Height()); + if (binningDesired == actualBinningX) return true; + else return false; +} + +void acquisition::Camera::calibrationParamsTest(int calibrationWidth, int calibrationHeight) { + if ( (pCam_ ->SensorWidth()) != calibrationWidth ) + ROS_WARN_STREAM(" Looks like your calibration is not done at full Sensor Resolution for cam_id = "< +#include + +PLUGINLIB_EXPORT_CLASS(acquisition::Capture, nodelet::Nodelet) acquisition::Capture::~Capture(){ // destructor - + ROS_DEBUG("Capture destructor started"); ifstream file(dump_img_.c_str()); if (file) if (remove(dump_img_.c_str()) != 0) @@ -18,114 +22,46 @@ acquisition::Capture::~Capture(){ camList_.Clear(); ROS_INFO_STREAM("Releasing camera pointers..."); - for (int i=0; iReleaseInstance(); delete dynamicReCfgServer_; - - ros::shutdown(); + // for boost thread + if (pubThread_) { + pubThread_->interrupt(); + pubThread_->join(); + } + // reset pubThread_ + pubThread_.reset(); + //reset it_ + it_.reset(); } -void handler(int i) { - - // Capture* obj = reinterpret_cast(object); - ROS_FATAL("HERE!!!"); - +acquisition::Capture::Capture() { + // } -acquisition::Capture::Capture(): it_(nh_), nh_pvt_ ("~") { - - // struct sigaction sigIntHandler; - - // sigIntHandler.sa_handler = handler; - // sigemptyset(&sigIntHandler.sa_mask); - // sigIntHandler.sa_flags = 0; - - // sigaction(SIGINT, &sigIntHandler, NULL); - - int mem; - ifstream usb_mem("/sys/module/usbcore/parameters/usbfs_memory_mb"); - if (usb_mem) { - usb_mem >> mem; - if (mem >= 1000) - ROS_INFO_STREAM("[ OK ] USB memory: "< /sys/module/usbcore/parameters/usbfs_memory_mb\"\n Terminating..."); - ros::shutdown(); - } - } else { - ROS_FATAL_STREAM("Could not check USB memory on system! Terminating..."); - ros::shutdown(); - } - - // default values for the parameters are set here. Should be removed eventually!! - exposure_time_ = 0 ; // default as 0 = auto exposure - soft_framerate_ = 20; //default soft framrate - ext_ = ".bmp"; - SOFT_FRAME_RATE_CTRL_ = false; - LIVE_ = false; - TIME_BENCHMARK_ = false; - MASTER_TIMESTAMP_FOR_ALL_ = true; - EXPORT_TO_ROS_ = false; - PUBLISH_CAM_INFO_ = false; - SAVE_ = false; - SAVE_BIN_ = false; - nframes_ = -1; - FIXED_NUM_FRAMES_ = false; - MAX_RATE_SAVE_ = false; - skip_num_ = 20; - init_delay_ = 1; - master_fps_ = 20.0; - binning_ = 1; - todays_date_ = todays_date(); - - dump_img_ = "dump" + ext_; - - grab_time_ = 0; - save_time_ = 0; - toMat_time_ = 0; - save_mat_time_ = 0; - export_to_ROS_time_ = 0; - achieved_time_ = 0; - - // decimation_ = 1; - - CAM_ = 0; - - // default flag values - - MANUAL_TRIGGER_ = false; - CAM_DIRS_CREATED_ = false; - - GRID_CREATED_ = false; - - - //read_settings(config_file); - read_parameters(); - - // Retrieve singleton reference to system object - ROS_INFO_STREAM("Creating system instance..."); - system_ = System::GetInstance(); - - load_cameras(); - - //initializing the ros publisher - acquisition_pub = nh_.advertise("camera", 1000); - //dynamic reconfigure - dynamicReCfgServer_ = new dynamic_reconfigure::Server(nh_pvt_); - - dynamic_reconfigure::Server::CallbackType dynamicReCfgServerCB_t; - - dynamicReCfgServerCB_t = boost::bind(&acquisition::Capture::dynamicReconfigureCallback,this, _1, _2); - dynamicReCfgServer_->setCallback(dynamicReCfgServerCB_t); +void acquisition::Capture::onInit() { + NODELET_INFO("Initializing nodelet"); + nh_ = this->getNodeHandle(); + nh_pvt_ = this->getPrivateNodeHandle(); + //it_.reset(new image_transport::ImageTransport(nh_)); + it_ = std::shared_ptr(new image_transport::ImageTransport(nh_)); + // set values to global class variables and register pub, sub to ros + init_variables_register_to_ros(); + init_array(); + // calling capture::run() in a different thread + pubThread_.reset(new boost::thread(boost::bind(&acquisition::Capture::run, this))); + NODELET_INFO("onInit Initialized"); } -acquisition::Capture::Capture(ros::NodeHandle nodehandl, ros::NodeHandle private_nh) : nh_ (nodehandl) , it_(nh_), nh_pvt_ (private_nh) { +void acquisition::Capture::init_variables_register_to_ros() { + // this is all stuff in previous contructor + // except for nodehandles assigned in onInit() int mem; ifstream usb_mem("/sys/module/usbcore/parameters/usbfs_memory_mb"); if (usb_mem) { @@ -144,11 +80,13 @@ acquisition::Capture::Capture(ros::NodeHandle nodehandl, ros::NodeHandle private // default values for the parameters are set here. Should be removed eventually!! exposure_time_ = 0 ; // default as 0 = auto exposure soft_framerate_ = 20; //default soft framrate + gain_ = 0; ext_ = ".bmp"; SOFT_FRAME_RATE_CTRL_ = false; LIVE_ = false; TIME_BENCHMARK_ = false; MASTER_TIMESTAMP_FOR_ALL_ = true; + EXTERNAL_TRIGGER_ = false; EXPORT_TO_ROS_ = false; PUBLISH_CAM_INFO_ = false; SAVE_ = false; @@ -156,11 +94,22 @@ acquisition::Capture::Capture(ros::NodeHandle nodehandl, ros::NodeHandle private nframes_ = -1; FIXED_NUM_FRAMES_ = false; MAX_RATE_SAVE_ = false; + region_of_interest_set_ = false; skip_num_ = 20; init_delay_ = 1; master_fps_ = 20.0; binning_ = 1; + SPINNAKER_GET_NEXT_IMAGE_TIMEOUT_ = 2000; todays_date_ = todays_date(); + + #ifdef trigger_msgs_FOUND + // Initialise the time variables for sync in with camera + latest_imu_trigger_time_ = ros::Time::now(); + prev_imu_trigger_count_ = 0; + latest_imu_trigger_count_ = 0; + #endif + + first_image_received = false; dump_img_ = "dump" + ext_; @@ -181,19 +130,42 @@ acquisition::Capture::Capture(ros::NodeHandle nodehandl, ros::NodeHandle private CAM_DIRS_CREATED_ = false; GRID_CREATED_ = false; - + VERIFY_BINNING_ = false; + //read_settings(config_file); read_parameters(); // Retrieve singleton reference to system object + ROS_INFO_STREAM("*** SYSTEM INFORMATION ***"); ROS_INFO_STREAM("Creating system instance..."); + ROS_INFO_STREAM("spinnaker_sdk_camera_driver package version: " << spinnaker_sdk_camera_driver_VERSION); system_ = System::GetInstance(); - + + const LibraryVersion spinnakerLibraryVersion = system_->GetLibraryVersion(); + ROS_INFO_STREAM("Spinnaker library version: " + << spinnakerLibraryVersion.major << "." + << spinnakerLibraryVersion.minor << "." + << spinnakerLibraryVersion.type << "." + << spinnakerLibraryVersion.build); + load_cameras(); //initializing the ros publisher acquisition_pub = nh_.advertise("camera", 1000); + + #ifdef trigger_msgs_FOUND + // initiliazing the trigger subscriber + if (EXTERNAL_TRIGGER_){ + timeStamp_sub = nh_.subscribe("/imu/sync_trigger", 1000, &acquisition::Capture::assignTimeStampCallback,this); + + for ( int i=0;i sync_message_queue; + sync_message_queue_vector_.push_back(sync_message_queue); + } + } + #endif + //dynamic reconfigure dynamicReCfgServer_ = new dynamic_reconfigure::Server(nh_pvt_); @@ -201,9 +173,8 @@ acquisition::Capture::Capture(ros::NodeHandle nodehandl, ros::NodeHandle private dynamicReCfgServerCB_t = boost::bind(&acquisition::Capture::dynamicReconfigureCallback,this, _1, _2); dynamicReCfgServer_->setCallback(dynamicReCfgServerCB_t); -} - +} void acquisition::Capture::load_cameras() { // Retrieve list of cameras from the system @@ -217,19 +188,23 @@ void acquisition::Capture::load_cameras() { for (int i=0; iadvertiseCamera("camera_array/"+cam_names_[j]+"/image_raw", 1)); //camera_info_pubs.push_back(nh_.advertise("camera_array/"+cam_names_[j]+"/camera_info", 1)); img_msgs.push_back(sensor_msgs::ImagePtr()); sensor_msgs::CameraInfoPtr ci_msg(new sensor_msgs::CameraInfo()); - int image_width = 0; - int image_height = 0; + + //int image_width = 0; + //int image_height = 0; + nh_pvt_.getParam("image_height", image_height_); + nh_pvt_.getParam("image_width", image_width_); + // full resolution image_size + ci_msg->height = image_height_; + ci_msg->width = image_width_; + std::string distortion_model = ""; - nh_pvt_.getParam("image_height", image_height); - nh_pvt_.getParam("image_width", image_width); nh_pvt_.getParam("distortion_model", distortion_model); - ci_msg->header.frame_id = "cam_"+to_string(j)+"_optical_frame"; - // full resolution image_size - ci_msg->height = image_height; - ci_msg->width = image_width; + // distortion ci_msg->distortion_model = distortion_model; // binning ci_msg->binning_x = binning_; ci_msg->binning_y = binning_; + + if (region_of_interest_set_ && (region_of_interest_width_!=0 || region_of_interest_height_!=0)){ + ci_msg->roi.do_rectify = true; + ci_msg->roi.width = region_of_interest_width_; + ci_msg->roi.height = region_of_interest_height_; + ci_msg->roi.x_offset = region_of_interest_x_offset_; + ci_msg->roi.y_offset = region_of_interest_y_offset_; + } if (PUBLISH_CAM_INFO_){ ci_msg->D = distortion_coeff_vec_[j]; @@ -307,22 +292,22 @@ void acquisition::Capture::load_cameras() { } cam_info_msgs.push_back(ci_msg); - cam_counter++; - } } if (!current_cam_found) ROS_WARN_STREAM(" Camera "<0){ + ROS_INFO("gain value set to:%.1f",gain_); + } + else ROS_INFO(" 'gain' Parameter was zero or negative, using Auto gain based on target grey value"); + } + else ROS_WARN(" 'gain' Parameter not set, using default behavior: Auto gain based on target grey value"); + if (nh_pvt_.getParam("target_grey_value", target_grey_value_)){ if (target_grey_value_ >0) ROS_INFO(" target_grey_value set to: %.1f",target_grey_value_); else ROS_INFO(" 'target_grey_value'=%0.f, Setting AutoExposureTargetGreyValueAuto to Continuous/ auto",target_grey_value_);} @@ -489,6 +527,33 @@ void acquisition::Capture::read_parameters() { }else ROS_INFO(" Frames will be recorded until user termination"); }else ROS_WARN(" 'frames' Parameter not set, using defult behavior, frames will be recorded until user termination"); } + + if (nh_pvt_.hasParam("tf_prefix")){ + nh_pvt_.param("tf_prefix", tf_prefix_, ""); + ROS_INFO_STREAM(" tf_prefix set to: "< 0) { cams[i].setEnumValue("ExposureAuto", "Off"); cams[i].setFloatValue("ExposureTime", exposure_time_); } else { cams[i].setEnumValue("ExposureAuto", "Continuous"); } + + if(gain_>0){ //fixed gain + cams[i].setEnumValue("GainAuto", "Off"); + double max_gain_allowed = cams[i].getFloatValueMax("Gain"); + if (gain_ <= max_gain_allowed) + cams[i].setFloatValue("Gain", gain_); + else { + cams[i].setFloatValue("Gain", max_gain_allowed); + ROS_WARN("Provided Gain value is higher than max allowed, setting gain to %f", max_gain_allowed); + } + target_grey_value_ = 50; + } else { + cams[i].setEnumValue("GainAuto","Continuous"); + } + if (target_grey_value_ > 4.0) { cams[i].setEnumValue("AutoExposureTargetGreyValueAuto", "Off"); cams[i].setFloatValue("AutoExposureTargetGreyValue", target_grey_value_); @@ -640,7 +731,7 @@ void acquisition::Capture::init_cameras(bool soft = false) { // cams[i].setFloatValue("AcquisitionFrameRate", 5.0); if (color_) - cams[i].setEnumValue("PixelFormat", "BGR8"); + cams[i].setEnumValue("PixelFormat", "BGR8"); else cams[i].setEnumValue("PixelFormat", "Mono8"); cams[i].setEnumValue("AcquisitionMode", "Continuous"); @@ -652,7 +743,7 @@ void acquisition::Capture::init_cameras(bool soft = false) { cams[i].setEnumValue("LineMode", "Output"); cams[i].setBoolValue("AcquisitionFrameRateEnable", false); //cams[i].setFloatValue("AcquisitionFrameRate", 170); - }else{ + } else{ cams[i].setEnumValue("TriggerMode", "On"); cams[i].setEnumValue("LineSelector", "Line2"); cams[i].setEnumValue("LineMode", "Output"); @@ -661,7 +752,9 @@ void acquisition::Capture::init_cameras(bool soft = false) { //cams[i].setEnumValue("LineSource", "ExposureActive"); - } else { + } else{ // sets the configuration for external trigger: used for all slave cameras + // in master slave setup. Also in the mode when another sensor such as IMU triggers + // the camera cams[i].setEnumValue("TriggerMode", "On"); cams[i].setEnumValue("LineSelector", "Line3"); cams[i].setEnumValue("TriggerSource", "Line3"); @@ -776,26 +869,49 @@ void acquisition::Capture::save_mat_frames(int dump) { void acquisition::Capture::export_to_ROS() { double t = ros::Time::now().toSec(); std_msgs::Header img_msg_header; - img_msg_header.stamp = mesg.header.stamp; + + #ifdef trigger_msgs_FOUND + if (EXTERNAL_TRIGGER_){ + if (latest_imu_trigger_count_ - prev_imu_trigger_count_ > 1 ){ + ROS_WARN("Difference in trigger count more than 1, latest_count = %d and prev_count = %d",latest_imu_trigger_count_,prev_imu_trigger_count_); + } + + else if (latest_imu_trigger_count_ - prev_imu_trigger_count_ == 0){ + double wait_time_start = ros::Time::now().toSec(); + ROS_WARN("Difference in trigger count zero, latest_count = %d and prev_count = %d",latest_imu_trigger_count_,prev_imu_trigger_count_); + while(latest_imu_trigger_count_ - prev_imu_trigger_count_ == 0){ + ros::Duration(0.0001).sleep(); + } + ROS_INFO_STREAM("Time gap for sync messages: "<header = img_msg_header; if(color_) img_msgs[i]=cv_bridge::CvImage(img_msg_header, "bgr8", frames_[i]).toImageMsg(); else img_msgs[i]=cv_bridge::CvImage(img_msg_header, "mono8", frames_[i]).toImageMsg(); - if (PUBLISH_CAM_INFO_){ - cam_info_msgs[i]->header.stamp = mesg.header.stamp; - } camera_image_pubs[i].publish(img_msgs[i],cam_info_msgs[i]); -/* - if (PUBLISH_CAM_INFO_){ - cam_info_msgs[i].header.stamp = mesg.header.stamp; - camera_info_pubs[i].publish(cam_info_msgs[i]); - } - */ + } export_to_ROS_time_ = ros::Time::now().toSec()-t;; } @@ -839,8 +955,8 @@ void acquisition::Capture::save_binary_frames(int dump) { void acquisition::Capture::get_mat_images() { //ros time stamp creation - mesg.header.stamp = ros::Time::now(); - mesg.time = ros::Time::now(); + //mesg.header.stamp = ros::Time::now(); + //mesg.time = ros::Time::now(); double t = ros::Time::now().toSec(); ostringstream ss; @@ -869,6 +985,8 @@ void acquisition::Capture::get_mat_images() { ss << cams[i].get_frame_id() << ", "; } + mesg.header.stamp = ros::Time::now(); + mesg.time = ros::Time::now(); string message = ss.str(); ROS_DEBUG_STREAM(message); @@ -891,7 +1009,10 @@ void acquisition::Capture::run_soft_trig() { int count = 0; - cams[MASTER_CAM_].trigger(); + if (!EXTERNAL_TRIGGER_) { + cams[MASTER_CAM_].trigger(); + } + get_mat_images(); if (SAVE_) { count++; @@ -901,6 +1022,20 @@ void acquisition::Capture::run_soft_trig() { save_mat_frames(0); } + if(!VERIFY_BINNING_){ + // Gets called only once, when first image is being triggered + for (unsigned int i = 0; i < numCameras_; i++) { + //verify if binning is set successfully + if (!region_of_interest_set_){ + ROS_ASSERT_MSG(cams[i].verifyBinning(binning_), " Failed to set Binning= %d, could be either due to Invalid binning value, try changing binning value or due to spinnaker api bug - failing to set lower binning than previously set value - solution: unplug usb camera and re-plug it back and run to node with desired valid binning", binning_); + } + // warn if full sensor resolution is not same as calibration resolution + cams[i].calibrationParamsTest(image_width_,image_height_); + } + VERIFY_BINNING_ = true; + } + + ros::Rate ros_rate(soft_framerate_); try{ while( ros::ok() ) { @@ -957,7 +1092,9 @@ void acquisition::Capture::run_soft_trig() { // Call update functions if (!MANUAL_TRIGGER_) { - cams[MASTER_CAM_].trigger(); + if (!EXTERNAL_TRIGGER_) { + cams[MASTER_CAM_].trigger(); + } get_mat_images(); } @@ -977,7 +1114,7 @@ void acquisition::Capture::run_soft_trig() { break; } } - + if (EXPORT_TO_ROS_) export_to_ROS(); //cams[MASTER_CAM_].targetGreyValueTest(); // ros publishing messages @@ -1001,11 +1138,13 @@ void acquisition::Capture::run_soft_trig() { } } catch(const std::exception &e){ - ROS_FATAL_STREAM("Excption: "<* img_q, int cam_n while (imageCnt < k_numImages){ // ROS_DEBUG_STREAM(" Write Queue to Disk for cam: "<< cam_no <<" size = "<size()); - if(img_q->empty()){ - boost::this_thread::sleep(boost::posix_time::milliseconds(5)); - continue; - } + #ifdef trigger_msgs_FOUND + // sleep for 5 milliseconds if the queue is empty + if(img_q->empty() || sync_message_queue_vector_.at(cam_no).empty()){ + boost::this_thread::sleep(boost::posix_time::milliseconds(5)); + continue; + } + #endif + + #ifndef trigger_msgs_FOUND + if(img_q->empty()){ + boost::this_thread::sleep(boost::posix_time::milliseconds(5)); + continue; + } + #endif ROS_DEBUG_STREAM(" Write Queue to Disk for cam: "<< cam_no <<" size = "<size()); if (img_q->size()>100) ROS_WARN_STREAM(" Queue "<size()); + #ifdef trigger_msgs_FOUND + if (abs((int)img_q->size() - (int)sync_message_queue_vector_.at(cam_no).size()) > 100){ + ROS_WARN_STREAM(" The camera image queue size is increasing, the sync trigger messages are not coming at the desired rate"); + } + #endif + ImagePtr convertedImage = img_q->front(); - uint64_t timeStamp = convertedImage->GetTimeStamp() * 1000; + + #ifdef trigger_msgs_FOUND + uint64_t timeStamp; + if (EXTERNAL_TRIGGER_){ + SyncInfo_ sync_info = sync_message_queue_vector_.at(cam_no).front(); + sync_info.latest_imu_trigger_count_; + timeStamp = sync_info.latest_imu_trigger_time_.toSec() * 1e6; + ROS_INFO("time Queue size for cam %d is = %d",cam_no,sync_message_queue_vector_.at(cam_no).size()); + sync_message_queue_vector_.at(cam_no).pop(); + } + else{ + timeStamp = convertedImage->GetTimeStamp() * 1e6; + } + #endif + + #ifndef trigger_msgs_FOUND + uint64_t timeStamp = convertedImage->GetTimeStamp() * 1e6; + #endif // Create a unique filename ostringstream filename; @@ -1081,7 +1253,9 @@ void acquisition::Capture::write_queue_to_disk(queue* img_q, int cam_n // ROS_DEBUG_STREAM("Writing to "<Save(filename.str().c_str()); - + // release the image before popping out to save memory + convertedImage->Release(); + ROS_INFO("image Queue size for cam %d is = %zu",cam_no, img_q->size()); queue_mutex_.lock(); img_q->pop(); queue_mutex_.unlock(); @@ -1103,15 +1277,26 @@ void acquisition::Capture::acquire_images_to_queue(vector>* img int k_numImages = nframes_; auto start = ros::Time::now().toSec(); auto elapsed = (ros::Time::now().toSec() - start)*1000; + + double flush_start_time = ros::Time::now().toSec(); + while ((ros::Time::now().toSec() - flush_start_time) < 3.0){ + for (int i = 0; i < numCameras_; i++) { + cams[i].grab_frame(); + } + ROS_DEBUG("Flushing time elapsed: %.3f",ros::Time::now().toSec() - flush_start_time); + } + + first_image_received = true; for (int imageCnt = 0; imageCnt < k_numImages; imageCnt++) { uint64_t timeStamp = 0; for (int i = 0; i < numCameras_; i++) { try { - ImagePtr pResultImage = cams[i].grab_frame(); - + // grab_frame() is a blocking call. It waits for the next image acquired by the camera + //ImagePtr pResultImage = cams[i].grab_frame(); + ImagePtr convertedImage = cams[i].grab_frame(); // Convert image to mono 8 - ImagePtr convertedImage = pResultImage->Convert(PixelFormat_Mono8, HQ_LINEAR); + //ImagePtr convertedImage = pResultImage->Convert(PixelFormat_Mono8, HQ_LINEAR); if(cams[i].is_master()) { mesg.header.stamp = ros::Time::now(); @@ -1132,7 +1317,7 @@ void acquisition::Capture::acquire_images_to_queue(vector>* img ROS_DEBUG_STREAM("Queue no. "<at(i).size()); // Release image - pResultImage->Release(); + //convertedImage->Release(); } catch (Spinnaker::Exception &e) { ROS_ERROR_STREAM(" Exception in Acquire to queue thread" << "\nError: " << e.what()); @@ -1169,22 +1354,24 @@ void acquisition::Capture::run_mt() { std::queue img_ptr_queue; image_queue_vector.push_back(img_ptr_queue); } - + + // start threads.create_thread(boost::bind(&Capture::acquire_images_to_queue, this, &image_queue_vector)); + // assign a new thread to write the nth image to disk acquired in a queue for (int i=0; iheader.stamp); + + SyncInfo_ sync_info; + latest_imu_trigger_count_ = msg->count; + latest_imu_trigger_time_ = msg->header.stamp; + sync_info.latest_imu_trigger_count_ = latest_imu_trigger_count_; + sync_info.latest_imu_trigger_time_ = latest_imu_trigger_time_; + ROS_DEBUG("Sync trigger receieved"); + if(first_image_received){ + for (int i = 0; i < numCameras_; i++){ + sync_message_queue_vector_.at(i).push(sync_info); + ROS_DEBUG("Sync trigger added to cam: %d, length of queue: %d",i,sync_message_queue_vector_.at(i).size()); + } + } + //double curr_time_msg_recieved = ros::Time::now().toSec(); + //sync_trigger_rate = 1/(curr_time_msg_recieved - last_time_msg_recieved); + //last_time_msg_recieved = curr_time_msg_recieved; + } +#endif diff --git a/src/capture_nodelet.cpp b/src/capture_nodelet.cpp deleted file mode 100644 index d1d6492..0000000 --- a/src/capture_nodelet.cpp +++ /dev/null @@ -1,31 +0,0 @@ -// -// Created by pushyami on 1/10/19. -// - -#include -#include -#include "spinnaker_sdk_camera_driver/capture_nodelet.h" - -namespace acquisition -{ - - void capture_nodelet::onInit() - { - //spinners - //ros::AsyncSpinner spinner(0); // Use max cores possible for mt - //spinner.start(); - - NODELET_INFO("Initializing nodelet"); - //acquisition::Capture cobj(getNodeHandle(), getPrivateNodeHandle()); - inst_.reset(new Capture(getNodeHandle(), getPrivateNodeHandle())); - inst_->init_array(); - pubThread_.reset(new boost::thread(boost::bind(&acquisition::Capture::run, inst_))); - // cobj.run(); - //NODELET_INFO("Initializing nodelet"); - } - - - -} - -PLUGINLIB_EXPORT_CLASS(acquisition::capture_nodelet, nodelet::Nodelet) diff --git a/src/nodelet_test.cpp b/src/nodelet_test.cpp deleted file mode 100644 index 4e0de50..0000000 --- a/src/nodelet_test.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// -// Created by auv on 1/10/19. -// - -#include "ros/ros.h" -#include "spinnaker_sdk_camera_driver/std_include.h" -#include -#include -#include "sensor_msgs/Image.h" - -int main(int argc, char **argv) -{ - - ros::init(argc, argv, "subscriber_nodelet"); - - nodelet::Loader nodelet; - nodelet::M_string remap(ros::names::getRemappings()); - nodelet::V_string nargv; - std::string nodelet_name = ros::this_node::getName(); - nodelet.load(nodelet_name, "acquisition/subscriber_nodelet", remap, nargv); - - return 0; -} \ No newline at end of file diff --git a/src/subscriber_node.cpp b/src/subscriber_node.cpp deleted file mode 100755 index ee12613..0000000 --- a/src/subscriber_node.cpp +++ /dev/null @@ -1,24 +0,0 @@ -#include -#include -#include -#include -#include - -#include - - -void imageCallback(const sensor_msgs::ImageConstPtr& msg) -{ - sensor_msgs::Image a =*msg; - double times =a.header.stamp.toSec(); - ROS_INFO_STREAM("THE DIFF IS :" << ros::Time::now().toSec() - times); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "subscriber_node"); - ros::NodeHandle nh; - image_transport::ImageTransport it(nh); - image_transport::Subscriber sub = it.subscribe("/camera_array/cam0/image_raw", 1, imageCallback); - ros::spin(); -} diff --git a/src/subscriber_nodelet.cpp b/src/subscriber_nodelet.cpp deleted file mode 100644 index 6241a90..0000000 --- a/src/subscriber_nodelet.cpp +++ /dev/null @@ -1,33 +0,0 @@ -// -// Created by pushyami on 1/15/19. -// - -#include -#include -#include "spinnaker_sdk_camera_driver/subscriber_nodelet.h" - - -//This is a test nodelet for measuring nodelet performance -namespace acquisition -{ - void subscriber_nodelet::chatterCallback(const sensor_msgs::Image::ConstPtr& msg) - { - NODELET_INFO_STREAM("diff time is "<< ros::Time::now().toSec() - msg->header.stamp.toSec()); - } - - void subscriber_nodelet::onInit() - { - - NODELET_INFO("Initializing nodelet"); - ros::NodeHandle& node = getNodeHandle(); - ros::NodeHandle& private_nh = getPrivateNodeHandle(); - ROS_INFO_STREAM("in sub nodelet"); - it_.reset(new image_transport::ImageTransport(node)); - image_sub_ = it_->subscribe("camera_array/cam0/image_raw",1, &subscriber_nodelet::chatterCallback, this) ; - } - - - -} - -PLUGINLIB_EXPORT_CLASS(acquisition::subscriber_nodelet, nodelet::Nodelet) diff --git a/src/utils.cpp b/src/utils.cpp deleted file mode 100755 index 7cb9a4d..0000000 --- a/src/utils.cpp +++ /dev/null @@ -1,341 +0,0 @@ -#include "spinnaker_sdk_camera_driver/utils.h" - -using namespace Spinnaker; -using namespace Spinnaker::GenApi; -using namespace Spinnaker::GenICam; -using namespace std; - -// This function acquires and saves 10 images from a device. -int AcquireImages(CameraList camList, int numCameras, int numImgsToCapture) { - - int result = 0; - - cout << endl << endl << "*** IMAGE ACQUISITION ***" << endl << endl; - - try { - - // Creating camera directories - for (int cam=0; cam < numCameras; cam++) { - ostringstream ss; - ss<<"cam"<GetNextImage(); - - // Check if the Image is complete - if (pResultImages[i]->IsIncomplete()) { - - cout << "Image incomplete with image status " << pResultImages[i]->GetImageStatus() << "..." << endl << endl; - return -1; - - } - // cout<<"frameID:"<< pResultImages[i]->GetFrameID()< skip_num) - // { - - // Print image information; height and width recorded in pixels - - size_t width = pResultImage->GetWidth(); - size_t height = pResultImage->GetHeight(); - - LOG(INFO) << "Grabbed image " << imageCnt << ", width = " << width << ", height = " << height << ", timestamp: " << int(result) << "-" << pResultImage->GetTimeStamp() << ", cam = " << i << ", frameID = "<< pResultImage->GetFrameID() << endl; - - // Extracting timestamp info - double t, it, ft; - t = double(pResultImage->GetTimeStamp())/1000000000; - ft = modf(t, &it); - - // Create a unique filename - ostringstream filename; - - char num[10]; - sprintf(num, "%06d", imageCnt); - - // filename<<"cam"<Save("dump.jpg"); - LOG(INFO) << "Skipping frame..."; - } else { - pResultImage->Save(filename.str().c_str()); - LOG(INFO) << "Image saved at " << filename.str() << endl; - } - - pResultImage->Release(); - - } - LOG(INFO)<<"Time taken to save images: "<GetFeatures(features); - - FeatureList_t::const_iterator it; - for (it = features.begin(); it != features.end(); ++it) { - - CNodePtr pfeatureNode = *it; - cout << pfeatureNode->GetName() << " : "; - CValuePtr pValue = (CValuePtr)pfeatureNode; - cout << (IsReadable(pValue) ? pValue->ToString() : "Node not readable"); - cout << endl; - } - - } - else { - cout << "Device control information not available." << endl; - } - } - - catch (Spinnaker::Exception &e) { - cout << "Error: " << e.what() << endl; - result = -1; - } - - return result; - -} - - -// This function acts as the body of the example; please see NodeMapInfo example -// for more in-depth comments on setting up cameras. -int initializeCameras(CameraList camList, int numCameras, int numImgsToCapture) { - - int result = 0; - - // Set camera1 frame rate - CameraPtr pCam = NULL; - - // Set cameras 1 to 4 to continuous - //for (unsigned int i = 0; i < numCameras; i++) - for (int i = numCameras-1 ; i >=0 ; i--) { - - CameraPtr pCam = NULL; - // Select camera - pCam = camList.GetByIndex(i); - - cout << endl << "Running example for camera " << i << "..." << endl; - - try { - - // Retrieve TL device nodemap and print device information - INodeMap & nodeMapTLDevice = pCam->GetTLDeviceNodeMap(); - - // result = PrintDeviceInfo(nodeMapTLDevice); - - // Initialize camera - pCam->Init(); - - // Retrieve GenICam nodemap - INodeMap & nodeMap = pCam->GetNodeMap(); - - // Retrieve enumeration node from nodemap - CEnumerationPtr ptrAcquisitionMode = nodeMap.GetNode("AcquisitionMode"); - if (!IsAvailable(ptrAcquisitionMode) || !IsWritable(ptrAcquisitionMode)) { - - cout << "Unable to set acquisition mode to continuous (enum retrieval). Aborting..." << endl << endl; - return -1; - } - - // Retrieve entry node from enumeration node - CEnumEntryPtr ptrAcquisitionModeContinuous = ptrAcquisitionMode->GetEntryByName("Continuous"); - if (!IsAvailable(ptrAcquisitionModeContinuous) || !IsReadable(ptrAcquisitionModeContinuous)) { - - cout << "Unable to set acquisition mode to continuous (entry retrieval). Aborting..." << endl << endl; - return -1; - } - - // Retrieve integer value from entry node - int64_t acquisitionModeContinuous = ptrAcquisitionModeContinuous->GetValue(); - - // Set integer value from entry node as new value of enumeration node - ptrAcquisitionMode->SetIntValue(acquisitionModeContinuous); - - LOG(INFO) << "Acquisition mode set to continuous for camera: " <BeginAcquisition(); - LOG(INFO) << "Acquiring images for camera: " <AcquisitionFrameRate.GetValue(); - - // LOG(INFO) << "Trigger: " << pCam->TriggerSource.GetValue(); - - // cout << "Camera " << i << " linkspeed is " << pCam->DeviceLinkSpeed << endl; - - - - } - - catch (Spinnaker::Exception &e) { - - cout << "Error: " << e.what() << endl; - result = -1; - } - - } - - sleep(2); - cout << "Sleep complete " << endl; - - // Acquire images for all the cameras - result = result | AcquireImages(camList, numCameras, numImgsToCapture); - - for (int i = 0; i < numCameras; i++) { - - CameraPtr pCam = NULL; - // Select camera - pCam = camList.GetByIndex(i); - - // End acquisition - // - // *** NOTES *** - // Ending acquisition appropriately helps ensure that devices clean up - // properly and do not need to be power-cycled to maintain integrity. - // - cout << "Camera "<GetNumImagesInUse()<<" "<EndAcquisition(); - - cout << "Camera "<DeInit(); - - } - - cout << "All cameras deinitialized!!!"<=0;acamno--) { - - pCam = camList.GetByIndex(acamno); - pCamID = pCam->GetUniqueID(); - - if (pCamID == CAMERA_SNs[camno]) { - - cout << "Camera serial is:"<GetUniqueID() << endl; - cout << "camList[1]: " << camList.GetByIndex(1)->GetUniqueID() << endl; - - camListcopy.Clear(); - camListnew.Clear(); - pCam = NULL; - - return camList; - -}