Expose packet configuration for ROS2 (#20)

* Expose packet configuration on ROS2
labforge · Feb 23, 2024 · 684f1ca · 684f1ca
1 parent 39f14d0
commit 684f1ca
Show file tree

Hide file tree

Showing 3 changed files with 42 additions and 8 deletions.
diff --git a/README.md b/README.md
@@ -156,6 +156,10 @@ ros2 run bottlenose_camera_driver bottlenose_camera_driver_node --ros-args -p ma
 | ```ResendRequestTimeout```               | Timeout for resend requests in (us)                                                    | ```5000```                       |                     |
 | ```RequestTimeout```                     | Maximum time that the data receiver waits for all the packets of a block (ms)          | ```1000```                       |                     |
 | ```ResetOnIdle```                        | Time without packets before resetting itself                                           | ```200```                        |                     |
+| ```RequestMissingPackets```              | If enabled, missing packets are requested.                                             | ```true```                       |                     |
+| ```ResendDelay```                        | Delay between resend requests in (us)                                                  | ```0```                          |                     |
+| ```GevSCPSPacketSize```                  | Maximum packet size in bytes for GigE Vision stream (0 = auto negotiate)               | ```0```                          |                     |
+| ```GevSCPD```                            | Delay between packets in (ns) for GigE Vision stream                                   | ```0```                          |                     |
 | ```Timeout```                            | Buffer reception timeout in (ms)                                                       | ```5000```                       |                     |
 
 

diff --git a/include/bottlenose_parameters.hpp b/include/bottlenose_parameters.hpp
@@ -79,6 +79,11 @@ const parameter_t bottlenose_parameters[] = {
   {"ResendRequestTimeout", rclcpp::ParameterValue(5000)},
   {"RequestTimeout", rclcpp::ParameterValue(1000)},
   {"ResetOnIdle", rclcpp::ParameterValue(200)},
+  {"RequestMissingPackets", rclcpp::ParameterValue(true)},
+  {"ResendDelay", rclcpp::ParameterValue(0)}, // us
+  {"GevSCPSPacketSize", rclcpp::ParameterValue(0)}, // 0 = Auto
+  {"GevSCPD", rclcpp::ParameterValue(0)}, // ns
+
 
   /* Calibration file parameters */
   {"camera_calibration_file", rclcpp::ParameterValue("")},

diff --git a/src/bottlenose_camera_driver.cpp b/src/bottlenose_camera_driver.cpp
@@ -626,12 +626,6 @@ bool CameraDriver::connect() {
     return false;
   }
   PvGenInteger *intval = dynamic_cast<PvGenInteger *>( device->GetParameters()->Get("GevSCPSPacketSize"));
-  assert(intval != nullptr);
-  int64_t val;
-  intval->GetValue(val);
-  if(val < 8000) {
-    RCLCPP_WARN(get_logger(), "Current MTU is %ld, please set to at least 8K for reliable image transfer", val);
-  }
   PvStream *stream = PvStream::CreateAndOpen( pDevice->GetConnectionID(), &res );
   if(res.IsFailure() || stream == nullptr) {
     RCLCPP_ERROR(get_logger(), "Could not open device %s, cause %s", m_mac_address.c_str(), res.GetCodeString().GetAscii());
@@ -651,6 +645,29 @@ bool CameraDriver::connect() {
     disconnect();
     return false;
   }
+  // Check if packet size should be adjusted manually
+  if(get_parameter("GevSCPSPacketSize").as_int() > 0) {
+    res = intval->SetValue(get_parameter("GevSCPSPacketSize").as_int());
+    if(res.IsFailure()) {
+      RCLCPP_ERROR_STREAM(get_logger(), "Could not set packet size to " << get_parameter("GevSCPSPacketSize").as_int() << " cause " << res.GetDescription().GetAscii());
+      disconnect();
+      return false;
+    }
+    RCLCPP_DEBUG_STREAM(get_logger(), "Set packet size to " << get_parameter("GevSCPSPacketSize").as_int());
+  } else {
+    int64_t val;
+    intval->GetValue(val);
+    RCLCPP_DEBUG_STREAM(get_logger(), "Using auto negotiated packet size of " << val << " bytes");
+  }
+  // Adjust inter-packet delay as needed
+  intval = static_cast<PvGenInteger *>( device->GetParameters()->Get("GevSCPD"));
+  res = intval->SetValue(get_parameter("GevSCPD").as_int());
+  if(res.IsFailure()) {
+    RCLCPP_ERROR_STREAM(get_logger(), "Could not set inter-packet delay to " << get_parameter("GevSCPD").as_int() << " cause " << res.GetDescription().GetAscii());
+    disconnect();
+    return false;
+  }
+  RCLCPP_DEBUG(get_logger(), "Set inter-packet delay to %ld", get_parameter("GevSCPD").as_int());
   res = m_device->SetStreamDestination( m_stream->GetLocalIPAddress(), m_stream->GetLocalPort() );
   if(res.IsFailure()) {
     RCLCPP_ERROR(get_logger(), "Could not set stream destination to localhost");
@@ -676,16 +693,24 @@ bool CameraDriver::connect() {
                            "MaximumResendRequestRetryByPacket",
                            "MaximumResendGroupSize",
                            "ResendRequestTimeout",
-                           "RequestTimeout"}) {
+                           "RequestTimeout",
+                           "ResendDelay"}) {
     intval = static_cast<PvGenInteger *>( stream->GetParameters()->Get(param));
     res = intval->SetValue(get_parameter(param).as_int());
     if (res.IsFailure()) {
-      RCLCPP_ERROR(get_logger(), "Could not adjust communication parameters");
+      RCLCPP_ERROR_STREAM(get_logger(), "Could not adjust communication parameter" << param << " to " << get_parameter(param).as_int() << " cause " << res.GetDescription().GetAscii());
       disconnect();
       return false;
     }
     RCLCPP_DEBUG_STREAM(get_logger(), "Set " << param << " to " << get_parameter(param).as_int());
   }
+  PvGenBoolean *boolval = static_cast<PvGenBoolean *>( stream->GetParameters()->Get("RequestMissingPackets"));
+  res = boolval->SetValue(get_parameter("RequestMissingPackets").as_bool());
+  if (res.IsFailure()) {
+    RCLCPP_ERROR(get_logger(), "Could not adjust RequestMissingPackets parameter");
+    disconnect();
+    return false;
+  }
 
   return true;
 }