From 302c8d4d3938c3b51b47753b519c401f12416cd2 Mon Sep 17 00:00:00 2001 From: mithunvarma Date: Thu, 20 Dec 2018 21:58:01 -0500 Subject: [PATCH 01/30] fixed imagetransport, changed from imagetransport:;publisher to imagetransport::camerapublisher and made caminfo to caminfoptr --- CMakeLists.txt | 10 ++- cfg/spinnaker_cam.cfg | 13 +++ include/spinnaker_sdk_camera_driver/camera.h | 1 + include/spinnaker_sdk_camera_driver/capture.h | 19 ++++- launch/acquisition.launch | 2 +- params/test_params.yaml | 18 ++++- src/acquisition_node.cpp | 5 +- src/camera.cpp | 13 +++ src/capture.cpp | 81 ++++++++++++------- 9 files changed, 124 insertions(+), 38 deletions(-) create mode 100755 cfg/spinnaker_cam.cfg diff --git a/CMakeLists.txt b/CMakeLists.txt index f9cd37d..f7a97db 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport sensor_msgs + dynamic_reconfigure ) #find_package(PCL REQUIRED) @@ -46,6 +47,11 @@ add_message_files( SpinnakerImageNames.msg ) +generate_dynamic_reconfigure_options( + cfg/spinnaker_cam.cfg + +) + generate_messages( DEPENDENCIES std_msgs @@ -75,11 +81,11 @@ add_library (acquilib SHARED 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_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 diff --git a/cfg/spinnaker_cam.cfg b/cfg/spinnaker_cam.cfg new file mode 100755 index 0000000..6c315e0 --- /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, 3, "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 fba1522..4bdca6d 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; @@ -58,6 +64,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(); @@ -121,13 +128,17 @@ namespace acquisition { // ros variables ros::NodeHandle nh_; ros::NodeHandle nh_pvt_; + //image_transport::ImageTransport it_; + image_transport::ImageTransport it_; + dynamic_reconfigure::Server server; 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/launch/acquisition.launch b/launch/acquisition.launch index b4712aa..1ce4854 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -3,7 +3,7 @@ - + diff --git a/params/test_params.yaml b/params/test_params.yaml index 11fda13..3e530f2 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -1,10 +1,12 @@ cam_ids: -- 17197555 +- 17197559 +- 17197547 cam_aliases: - cam0 -master_cam: 17197555 +- cam1 +master_cam: 17197559 skip: 20 -delay: 0 +delay: 1.0 # Assign all the follwing via launch file to prevent confusion and conflict @@ -16,3 +18,13 @@ delay: 0 #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 + +#Camera info message details +distortion_model: plumb_bob +distortion_coeffs: +- [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725] +- [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725] +#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] +- [1886.9232141485886, 0.0, 604.7214878709341, 0.0, 1886.6668765711668, 493.47726714719823, 0.0, 0.0, 1.0] \ No newline at end of file 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/camera.cpp b/src/camera.cpp index c189032..9d60fac 100755 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -301,6 +301,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)){ diff --git a/src/capture.cpp b/src/capture.cpp index a8151c2..79a0ff7 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -35,7 +35,7 @@ void handler(int i) { } -acquisition::Capture::Capture():nh_(),nh_pvt_ ("~") { +acquisition::Capture::Capture(): it_(nh_), nh_pvt_ ("~") { // struct sigaction sigIntHandler; @@ -91,7 +91,7 @@ acquisition::Capture::Capture():nh_(),nh_pvt_ ("~") { achieved_time_ = 0; // decimation_ = 1; - + CAM_ = 0; // default flag values @@ -113,6 +113,12 @@ acquisition::Capture::Capture():nh_(),nh_pvt_ ("~") { //initializing the ros publisher acquisition_pub = nh_.advertise("camera", 1000); + //dynamic reconfigure + //dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; + + f = boost::bind(&acquisition::Capture::dynamicReconfigureCallback,this, _1, _2); + server.setCallback(f); } void acquisition::Capture::load_cameras() { @@ -133,6 +139,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; - - ci_msg.height = 1024; - ci_msg.width = 1280; - ci_msg.distortion_model = "plumb_bob"; - ci_msg.D = distortion_coeff_vec_[j]; - ci_msg.binning_x = binning_; - ci_msg.binning_y = binning_; + sensor_msgs::CameraInfoPtr ci_msg(new sensor_msgs::CameraInfo()); + ci_msg->height = 1024; + ci_msg->width = 1280; + ci_msg->distortion_model = "plumb_bob"; + ci_msg->D = distortion_coeff_vec_[j]; + ci_msg->binning_x = binning_; + ci_msg->binning_y = binning_; for (int count = 0; countK[count] = intrinsic_coeff_vec_[j][count]; + ci_msg->header.frame_id = "cam_"+to_string(j)+"_optical_frame"; + 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}; cam_info_msgs.push_back(ci_msg); } cam_counter++; @@ -390,8 +396,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(); @@ -599,15 +605,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;; } @@ -767,7 +777,7 @@ void acquisition::Capture::run_soft_trig() { } double disp_time_ = ros::Time::now().toSec() - t; - + // Call update functions if (!MANUAL_TRIGGER_) { cams[MASTER_CAM_].trigger(); @@ -792,7 +802,7 @@ void acquisition::Capture::run_soft_trig() { } if (EXPORT_TO_ROS_) export_to_ROS(); - + //cams[MASTER_CAM_].targetGreyValueTest(); // ros publishing messages acquisition_pub.publish(mesg); @@ -1008,3 +1018,20 @@ std::string acquisition::Capture::todays_date() std::string td(out); return td; } + +void acquisition::Capture::dynamicReconfigureCallback(spinnaker_sdk_camera_driver::spinnaker_camConfig &config, uint32_t level){ + ROS_INFO_STREAM("Dynamic Reconfigure: Target grey value : " << config.target_grey_val <<"Exposure "< 0){ + cams[MASTER_CAM_].setEnumValue("ExposureAuto", "Off"); + cams[MASTER_CAM_].setEnumValue("ExposureMode", "Timed"); + cams[MASTER_CAM_].setFloatValue("ExposureTime", config.exposure_time); + } + + +} \ No newline at end of file From a5966abd9160a1e52926e63df4587c87be19e945 Mon Sep 17 00:00:00 2001 From: mithunvarma Date: Fri, 4 Jan 2019 00:19:54 -0500 Subject: [PATCH 02/30] changed dynamic reconfig levels --- cfg/spinnaker_cam.cfg | 2 +- launch/acquisition.launch | 38 +++++++++++++++++++------------------- params/test_params.yaml | 10 ++++------ src/camera.cpp | 13 ++++--------- src/capture.cpp | 29 ++++++++++++++++------------- 5 files changed, 44 insertions(+), 48 deletions(-) diff --git a/cfg/spinnaker_cam.cfg b/cfg/spinnaker_cam.cfg index 6c315e0..673f142 100755 --- a/cfg/spinnaker_cam.cfg +++ b/cfg/spinnaker_cam.cfg @@ -7,7 +7,7 @@ gen = ParameterGenerator() gen.add("target_grey_val", double_t, 1, "Set Target Grey Value", 50, 4, 90) -gen.add("exposure_time", int_t, 3, "Set Exposure time (0:auto)", 0, 0, 15000) +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/launch/acquisition.launch b/launch/acquisition.launch index 1ce4854..905b141 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -1,31 +1,31 @@ - + - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + args=""> + override what is in the yaml file. Thus use it to set parameters camer_array configuration params --> @@ -47,4 +47,4 @@ - + \ No newline at end of file diff --git a/params/test_params.yaml b/params/test_params.yaml index 3e530f2..da7b8e3 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -1,10 +1,8 @@ cam_ids: -- 17197559 -- 17197547 +- 17197557 cam_aliases: - cam0 -- cam1 -master_cam: 17197559 +master_cam: 17197557 skip: 20 delay: 1.0 @@ -23,8 +21,8 @@ delay: 1.0 distortion_model: plumb_bob distortion_coeffs: - [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725] -- [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725] +#- [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725] #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] -- [1886.9232141485886, 0.0, 604.7214878709341, 0.0, 1886.6668765711668, 493.47726714719823, 0.0, 0.0, 1.0] \ No newline at end of file +#- [1886.9232141485886, 0.0, 604.7214878709341, 0.0, 1886.6668765711668, 493.47726714719823, 0.0, 0.0, 1.0] diff --git a/src/camera.cpp b/src/camera.cpp index 9d60fac..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; @@ -323,4 +318,4 @@ void acquisition::Camera::exposureTest() { float expTime=ptrExpTest->GetValue(); ROS_DEBUG_STREAM("Exposure Time: "<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]); @@ -673,9 +673,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); + if (level == 2 || level ==3){ + ROS_INFO_STREAM("Exposure "< 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 From c64ad05fcec0cbae310e9e7c1482448e2770c20c Mon Sep 17 00:00:00 2001 From: mithun Date: Thu, 11 Feb 2016 11:31:58 -0500 Subject: [PATCH 03/30] uas arm modifications --- CMakeLists.txt | 8 ++++---- launch/acquisition.launch | 12 +++++++----- params/test_params.yaml | 28 +++++++++++++--------------- 3 files changed, 24 insertions(+), 24 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f7a97db..5d6fd30 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,7 +32,7 @@ find_package(catkin REQUIRED COMPONENTS ### # Find Packages find_package(OpenCV REQUIRED) -find_package(LibUnwind REQUIRED) +#find_package(LibUnwind REQUIRED) find_package(Boost REQUIRED) if(Boost_FOUND) @@ -60,7 +60,7 @@ generate_messages( catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS roscpp std_msgs message_runtime - DEPENDS OpenCV LibUnwind + DEPENDS OpenCV #LibUnwind ) include_directories( @@ -69,12 +69,12 @@ include_directories( ${SPINNAKER_INCLUDE_DIR} ${OpenCV_INCLUDE_DIRS} ${Boost_INCLUDE_DIR} - ${LibUnwind_INCLUDE_DIRS} + #${LibUnwind_INCLUDE_DIRS} ) link_directories( ${SPINNAKER_LIB_DIR} ) -set (LIBS ${LibUnwind_LIBRARIES} Spinnaker ${OpenCV_LIBS} ${Boost_GENERAL}) +set (LIBS Spinnaker ${OpenCV_LIBS} ${Boost_GENERAL}) add_library (acquilib SHARED src/capture.cpp diff --git a/launch/acquisition.launch b/launch/acquisition.launch index 905b141..e1685f8 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -1,9 +1,12 @@ - + - + + + + @@ -21,8 +24,7 @@ - + @@ -47,4 +49,4 @@ - \ No newline at end of file + diff --git a/params/test_params.yaml b/params/test_params.yaml index da7b8e3..6883d81 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -1,28 +1,26 @@ cam_ids: -- 17197557 +- 17197556 cam_aliases: - cam0 -master_cam: 17197557 -skip: 20 -delay: 1.0 +master_cam: 17197556 +skip: 5 +delay: 0 + +#Camera info message details +distortion_model: plumb_bob +distortion_coeffs: +- [-0.17465, 0.03501, 0.00064, -0.00036] +#specified as [fx 0 cx 0 fy cy 0 0 1] +intrinsic_coeffs: +- [638.71023, 0.0, 315.25919, 0.0, 641.21041, 248.26077, 0.0, 0.0, 1.0] # 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 +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 - -#Camera info message details -distortion_model: plumb_bob -distortion_coeffs: -- [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725] -#- [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725] -#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] -#- [1886.9232141485886, 0.0, 604.7214878709341, 0.0, 1886.6668765711668, 493.47726714719823, 0.0, 0.0, 1.0] From bfd791df7f0b55952edb5c3fb87db4e2f22e06a9 Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 5 Dec 2018 18:29:09 -0500 Subject: [PATCH 04/30] image_transport added in - seg faults --- include/capture.h | 6 +++++- launch/acquisition.launch | 2 +- params/test_params.yaml | 4 ++-- src/capture.cpp | 8 +++++--- 4 files changed, 13 insertions(+), 7 deletions(-) diff --git a/include/capture.h b/include/capture.h index 8ff5eec..4cbd207 100755 --- a/include/capture.h +++ b/include/capture.h @@ -12,6 +12,7 @@ #include "spinnaker_sdk_camera_driver/spinnaker_image_names.h" #include +#include using namespace Spinnaker; using namespace Spinnaker::GenApi; @@ -121,9 +122,12 @@ namespace acquisition { // ros variables ros::NodeHandle nh_; ros::NodeHandle nh_pvt_; + //image_transport::ImageTransport it_; + std::shared_ptr it_; ros::Publisher acquisition_pub; - vector camera_image_pubs; + + vector camera_image_pubs; vector camera_info_pubs; diff --git a/launch/acquisition.launch b/launch/acquisition.launch index b4712aa..ebb9e72 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -22,7 +22,7 @@ + args="" launch-prefix="xterm -e gdb --args"> diff --git a/params/test_params.yaml b/params/test_params.yaml index 11fda13..8cafc56 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -1,8 +1,8 @@ cam_ids: -- 17197555 +- 18080155 cam_aliases: - cam0 -master_cam: 17197555 +master_cam: 18080155 skip: 20 delay: 0 diff --git a/src/capture.cpp b/src/capture.cpp index c0b4cd3..b9aa059 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -35,7 +35,7 @@ void handler(int i) { } -acquisition::Capture::Capture():nh_(),nh_pvt_ ("~") { +acquisition::Capture::Capture(): it_(nh_), nh_pvt_ ("~") { // struct sigaction sigIntHandler; @@ -91,7 +91,7 @@ acquisition::Capture::Capture():nh_(),nh_pvt_ ("~") { achieved_time_ = 0; // decimation_ = 1; - + CAM_ = 0; // default flag values @@ -133,6 +133,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_image_pubs.push_back(it_->advertise("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()); From 6391a5a99bc3792de9fd99de58b6545ca1a8dc2b Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 5 Dec 2018 18:29:09 -0500 Subject: [PATCH 05/30] image_transport added in - seg faults --- include/capture.h | 7 ++++++- launch/acquisition.launch | 2 +- params/test_params.yaml | 4 ++-- src/capture.cpp | 8 +++++--- 4 files changed, 14 insertions(+), 7 deletions(-) diff --git a/include/capture.h b/include/capture.h index 9780078..76922d9 100755 --- a/include/capture.h +++ b/include/capture.h @@ -12,6 +12,7 @@ #include "spinnaker_sdk_camera_driver/spinnaker_image_names.h" #include +#include using namespace Spinnaker; using namespace Spinnaker::GenApi; @@ -121,9 +122,13 @@ namespace acquisition { // ros variables ros::NodeHandle nh_; ros::NodeHandle nh_pvt_; + //image_transport::ImageTransport it_; + std::shared_ptr it_; ros::Publisher acquisition_pub; - vector camera_image_pubs; + + //vector camera_image_pubs; + vector camera_image_pubs; vector camera_info_pubs; vector img_msgs; diff --git a/launch/acquisition.launch b/launch/acquisition.launch index b4712aa..ebb9e72 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -22,7 +22,7 @@ + args="" launch-prefix="xterm -e gdb --args"> diff --git a/params/test_params.yaml b/params/test_params.yaml index 11fda13..8cafc56 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -1,8 +1,8 @@ cam_ids: -- 17197555 +- 18080155 cam_aliases: - cam0 -master_cam: 17197555 +master_cam: 18080155 skip: 20 delay: 0 diff --git a/src/capture.cpp b/src/capture.cpp index d83dda9..2cfaa83 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -35,7 +35,7 @@ void handler(int i) { } -acquisition::Capture::Capture():nh_(),nh_pvt_ ("~") { +acquisition::Capture::Capture(): it_(nh_), nh_pvt_ ("~") { // struct sigaction sigIntHandler; @@ -91,7 +91,7 @@ acquisition::Capture::Capture():nh_(),nh_pvt_ ("~") { achieved_time_ = 0; // decimation_ = 1; - + CAM_ = 0; // default flag values @@ -133,6 +133,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_image_pubs.push_back(it_->advertise("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()); From 290c41ed2611c0571b4be9259129577b5dfbef41 Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 19 Dec 2018 11:04:12 -0500 Subject: [PATCH 06/30] Removing shared pointer for image transport and removing gdb --- include/capture.h | 2 +- launch/acquisition.launch | 2 +- params/test_params.yaml | 6 +++--- src/capture.cpp | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/capture.h b/include/capture.h index 76922d9..83b062a 100755 --- a/include/capture.h +++ b/include/capture.h @@ -123,7 +123,7 @@ namespace acquisition { ros::NodeHandle nh_; ros::NodeHandle nh_pvt_; //image_transport::ImageTransport it_; - std::shared_ptr it_; + image_transport::ImageTransport it_; ros::Publisher acquisition_pub; diff --git a/launch/acquisition.launch b/launch/acquisition.launch index ebb9e72..b4712aa 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -22,7 +22,7 @@ + args=""> diff --git a/params/test_params.yaml b/params/test_params.yaml index 8cafc56..d0b4c58 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -1,10 +1,10 @@ cam_ids: -- 18080155 +- 17197559 cam_aliases: - cam0 -master_cam: 18080155 +master_cam: 17197559 skip: 20 -delay: 0 +delay: 1 # Assign all the follwing via launch file to prevent confusion and conflict diff --git a/src/capture.cpp b/src/capture.cpp index 2cfaa83..40b0a1e 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -158,7 +158,7 @@ void acquisition::Capture::load_cameras() { cams.push_back(cam); - camera_image_pubs.push_back(it_->advertise("camera_array/"+cam_names_[j]+"/image_raw", 1)); + camera_image_pubs.push_back(it_.advertise("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()); From 08d6329bd114a25059fe0a1d748b2980c7d66aeb Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 19 Dec 2018 11:21:51 -0500 Subject: [PATCH 07/30] A couple little tweaks --- launch/acquisition.launch | 2 +- params/test_params.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/acquisition.launch b/launch/acquisition.launch index b4712aa..1ce4854 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -3,7 +3,7 @@ - + diff --git a/params/test_params.yaml b/params/test_params.yaml index d0b4c58..d08afc6 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -4,7 +4,7 @@ cam_aliases: - cam0 master_cam: 17197559 skip: 20 -delay: 1 +delay: 1.0 # Assign all the follwing via launch file to prevent confusion and conflict From 0444cf5ba418f4978e7a8b585530f65e63657640 Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 19 Dec 2018 11:34:43 -0500 Subject: [PATCH 08/30] removing gdb --- launch/acquisition.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/acquisition.launch b/launch/acquisition.launch index 0cb31af..1ce4854 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -22,7 +22,7 @@ + args=""> From f3fa0da4defedf34aa1a099601e7c35b47c1f6e6 Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 5 Dec 2018 18:29:09 -0500 Subject: [PATCH 09/30] image_transport added in - seg faults --- include/spinnaker_sdk_camera_driver/capture.h | 6 +++++- launch/acquisition.launch | 2 +- params/test_params.yaml | 4 ++-- src/capture.cpp | 8 +++++--- 4 files changed, 13 insertions(+), 7 deletions(-) diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h index fba1522..74a1340 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -12,6 +12,7 @@ #include "spinnaker_sdk_camera_driver/SpinnakerImageNames.h" #include +#include using namespace Spinnaker; using namespace Spinnaker::GenApi; @@ -121,9 +122,12 @@ namespace acquisition { // ros variables ros::NodeHandle nh_; ros::NodeHandle nh_pvt_; + //image_transport::ImageTransport it_; + std::shared_ptr it_; ros::Publisher acquisition_pub; - vector camera_image_pubs; + + vector camera_image_pubs; vector camera_info_pubs; vector img_msgs; diff --git a/launch/acquisition.launch b/launch/acquisition.launch index b4712aa..ebb9e72 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -22,7 +22,7 @@ + args="" launch-prefix="xterm -e gdb --args"> diff --git a/params/test_params.yaml b/params/test_params.yaml index 11fda13..8cafc56 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -1,8 +1,8 @@ cam_ids: -- 17197555 +- 18080155 cam_aliases: - cam0 -master_cam: 17197555 +master_cam: 18080155 skip: 20 delay: 0 diff --git a/src/capture.cpp b/src/capture.cpp index a8151c2..4e7b960 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -35,7 +35,7 @@ void handler(int i) { } -acquisition::Capture::Capture():nh_(),nh_pvt_ ("~") { +acquisition::Capture::Capture(): it_(nh_), nh_pvt_ ("~") { // struct sigaction sigIntHandler; @@ -91,7 +91,7 @@ acquisition::Capture::Capture():nh_(),nh_pvt_ ("~") { achieved_time_ = 0; // decimation_ = 1; - + CAM_ = 0; // default flag values @@ -133,6 +133,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_image_pubs.push_back(it_->advertise("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()); From b00e9dca0704f9fc9d1e1119722c61509a6bedd1 Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 5 Dec 2018 18:29:09 -0500 Subject: [PATCH 10/30] image_transport added in - seg faults --- include/spinnaker_sdk_camera_driver/capture.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h index 74a1340..31f8f98 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -126,7 +126,8 @@ namespace acquisition { std::shared_ptr it_; ros::Publisher acquisition_pub; - + + //vector camera_image_pubs; vector camera_image_pubs; vector camera_info_pubs; From a65fc29fca2f0b6ef2f0db9f7b31143eea333653 Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 19 Dec 2018 11:04:12 -0500 Subject: [PATCH 11/30] Removing shared pointer for image transport and removing gdb --- include/spinnaker_sdk_camera_driver/capture.h | 2 +- launch/acquisition.launch | 2 +- params/test_params.yaml | 6 +++--- src/capture.cpp | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h index 31f8f98..8aeefc3 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -123,7 +123,7 @@ namespace acquisition { ros::NodeHandle nh_; ros::NodeHandle nh_pvt_; //image_transport::ImageTransport it_; - std::shared_ptr it_; + image_transport::ImageTransport it_; ros::Publisher acquisition_pub; diff --git a/launch/acquisition.launch b/launch/acquisition.launch index ebb9e72..b4712aa 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -22,7 +22,7 @@ + args=""> diff --git a/params/test_params.yaml b/params/test_params.yaml index 8cafc56..d0b4c58 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -1,10 +1,10 @@ cam_ids: -- 18080155 +- 17197559 cam_aliases: - cam0 -master_cam: 18080155 +master_cam: 17197559 skip: 20 -delay: 0 +delay: 1 # Assign all the follwing via launch file to prevent confusion and conflict diff --git a/src/capture.cpp b/src/capture.cpp index 4e7b960..3296c19 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -158,7 +158,7 @@ void acquisition::Capture::load_cameras() { cams.push_back(cam); - camera_image_pubs.push_back(it_->advertise("camera_array/"+cam_names_[j]+"/image_raw", 1)); + camera_image_pubs.push_back(it_.advertise("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()); From e0722c1fd35bf18fc42f81182d43945674787ebb Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 19 Dec 2018 11:21:51 -0500 Subject: [PATCH 12/30] A couple little tweaks --- launch/acquisition.launch | 2 +- params/test_params.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/acquisition.launch b/launch/acquisition.launch index b4712aa..1ce4854 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -3,7 +3,7 @@ - + diff --git a/params/test_params.yaml b/params/test_params.yaml index d0b4c58..d08afc6 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -4,7 +4,7 @@ cam_aliases: - cam0 master_cam: 17197559 skip: 20 -delay: 1 +delay: 1.0 # Assign all the follwing via launch file to prevent confusion and conflict From 455844d3b2cdf41fe3daf80d2c1ee8848f1d70ea Mon Sep 17 00:00:00 2001 From: pushyamikaveti Date: Wed, 19 Dec 2018 22:11:14 -0500 Subject: [PATCH 13/30] support to change target grey value and dynamic reconfigure --- CMakeLists.txt | 10 +++++-- cfg/spinnaker_cam.cfg | 13 +++++++++ include/spinnaker_sdk_camera_driver/camera.h | 1 + include/spinnaker_sdk_camera_driver/capture.h | 10 +++++-- src/acquisition_node.cpp | 5 +++- src/camera.cpp | 13 +++++++++ src/capture.cpp | 27 +++++++++++++++++-- 7 files changed, 72 insertions(+), 7 deletions(-) create mode 100755 cfg/spinnaker_cam.cfg diff --git a/CMakeLists.txt b/CMakeLists.txt index f9cd37d..f7a97db 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport sensor_msgs + dynamic_reconfigure ) #find_package(PCL REQUIRED) @@ -46,6 +47,11 @@ add_message_files( SpinnakerImageNames.msg ) +generate_dynamic_reconfigure_options( + cfg/spinnaker_cam.cfg + +) + generate_messages( DEPENDENCIES std_msgs @@ -75,11 +81,11 @@ add_library (acquilib SHARED 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_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 diff --git a/cfg/spinnaker_cam.cfg b/cfg/spinnaker_cam.cfg new file mode 100755 index 0000000..6c315e0 --- /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, 3, "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 8aeefc3..6efe0b1 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -7,8 +7,13 @@ #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 @@ -59,6 +64,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(); @@ -124,9 +130,9 @@ namespace acquisition { ros::NodeHandle nh_pvt_; //image_transport::ImageTransport it_; image_transport::ImageTransport it_; + dynamic_reconfigure::Server server; ros::Publisher acquisition_pub; - //vector camera_image_pubs; vector camera_image_pubs; vector camera_info_pubs; 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/camera.cpp b/src/camera.cpp index c189032..9d60fac 100755 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -301,6 +301,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)){ diff --git a/src/capture.cpp b/src/capture.cpp index 3296c19..6aa59a3 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -113,6 +113,12 @@ acquisition::Capture::Capture(): it_(nh_), nh_pvt_ ("~") { //initializing the ros publisher acquisition_pub = nh_.advertise("camera", 1000); + //dynamic reconfigure + //dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; + + f = boost::bind(&acquisition::Capture::dynamicReconfigureCallback,this, _1, _2); + server.setCallback(f); } void acquisition::Capture::load_cameras() { @@ -769,7 +775,7 @@ void acquisition::Capture::run_soft_trig() { } double disp_time_ = ros::Time::now().toSec() - t; - + // Call update functions if (!MANUAL_TRIGGER_) { cams[MASTER_CAM_].trigger(); @@ -794,7 +800,7 @@ void acquisition::Capture::run_soft_trig() { } if (EXPORT_TO_ROS_) export_to_ROS(); - + //cams[MASTER_CAM_].targetGreyValueTest(); // ros publishing messages acquisition_pub.publish(mesg); @@ -1010,3 +1016,20 @@ std::string acquisition::Capture::todays_date() std::string td(out); return td; } + +void acquisition::Capture::dynamicReconfigureCallback(spinnaker_sdk_camera_driver::spinnaker_camConfig &config, uint32_t level){ + ROS_INFO_STREAM("Dynamic Reconfigure: Target grey value : " << config.target_grey_val <<"Exposure "< 0){ + cams[MASTER_CAM_].setEnumValue("ExposureAuto", "Off"); + cams[MASTER_CAM_].setEnumValue("ExposureMode", "Timed"); + cams[MASTER_CAM_].setFloatValue("ExposureTime", config.exposure_time); + } + + +} \ No newline at end of file From b836452e226f202b90eeae536ca8002fff681eeb Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 5 Dec 2018 18:29:09 -0500 Subject: [PATCH 14/30] image_transport added in - seg faults --- include/spinnaker_sdk_camera_driver/capture.h | 6 +++++- launch/acquisition.launch | 2 +- params/test_params.yaml | 4 ++-- src/capture.cpp | 8 +++++--- 4 files changed, 13 insertions(+), 7 deletions(-) diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h index 5a53270..5b94fbc 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -12,6 +12,7 @@ #include "spinnaker_sdk_camera_driver/SpinnakerImageNames.h" #include +#include using namespace Spinnaker; using namespace Spinnaker::GenApi; @@ -123,9 +124,12 @@ namespace acquisition { // ros variables ros::NodeHandle nh_; ros::NodeHandle nh_pvt_; + //image_transport::ImageTransport it_; + std::shared_ptr it_; ros::Publisher acquisition_pub; - vector camera_image_pubs; + + vector camera_image_pubs; vector camera_info_pubs; vector img_msgs; diff --git a/launch/acquisition.launch b/launch/acquisition.launch index b4712aa..ebb9e72 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -22,7 +22,7 @@ + args="" launch-prefix="xterm -e gdb --args"> diff --git a/params/test_params.yaml b/params/test_params.yaml index 11fda13..8cafc56 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -1,8 +1,8 @@ cam_ids: -- 17197555 +- 18080155 cam_aliases: - cam0 -master_cam: 17197555 +master_cam: 18080155 skip: 20 delay: 0 diff --git a/src/capture.cpp b/src/capture.cpp index 0cf5d28..88af161 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -35,7 +35,7 @@ void handler(int i) { } -acquisition::Capture::Capture():nh_(),nh_pvt_ ("~") { +acquisition::Capture::Capture(): it_(nh_), nh_pvt_ ("~") { // struct sigaction sigIntHandler; @@ -91,7 +91,7 @@ acquisition::Capture::Capture():nh_(),nh_pvt_ ("~") { achieved_time_ = 0; // decimation_ = 1; - + CAM_ = 0; // default flag values @@ -133,6 +133,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_image_pubs.push_back(it_->advertise("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()); From 789c7396a413de0a776b855c1cbe527453070cdd Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 5 Dec 2018 18:29:09 -0500 Subject: [PATCH 15/30] image_transport added in - seg faults --- include/spinnaker_sdk_camera_driver/capture.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h index 5b94fbc..23369a9 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -129,6 +129,7 @@ namespace acquisition { ros::Publisher acquisition_pub; + //vector camera_image_pubs; vector camera_image_pubs; vector camera_info_pubs; From c5d09eb23b6d1325b91e7f6242c3bac31b3267c7 Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 19 Dec 2018 11:04:12 -0500 Subject: [PATCH 16/30] Removing shared pointer for image transport and removing gdb --- include/spinnaker_sdk_camera_driver/capture.h | 2 +- launch/acquisition.launch | 2 +- params/test_params.yaml | 6 +++--- src/capture.cpp | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h index 23369a9..7f69f58 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -125,7 +125,7 @@ namespace acquisition { ros::NodeHandle nh_; ros::NodeHandle nh_pvt_; //image_transport::ImageTransport it_; - std::shared_ptr it_; + image_transport::ImageTransport it_; ros::Publisher acquisition_pub; diff --git a/launch/acquisition.launch b/launch/acquisition.launch index ebb9e72..b4712aa 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -22,7 +22,7 @@ + args=""> diff --git a/params/test_params.yaml b/params/test_params.yaml index 8cafc56..d0b4c58 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -1,10 +1,10 @@ cam_ids: -- 18080155 +- 17197559 cam_aliases: - cam0 -master_cam: 18080155 +master_cam: 17197559 skip: 20 -delay: 0 +delay: 1 # Assign all the follwing via launch file to prevent confusion and conflict diff --git a/src/capture.cpp b/src/capture.cpp index 88af161..f784498 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -158,7 +158,7 @@ void acquisition::Capture::load_cameras() { cams.push_back(cam); - camera_image_pubs.push_back(it_->advertise("camera_array/"+cam_names_[j]+"/image_raw", 1)); + camera_image_pubs.push_back(it_.advertise("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()); From c2a1e3f4848dc60d0b9a06d1557b08e5d4525078 Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 19 Dec 2018 11:21:51 -0500 Subject: [PATCH 17/30] A couple little tweaks --- launch/acquisition.launch | 2 +- params/test_params.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/acquisition.launch b/launch/acquisition.launch index b4712aa..1ce4854 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -3,7 +3,7 @@ - + diff --git a/params/test_params.yaml b/params/test_params.yaml index d0b4c58..d08afc6 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -4,7 +4,7 @@ cam_aliases: - cam0 master_cam: 17197559 skip: 20 -delay: 1 +delay: 1.0 # Assign all the follwing via launch file to prevent confusion and conflict From cd70f89514b42414b3452caf27913177de18176d Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 5 Dec 2018 18:29:09 -0500 Subject: [PATCH 18/30] image_transport added in - seg faults --- include/spinnaker_sdk_camera_driver/capture.h | 2 +- launch/acquisition.launch | 2 +- src/capture.cpp | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h index 7f69f58..8e5eadd 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -124,12 +124,12 @@ namespace acquisition { // ros variables ros::NodeHandle nh_; ros::NodeHandle nh_pvt_; - //image_transport::ImageTransport it_; image_transport::ImageTransport it_; ros::Publisher acquisition_pub; //vector camera_image_pubs; + vector camera_image_pubs; vector camera_info_pubs; diff --git a/launch/acquisition.launch b/launch/acquisition.launch index 1ce4854..0cb31af 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -22,7 +22,7 @@ + args="" launch-prefix="xterm -e gdb --args"> diff --git a/src/capture.cpp b/src/capture.cpp index f784498..d292e6e 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -159,6 +159,7 @@ void acquisition::Capture::load_cameras() { cams.push_back(cam); camera_image_pubs.push_back(it_.advertise("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()); From 2018c1a1491820a9ac3ebb20775e2f5177f67ef0 Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 5 Dec 2018 18:29:09 -0500 Subject: [PATCH 19/30] image_transport added in - seg faults --- include/spinnaker_sdk_camera_driver/capture.h | 1 - 1 file changed, 1 deletion(-) diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h index 8e5eadd..e1bcb48 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -129,7 +129,6 @@ namespace acquisition { ros::Publisher acquisition_pub; //vector camera_image_pubs; - vector camera_image_pubs; vector camera_info_pubs; From 7a047c11127be96c2a2a5436e292bba4dec45f66 Mon Sep 17 00:00:00 2001 From: Vikrant Shah Date: Wed, 19 Dec 2018 11:04:12 -0500 Subject: [PATCH 20/30] Removing shared pointer for image transport and removing gdb --- include/spinnaker_sdk_camera_driver/capture.h | 1 + launch/acquisition.launch | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h index e1bcb48..7f69f58 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -124,6 +124,7 @@ namespace acquisition { // ros variables ros::NodeHandle nh_; ros::NodeHandle nh_pvt_; + //image_transport::ImageTransport it_; image_transport::ImageTransport it_; ros::Publisher acquisition_pub; diff --git a/launch/acquisition.launch b/launch/acquisition.launch index 0cb31af..1ce4854 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -22,7 +22,7 @@ + args=""> From 419ea3db24c81e31bba974dcebb1a4abd9ca5eac Mon Sep 17 00:00:00 2001 From: pushyamikaveti Date: Wed, 19 Dec 2018 22:11:14 -0500 Subject: [PATCH 21/30] support to change target grey value and dynamic reconfigure --- CMakeLists.txt | 10 +++++-- cfg/spinnaker_cam.cfg | 13 +++++++++ include/spinnaker_sdk_camera_driver/camera.h | 1 + include/spinnaker_sdk_camera_driver/capture.h | 10 +++++-- src/acquisition_node.cpp | 5 +++- src/camera.cpp | 13 +++++++++ src/capture.cpp | 27 +++++++++++++++++-- 7 files changed, 72 insertions(+), 7 deletions(-) create mode 100755 cfg/spinnaker_cam.cfg diff --git a/CMakeLists.txt b/CMakeLists.txt index f9cd37d..f7a97db 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport sensor_msgs + dynamic_reconfigure ) #find_package(PCL REQUIRED) @@ -46,6 +47,11 @@ add_message_files( SpinnakerImageNames.msg ) +generate_dynamic_reconfigure_options( + cfg/spinnaker_cam.cfg + +) + generate_messages( DEPENDENCIES std_msgs @@ -75,11 +81,11 @@ add_library (acquilib SHARED 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_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 diff --git a/cfg/spinnaker_cam.cfg b/cfg/spinnaker_cam.cfg new file mode 100755 index 0000000..6c315e0 --- /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, 3, "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 7f69f58..cf638d4 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -7,8 +7,13 @@ #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 @@ -59,6 +64,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(); @@ -126,9 +132,9 @@ namespace acquisition { ros::NodeHandle nh_pvt_; //image_transport::ImageTransport it_; image_transport::ImageTransport it_; + dynamic_reconfigure::Server server; ros::Publisher acquisition_pub; - //vector camera_image_pubs; vector camera_image_pubs; vector camera_info_pubs; 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/camera.cpp b/src/camera.cpp index c189032..9d60fac 100755 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -301,6 +301,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)){ diff --git a/src/capture.cpp b/src/capture.cpp index d292e6e..620103b 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -113,6 +113,12 @@ acquisition::Capture::Capture(): it_(nh_), nh_pvt_ ("~") { //initializing the ros publisher acquisition_pub = nh_.advertise("camera", 1000); + //dynamic reconfigure + //dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; + + f = boost::bind(&acquisition::Capture::dynamicReconfigureCallback,this, _1, _2); + server.setCallback(f); } void acquisition::Capture::load_cameras() { @@ -819,7 +825,7 @@ void acquisition::Capture::run_soft_trig() { } double disp_time_ = ros::Time::now().toSec() - t; - + // Call update functions if (!MANUAL_TRIGGER_) { cams[MASTER_CAM_].trigger(); @@ -844,7 +850,7 @@ void acquisition::Capture::run_soft_trig() { } if (EXPORT_TO_ROS_) export_to_ROS(); - + //cams[MASTER_CAM_].targetGreyValueTest(); // ros publishing messages acquisition_pub.publish(mesg); @@ -1060,3 +1066,20 @@ std::string acquisition::Capture::todays_date() std::string td(out); return td; } + +void acquisition::Capture::dynamicReconfigureCallback(spinnaker_sdk_camera_driver::spinnaker_camConfig &config, uint32_t level){ + ROS_INFO_STREAM("Dynamic Reconfigure: Target grey value : " << config.target_grey_val <<"Exposure "< 0){ + cams[MASTER_CAM_].setEnumValue("ExposureAuto", "Off"); + cams[MASTER_CAM_].setEnumValue("ExposureMode", "Timed"); + cams[MASTER_CAM_].setFloatValue("ExposureTime", config.exposure_time); + } + + +} \ No newline at end of file From 70e4f4b1d73f60497ff437c2307983e84cd80fa3 Mon Sep 17 00:00:00 2001 From: pushyamikaveti Date: Thu, 10 Jan 2019 18:37:31 -0500 Subject: [PATCH 22/30] added support for nodelets --- CMakeLists.txt | 17 +++- include/spinnaker_sdk_camera_driver/capture.h | 1 + .../capture_nodelet.h | 25 ++++++ launch/acquisition.launch | 2 +- launch/acquisition_nodelet.launch | 55 +++++++++++++ nodelet_plugins.xml | 7 ++ package.xml | 5 ++ params/test_params.yaml | 27 ++++++- src/acquisition_nodelet.cpp | 29 +++++++ src/capture.cpp | 79 +++++++++++++++++++ src/capture_nodelet.cpp | 29 +++++++ src/nodelet_test.cpp | 33 ++++++++ 12 files changed, 305 insertions(+), 4 deletions(-) create mode 100644 include/spinnaker_sdk_camera_driver/capture_nodelet.h create mode 100644 launch/acquisition_nodelet.launch create mode 100644 nodelet_plugins.xml create mode 100644 src/acquisition_nodelet.cpp create mode 100644 src/capture_nodelet.cpp create mode 100644 src/nodelet_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index f7a97db..eb7e221 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(catkin REQUIRED COMPONENTS image_transport sensor_msgs dynamic_reconfigure + nodelet ) #find_package(PCL REQUIRED) @@ -59,7 +60,7 @@ generate_messages( catkin_package( INCLUDE_DIRS include - CATKIN_DEPENDS roscpp std_msgs message_runtime + CATKIN_DEPENDS roscpp std_msgs message_runtime nodelet DEPENDS OpenCV LibUnwind ) @@ -84,11 +85,23 @@ add_library (acquilib SHARED 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) +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}) -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 ${LIBS} ${catkin_LIBRARIES}) + +install(TARGETS acquilib acquisition_node capture_nodelet acquisition_nodelet ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h index cf638d4..95170b7 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -33,6 +33,7 @@ namespace acquisition { ~Capture(); Capture(); + Capture( ros::NodeHandle node,ros::NodeHandle private_nh); void load_cameras(); 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..6a572d1 --- /dev/null +++ b/include/spinnaker_sdk_camera_driver/capture_nodelet.h @@ -0,0 +1,25 @@ +// +// Created by auv 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(){} + virtual void onInit(); + //boost::shared_ptr inst_; + + }; + +} + diff --git a/launch/acquisition.launch b/launch/acquisition.launch index 1ce4854..83f9a2e 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -13,7 +13,7 @@ - + diff --git a/launch/acquisition_nodelet.launch b/launch/acquisition_nodelet.launch new file mode 100644 index 0000000..f874627 --- /dev/null +++ b/launch/acquisition_nodelet.launch @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml new file mode 100644 index 0000000..e65439b --- /dev/null +++ b/nodelet_plugins.xml @@ -0,0 +1,7 @@ + + + + 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 d08afc6..a70d69e 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -1,7 +1,9 @@ cam_ids: - 17197559 +- 17197547 cam_aliases: - cam0 +- cam1 master_cam: 17197559 skip: 20 delay: 1.0 @@ -13,6 +15,29 @@ delay: 1.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_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/capture.cpp b/src/capture.cpp index 810055e..351598c 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -121,6 +121,85 @@ acquisition::Capture::Capture(): it_(nh_), nh_pvt_ ("~") { server.setCallback(f); } +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 + //dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; + + f = boost::bind(&acquisition::Capture::dynamicReconfigureCallback,this, _1, _2); + server.setCallback(f); +} + + void acquisition::Capture::load_cameras() { // Retrieve list of cameras from the system diff --git a/src/capture_nodelet.cpp b/src/capture_nodelet.cpp new file mode 100644 index 0000000..3db9d79 --- /dev/null +++ b/src/capture_nodelet.cpp @@ -0,0 +1,29 @@ +// +// Created by auv 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_DEBUG("Initializing nodelet"); + acquisition::Capture cobj(getNodeHandle(), getPrivateNodeHandle()); + //inst_.reset(new Capture(getNodeHandle(), getPrivateNodeHandle())); + cobj.init_array(); + cobj.run(); + } + + + +} + +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..d7423da --- /dev/null +++ b/src/nodelet_test.cpp @@ -0,0 +1,33 @@ +// +// Created by auv on 1/10/19. +// + +#include "ros/ros.h" +#include "spinnaker_sdk_camera_driver/std_include.h" +#include "std_msgs/String.h" +#include +#include "sensor_msgs/Image.h" + +/** + * This tutorial demonstrates simple receipt of messages over the ROS system. + */ +void chatterCallback(const sensor_msgs::Image::ConstPtr& msg) +{ + ROS_INFO_STREAM("diff time is "<< ros::Time::now().toSec() - msg->header.stamp.toSec()); +} + +int main(int argc, char **argv) +{ + + ros::init(argc, argv, "listener"); + + + ros::NodeHandle node; + + image_transport::ImageTransport it_(node); + image_transport::Subscriber image_sub_ = it_.subscribe("/camera_array/cam0/image_raw",1, chatterCallback) ; + + ros::spin(); + + return 0; +} \ No newline at end of file From 15573550d13fadc0714d44a7baae92505a0b710a Mon Sep 17 00:00:00 2001 From: pushyamikaveti Date: Wed, 16 Jan 2019 15:11:53 -0500 Subject: [PATCH 23/30] debugging nodelets and added a tester node to measure performance --- CMakeLists.txt | 6 ++-- .../capture_nodelet.h | 13 ++++++-- .../spinnaker_sdk_camera_driver/std_include.h | 3 ++ .../subscriber_nodelet.h | 26 +++++++++++++++ launch/acquisition.launch | 2 +- launch/acquisition_nodelet.launch | 6 ++-- nodelet_plugins.xml | 5 +++ src/capture_nodelet.cpp | 20 ++++++----- src/nodelet_test.cpp | 24 ++++---------- src/subscriber_nodelet.cpp | 33 +++++++++++++++++++ 10 files changed, 103 insertions(+), 35 deletions(-) create mode 100644 include/spinnaker_sdk_camera_driver/subscriber_nodelet.h create mode 100644 src/subscriber_nodelet.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index eb7e221..cbfe1f3 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -85,7 +85,7 @@ add_library (acquilib SHARED 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) +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) @@ -99,9 +99,9 @@ target_link_libraries (acquisition_nodelet acquilib capture_nodelet ${LIBS} ${ca add_executable (nodelet_test src/nodelet_test.cpp) add_dependencies(nodelet_test ${catkin_EXPORTED_TARGETS}) -target_link_libraries (nodelet_test ${LIBS} ${catkin_LIBRARIES}) +target_link_libraries (nodelet_test capture_nodelet ${LIBS} ${catkin_LIBRARIES}) -install(TARGETS acquilib acquisition_node capture_nodelet acquisition_nodelet +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/include/spinnaker_sdk_camera_driver/capture_nodelet.h b/include/spinnaker_sdk_camera_driver/capture_nodelet.h index 6a572d1..0955c99 100644 --- a/include/spinnaker_sdk_camera_driver/capture_nodelet.h +++ b/include/spinnaker_sdk_camera_driver/capture_nodelet.h @@ -1,5 +1,5 @@ // -// Created by auv on 1/10/19. +// Created by pushyami on 1/10/19. // #ifndef SPINNAKER_SDK_CAMERA_DRIVER_CAPTURE_NODELET_H @@ -15,9 +15,16 @@ namespace acquisition { public: capture_nodelet(){} - ~capture_nodelet(){} + ~capture_nodelet(){ + if (pubThread_) { + pubThread_->interrupt(); + pubThread_->join(); + } + } virtual void onInit(); - //boost::shared_ptr inst_; + + 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 83f9a2e..1ce4854 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -13,7 +13,7 @@ - + diff --git a/launch/acquisition_nodelet.launch b/launch/acquisition_nodelet.launch index f874627..2b3468b 100644 --- a/launch/acquisition_nodelet.launch +++ b/launch/acquisition_nodelet.launch @@ -24,7 +24,7 @@ --> - + @@ -50,6 +50,8 @@ - + + diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index e65439b..ad24255 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -4,4 +4,9 @@ This is my nodelet. + + + This is my nodelet. + + diff --git a/src/capture_nodelet.cpp b/src/capture_nodelet.cpp index 3db9d79..d1d6492 100644 --- a/src/capture_nodelet.cpp +++ b/src/capture_nodelet.cpp @@ -1,5 +1,5 @@ // -// Created by auv on 1/10/19. +// Created by pushyami on 1/10/19. // #include @@ -12,14 +12,16 @@ namespace acquisition void capture_nodelet::onInit() { //spinners - ros::AsyncSpinner spinner(0); // Use max cores possible for mt - spinner.start(); - - NODELET_DEBUG("Initializing nodelet"); - acquisition::Capture cobj(getNodeHandle(), getPrivateNodeHandle()); - //inst_.reset(new Capture(getNodeHandle(), getPrivateNodeHandle())); - cobj.init_array(); - cobj.run(); + //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"); } diff --git a/src/nodelet_test.cpp b/src/nodelet_test.cpp index d7423da..4e0de50 100644 --- a/src/nodelet_test.cpp +++ b/src/nodelet_test.cpp @@ -4,30 +4,20 @@ #include "ros/ros.h" #include "spinnaker_sdk_camera_driver/std_include.h" -#include "std_msgs/String.h" +#include #include #include "sensor_msgs/Image.h" -/** - * This tutorial demonstrates simple receipt of messages over the ROS system. - */ -void chatterCallback(const sensor_msgs::Image::ConstPtr& msg) -{ - ROS_INFO_STREAM("diff time is "<< ros::Time::now().toSec() - msg->header.stamp.toSec()); -} - int main(int argc, char **argv) { - ros::init(argc, argv, "listener"); - - - ros::NodeHandle node; - - image_transport::ImageTransport it_(node); - image_transport::Subscriber image_sub_ = it_.subscribe("/camera_array/cam0/image_raw",1, chatterCallback) ; + ros::init(argc, argv, "subscriber_nodelet"); - ros::spin(); + 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_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) From d78e4ea89cf977772977a90ee5c80b2fc548aee7 Mon Sep 17 00:00:00 2001 From: mithun Date: Mon, 25 Feb 2019 22:14:07 -0500 Subject: [PATCH 24/30] addind params --- launch/acquisition.launch | 2 +- .../{test_params.yaml => cam_17197553.yaml} | 10 ++++--- params/cam_17197558.yaml | 28 +++++++++++++++++++ src/subscriber_node.cpp | 24 ++++++++++++++++ 4 files changed, 59 insertions(+), 5 deletions(-) rename params/{test_params.yaml => cam_17197553.yaml} (69%) create mode 100644 params/cam_17197558.yaml create mode 100755 src/subscriber_node.cpp diff --git a/launch/acquisition.launch b/launch/acquisition.launch index e1685f8..e07b106 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -21,7 +21,7 @@ - + diff --git a/params/test_params.yaml b/params/cam_17197553.yaml similarity index 69% rename from params/test_params.yaml rename to params/cam_17197553.yaml index 6883d81..245314b 100644 --- a/params/test_params.yaml +++ b/params/cam_17197553.yaml @@ -1,18 +1,20 @@ cam_ids: -- 17197556 +- 17197553 cam_aliases: - cam0 -master_cam: 17197556 +master_cam: 17197553 skip: 5 delay: 0 #Camera info message details distortion_model: plumb_bob distortion_coeffs: -- [-0.17465, 0.03501, 0.00064, -0.00036] + - [-0.22001, 0.16693, 0.00261, -0.00099, 0.00000] + #- [-0.17465, 0.03501, 0.00064, -0.00036] #specified as [fx 0 cx 0 fy cy 0 0 1] intrinsic_coeffs: -- [638.71023, 0.0, 315.25919, 0.0, 641.21041, 248.26077, 0.0, 0.0, 1.0] + - [1278.71483, 0.0, 623.08283, 0.0, 1284.06247, 515.71835, 0.0, 0.0, 1.0] + #- [638.71023, 0.0, 315.25919, 0.0, 641.21041, 248.26077, 0.0, 0.0, 1.0] # Assign all the follwing via launch file to prevent confusion and conflict diff --git a/params/cam_17197558.yaml b/params/cam_17197558.yaml new file mode 100644 index 0000000..ad3d123 --- /dev/null +++ b/params/cam_17197558.yaml @@ -0,0 +1,28 @@ +cam_ids: +- 17197558 +cam_aliases: +- cam0 +master_cam: 17197558 +skip: 5 +delay: 0 + +#Camera info message details +distortion_model: plumb_bob +distortion_coeffs: + - [-0.22001, 0.16693, 0.00261, -0.00099, 0.00000] + #- [-0.17465, 0.03501, 0.00064, -0.00036] +#specified as [fx 0 cx 0 fy cy 0 0 1] +intrinsic_coeffs: + - [1278.71483, 0.0, 623.08283, 0.0, 1284.06247, 515.71835, 0.0, 0.0, 1.0] + #- [638.71023, 0.0, 315.25919, 0.0, 641.21041, 248.26077, 0.0, 0.0, 1.0] + +# 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/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(); +} From 300396d5a8b028c0c30389310e0f70337ac1e815 Mon Sep 17 00:00:00 2001 From: mithun Date: Tue, 26 Feb 2019 03:56:16 -0500 Subject: [PATCH 25/30] added respawn --- launch/acquisition.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/acquisition.launch b/launch/acquisition.launch index e07b106..992a756 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -24,7 +24,7 @@ - + From c26ee1ac3c80fee6f53e826743b5f2393b0a4285 Mon Sep 17 00:00:00 2001 From: Shri1304 Date: Wed, 12 Jun 2019 01:08:25 -0400 Subject: [PATCH 26/30] fix CMakeList for aarch64 flag --- CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 563cf83..1e7bafb 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,9 +39,9 @@ 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 arm) +if(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm) message("Detected ARM architecture") -endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES arm) +endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm) find_package(Boost REQUIRED) if(Boost_FOUND) @@ -87,7 +87,7 @@ if(${CMAKE_SYSTEM_PROCESSOR} MATCHES x86_64 OR x86_32) set (LIBS ${LibUnwind_LIBRARIES} Spinnaker ${OpenCV_LIBS} ${Boost_GENERAL}) endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES x86_64 OR x86_32) -if(${CMAKE_SYSTEM_PROCESSOR} MATCHES arm) +if(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm) catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS roscpp std_msgs message_runtime nodelet @@ -106,7 +106,7 @@ if(${CMAKE_SYSTEM_PROCESSOR} MATCHES arm) set (LIBS Spinnaker ${OpenCV_LIBS} ${Boost_GENERAL}) -endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES arm) +endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm) add_library (acquilib SHARED src/capture.cpp From 9a9d03ce2e155745a1e27cab4e57102f436338f5 Mon Sep 17 00:00:00 2001 From: Shri1304 Date: Wed, 12 Jun 2019 01:12:50 -0400 Subject: [PATCH 27/30] cleaned acquisition_nodelet launch file and added nodelet_test laucnh file for the same --- launch/acquisition_nodelet.launch | 4 +-- launch/nodelet_test.launch | 57 +++++++++++++++++++++++++++++++ 2 files changed, 59 insertions(+), 2 deletions(-) create mode 100644 launch/nodelet_test.launch diff --git a/launch/acquisition_nodelet.launch b/launch/acquisition_nodelet.launch index c880300..a7ff269 100644 --- a/launch/acquisition_nodelet.launch +++ b/launch/acquisition_nodelet.launch @@ -51,7 +51,7 @@ - + diff --git a/launch/nodelet_test.launch b/launch/nodelet_test.launch new file mode 100644 index 0000000..fa31da0 --- /dev/null +++ b/launch/nodelet_test.launch @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From c4b8793c4d8db4a695ff4ec5a432e65167d4975d Mon Sep 17 00:00:00 2001 From: mithunvarma Date: Wed, 12 Jun 2019 14:23:51 -0400 Subject: [PATCH 28/30] changed Readme on instructions to use nodelets and changed launch files for nodelets and added example for multiple nodelets --- README.md | 24 +++++++++++ launch/acquisition_nodelet.launch | 26 +++++++----- launch/multiple_nodelet_example.launch | 20 +++++++++ launch/nodelet_test.launch | 56 ++++++++++++++------------ params/cam_17197558.yaml | 28 ------------- 5 files changed, 89 insertions(+), 65 deletions(-) create mode 100644 launch/multiple_nodelet_example.launch delete mode 100644 params/cam_17197558.yaml diff --git a/README.md b/README.md index 009fbdc..efaaffd 100644 --- a/README.md +++ b/README.md @@ -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,14 @@ 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_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 ``` @@ -88,6 +102,14 @@ This is the names that would be given to the cameras for filenames and rostopics * ~delay (float) Secs to wait in the deinit/init sequence + +### 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) * ~image_height (int) @@ -100,9 +122,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 Monocular camera case, projection_coeffs if 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/launch/acquisition_nodelet.launch b/launch/acquisition_nodelet.launch index a7ff269..1f70eb9 100644 --- a/launch/acquisition_nodelet.launch +++ b/launch/acquisition_nodelet.launch @@ -2,7 +2,7 @@ - + @@ -20,15 +20,21 @@ - + + + - + + - + + + + + @@ -50,8 +56,6 @@ - - - + + diff --git a/launch/multiple_nodelet_example.launch b/launch/multiple_nodelet_example.launch new file mode 100644 index 0000000..e12f59a --- /dev/null +++ b/launch/multiple_nodelet_example.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + diff --git a/launch/nodelet_test.launch b/launch/nodelet_test.launch index fa31da0..f8a4886 100644 --- a/launch/nodelet_test.launch +++ b/launch/nodelet_test.launch @@ -2,35 +2,39 @@ - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + - + + + - - + + + + + + override what is in the yaml file. Thus use it to set parameters camer_array configuration params --> @@ -50,8 +54,8 @@ - + + - + args="load acquisition/subscriber_nodelet $(arg nodelet_manager_name)" output="screen" /> diff --git a/params/cam_17197558.yaml b/params/cam_17197558.yaml deleted file mode 100644 index ad3d123..0000000 --- a/params/cam_17197558.yaml +++ /dev/null @@ -1,28 +0,0 @@ -cam_ids: -- 17197558 -cam_aliases: -- cam0 -master_cam: 17197558 -skip: 5 -delay: 0 - -#Camera info message details -distortion_model: plumb_bob -distortion_coeffs: - - [-0.22001, 0.16693, 0.00261, -0.00099, 0.00000] - #- [-0.17465, 0.03501, 0.00064, -0.00036] -#specified as [fx 0 cx 0 fy cy 0 0 1] -intrinsic_coeffs: - - [1278.71483, 0.0, 623.08283, 0.0, 1284.06247, 515.71835, 0.0, 0.0, 1.0] - #- [638.71023, 0.0, 315.25919, 0.0, 641.21041, 248.26077, 0.0, 0.0, 1.0] - -# 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 From a3008106ec7513b9ddbce38b286843f7c26c9914 Mon Sep 17 00:00:00 2001 From: mithun Varma Diddi Date: Wed, 12 Jun 2019 14:29:00 -0400 Subject: [PATCH 29/30] update Readme for typos --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index efaaffd..0152559 100644 --- a/README.md +++ b/README.md @@ -14,7 +14,7 @@ The pre-requisites for this repo include: * ros-kinetic-cv-bridge * ros-kinetic-image-transport -#### Incase of x86_64 or x86_32 architecture, install the following: #### +# Incase of x86_64 or x86_32 architecture, install the following: * libunwind-dev ```bash @@ -122,7 +122,7 @@ 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 Monocular camera case, projection_coeffs if not set, intrinsic_coeff will be used to set P[1:3,1:3]=K + 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. From f68714966c719c03ee05f647ecb9f30f75b02ac8 Mon Sep 17 00:00:00 2001 From: mithunvarma Date: Wed, 12 Jun 2019 20:35:54 -0400 Subject: [PATCH 30/30] will make changes as suggested after review in pl #30, changing names of launch files, changing dynamic recfg server namespace to fix ns issue for nodelets, nodelet manager starts by default in acquisition.launch --- README.md | 22 ++++++++++--- include/spinnaker_sdk_camera_driver/capture.h | 2 +- launch/acquisition.launch | 28 +++++++++------- ...nodelet.launch => acquisition_node.launch} | 32 ++++++------------- launch/multiple_nodelet_example.launch | 4 +-- ...unch => nodelet_subscriber_example.launch} | 5 ++- params/test_params.yaml | 16 +++++----- src/capture.cpp | 26 ++++++++------- 8 files changed, 72 insertions(+), 63 deletions(-) rename launch/{acquisition_nodelet.launch => acquisition_node.launch} (69%) rename launch/{nodelet_test.launch => nodelet_subscriber_example.launch} (95%) diff --git a/README.md b/README.md index cbcf768..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. @@ -64,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) @@ -87,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) @@ -99,9 +99,21 @@ 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. diff --git a/include/spinnaker_sdk_camera_driver/capture.h b/include/spinnaker_sdk_camera_driver/capture.h index 8c7239e..e248af0 100755 --- a/include/spinnaker_sdk_camera_driver/capture.h +++ b/include/spinnaker_sdk_camera_driver/capture.h @@ -133,7 +133,7 @@ namespace acquisition { ros::NodeHandle nh_pvt_; //image_transport::ImageTransport it_; image_transport::ImageTransport it_; - dynamic_reconfigure::Server server; + dynamic_reconfigure::Server* dynamicReCfgServer_; ros::Publisher acquisition_pub; //vector camera_image_pubs; diff --git a/launch/acquisition.launch b/launch/acquisition.launch index 8833b2e..31c29c9 100644 --- a/launch/acquisition.launch +++ b/launch/acquisition.launch @@ -2,11 +2,10 @@ - - + - + @@ -14,25 +13,32 @@ - + - - + + + + + + + + + override what is in the yaml file. Thus use it to set parameters camer_array configuration params --> - + - @@ -45,6 +51,6 @@ - - + + diff --git a/launch/acquisition_nodelet.launch b/launch/acquisition_node.launch similarity index 69% rename from launch/acquisition_nodelet.launch rename to launch/acquisition_node.launch index 1f70eb9..8ea0338 100644 --- a/launch/acquisition_nodelet.launch +++ b/launch/acquisition_node.launch @@ -2,10 +2,11 @@ - + + - + @@ -13,37 +14,24 @@ - + - - - - - - - - + + - - - - + override what is in the yaml file. Thus use it to set parameters camer_array configuration params --> - + - @@ -56,6 +44,6 @@ - - + + diff --git a/launch/multiple_nodelet_example.launch b/launch/multiple_nodelet_example.launch index e12f59a..ba96a9e 100644 --- a/launch/multiple_nodelet_example.launch +++ b/launch/multiple_nodelet_example.launch @@ -7,9 +7,9 @@ - + - + diff --git a/launch/nodelet_test.launch b/launch/nodelet_subscriber_example.launch similarity index 95% rename from launch/nodelet_test.launch rename to launch/nodelet_subscriber_example.launch index f8a4886..31f2a69 100644 --- a/launch/nodelet_test.launch +++ b/launch/nodelet_subscriber_example.launch @@ -5,7 +5,7 @@ - + @@ -38,10 +38,9 @@ - + - diff --git a/params/test_params.yaml b/params/test_params.yaml index 9f93386..a8a9a45 100644 --- a/params/test_params.yaml +++ b/params/test_params.yaml @@ -1,10 +1,10 @@ cam_ids: -- 17197559 -- 17197547 +- 18080155 +#- 17197547 cam_aliases: - cam0 -- cam1 -master_cam: 17197559 +#- cam1 +master_cam: 18080155 skip: 20 delay: 1.0 @@ -25,18 +25,18 @@ image_height: 1536 image_width: 2048 distortion_coeffs: - [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725] -- [-0.06001035588294054, 0.3466585291091072, -1.0932356940133992, 3.4511959617065253] +#- [-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] +#- [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] +#- [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] +#- [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/capture.cpp b/src/capture.cpp index 0857dbc..1de4559 100755 --- a/src/capture.cpp +++ b/src/capture.cpp @@ -23,6 +23,8 @@ acquisition::Capture::~Capture(){ ROS_INFO_STREAM("Releasing system instance..."); system_->ReleaseInstance(); + + delete dynamicReCfgServer_; ros::shutdown(); @@ -114,11 +116,12 @@ acquisition::Capture::Capture(): it_(nh_), nh_pvt_ ("~") { //initializing the ros publisher acquisition_pub = nh_.advertise("camera", 1000); //dynamic reconfigure - //dynamic_reconfigure::Server server; - dynamic_reconfigure::Server::CallbackType f; + dynamicReCfgServer_ = new dynamic_reconfigure::Server(nh_pvt_); + + dynamic_reconfigure::Server::CallbackType dynamicReCfgServerCB_t; - f = boost::bind(&acquisition::Capture::dynamicReconfigureCallback,this, _1, _2); - server.setCallback(f); + 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) { @@ -192,11 +195,12 @@ acquisition::Capture::Capture(ros::NodeHandle nodehandl, ros::NodeHandle private //initializing the ros publisher acquisition_pub = nh_.advertise("camera", 1000); //dynamic reconfigure - //dynamic_reconfigure::Server server; - dynamic_reconfigure::Server::CallbackType f; + dynamicReCfgServer_ = new dynamic_reconfigure::Server(nh_pvt_); + + dynamic_reconfigure::Server::CallbackType dynamicReCfgServerCB_t; - f = boost::bind(&acquisition::Capture::dynamicReconfigureCallback,this, _1, _2); - server.setCallback(f); + dynamicReCfgServerCB_t = boost::bind(&acquisition::Capture::dynamicReconfigureCallback,this, _1, _2); + dynamicReCfgServer_->setCallback(dynamicReCfgServerCB_t); } @@ -426,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_);