diff --git a/CMakeLists.txt b/CMakeLists.txt index f9cd37d..1e7bafb 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,8 @@ find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport sensor_msgs + dynamic_reconfigure + nodelet ) #find_package(PCL REQUIRED) @@ -31,7 +33,15 @@ find_package(catkin REQUIRED COMPONENTS ### # Find Packages find_package(OpenCV REQUIRED) -find_package(LibUnwind REQUIRED) +# use LibUnwind only for x86_64 or x86_32 architecture +# do not use LibUnwind for arm architecture +if(${CMAKE_SYSTEM_PROCESSOR} MATCHES x86_64 OR x86_32) + message("uses LibUnwind for x86_64 or x86_32 architecture") + find_package(LibUnwind REQUIRED) +endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES x86_64 OR x86_32) +if(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm) + message("Detected ARM architecture") +endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm) find_package(Boost REQUIRED) if(Boost_FOUND) @@ -46,43 +56,83 @@ add_message_files( SpinnakerImageNames.msg ) -generate_messages( - DEPENDENCIES - std_msgs -) +generate_dynamic_reconfigure_options( + cfg/spinnaker_cam.cfg -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp std_msgs message_runtime - DEPENDS OpenCV LibUnwind ) -include_directories( - ${PROJECT_INCLUDE_DIR} - ${catkin_INCLUDE_DIRS} - ${SPINNAKER_INCLUDE_DIR} - ${OpenCV_INCLUDE_DIRS} - ${Boost_INCLUDE_DIR} - ${LibUnwind_INCLUDE_DIRS} +generate_messages( + DEPENDENCIES + std_msgs ) - -link_directories( ${SPINNAKER_LIB_DIR} ) - -set (LIBS ${LibUnwind_LIBRARIES} Spinnaker ${OpenCV_LIBS} ${Boost_GENERAL}) +## setting catkin_package, include_directories and libs based on architecture +if(${CMAKE_SYSTEM_PROCESSOR} MATCHES x86_64 OR x86_32) + catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS roscpp std_msgs message_runtime nodelet + DEPENDS OpenCV LibUnwind + ) + + include_directories( + ${PROJECT_INCLUDE_DIR} + ${catkin_INCLUDE_DIRS} + ${SPINNAKER_INCLUDE_DIR} + ${OpenCV_INCLUDE_DIRS} + ${Boost_INCLUDE_DIR} + ${LibUnwind_INCLUDE_DIRS} + ) + + link_directories( ${SPINNAKER_LIB_DIR} ) + + set (LIBS ${LibUnwind_LIBRARIES} Spinnaker ${OpenCV_LIBS} ${Boost_GENERAL}) + +endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES x86_64 OR x86_32) +if(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm) + catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS roscpp std_msgs message_runtime nodelet + DEPENDS OpenCV + ) + + include_directories( + ${PROJECT_INCLUDE_DIR} + ${catkin_INCLUDE_DIRS} + ${SPINNAKER_INCLUDE_DIR} + ${OpenCV_INCLUDE_DIRS} + ${Boost_INCLUDE_DIR} + ) + + link_directories( ${SPINNAKER_LIB_DIR} ) + + set (LIBS Spinnaker ${OpenCV_LIBS} ${Boost_GENERAL}) + +endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm) add_library (acquilib SHARED src/capture.cpp src/camera.cpp ) -add_dependencies(acquilib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +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}) +add_dependencies(acquisition_node acquilib ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) target_link_libraries (acquisition_node acquilib ${LIBS} ${catkin_LIBRARIES}) -install(TARGETS acquilib acquisition_node + +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 ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/README.md b/README.md index 0a52322..020c89e 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# spinnaker_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. @@ -13,6 +13,8 @@ The pre-requisites for this repo include: * spinnaker (download from [Pt Grey's website](https://www.ptgrey.com/support/downloads)) * ros-kinetic-cv-bridge * ros-kinetic-image-transport + +# Incase of x86_64 or x86_32 architecture, install the following: * libunwind-dev ```bash @@ -21,6 +23,11 @@ The pre-requisites for this repo include: # after installing ros, install other pre-requisites with: sudo apt install libunwind-dev ros-kinetic-cv-bridge ros-kinetic-image-transport + +# if you use arm64 (aarch64), install pre-requisites with: + +sudo apt install ros-kinetic-cv-bridge ros-kinetic-image-transport + ``` ### Installing @@ -39,7 +46,13 @@ 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 -roslaunch spinnaker_sdk_camera_driver acquisition.launch +# To launch nodelet verison of driver, use # + +roslaunch spinnaker_camera_driver acquisition_nodelet.launch + +# To launch node version of driver, use + +roslaunch spinnaker_camera_driver acquisition.launch # Test that the images are being published by running rqt_image_view ``` @@ -51,8 +64,8 @@ All the parameters can be set via the launch file or via the yaml config_file. Binning for cameras, when changing from 2 to 1 cameras need to be unplugged and replugged * ~color (bool, default: false) Should color images be used (only works on models that support color images) -* ~exp (int, default: 0) - Exposure setting for cameras +* ~exposure_time (int, default: 0, 0:auto) + Exposure setting for cameras, also available as dynamic reconfiguarble parameter. * ~frames (int, default: 50) Number of frames to save/view 0=ON * ~live (bool, default: false) @@ -74,7 +87,7 @@ All the parameters can be set via the launch file or via the yaml config_file. * ~utstamps (bool, default:false) 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" + 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 ### System configuration parameters * ~cam_ids (yaml sequence or array) @@ -86,7 +99,27 @@ This is the names that would be given to the cameras for filenames and rostopics * ~skip (int) Number of frames to be skipped initially to flush the buffer * ~delay (float) - Secs to wait in the deinit/init sequence + Secs to wait in the deinit/init sequence. + + +### Dynamic Reconfigure parameters +* ~target_grey_val (double) + Target Grey Value is a parameter that helps to compensate for various lighting conditions by adjusting brightness to achieve optimal imaging results. The value is linear and is a percentage of the maximum pixel value. + Explained in detail at [FLIR webpage](https://www.flir.com/support-center/iis/machine-vision/application-note/using-auto-exposure-in-blackfly-s/). + + Setting target_grey_val invokes setting AutoExposureTargetGreyValueAuto to 'off' and AutoExposureTargetGreyValue is set to target_grey_val. +* ~exposure_time (int, default= 0, 0:auto) + Exposure time for the sensor. + When Exposure time is set within minimum and maximum exposure time limits(varies with camera model), ExposureAuto is set to 'off' and ExposureTime is set to exposure_time param value. + + 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) + 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). ### Camera info message details * ~image_width (int) @@ -100,9 +133,11 @@ This is the names that would be given to the cameras for filenames and rostopics Specified as [fx 0 cx 0 fy cy 0 0 1] * ~projection_coeffs (array of arrays) Projection coefficients of all the cameras in the array. Must match the number of cam_ids provided. + For case of Monocular camera, if projection_coeffs is not set, intrinsic_coeff will be used to set, P[1:3,1:3]=K * ~rectification_coeffs (array of arrays) Rectification coefficients of all the cameras in the array. Must match the number of cam_ids provided. + ## Multicamera Master-Slave Setup When using multiple cameras, we have found that the only way to keep images between different cameras synched is by using a master-slave setup using the GPIO connector. So this is the only way we support multicamera operation with this code. A general guide for multi camera setup is available at https://www.ptgrey.com/tan/11052, however note that we use a slightly different setup with our package. Refer to the `params/multi-cam_example.yaml` for an example on how to setup the configuration. You must specify a master_cam which must be one of the cameras in the cam_ids list. This master camera is the camera that is either explicitly software triggered by the code or triggered internally via a counter at a given frame rate. All the other cameras are triggered externally when the master camera triggers. In order to make this work, the wiring must be such that the external signal from the master camera **Line2** is connected to **Line3** on all slave cameras. To connect cameras in this way: diff --git a/cfg/spinnaker_cam.cfg b/cfg/spinnaker_cam.cfg new file mode 100755 index 0000000..673f142 --- /dev/null +++ b/cfg/spinnaker_cam.cfg @@ -0,0 +1,13 @@ +#!/usr/bin/env python +PACKAGE = "spinnaker_sdk_camera_driver" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + + +gen.add("target_grey_val", double_t, 1, "Set Target Grey Value", 50, 4, 90) +gen.add("exposure_time", int_t, 2, "Set Exposure time (0:auto)", 0, 0, 15000) + + +exit(gen.generate(PACKAGE, "spinnaker_sdk_camera_driver", "spinnaker_cam")) diff --git a/include/spinnaker_sdk_camera_driver/camera.h b/include/spinnaker_sdk_camera_driver/camera.h index 440b3ce..a3b37f6 100755 --- a/include/spinnaker_sdk_camera_driver/camera.h +++ b/include/spinnaker_sdk_camera_driver/camera.h @@ -45,6 +45,7 @@ namespace acquisition { void setResolutionPixels(int width, int height); void setBufferSize(int numBuf); void adcBitDepth(gcstring bitDep); + void targetGreyValueTest(); // void set_acquisition_mode_continuous(); // void set_frame_rate(float); diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h index 5a53270..e248af0 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -7,11 +7,17 @@ #include #include - +//ROS +#include "std_msgs/Float64.h" #include "std_msgs/String.h" +//Dynamic reconfigure +#include +#include + #include "spinnaker_sdk_camera_driver/SpinnakerImageNames.h" #include +#include using namespace Spinnaker; using namespace Spinnaker::GenApi; @@ -27,6 +33,7 @@ namespace acquisition { ~Capture(); Capture(); + Capture( ros::NodeHandle node,ros::NodeHandle private_nh); void load_cameras(); @@ -58,6 +65,7 @@ namespace acquisition { void get_mat_images(); void update_grid(); void export_to_ROS(); + void dynamicReconfigureCallback(spinnaker_sdk_camera_driver::spinnaker_camConfig &config, uint32_t level); float mem_usage(); @@ -123,13 +131,18 @@ namespace acquisition { // ros variables ros::NodeHandle nh_; ros::NodeHandle nh_pvt_; + //image_transport::ImageTransport it_; + image_transport::ImageTransport it_; + dynamic_reconfigure::Server* dynamicReCfgServer_; ros::Publisher acquisition_pub; - vector camera_image_pubs; - vector camera_info_pubs; + //vector camera_image_pubs; + vector camera_image_pubs; + //vector camera_info_pubs; + vector img_msgs; - vector cam_info_msgs; + vector cam_info_msgs; spinnaker_sdk_camera_driver::SpinnakerImageNames mesg; boost::mutex queue_mutex_; }; diff --git a/include/spinnaker_sdk_camera_driver/capture_nodelet.h b/include/spinnaker_sdk_camera_driver/capture_nodelet.h new file mode 100644 index 0000000..0955c99 --- /dev/null +++ b/include/spinnaker_sdk_camera_driver/capture_nodelet.h @@ -0,0 +1,32 @@ +// +// 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/std_include.h b/include/spinnaker_sdk_camera_driver/std_include.h index e9f377e..24c196b 100755 --- a/include/spinnaker_sdk_camera_driver/std_include.h +++ b/include/spinnaker_sdk_camera_driver/std_include.h @@ -15,6 +15,9 @@ #include #include "sensor_msgs/Image.h" #include "sensor_msgs/CameraInfo.h" +#include "std_msgs/String.h" +#include +#include "sensor_msgs/Image.h" // Standard Libs diff --git a/include/spinnaker_sdk_camera_driver/subscriber_nodelet.h b/include/spinnaker_sdk_camera_driver/subscriber_nodelet.h new file mode 100644 index 0000000..a94a44a --- /dev/null +++ b/include/spinnaker_sdk_camera_driver/subscriber_nodelet.h @@ -0,0 +1,26 @@ +// +// 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/launch/acquisition.launch b/launch/acquisition.launch index b4712aa..31c29c9 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -2,10 +2,10 @@ - - + + - + @@ -13,26 +13,32 @@ - + - - + + + + + + + + - + - @@ -45,6 +51,6 @@ - - + + diff --git a/launch/acquisition_node.launch b/launch/acquisition_node.launch new file mode 100644 index 0000000..8ea0338 --- /dev/null +++ b/launch/acquisition_node.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/multiple_nodelet_example.launch b/launch/multiple_nodelet_example.launch new file mode 100644 index 0000000..ba96a9e --- /dev/null +++ b/launch/multiple_nodelet_example.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + diff --git a/launch/nodelet_subscriber_example.launch b/launch/nodelet_subscriber_example.launch new file mode 100644 index 0000000..31f2a69 --- /dev/null +++ b/launch/nodelet_subscriber_example.launch @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml new file mode 100644 index 0000000..ad24255 --- /dev/null +++ b/nodelet_plugins.xml @@ -0,0 +1,12 @@ + + + + This is my nodelet. + + + + + This is my nodelet. + + + diff --git a/package.xml b/package.xml index 059f84e..06f3ed1 100755 --- a/package.xml +++ b/package.xml @@ -24,6 +24,11 @@ cv_bridge image_transport sensor_msgs + nodelet + + + + message_runtime diff --git a/params/test_params.yaml b/params/test_params.yaml index 11fda13..a8a9a45 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -1,10 +1,12 @@ cam_ids: -- 17197555 +- 18080155 +#- 17197547 cam_aliases: - cam0 -master_cam: 17197555 +#- cam1 +master_cam: 18080155 skip: 20 -delay: 0 +delay: 1.0 # Assign all the follwing via launch file to prevent confusion and conflict @@ -13,6 +15,28 @@ delay: 0 #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 +#soft_framerate: 20 # 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 + +#Camera info message details +distortion_model: plumb_bob +image_height: 1536 +image_width: 2048 +distortion_coeffs: +- [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725] +#- [-0.06001035588294054, 0.3466585291091072, -1.0932356940133992, 3.4511959617065253] + + +#specified as [fx 0 cx 0 fy cy 0 0 1] +intrinsic_coeffs: +- [1886.9232141485886, 0.0, 604.7214878709341, 0.0, 1886.6668765711668, 493.47726714719823, 0.0, 0.0, 1.0] +#- [1872.1392958276606, 0.0, 583.2925844388025, 0.0, 1872.6168324818198, 483.72597760530056, 0.0, 0.0, 1.0] + +rectification_coeffs: +- [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] +#- [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] + +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] +#- [703.910323, 0.000000, 815.113302, 0.000000, 0.000000, 958.319231, 636.871993, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/src/acquisition_node.cpp b/src/acquisition_node.cpp index ed5ee3e..098ed45 100755 --- a/src/acquisition_node.cpp +++ b/src/acquisition_node.cpp @@ -11,7 +11,10 @@ int main(int argc, char** argv) { // 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(); diff --git a/src/acquisition_nodelet.cpp b/src/acquisition_nodelet.cpp new file mode 100644 index 0000000..6caecbd --- /dev/null +++ b/src/acquisition_nodelet.cpp @@ -0,0 +1,29 @@ +#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 c189032..de4c37d 100755 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -40,8 +40,8 @@ void acquisition::Camera::deinit() { ImagePtr acquisition::Camera::grab_frame() { 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() << "!"); @@ -60,11 +60,9 @@ ImagePtr acquisition::Camera::grab_frame() { } } - + ROS_DEBUG_STREAM("Grabbed frame from camera " << get_id() << " with timestamp " << timestamp_*1000); - - return pResultImage; - + return pResultImage; } // Returns last timestamp @@ -96,19 +94,16 @@ Mat acquisition::Camera::convert_to_mat(ImagePtr pImage) { convertedImage = pImage->Convert(PixelFormat_BGR8); //, NEAREST_NEIGHBOR); else convertedImage = pImage->Convert(PixelFormat_Mono8); //, NEAREST_NEIGHBOR); - unsigned int XPadding = convertedImage->GetXPadding(); unsigned int YPadding = convertedImage->GetYPadding(); unsigned int rowsize = convertedImage->GetWidth(); unsigned int colsize = convertedImage->GetHeight(); - //image data contains padding. When allocating Mat container size, you need to account for the X,Y image data padding. Mat img; if (COLOR_) img = Mat(colsize + YPadding, rowsize + XPadding, CV_8UC3, convertedImage->GetData(), convertedImage->GetStride()); else img = Mat(colsize + YPadding, rowsize + XPadding, CV_8UC1, convertedImage->GetData(), convertedImage->GetStride()); - return img.clone(); // return img; @@ -301,6 +296,19 @@ void acquisition::Camera::trigger() { } + +void acquisition::Camera::targetGreyValueTest() { + CFloatPtr ptrExpTest =pCam_->GetNodeMap().GetNode("AutoExposureTargetGreyValue"); + //CFloatPtr ptrExpTest=pCam_->GetNodeMap().GetNode("ExposureTime"); + if (!IsAvailable(ptrExpTest) || !IsReadable(ptrExpTest)){ + ROS_FATAL_STREAM("Unable to set exposure " << "). Aborting..." << endl << endl); + return ; + } + ptrExpTest->SetValue(90.0); + ROS_INFO_STREAM("target grey mode Time: "<GetValue()<GetNodeMap().GetNode("ExposureTime"); if (!IsAvailable(ptrExpTest) || !IsReadable(ptrExpTest)){ @@ -310,4 +318,4 @@ void acquisition::Camera::exposureTest() { float expTime=ptrExpTest->GetValue(); ROS_DEBUG_STREAM("Exposure Time: "<ReleaseInstance(); + + delete dynamicReCfgServer_; ros::shutdown(); @@ -35,7 +37,7 @@ void handler(int i) { } -acquisition::Capture::Capture():nh_(),nh_pvt_ ("~") { +acquisition::Capture::Capture(): it_(nh_), nh_pvt_ ("~") { // struct sigaction sigIntHandler; @@ -91,7 +93,7 @@ acquisition::Capture::Capture():nh_(),nh_pvt_ ("~") { achieved_time_ = 0; // decimation_ = 1; - + CAM_ = 0; // default flag values @@ -113,8 +115,95 @@ acquisition::Capture::Capture():nh_(),nh_pvt_ ("~") { //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); +} + +acquisition::Capture::Capture(ros::NodeHandle nodehandl, ros::NodeHandle private_nh) : nh_ (nodehandl) , it_(nh_), nh_pvt_ (private_nh) { + + 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::load_cameras() { // Retrieve list of cameras from the system @@ -133,6 +222,8 @@ void acquisition::Capture::load_cameras() { bool master_set = false; int cam_counter = 0; + + for (int j=0; j("camera_array/"+cam_names_[j]+"/image_raw", 1)); - camera_info_pubs.push_back(nh_.advertise("camera_array/"+cam_names_[j]+"/camera_info", 1)); + camera_image_pubs.push_back(it_.advertiseCamera("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()); - if (PUBLISH_CAM_INFO_){ - sensor_msgs::CameraInfo ci_msg; + sensor_msgs::CameraInfoPtr ci_msg(new sensor_msgs::CameraInfo()); int image_width = 0; int image_height = 0; 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"; - ci_msg.width = image_width; - ci_msg.height = image_height; - ci_msg.distortion_model = distortion_model; - ci_msg.binning_x = binning_; - ci_msg.binning_y = binning_; - - ci_msg.D = distortion_coeff_vec_[j]; + 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; + ci_msg->D = distortion_coeff_vec_[j]; + // intrinsic coefficients for (int count = 0; countK[count] = intrinsic_coeff_vec_[j][count]; + // Rectification matrix + if (!rect_coeff_vec_.empty()) + ci_msg->R = { + rect_coeff_vec_[j][0], rect_coeff_vec_[j][1], + rect_coeff_vec_[j][2], rect_coeff_vec_[j][3], + rect_coeff_vec_[j][4], rect_coeff_vec_[j][5], + rect_coeff_vec_[j][6], rect_coeff_vec_[j][7], + rect_coeff_vec_[j][8]}; + // Projection/camera matrix + if (!proj_coeff_vec_.empty()){ + ci_msg->P = { + proj_coeff_vec_[j][0], proj_coeff_vec_[j][1], + proj_coeff_vec_[j][2], proj_coeff_vec_[j][3], + proj_coeff_vec_[j][4], proj_coeff_vec_[j][5], + proj_coeff_vec_[j][6], proj_coeff_vec_[j][7], + proj_coeff_vec_[j][8], proj_coeff_vec_[j][9], + proj_coeff_vec_[j][10], proj_coeff_vec_[j][11]}; + } + else if(numCameras_ == 1){ + // for case of monocular camera, P[1:3,1:3]=K + ci_msg->P = { + intrinsic_coeff_vec_[j][0], intrinsic_coeff_vec_[j][1], + intrinsic_coeff_vec_[j][2], 0, + intrinsic_coeff_vec_[j][3], intrinsic_coeff_vec_[j][4], + intrinsic_coeff_vec_[j][5], 0, + intrinsic_coeff_vec_[j][6], intrinsic_coeff_vec_[j][7], + intrinsic_coeff_vec_[j][8], 0}; + } + // binning + ci_msg->binning_x = binning_; + ci_msg->binning_y = binning_; cam_info_msgs.push_back(ci_msg); } @@ -318,10 +430,10 @@ void acquisition::Capture::read_parameters() { } else ROS_WARN(" 'fps' Parameter not set, using default behavior: fps=%0.2f",master_fps_); - if (nh_pvt_.getParam("exp", exposure_time_)){ + if (nh_pvt_.getParam("exposure_time", exposure_time_)){ if (exposure_time_ >0) ROS_INFO(" Exposure set to: %.1f",exposure_time_); - else ROS_INFO(" 'exp'=%0.f, Setting autoexposure",exposure_time_); - } else ROS_WARN(" 'exp' Parameter not set, using default behavior: Automatic Exposure "); + else ROS_INFO(" 'exposure_time'=%0.f, Setting autoexposure",exposure_time_); + } else ROS_WARN(" 'exposure_time' Parameter not set, using default behavior: Automatic Exposure "); if (nh_pvt_.getParam("binning", binning_)){ if (binning_ >0) ROS_INFO(" Binning set to: %d",binning_); @@ -439,8 +551,8 @@ void acquisition::Capture::read_parameters() { PUBLISH_CAM_INFO_ = intrinsics_list_provided && distort_list_provided; if (PUBLISH_CAM_INFO_) ROS_INFO(" Camera coeffs provided, camera info messges will be published."); - else - ROS_INFO(" Camera coeffs not provided correctly, camera info messges will not be published."); + else + ROS_INFO(" Camera coeffs not provided correctly, camera info messges will not be published."); // ROS_ASSERT_MSG(my_list.getType() // int num_ids = cam_id_vec.size(); @@ -648,15 +760,19 @@ void acquisition::Capture::export_to_ROS() { 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(); - - camera_image_pubs[i].publish(img_msgs[i]); + 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;; } @@ -712,9 +828,9 @@ void acquisition::Capture::get_mat_images() { for (int i=0; i 0){ + cams[MASTER_CAM_].setEnumValue("ExposureAuto", "Off"); + cams[MASTER_CAM_].setEnumValue("ExposureMode", "Timed"); + cams[MASTER_CAM_].setFloatValue("ExposureTime", config.exposure_time); + } + else if(config.exposure_time ==0){ + cams[MASTER_CAM_].setEnumValue("ExposureAuto", "Continuous"); + cams[MASTER_CAM_].setEnumValue("ExposureMode", "Timed"); + } + } +} \ No newline at end of file diff --git a/src/capture_nodelet.cpp b/src/capture_nodelet.cpp new file mode 100644 index 0000000..d1d6492 --- /dev/null +++ b/src/capture_nodelet.cpp @@ -0,0 +1,31 @@ +// +// 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 new file mode 100644 index 0000000..4e0de50 --- /dev/null +++ b/src/nodelet_test.cpp @@ -0,0 +1,23 @@ +// +// 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 new file mode 100755 index 0000000..ee12613 --- /dev/null +++ b/src/subscriber_node.cpp @@ -0,0 +1,24 @@ +#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 new file mode 100644 index 0000000..6241a90 --- /dev/null +++ b/src/subscriber_nodelet.cpp @@ -0,0 +1,33 @@ +// +// 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)