From 0f00badac8ec7472430e211760a9fe42af335cec Mon Sep 17 00:00:00 2001
From: Bernd Pfrommer <bernd.pfrommer@gmail.com>
Date: Thu, 23 Nov 2023 12:53:37 +0000
Subject: [PATCH] fixed bug when playing back from file

---
 .../metavision_driver/metavision_wrapper.h    |  1 +
 src/driver_ros1.cpp                           | 14 +++++++------
 src/driver_ros2.cpp                           | 20 ++++++++++---------
 src/metavision_wrapper.cpp                    |  2 +-
 4 files changed, 21 insertions(+), 16 deletions(-)

diff --git a/include/metavision_driver/metavision_wrapper.h b/include/metavision_driver/metavision_wrapper.h
index 4b7b3bb..74598bd 100644
--- a/include/metavision_driver/metavision_wrapper.h
+++ b/include/metavision_driver/metavision_wrapper.h
@@ -88,6 +88,7 @@ class MetavisionWrapper
   const std::string & getExternalTriggerInMode() const { return (triggerInMode_); }
   const std::string & getSyncMode() const { return (syncMode_); }
   const std::string & getSensorVersion() const { return (sensorVersion_); }
+  const std::string & getFromFile() const { return (fromFile_); }
 
   void setSerialNumber(const std::string & sn) { serialNumber_ = sn; }
   void setFromFile(const std::string & f) { fromFile_ = f; }
diff --git a/src/driver_ros1.cpp b/src/driver_ros1.cpp
index bc68fce..c6a0fb5 100644
--- a/src/driver_ros1.cpp
+++ b/src/driver_ros1.cpp
@@ -158,13 +158,15 @@ void DriverROS1::start()
   // ------ start camera, may get callbacks from then on
   wrapper_->startCamera(this);
 
-  initializeBiasParameters(wrapper_->getSensorVersion());
-  // hook up dynamic config server *after* the camera has
-  // been initialized so we can read the bias values
-  configServer_.reset(new dynamic_reconfigure::Server<Config>(nh_));
-  configServer_->setCallback(boost::bind(&DriverROS1::configure, this, _1, _2));
+  if (wrapper_->getFromFile().empty()) {
+    initializeBiasParameters(wrapper_->getSensorVersion());
+    // hook up dynamic config server *after* the camera has
+    // been initialized so we can read the bias values
+    configServer_.reset(new dynamic_reconfigure::Server<Config>(nh_));
+    configServer_->setCallback(boost::bind(&DriverROS1::configure, this, _1, _2));
 
-  saveBiasService_ = nh_.advertiseService("save_biases", &DriverROS1::saveBiases, this);
+    saveBiasService_ = nh_.advertiseService("save_biases", &DriverROS1::saveBiases, this);
+  }
 }
 
 void DriverROS1::initializeBiasParameters(const std::string & sensorVersion)
diff --git a/src/driver_ros2.cpp b/src/driver_ros2.cpp
index d96b36f..bb7ec44 100644
--- a/src/driver_ros2.cpp
+++ b/src/driver_ros2.cpp
@@ -252,16 +252,18 @@ void DriverROS2::start()
   // ------ start camera, may get callbacks from then on
   wrapper_->startCamera(this);
 
-  declareBiasParameters(wrapper_->getSensorVersion());
-  callbackHandle_ = this->add_on_set_parameters_callback(
-    std::bind(&DriverROS2::parameterChanged, this, std::placeholders::_1));
-  parameterSubscription_ = rclcpp::AsyncParametersClient::on_parameter_event(
-    this->get_node_topics_interface(),
-    std::bind(&DriverROS2::onParameterEvent, this, std::placeholders::_1));
+  if (wrapper_->getFromFile().empty()) {
+    declareBiasParameters(wrapper_->getSensorVersion());
+    callbackHandle_ = this->add_on_set_parameters_callback(
+      std::bind(&DriverROS2::parameterChanged, this, std::placeholders::_1));
+    parameterSubscription_ = rclcpp::AsyncParametersClient::on_parameter_event(
+      this->get_node_topics_interface(),
+      std::bind(&DriverROS2::onParameterEvent, this, std::placeholders::_1));
 
-  saveBiasesService_ = this->create_service<Trigger>(
-    "save_biases",
-    std::bind(&DriverROS2::saveBiases, this, std::placeholders::_1, std::placeholders::_2));
+    saveBiasesService_ = this->create_service<Trigger>(
+      "save_biases",
+      std::bind(&DriverROS2::saveBiases, this, std::placeholders::_1, std::placeholders::_2));
+  }
 }
 
 bool DriverROS2::stop()
diff --git a/src/metavision_wrapper.cpp b/src/metavision_wrapper.cpp
index a4b5c13..4852545 100644
--- a/src/metavision_wrapper.cpp
+++ b/src/metavision_wrapper.cpp
@@ -336,7 +336,7 @@ bool MetavisionWrapper::initializeCamera()
         LOG_WARN_NAMED("reading bias file failed with error: " << e.what());
         LOG_WARN_NAMED("continuing with default biases!");
       }
-    } else {
+    } else if (fromFile_.empty()) {  // only load biases when not playing from file!
       LOG_INFO_NAMED("no bias file provided, using camera defaults:");
       const Metavision::Biases biases = cam_.biases();
       Metavision::I_LL_Biases * hw_biases = biases.get_facility();