Added timeout function

Signed-off-by: Jakub Delicat <[email protected]>
husarion · Dec 6, 2024 · 72fd4ce · 72fd4ce
1 parent cb249d6
commit 72fd4ce
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 16 deletions.
diff --git 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
@@ -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"