From 302c8d4d3938c3b51b47753b519c401f12416cd2 Mon Sep 17 00:00:00 2001 From: mithunvarma Date: Thu, 20 Dec 2018 21:58:01 -0500 Subject: [PATCH 1/5] 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 2/5] 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 3/5] 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 d78e4ea89cf977772977a90ee5c80b2fc548aee7 Mon Sep 17 00:00:00 2001 From: mithun Date: Mon, 25 Feb 2019 22:14:07 -0500 Subject: [PATCH 4/5] 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 5/5] 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 @@ - +