From 72fd4cecab23668f23bab804de69e89dab7db081 Mon Sep 17 00:00:00 2001
From: Jakub Delicat <jakub.delicat@husarion.com>
Date: Fri, 6 Dec 2024 13:11:19 +0000
Subject: [PATCH] Added timeout function

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>
---
 .../panther_docking/panther_charging_dock.hpp |  2 +
 panther_docking/src/panther_charging_dock.cpp | 41 +++++++++++--------
 2 files changed, 27 insertions(+), 16 deletions(-)

diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp
index f0137681..c58ed184 100644
--- a/panther_docking/include/panther_docking/panther_charging_dock.hpp
+++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp
@@ -179,6 +179,8 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock
    */
   void setDockPosePublisherState(std::uint8_t state);
 
+  bool IsWiboticInfoTimeout();
+
   std::string base_frame_name_;
   std::string fixed_frame_name_;
   std::string dock_frame_;
diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp
index efb28252..b0eb1df5 100644
--- a/panther_docking/src/panther_charging_dock.cpp
+++ b/panther_docking/src/panther_charging_dock.cpp
@@ -204,22 +204,7 @@ bool PantherChargingDock::isCharging()
       return false;
     }
 
-    if (!wibotic_info_) {
-      setDockPosePublisherState(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE);
-      throw opennav_docking_core::FailedToCharge("No Wibotic info received.");
-    }
-
-    rclcpp::Time requested_wibotic_info_time;
-    {
-      auto node = node_.lock();
-      requested_wibotic_info_time = node->now();
-    }
-
-    const auto duration = requested_wibotic_info_time - wibotic_info_->header.stamp;
-    if (duration > rclcpp::Duration::from_seconds(wibotic_info_timeout_)) {
-      RCLCPP_WARN_STREAM(
-        logger_, "Wibotic info is outdated. Time difference is: "
-                   << duration.seconds() << "s but timeout is " << wibotic_info_timeout_ << "s.");
+    if (IsWiboticInfoTimeout()) {
       return false;
     }
 
@@ -285,6 +270,30 @@ void PantherChargingDock::setDockPosePublisherState(std::uint8_t state)
   dock_pose_publisher_change_state_client_->async_send_request(request);
 }
 
+bool PantherChargingDock::IsWiboticInfoTimeout()
+{
+  if (!wibotic_info_) {
+    RCLCPP_ERROR_STREAM(
+      logger_, "Wibotic info is not set. This should not happen. Check the Wibotic info topic.");
+    return true;
+  }
+
+  rclcpp::Time requested_wibotic_info_time;
+  {
+    auto node = node_.lock();
+    requested_wibotic_info_time = node->now();
+  }
+
+  const auto duration = requested_wibotic_info_time - wibotic_info_->header.stamp;
+  if (duration > rclcpp::Duration::from_seconds(wibotic_info_timeout_)) {
+    RCLCPP_WARN_STREAM(
+      logger_, "Wibotic info is outdated. Time difference is: "
+                 << duration.seconds() << "s but timeout is " << wibotic_info_timeout_ << "s.");
+    return true;
+  }
+  return false;
+}
+
 }  // namespace panther_docking
 
 #include "pluginlib/class_list_macros.hpp"