From ef5896398cb632514ec0d8c74241ec79cfb01c97 Mon Sep 17 00:00:00 2001 From: Thomas Reidemeister Date: Fri, 23 Feb 2024 14:31:25 -0500 Subject: [PATCH] Expose packet configuration for ROS2 --- README.md | 4 +++ include/bottlenose_parameters.hpp | 5 ++++ src/bottlenose_camera_driver.cpp | 41 +++++++++++++++++++++++++------ 3 files changed, 42 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index bbf60f8..c44dd28 100644 --- 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 index 3581108..a0eb642 100644 --- 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 index 93719de..3109eec 100644 --- 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( 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( 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( 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( 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; }