fixed imagetransport, changed from imagetransport:;publisher to imagetransport::camerapublisher and made caminfo to caminfoptr

Author: mithundiddi

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

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"))

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);

include/spinnaker_sdk_camera_driver/capture.h

@@ -7,11 +7,17 @@
 
 #include <boost/archive/binary_oarchive.hpp>
 #include <boost/filesystem.hpp>
-
+//ROS
+#include "std_msgs/Float64.h"
 #include "std_msgs/String.h"
+//Dynamic reconfigure
+#include <dynamic_reconfigure/server.h>
+#include <spinnaker_sdk_camera_driver/spinnaker_camConfig.h>
+
 #include "spinnaker_sdk_camera_driver/SpinnakerImageNames.h"
 
 #include <sstream>
+#include <image_transport/image_transport.h>
 
 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<spinnaker_sdk_camera_driver::spinnaker_camConfig> server;
 
         ros::Publisher acquisition_pub;
-        vector<ros::Publisher> camera_image_pubs;
-        vector<ros::Publisher> camera_info_pubs;
+        //vector<ros::Publisher> camera_image_pubs;
+        vector<image_transport::CameraPublisher> camera_image_pubs;
+        //vector<ros::Publisher> camera_info_pubs;
 
         vector<sensor_msgs::ImagePtr> img_msgs;
-        vector<sensor_msgs::CameraInfo> cam_info_msgs;
+        vector<sensor_msgs::CameraInfoPtr> cam_info_msgs;
         spinnaker_sdk_camera_driver::SpinnakerImageNames mesg;
         boost::mutex queue_mutex_;  
     };

launch/acquisition.launch

@@ -3,7 +3,7 @@
   <env name="ROSCONSOLE_CONFIG_FILE" value="$(find spinnaker_sdk_camera_driver)/cfg/std_console.conf"/>
 
   <!-- acquisition.lauynch -->
-  <arg name="binning"		    default="2"	doc="Binning for cameras, when changing from 2 to 1 cameras need to be unplugged and replugged"/>
+  <arg name="binning"		    default="1"	doc="Binning for cameras, when changing from 2 to 1 cameras need to be unplugged and replugged"/>
   <arg name="color"		        default="false"	doc="Should color images be used (only works on models that support color images)"/>
   <arg name="exp"               default="0"	doc="Exposure setting for cameras"/>
   <arg name="frames"            default="3400"	doc="Numer of frames to save/view 0=ON"/>

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]

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();

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: "<<ptrExpTest->GetValue()<<endl);
+
+}
+
 void acquisition::Camera::exposureTest() {
     CFloatPtr ptrExpTest=pCam_->GetNodeMap().GetNode("ExposureTime");
     if (!IsAvailable(ptrExpTest) || !IsReadable(ptrExpTest)){

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<spinnaker_sdk_camera_driver::SpinnakerImageNames>("camera", 1000);
+    //dynamic reconfigure
+    //dynamic_reconfigure::Server<spinnaker_sdk_camera_driver::spinnaker_camConfig> server;
+    dynamic_reconfigure::Server<spinnaker_sdk_camera_driver::spinnaker_camConfig>::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<cam_ids_.size(); j++) {
         bool current_cam_found=false;
         for (int i=0; i<numCameras_; i++) {
@@ -156,26 +164,24 @@ void acquisition::Capture::load_cameras() {
 
                 cams.push_back(cam);
 
-                camera_image_pubs.push_back(nh_.advertise<sensor_msgs::Image>("camera_array/"+cam_names_[j]+"/image_raw", 1));
-                camera_info_pubs.push_back(nh_.advertise<sensor_msgs::CameraInfo>("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<sensor_msgs::CameraInfo>("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; count<intrinsic_coeff_vec_[j].size();count++)
-                        ci_msg.K[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};
-
+                        ci_msg->K[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 "<<config.exposure_time);
+    ROS_INFO_STREAM("Dynamic Reconfigure: Level : " << level);
+
+    if(level == 1){
+        cams[MASTER_CAM_].setEnumValue("AutoExposureTargetGreyValueAuto", "Off");
+        cams[MASTER_CAM_].setFloatValue("AutoExposureTargetGreyValue", config.target_grey_val);
+    }
+    else if (level == 3 && config.exposure_time > 0){
+        cams[MASTER_CAM_].setEnumValue("ExposureAuto", "Off");
+        cams[MASTER_CAM_].setEnumValue("ExposureMode", "Timed");
+        cams[MASTER_CAM_].setFloatValue("ExposureTime", config.exposure_time);
+    }
+    
+
+}