From 6cc521dfba60f9cdfb9f17cb8d1edef727e190b6 Mon Sep 17 00:00:00 2001
From: psmithcrl <patrick.smith@carnegierobotics.com>
Date: Tue, 6 Aug 2024 12:29:11 -0400
Subject: [PATCH] Make sure the fps change has been accepted by camera before
 finishing reconfiguration

---
 .../include/multisense_ros/camera.h           |  7 ++++
 multisense_ros/src/camera.cpp                 | 36 ++++++++++++++++++-
 2 files changed, 42 insertions(+), 1 deletion(-)

diff --git a/multisense_ros/include/multisense_ros/camera.h b/multisense_ros/include/multisense_ros/camera.h
index f9d2bd6..8c73b57 100644
--- a/multisense_ros/include/multisense_ros/camera.h
+++ b/multisense_ros/include/multisense_ros/camera.h
@@ -165,6 +165,13 @@ class Camera : public rclcpp::Node
 
     void initalizeParameters(const crl::multisense::image::Config& config);
 
+    //
+    // Helper function to verify that an fps change has been accepted by the camera.
+    // FPS Changes can take time on certain versions of camera firmware.
+    // To verify changes have been accepted by the camera we will poll the value
+    // up to 0.5s.
+    crl::multisense::Status verifyFpsChange(const crl::multisense::image::Config& config);
+
     //
     // Parameter management
 
diff --git a/multisense_ros/src/camera.cpp b/multisense_ros/src/camera.cpp
index aa578de..1b81ee3 100644
--- a/multisense_ros/src/camera.cpp
+++ b/multisense_ros/src/camera.cpp
@@ -1927,6 +1927,35 @@ void Camera::timerCallback()
     }
 }
 
+crl::multisense::Status Camera::verifyFpsChange(const crl::multisense::image::Config& config)
+{
+    int timeout = 50; //500ms
+    if (const auto status = driver_->setImageConfig(config) != Status_Ok)
+    {
+        RCLCPP_ERROR(get_logger(), "Error: Camera failed to set fps");
+        return status;
+    }
+
+    do {
+
+        image::Config i;
+        if (const auto status = driver_->getImageConfig(i); status != Status_Ok) {
+            RCLCPP_ERROR(get_logger(), "Camera: failed to query sensor configuration: %s",
+                         Channel::statusString(status));
+            return status;
+        }
+
+        if (std::abs(config.fps() - i.fps()) < 1.0f) {
+            return Status_Ok;
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    } while((timeout-->0));
+
+    return Status_TimedOut;
+}
+
+
 void Camera::initalizeParameters(const image::Config& config)
 {
     //
@@ -2741,7 +2770,12 @@ rcl_interfaces::msg::SetParametersResult Camera::parameterCallback(const std::ve
             if (image_config.fps() != value)
             {
                 image_config.setFps(value);
-                update_image_config = true;
+                if (const auto status = verifyFpsChange(image_config); status != Status_Ok) {
+                  RCLCPP_ERROR(get_logger(), "Error: failed to update fps on the camera!");
+                  return result.set__successful(false).set__reason(Channel::statusString(status));
+                }
+
+                updateConfig(image_config);
             }
         }
         else if(name == "stereo_post_filtering")