ros-drivers · RomainV92 · May 30, 2022 · May 30, 2022 · May 30, 2022
diff --git a/spinnaker_camera_driver/cfg/Spinnaker.cfg b/spinnaker_camera_driver/cfg/Spinnaker.cfg
@@ -306,6 +306,11 @@ auto_lighting_mode = gen.enum([
 ], "Auto algorithms lighting modes")
 gen.add("auto_exposure_lighting_mode", str_t, SensorLevels.RECONFIGURE_RUNNING, "Auto exposure lighting mode.", "Normal", edit_method=auto_lighting_mode)
 
+# GigE camera parameters
+gen.add("gige_parameter_enable",               bool_t, SensorLevels.RECONFIGURE_STOP,    "GigE camera parameters enabled",      False)
+gen.add("device_link_throughput_limit",        int_t,  SensorLevels.RECONFIGURE_RUNNING, "Device link throughput limit",        14353165, 0)
+gen.add("camera_packet_size",                  int_t,  SensorLevels.RECONFIGURE_STOP,    "GigE camera packet size",             1400,     0)
+
 # Other
 gen.add("time_offset", double_t, SensorLevels.RECONFIGURE_RUNNING, "Time offset to add to image time stamps.", 0.0, -5.0, 5.0)
 

diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h
@@ -52,6 +52,7 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <mutex>
 #include <sstream>
 #include <string>
+#include <thread>
 
 // Header generated by dynamic_reconfigure
 #include <spinnaker_camera_driver/SpinnakerConfig.h>
@@ -160,6 +161,36 @@ class SpinnakerCamera
   */
   void setDesiredCamera(const uint32_t& id);
 
+  /*!
+  * \brief Used to set the IP of the camera you wish to connect to.
+  *
+  * Sets the desired IP.  If this value is not set, the driver will try to automatically 
+  * assign an IP address to the camera.
+  * This function should be called before connect().
+  * \param ip IP for the camera.  Should be something like 192.168.0.2.
+  */
+  void setDesiredIP(const std::string& ip);
+
+  /*!
+  * \brief Used to set the subnet mask of the network the camera is connected to.
+  *
+  * Sets the subnet mask.  If this value is not set, the driver will assign the default subnet mask
+  * '255.255.255.0'.
+  * This function should be called before connect().
+  * \param mask Subnet mask of the network.  Should be something like 255.255.255.0.
+  */
+  void setDesiredSubnetMask(const std::string& mask);
+
+  /*!
+  * \brief Used to set the gateway of the subnet the camera will connect to.
+  *
+  * Sets the desired gateway.  If this value is not set, the driver will automatically 
+  * assign the gateway to the first address of the subnet.
+  * This function should be called before connect().
+  * \param gateway Gateway address on the subnet.  Should be something like 192.168.0.1.
+  */
+  void setDesiredGateway(const std::string& gateway);
+
   void setGain(const float& gain);
   int getHeightMax();
   int getWidthMax();
@@ -171,8 +202,27 @@ class SpinnakerCamera
     return serial_;
   }
 
+protected:
+  // Routine to find the desired camera to connect to.
+  void findCameraPtr();
+  // Routine to update camera the desired camera's pointer after changing a camera parameter (e.g. IP address)
+  void updateCameraPtr();
+  int64_t fromStrToIP(const std::string ip_str);
+  std::string convertIPtoStr(const int64_t ip_int);
+
+  /*!
+  * \brief Used to apply the desired subnet configuration to the camera.
+  *
+  * This function sets the IP of the camera, the subnet mask and the gateway address for the network being used.
+  */
+  void setIP();
+  void tryAutoForceIP();
+
 private:
   uint32_t serial_;  ///< A variable to hold the serial number of the desired camera.
+  int64_t ip_;       ///< A variable to hold the IP to associate to the desired camera.
+  int64_t mask_;     ///< A variable to hold the subnet mask of the network the desired camera is attached to.
+  int64_t gateway_;  ///< A variable to hold the address of the gateway in the subnet the camera is connected to.
 
   Spinnaker::SystemPtr system_;
   Spinnaker::CameraList camList_;

diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h
@@ -63,6 +63,7 @@ class Camera
   static const uint8_t LEVEL_RECONFIGURE_RUNNING = 0;
 
   virtual void setGain(const float& gain);
+  inline void setPacketSizeMax(const unsigned int size) { packet_size_max_ = size; };
   int getHeightMax();
   int getWidthMax();
 
@@ -78,6 +79,7 @@ class Camera
 
   int height_max_;
   int width_max_;
+  unsigned int packet_size_max_;
 
   /*!
   * \brief Changes the video mode of the connected camera.
@@ -87,6 +89,8 @@ class Camera
   */
   virtual void setFrameRate(const float frame_rate);
   virtual void setImageControlFormats(const spinnaker_camera_driver::SpinnakerConfig& config);
+
+  void setGigEPacketSize(const int size);
   /*!
   * \brief Set parameters relative to GigE cameras.
   *

diff --git a/spinnaker_camera_driver/launch/camera_gige.launch b/spinnaker_camera_driver/launch/camera_gige.launch
@@ -0,0 +1,131 @@
+<?xml version="1.0"?>
+<!--
+Software License Agreement (BSD)
+
+\file      camera.launch
+\authors   Michael Hosmar <[email protected]>
+\copyright Copyright (c) 2018, Clearpath Robotics, Inc., All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the
+   following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 
+   following disclaimer in the documentation and/or other materials provided with the distribution.
+ * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
+   products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
+RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
+DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<launch>
+   <!-- Determine this using rosrun spinnaker_camera_driver list_cameras.
+       If not specified, defaults to first camera found. -->
+  <arg name="camera_name"               default="camera" />
+  <arg name="camera_serial"             default="0" />
+
+  <!-- ### Use these parameters to set the subnet configuration ONCE, and while only one camera is connected ### -->
+  <arg name="camera_IP"                 default="0" />              <!-- Relevant only when using a GigE camera -->
+  <arg name="camera_subnet_mask"        default="255.255.255.0" />  <!-- Relevant only when using a GigE camera -->
+  <arg name="subnet_gateway"            default="0" />              <!-- Relevant only when using a GigE camera -->
+  <!-- ###### -->
+  <arg name="calibrated"                default="0" />
+  <arg name="device_type"               default="GigE" /> <!-- USB3 or GigE -->
+
+  <!-- When unspecified, the driver will use the default framerate as given by the
+      camera itself. Use the parameter 'control_frame_rate' to enable manual frame 
+      rate control, and 'frame_rate' to set the frame rate value. -->
+  <arg name="control_frame_rate"        default="False" />
+  <arg name="frame_rate"                default="30" />
+
+  <!-- Disabling ISP will dramatically increase frame-rate. However, it can only be 
+      disabled when using Bayer encoding (e.g. BayerRG8)-->
+  <arg name="isp_enable"                default="False" /> 
+  <arg name="encoding"                  default="Mono8" />
+  <arg name="color_balance"             default="Continuous" /> <!-- Off, Once, or Continuous -->
+  <!-- Available Encodings:
+        Mono:              YUV:              YCbCr:          Other:
+        - Mono8            - YUV411Packed    - YCbCr8        - BGR8
+        - Mono16           - YUV422Packed    - YCbCr422_8    - BGRa8
+        - Mono12p          - YUV444Packed    - YCbCr411_8    - RGB8Packed
+        - Mono12Packed
+
+        Bayer:
+        - BayerGR8         - BayerGR12p
+        - BayerRG8         - BayerRG12p
+        - BayerGB8         - BayerGB12p
+        - BayerBG8         - BayerBG12p
+        - BayerGR16        - BayerGR12Packed
+        - BayerRG16        - BayerRG12Packed
+        - BayerGB16        - BayerGB12Packed
+        - BayerBG16        - BayerBG12Packed
+  -->
+
+  <!-- Parameters for GigE cameras -->
+  <arg name="enable_gige_parameters"    default="False" />
+  <arg name="throughput_limit"          default="125000000" />
+  <arg name="packet_size"               default="1400" />
+
+  <group ns="$(arg camera_name)">
+    <!-- Nodelet manager -->
+    <node pkg="nodelet" type="nodelet" name="camera_nodelet_manager" args="manager" cwd="node" output="screen"/>
+
+    <!-- Camera nodelet -->
+    <node pkg="nodelet" type="nodelet" name="spinnaker_camera_nodelet"
+          args="load spinnaker_camera_driver/SpinnakerCameraNodelet camera_nodelet_manager" >
+
+      <param name="frame_id"                            value="$(arg camera_name)" />
+      <param name="serial"                              value="$(arg camera_serial)" />
+      <param name="camera_ip"                           value="$(arg camera_IP)" />
+      <param name="camera_mask"                         value="$(arg camera_subnet_mask)" />
+      <param name="subnet_gateway"                      value="$(arg subnet_gateway)" />
+      <param name="device_type"                         value="$(arg device_type)" />
+
+      <!-- Frame rate -->
+      <param name="acquisition_frame_rate_enable"       value="$(arg control_frame_rate)" />
+      <param name="acquisition_frame_rate"              value="$(arg frame_rate)" />
+
+      <!-- Image Processing -->
+      <param name="isp_enable"                          value="$(arg isp_enable)" />
+      <param name="auto_white_balance"                  value="$(arg color_balance)" />
+      <param name="image_format_color_coding"           value="$(arg encoding)" />
+
+      <param name="gige_parameter_enable"               value="$(arg enable_gige_parameters)" />
+      <param name="device_link_throughput_limit"        value="$(arg throughput_limit)" />
+      <param name="camera_packet_size"                  value="$(arg packet_size)" />
+
+      <!-- Image Resolution -->
+      <!-- Height and width pixel size cannot be set directly. Instead use the 
+          binning, offset, and region of interest options. 
+          - RoI: range of pixels to select from original image
+                (Note: RoI is defined from image pixel origin (i.e. top left))
+          - Binning: reduces resolution by a factor of 1, 2, 4, or 8
+          - Offset: moves the pixel origin
+                x-offset = max_width/x_binning - roi_width/2
+                y-offset = max_height/y_binning - roi_height/2
+      -->
+      <!--
+      <param name="image_format_x_binning" value="2" />
+      <param name="image_format_y_binning" value="2" />
+      <param name="image_format_x_offset" value="128" />
+      <param name="image_format_y_offset" value="122" />
+      <param name="image_format_roi_width" value="1280" />
+      <param name="image_format_roi_height" value="720" />
+      -->
+
+      <!-- Use the camera_calibration package to create this file -->
+      <param name="camera_info_url" if="$(arg calibrated)"
+             value="file://$(env HOME)/.ros/camera_info/$(arg camera_serial).yaml" />
+    </node>
+
+    <!-- Debayering nodelet -->
+    <!-- <node pkg="nodelet" type="nodelet" name="image_proc_debayer"
+          args="load image_proc/debayer camera_nodelet_manager"> -->
+    <!-- </node> -->
+  </group>
+</launch>
diff --git a/spinnaker_camera_driver/launch/stereo.launch b/spinnaker_camera_driver/launch/stereo.launch
@@ -1,11 +1,9 @@
 <?xml version="1.0"?>
 <!--
 Software License Agreement (BSD)
-
 \file      stereo.launch
 \authors   Michael Hosmar <[email protected]>
 \copyright Copyright (c) 2018, Clearpath Robotics, Inc., All rights reserved.
-
 Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 the following conditions are met:
  * Redistributions of source code must retain the above copyright notice, this list of conditions and the
@@ -14,7 +12,6 @@ the following conditions are met:
    following disclaimer in the documentation and/or other materials provided with the distribution.
  * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
    products derived from this software without specific prior written permission.
-
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
 RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-