Merge pull request #117 from neufieldrobotics/feature/gain_parameter2

Feature/gain parameter2

CMakeLists.txt

@@ -113,6 +113,9 @@ if(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm)
 
 endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm)
 
+# Make package version available in CPP as a defination
+add_definitions("-D${PROJECT_NAME}_VERSION=\"${${PROJECT_NAME}_VERSION}\"")
+
 add_library (acquilib SHARED
   src/capture.cpp
   src/camera.cpp

include/spinnaker_sdk_camera_driver/camera.h

@@ -59,7 +59,8 @@ namespace acquisition {
         // void setTrigMode();
         // void setTriggerOverlapOff();
 
-        string get_node_value(string node_string);
+        string getTLNodeStringValue(string node_string);
+        double getFloatValueMax(string node_string);
         string get_id();
         void make_master() { MASTER_ = true; ROS_DEBUG_STREAM( "camera " << get_id() << " set as master"); }
         bool is_master() { return MASTER_; }

include/spinnaker_sdk_camera_driver/capture.h

@@ -115,6 +115,7 @@ namespace acquisition {
         string dump_img_;
         string ext_;
         float exposure_time_;
+        float gain_;
         double target_grey_value_;
         bool first_image_received;
         // int decimation_;

launch/acquisition.launch

@@ -3,23 +3,29 @@
   <env name="ROSCONSOLE_CONFIG_FILE" value="$(find spinnaker_sdk_camera_driver)/cfg/std_console.conf"/>
 
   <!-- acquisition spinnaker params-->
-  <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="exposure_time"     default="0"	doc="Exposure_time setting for cameras"/>
-  <arg name="external_trigger"  default="false"	doc="External trigger (No camera is master)"/>
-  <arg name="target_grey_value"   default="0"   doc="AutoExposureTargetGreyValue min: 4 max: 99 if set below 4, AutoExposureTargetGreyValueAuto will be set to continuous(auto) also available as dynamic reconfigurable parameter" />
-  <arg name="frames"            default="3400"	doc="Numer of frames to save/view 0=ON"/>
-  <arg name="live"              default="false"	doc="Show images on screen GUI"/>
-  <arg name="live_grid"         default="false"	doc="Show images on screen GUI in a grid"/>
-  <arg name="output"            default="screen"	doc="display output to screen or log file"/>
-  <arg name="save"              default="false"	doc="flag whether images should be saved or not"/>
-  <arg name="save_path"         default="~"	doc="location to save the image data"/>
-  <arg name="save_type"         default="bmp"	doc="Type of file type to save to when saving images locally: binary, tiff, bmp, jpeg etc." />
-  <arg name="soft_framerate"    default="30"	doc="When hybrid software triggering is used, this controls the FPS, 0=as fast as possible"/>
-  <arg name="time"              default="false"	doc="Show time/FPS on output"/>
-  <arg name="to_ros"            default="true"	doc="Flag whether images should be published to ROS" />
-  <arg name="utstamps"          default="false"	doc="Flag whether each image should have Unique timestamps vs the master cams time stamp for all" />
-  <arg name="max_rate_save"     default="false"	doc="Flag for max rate mode which is when the master triggerst the slaves and saves images at maximum rate possible" />
+  <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="exposure_time"     default="0"      doc="Exposure_time setting for cameras"/>
+  <arg name="external_trigger"  default="false"  doc="External trigger (No camera is master)"/>
+  <arg name="gain"              default="0"      doc="Gain setting. Gain should be positive (full range - 0 - 18 dB for blackfly-s camera) 
+                                                      or zero (auto gain). if gain > max, it will be set to max allowed value.
+                                                      Default is 0, auto gain which is set according to target grey value or autoexposure settings"/>
+  <arg name="target_grey_value"   default="0"    doc="AutoExposureTargetGreyValue min: 4 max: 99 if set below 4,
+                                                      AutoExposureTargetGreyValueAuto will be set to continuous(auto)
+                                                      also available as dynamic reconfigurable parameter.
+                                                      This parameter has no meaning when auto exposure and auto gain are off" />
+  <arg name="frames"            default="3400"   doc="Numer of frames to save/view 0=ON"/>
+  <arg name="live"              default="false"  doc="Show images on screen GUI"/>
+  <arg name="live_grid"         default="false"  doc="Show images on screen GUI in a grid"/>
+  <arg name="output"            default="screen" doc="display output to screen or log file"/>
+  <arg name="save"              default="false"  doc="flag whether images should be saved or not"/>
+  <arg name="save_path"         default="~"      doc="location to save the image data"/>
+  <arg name="save_type"         default="bmp"    doc="Type of file type to save to when saving images locally: binary, tiff, bmp, jpeg etc." />
+  <arg name="soft_framerate"    default="30"     doc="When hybrid software triggering is used, this controls the FPS, 0=as fast as possible"/>
+  <arg name="time"              default="false"  doc="Show time/FPS on output"/>
+  <arg name="to_ros"            default="true"   doc="Flag whether images should be published to ROS" />
+  <arg name="utstamps"          default="false"  doc="Flag whether each image should have Unique timestamps vs the master cams time stamp for all" />
+  <arg name="max_rate_save"     default="false"  doc="Flag for max rate mode which is when the master triggerst the slaves and saves images at maximum rate possible" />
 
 
   <!-- nodelet manager params-->
@@ -29,7 +35,7 @@
 
   <!-- Capture nodelet params-->
   <arg name="tf_prefix" default="" doc="will prefix (arg tf_prefix)/ to existing frame_id in Image msg and camerainfo msg" />
-  <arg name="config_file"       default="$(find spinnaker_sdk_camera_driver)/params/test_params.yaml" doc="File specifying the parameters of the camera_array"/>
+  <arg name="config_file"          default="$(find spinnaker_sdk_camera_driver)/params/test_params.yaml" doc="File specifying the parameters of the camera_array"/>
 
    <!-- start the nodelet manager if $(arg start_nodelet_manager) is true-->
   <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen" unless="$(arg external_manager)" />
@@ -38,12 +44,13 @@
   <node pkg="nodelet" type="nodelet" name="acquisition_node"
           args="load acquisition/Capture $(arg manager)" >
     <!-- load the acquisition node parameters file.  Note any parameters provided in this file will 
-	 override what is in the yaml file.  Thus use it to set parameters camer_array configuration params -->
+         override what is in the yaml file.  Thus use it to set parameters camer_array configuration params -->
     <rosparam command="load"        file="$(arg config_file)" />
 
     <!-- Load parameters onto server using argument or default values above -->
     <param name="exposure_time"     value="$(arg exposure_time)" />
     <param name="external_trigger"     value="$(arg external_trigger)" />
+    <param name="gain"              value="$(arg gain)"/>
     <param name="target_grey_value" value="$(arg target_grey_value)" />
     <param name="binning"           value="$(arg binning)" />
     <param name="color"             value="$(arg color)" />

launch/acquisition_external_trigger.launch

@@ -0,0 +1,72 @@
+<launch>
+  <!-- configure console output verbosity mode:debug_console.conf or std_console.conf -->
+  <env name="ROSCONSOLE_CONFIG_FILE" value="$(find spinnaker_sdk_camera_driver)/cfg/std_console.conf"/>
+
+  <!-- acquisition spinnaker params-->
+  <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="exposure_time"     default="0"      doc="Exposure_time setting for cameras"/>
+  <arg name="external_trigger"  default="true"   doc="External trigger (No camera is master)"/>
+  <arg name="gain"              default="0"      doc="Gain setting. Gain should be positive (full range - 0 - 18 dB for blackfly-s camera) 
+                                                      or zero (auto gain). if gain > max, it will be set to max allowed value.
+                                                      Default is 0, auto gain which is set according to target grey value or autoexposure settings"/>
+  <arg name="target_grey_value"   default="0"    doc="AutoExposureTargetGreyValue min: 4 max: 99 if set below 4,
+                                                      AutoExposureTargetGreyValueAuto will be set to continuous(auto)
+                                                      also available as dynamic reconfigurable parameter.
+                                                      This parameter has no meaning when auto exposure and auto gain are off" />
+  <arg name="frames"            default="3400"   doc="Numer of frames to save/view 0=ON"/>
+  <arg name="live"              default="false"  doc="Show images on screen GUI"/>
+  <arg name="live_grid"         default="false"  doc="Show images on screen GUI in a grid"/>
+  <arg name="output"            default="screen" doc="display output to screen or log file"/>
+  <arg name="save"              default="false"  doc="flag whether images should be saved or not"/>
+  <arg name="save_path"         default="~"      doc="location to save the image data"/>
+  <arg name="save_type"         default="bmp"    doc="Type of file type to save to when saving images locally: binary, tiff, bmp, jpeg etc." />
+  <arg name="soft_framerate"    default="30"     doc="When hybrid software triggering is used, this controls the FPS, 0=as fast as possible"/>
+  <arg name="time"              default="false"  doc="Show time/FPS on output"/>
+  <arg name="to_ros"            default="true"   doc="Flag whether images should be published to ROS" />
+  <arg name="utstamps"          default="false"  doc="Flag whether each image should have Unique timestamps vs the master cams time stamp for all" />
+  <arg name="max_rate_save"     default="false"  doc="Flag for max rate mode which is when the master triggerst the slaves and saves images at maximum rate possible" />
+
+
+  <!-- nodelet manager params-->
+  <arg name="manager" default="vision_nodelet_manager" doc="name of the nodelet manager, comes handy when launching multiple nodelets from different launch files" />
+  <arg name="external_manager" default="false" doc="If set to False(default), creates a nodelet manager with $(arg manager).
+  If True, the acquisition/Capture waits for the nodelet_manager name $(arg manager)" />
+
+  <!-- Capture nodelet params-->
+  <arg name="tf_prefix" default="" doc="will prefix (arg tf_prefix)/ to existing frame_id in Image msg and camerainfo msg" />
+  <arg name="config_file"          default="$(find spinnaker_sdk_camera_driver)/params/external_trigger_params.yaml" doc="File specifying the parameters of the camera_array"/>
+
+  <!-- start the nodelet manager if $(arg start_nodelet_manager) is true-->
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen" unless="$(arg external_manager)" />
+
+  <!-- load the acquisition nodelet -->
+  <node pkg="nodelet" type="nodelet" name="acquisition_node"
+          args="load acquisition/Capture $(arg manager)" >
+    <!-- load the acquisition node parameters file.  Note any parameters provided in this file will 
+         override what is in the yaml file.  Thus use it to set parameters camer_array configuration params -->
+    <rosparam command="load"        file="$(arg config_file)" />
+
+    <!-- Load parameters onto server using argument or default values above -->
+    <param name="exposure_time"     value="$(arg exposure_time)" />
+    <param name="external_trigger"  value="$(arg external_trigger)" />
+    <param name="gain"              value="$(arg gain)"/>
+    <param name="target_grey_value" value="$(arg target_grey_value)" />
+    <param name="binning"           value="$(arg binning)" />
+    <param name="color"             value="$(arg color)" />
+    <param name="frames"            value="$(arg frames)" />
+    <param name="live"              value="$(arg live)" />
+    <param name="live_grid"         value="$(arg live_grid)" />
+    <param name="save"              value="$(arg save)" />
+    <param name="save_type"         value="$(arg save_type)" />
+    <param name="save_path"         value="$(arg save_path)" />
+    <param name="soft_framerate"    value="$(arg soft_framerate)" />
+    <param name="time"              value="$(arg time)" />
+    <param name="utstamps"          value="$(arg utstamps)" />
+    <param name="to_ros"            value="$(arg to_ros)"/>
+    <param name="max_rate_save"     value="$(arg max_rate_save)" />
+    <param name="tf_prefix"         value="$(arg tf_prefix)" />
+
+  </node>
+
+</launch>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>spinnaker_sdk_camera_driver</name>
-  <version>0.0.0</version>
+  <version>1.1.0</version>
   <description>Point Grey (FLIR) Spinnaker based camera driver (Blackfly S etc.)</description>
 
   <maintainer email="[email protected]">Vikrant Shah</maintainer>

src/camera.cpp

@@ -306,17 +306,34 @@ void acquisition::Camera::trigger() {
 
 }
 
-string acquisition::Camera::get_node_value(string node_string) {
+double acquisition::Camera::getFloatValueMax(string node_string) {
+    INodeMap& nodeMap = pCam_->GetNodeMap();
+
+    CFloatPtr ptrNodeValue = nodeMap.GetNode(node_string.c_str());
+
+    if (IsAvailable(ptrNodeValue)){
+      //cout << "\tMax " << ptrNodeValue->GetValue() << "..." << endl;
+      return ptrNodeValue->GetMax();
+    } else {
+        ROS_FATAL_STREAM("Node " << node_string << " not available" << endl);
+        return -1;
+    }
+}
+
+
+string acquisition::Camera::getTLNodeStringValue(string node_string) {
     INodeMap& nodeMap = pCam_->GetTLDeviceNodeMap();
     CStringPtr ptrNodeValue = nodeMap.GetNode(node_string.c_str());
     if (IsReadable(ptrNodeValue)){
         return string(ptrNodeValue->GetValue());
+    } else{
+        ROS_FATAL_STREAM("Node " << node_string << " not readable" << endl);
+        return "";
     }
-    return "";
 }
 
 string acquisition::Camera::get_id() {
-    return get_node_value("DeviceSerialNumber");
+    return getTLNodeStringValue("DeviceSerialNumber");
 }
 
 void acquisition::Camera::targetGreyValueTest() {

src/capture.cpp

@@ -80,6 +80,7 @@ void acquisition::Capture::init_variables_register_to_ros() {
     // default values for the parameters are set here. Should be removed eventually!!
     exposure_time_ = 0 ; // default as 0 = auto exposure
     soft_framerate_ = 20; //default soft framrate
+    gain_ = 0;
     ext_ = ".bmp";
     SOFT_FRAME_RATE_CTRL_ = false;
     LIVE_ = false;
@@ -138,6 +139,7 @@ void acquisition::Capture::init_variables_register_to_ros() {
     // Retrieve singleton reference to system object
     ROS_INFO_STREAM("*** SYSTEM INFORMATION ***");
     ROS_INFO_STREAM("Creating system instance...");
+    ROS_INFO_STREAM("spinnaker_sdk_camera_driver package version: " << spinnaker_sdk_camera_driver_VERSION);
     system_ = System::GetInstance();
 
     const LibraryVersion spinnakerLibraryVersion = system_->GetLibraryVersion();
@@ -187,8 +189,8 @@ void acquisition::Capture::load_cameras() {
     for (int i=0; i<numCameras_; i++) {
         acquisition::Camera cam(camList_.GetByIndex(i));
         ROS_INFO_STREAM("  -"<< cam.get_id()
-                             <<" "<< cam.get_node_value("DeviceModelName")
-                             <<" "<< cam.get_node_value("DeviceVersion") );
+                             <<" "<< cam.getTLNodeStringValue("DeviceModelName")
+                             <<" "<< cam.getTLNodeStringValue("DeviceVersion") );
     }
 
     bool master_set = false;
@@ -297,9 +299,6 @@ void acquisition::Capture::load_cameras() {
     }
     ROS_ASSERT_MSG(cams.size(),"None of the connected cameras are in the config list!");
 
-    if (!EXTERNAL_TRIGGER_){
-        ROS_ASSERT_MSG(master_set,"The camera supposed to be the master isn't connected!");
-    }
     // Setting numCameras_ variable to reflect number of camera objects used.
     // numCameras_ variable is used in other methods where it means size of cams list.
     numCameras_ = cams.size();
@@ -477,6 +476,14 @@ void acquisition::Capture::read_parameters() {
         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("gain", gain_)){
+       if(gain_>0){
+          ROS_INFO("gain value set to:%.1f",gain_);
+       }
+       else ROS_INFO("  'gain' Parameter was zero or negative, using Auto gain based on target grey value");
+    } 
+    else ROS_WARN("  'gain' Parameter not set, using default behavior: Auto gain based on target grey value");
+
     if (nh_pvt_.getParam("target_grey_value", target_grey_value_)){
         if (target_grey_value_ >0) ROS_INFO("  target_grey_value set to: %.1f",target_grey_value_);
         else ROS_INFO("  'target_grey_value'=%0.f, Setting AutoExposureTargetGreyValueAuto to Continuous/ auto",target_grey_value_);} 
@@ -697,6 +704,21 @@ void acquisition::Capture::init_cameras(bool soft = false) {
                 } else {
                     cams[i].setEnumValue("ExposureAuto", "Continuous");
                 }
+
+                if(gain_>0){ //fixed gain
+                    cams[i].setEnumValue("GainAuto", "Off");
+                    double max_gain_allowed = cams[i].getFloatValueMax("Gain");
+                    if (gain_ <= max_gain_allowed)
+                        cams[i].setFloatValue("Gain", gain_);
+                    else {
+                        cams[i].setFloatValue("Gain", max_gain_allowed);
+                        ROS_WARN("Provided Gain value is higher than max allowed, setting gain to %f", max_gain_allowed);
+                    }
+                    target_grey_value_ = 50;
+                } else {
+                    cams[i].setEnumValue("GainAuto","Continuous");                   
+                }
+
                 if (target_grey_value_ > 4.0) {
                     cams[i].setEnumValue("AutoExposureTargetGreyValueAuto", "Off");
                     cams[i].setFloatValue("AutoExposureTargetGreyValue", target_grey_value_);
@@ -709,7 +731,7 @@ void acquisition::Capture::init_cameras(bool soft = false) {
                 // cams[i].setFloatValue("AcquisitionFrameRate", 5.0);
 
                 if (color_)
-                    cams[i].setEnumValue("PixelFormat", "BGR8");
+                        cams[i].setEnumValue("PixelFormat", "BGR8");
                     else
                         cams[i].setEnumValue("PixelFormat", "Mono8");
                 cams[i].setEnumValue("AcquisitionMode", "Continuous");
@@ -1233,7 +1255,7 @@ void acquisition::Capture::write_queue_to_disk(queue<ImagePtr>* img_q, int cam_n
         convertedImage->Save(filename.str().c_str());
         // release the image before popping out to save memory
         convertedImage->Release();
-        ROS_INFO("image Queue size for cam %d is = %d",cam_no,img_q->size());
+        ROS_INFO("image Queue size for cam %d is = %zu",cam_no, img_q->size());
         queue_mutex_.lock();
         img_q->pop();
         queue_mutex_.unlock();