From dad30740be99f32cd84bdf32df7d7917221ee7d0 Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Tue, 20 May 2025 20:12:08 -0400
Subject: [PATCH] clean exit if can't open pcb

---
 .../modelec_com/pcb_action_interface.hpp      |  2 +
 .../modelec_com/pcb_alim_interface.hpp        |  2 +
 .../include/modelec_com/pcb_odo_interface.hpp |  2 +
 src/modelec_com/src/pcb_action_interface.cpp  |  7 ++-
 src/modelec_com/src/pcb_alim_interface.cpp    | 49 ++++++++++++++++-
 src/modelec_com/src/pcb_odo_interface.cpp     | 52 ++++++++++++++++++-
 6 files changed, 111 insertions(+), 3 deletions(-)

diff --git a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp
index ca8850a..1ab53d0 100644
--- a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp
+++ b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp
@@ -60,6 +60,8 @@ namespace Modelec
         rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_res_pub_;
         rclcpp::Publisher<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_move_res_pub_;
 
+
+        bool isOk = false;
     public:
         void SendToPCB(const std::string& data) const;
         void SendToPCB(const std::string& order, const std::string& elem,
diff --git a/src/modelec_com/include/modelec_com/pcb_alim_interface.hpp b/src/modelec_com/include/modelec_com/pcb_alim_interface.hpp
index b3ff29b..3f47b42 100644
--- a/src/modelec_com/include/modelec_com/pcb_alim_interface.hpp
+++ b/src/modelec_com/include/modelec_com/pcb_alim_interface.hpp
@@ -96,6 +96,8 @@ namespace Modelec
         void ResolveSetPCBEmgRequest(bool value);
         void ResolveGetPCBTempRequest(const PCBData& value);
 
+        bool isOk = false;
+
     public:
         void SendToPCB(const std::string& data) const;
         void SendToPCB(const std::string& order, const std::string& elem,
diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
index 87697ee..90f4779 100644
--- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
+++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
@@ -135,6 +135,8 @@ namespace Modelec
         int timeout_ms = 1000;
         int attempt = 5;
 
+        bool isOk = false;
+
     public:
         void SendToPCB(const std::string& data) const;
         void SendToPCB(const std::string& order, const std::string& elem,
diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp
index ea6106c..3ec3792 100644
--- a/src/modelec_com/src/pcb_action_interface.cpp
+++ b/src/modelec_com/src/pcb_action_interface.cpp
@@ -58,6 +58,8 @@ namespace Modelec
                     });
 
                     pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
+
+                    isOk = true;
                 }
                 else
                 {
@@ -214,7 +216,10 @@ namespace Modelec
 
     PCBActionInterface::~PCBActionInterface()
     {
-        pcb_executor_->cancel();
+        if (pcb_executor_)
+        {
+            pcb_executor_->cancel();
+        }
         if (pcb_executor_thread_.joinable())
         {
             pcb_executor_thread_.join();
diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp
index fd9e231..ca6b65a 100644
--- a/src/modelec_com/src/pcb_alim_interface.cpp
+++ b/src/modelec_com/src/pcb_alim_interface.cpp
@@ -58,6 +58,8 @@ namespace Modelec
                     });
 
                     pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
+
+                    isOk = true;
                 }
                 else
                 {
@@ -131,7 +133,10 @@ namespace Modelec
 
     PCBAlimInterface::~PCBAlimInterface()
     {
-        pcb_executor_->cancel();
+        if (pcb_executor_)
+        {
+            pcb_executor_->cancel();
+        }
         if (pcb_executor_thread_.joinable())
         {
             pcb_executor_thread_.join();
@@ -220,6 +225,13 @@ namespace Modelec
     void PCBAlimInterface::HandleGetPCBOutData(const std::shared_ptr<modelec_interfaces::srv::AlimOut::Request> request,
                                                std::shared_ptr<modelec_interfaces::srv::AlimOut::Response> response)
     {
+        if (!isOk)
+        {
+            RCLCPP_ERROR(get_logger(), "PCB not initialized");
+            response->success = false;
+            return;
+        }
+
         std::promise<PCBData> promise;
         auto future = promise.get_future();
 
@@ -239,6 +251,13 @@ namespace Modelec
     void PCBAlimInterface::HandleSetPCBOutData(const std::shared_ptr<modelec_interfaces::srv::AlimOut::Request> request,
                                                std::shared_ptr<modelec_interfaces::srv::AlimOut::Response> response)
     {
+        if (!isOk)
+        {
+            RCLCPP_ERROR(get_logger(), "PCB not initialized");
+            response->success = false;
+            return;
+        }
+
         std::promise<PCBData> promise;
         auto future = promise.get_future();
 
@@ -258,6 +277,13 @@ namespace Modelec
     void PCBAlimInterface::HandleGetPCBInData(const std::shared_ptr<modelec_interfaces::srv::AlimIn::Request> request,
                                               std::shared_ptr<modelec_interfaces::srv::AlimIn::Response> response)
     {
+        if (!isOk)
+        {
+            RCLCPP_ERROR(get_logger(), "PCB not initialized");
+            response->success = false;
+            return;
+        }
+
         std::promise<PCBData> promise;
         auto future = promise.get_future();
 
@@ -277,6 +303,13 @@ namespace Modelec
     void PCBAlimInterface::HandleGetPCBBauData(const std::shared_ptr<modelec_interfaces::srv::AlimBau::Request>,
                                                std::shared_ptr<modelec_interfaces::srv::AlimBau::Response> response)
     {
+        if (!isOk)
+        {
+            RCLCPP_ERROR(get_logger(), "PCB not initialized");
+            response->success = false;
+            return;
+        }
+
         std::promise<PCBBau> promise;
         auto future = promise.get_future();
 
@@ -295,6 +328,13 @@ namespace Modelec
     void PCBAlimInterface::HandleSetPCBEmgData(const std::shared_ptr<modelec_interfaces::srv::AlimEmg::Request> request,
                                                std::shared_ptr<modelec_interfaces::srv::AlimEmg::Response> response)
     {
+        if (!isOk)
+        {
+            RCLCPP_ERROR(get_logger(), "PCB not initialized");
+            response->success = false;
+            return;
+        }
+
         std::promise<bool> promise;
         auto future = promise.get_future();
 
@@ -312,6 +352,13 @@ namespace Modelec
         const std::shared_ptr<modelec_interfaces::srv::AlimTemp::Request>,
         std::shared_ptr<modelec_interfaces::srv::AlimTemp::Response> response)
     {
+        if (!isOk)
+        {
+            RCLCPP_ERROR(get_logger(), "PCB not initialized");
+            response->success = false;
+            return;
+        }
+
         std::promise<PCBData> promise;
         auto future = promise.get_future();
 
diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp
index ff1987f..4572fde 100644
--- a/src/modelec_com/src/pcb_odo_interface.cpp
+++ b/src/modelec_com/src/pcb_odo_interface.cpp
@@ -60,6 +60,8 @@ namespace Modelec
                     });
 
                     pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
+
+                    isOk = true;
                 }
                 else
                 {
@@ -172,7 +174,10 @@ namespace Modelec
 
     PCBOdoInterface::~PCBOdoInterface()
     {
-        pcb_executor_->cancel();
+        if (pcb_executor_)
+        {
+            pcb_executor_->cancel();
+        }
         if (pcb_executor_thread_.joinable())
         {
             pcb_executor_thread_.join();
@@ -327,6 +332,12 @@ namespace Modelec
         const std::shared_ptr<modelec_interfaces::srv::OdometryToF::Request> request,
         std::shared_ptr<modelec_interfaces::srv::OdometryToF::Response> response)
     {
+        if (!isOk)
+        {
+            RCLCPP_ERROR(get_logger(), "PCB not initialized");
+            return;
+        }
+
         std::promise<long> promise;
         auto future = promise.get_future();
 
@@ -344,6 +355,12 @@ namespace Modelec
         const std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Request>,
         std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Response> response)
     {
+        if (!isOk)
+        {
+            RCLCPP_ERROR(get_logger(), "PCB not initialized");
+            return;
+        }
+
         std::promise<OdometryData> promise;
         auto future = promise.get_future();
 
@@ -364,6 +381,12 @@ namespace Modelec
         const std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Request>,
         std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Response> response)
     {
+        if (!isOk)
+        {
+            RCLCPP_ERROR(get_logger(), "PCB not initialized");
+            return;
+        }
+
         std::promise<OdometryData> promise;
         auto future = promise.get_future();
 
@@ -383,6 +406,13 @@ namespace Modelec
     void PCBOdoInterface::HandleGetStart(const std::shared_ptr<modelec_interfaces::srv::OdometryStart::Request> request,
                                          std::shared_ptr<modelec_interfaces::srv::OdometryStart::Response> response)
     {
+        if (!isOk)
+        {
+            RCLCPP_ERROR(get_logger(), "PCB not initialized");
+            response->success = false;
+            return;
+        }
+
         std::promise<bool> promise;
         auto future = promise.get_future();
 
@@ -398,6 +428,12 @@ namespace Modelec
     void PCBOdoInterface::HandleGetPID(const std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Request>,
                                        std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response)
     {
+        if (!isOk)
+        {
+            RCLCPP_ERROR(get_logger(), "PCB not initialized");
+            return;
+        }
+
         std::promise<PIDData> promise;
         auto future = promise.get_future();
 
@@ -418,6 +454,13 @@ namespace Modelec
     void PCBOdoInterface::HandleSetPID(const std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Request> request,
                                        std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Response> response)
     {
+        if (!isOk)
+        {
+            RCLCPP_ERROR(get_logger(), "PCB not initialized");
+            response->success = false;
+            return;
+        }
+
         std::promise<bool> promise;
         auto future = promise.get_future();
 
@@ -436,6 +479,13 @@ namespace Modelec
         const std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Request> request,
         std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Response> response)
     {
+        if (!isOk)
+        {
+            RCLCPP_ERROR(get_logger(), "PCB not initialized");
+            response->success = false;
+            return;
+        }
+
         std::promise<bool> promise;
         auto future = promise.get_future();