fixed bug when playing back from file

ros-event-camera · Nov 23, 2023 · 0f00bad · 0f00bad
1 parent bb53c1c
commit 0f00bad
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 16 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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();