pcb alim

src/modelec/include/modelec/pcb_alim_interface.hpp

@@ -2,6 +2,11 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <std_msgs/msg/string.hpp>
+#include <modelec_interface/srv/alim_out.hpp>
+#include <modelec_interface/srv/alim_in.hpp>
+#include <modelec_interface/srv/alim_bau.hpp>
+#include <modelec_interface/srv/alim_emg.hpp>
+#include <modelec_interface/srv/alim_temp.hpp>
 
 namespace Modelec
 {
@@ -15,6 +20,17 @@ public:
 
     ~PCBAlimInterface() override;
 
+    struct PCBData
+    {
+        bool success;
+        int value;
+    };
+
+    struct PCBBau
+    {
+        bool success;
+        bool activate;
+    };
 private:
 
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pcb_publisher_;
@@ -22,40 +38,64 @@ private:
 
     void PCBCallback(const std_msgs::msg::String::SharedPtr msg);
 
-    void SendToPCB(const std::string &data);
-    void SendToPCB(const std::string& order, const std::string& elem, const std::string& data, const std::string& value = std::string());
+    rclcpp::Service<modelec_interface::srv::AlimOut>::SharedPtr pcb_out_service_;
+    rclcpp::Service<modelec_interface::srv::AlimIn>::SharedPtr pcb_in_service_;
+    rclcpp::Service<modelec_interface::srv::AlimBau>::SharedPtr pcb_bau_service_;
+    rclcpp::Service<modelec_interface::srv::AlimEmg>::SharedPtr pcb_emg_service_;
+    rclcpp::Service<modelec_interface::srv::AlimTemp>::SharedPtr pcb_temp_service_;
 
-    void GetData(const std::string &elem, const std::string& data, const std::string& value = std::string());
+    std::queue<std::promise<PCBData>> pcb_out_promises_;
+    std::mutex pcb_out_mutex_;
 
-    void SendOrder(const std::string &elem, const std::string& data, const std::string& value = std::string());
+    std::queue<std::promise<PCBData>> pcb_in_promises_;
+    std::mutex pcb_in_mutex_;
 
-    // get data
-    void GetEmergencyStopButtonState();
-    void GetEntryVoltage(int entry);
-    void GetEntryCurrent(int entry);
-    void GetEntryState(int entry);
-    void GetEntryIsValide(int entry);
-    void GetPCBTemperature();
-    
-    void GetOutput5VState();
-    void GetOutput5VVoltage();
-    void GetOutput5VCurrent();
+    std::queue<std::promise<PCBBau>> pcb_bau_promises_;
+    std::mutex pcb_bau_mutex_;
 
-    void GetOutput5V1State();
-    void GetOutput5V1Voltage();
-    void GetOutput5V1Current();
+    std::queue<std::promise<bool>> pcb_emg_promises_;
+    std::mutex pcb_emg_mutex_;
 
-    void GetOutput12VState();
-    void GetOutput12VVoltage();
-    void GetOutput12VCurrent();
+    std::queue<std::promise<PCBData>> pcb_temp_promises_;
+    std::mutex pcb_temp_mutex_;
 
-    void GetOutput24VState();
-    void GetOutput24VVoltage();
-    void GetOutput24VCurrent();
+    void HandleGetPCBOutData(
+        const std::shared_ptr<modelec_interface::srv::AlimOut::Request> request,
+        std::shared_ptr<modelec_interface::srv::AlimOut::Response> response);
+
+    void HandleSetPCBOutData(
+        const std::shared_ptr<modelec_interface::srv::AlimOut::Request> request,
+        std::shared_ptr<modelec_interface::srv::AlimOut::Response> response);
+
+    void HandleGetPCBInData(
+        const std::shared_ptr<modelec_interface::srv::AlimIn::Request> request,
+        std::shared_ptr<modelec_interface::srv::AlimIn::Response> response);
+
+    void HandleGetPCBBauData(
+        const std::shared_ptr<modelec_interface::srv::AlimBau::Request> request,
+        std::shared_ptr<modelec_interface::srv::AlimBau::Response> response);
+
+    void HandleSetPCBEmgData(
+        const std::shared_ptr<modelec_interface::srv::AlimEmg::Request> request,
+        std::shared_ptr<modelec_interface::srv::AlimEmg::Response> response);
+
+    void HandleGetPCBTempData(
+        const std::shared_ptr<modelec_interface::srv::AlimTemp::Request> request,
+        std::shared_ptr<modelec_interface::srv::AlimTemp::Response> response);
+
+    void ResolveGetPCBOutRequest(const PCBData& value);
+    void ResolveSetPCBOutRequest(const PCBData& value);
+    void ResolveGetPCBInRequest(const PCBData& value);
+    void ResolveGetPCBBauRequest(const PCBBau& value);
+    void ResolveSetPCBEmgRequest(const PCBData& value);
+    void ResolveGetPCBTempRequest(const PCBData& value);
+
+public:
+    void SendToPCB(const std::string &data) const;
+    void SendToPCB(const std::string& order, const std::string& elem, const std::vector<std::string>& data = {}) const;
+
+    void GetData(const std::string& elem, const std::vector<std::string>& data = {}) const;
+    void SendOrder(const std::string &elem, const std::vector<std::string> &data = {}) const;
 
-    void SetSoftwareEmergencyStop(bool state);
-    void Set5VState(bool state);
-    void Set12VState(bool state);
-    void Set24VState(bool state);
 };
 }  // namespace Modelec

src/modelec/src/pcb_alim_interface.cpp

@@ -1,4 +1,5 @@
 #include <modelec/pcb_alim_interface.hpp>
+#include <modelec/utils.hpp>
 #include <modelec_interface/srv/add_serial_listener.hpp>
 
 namespace Modelec
@@ -38,12 +39,16 @@ namespace Modelec
 
                     pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
                         result.get()->publisher, 10,
-                        std::bind(&PCBAlimInterface::PCBCallback, this, std::placeholders::_1), options);
+                        [this](const std_msgs::msg::String::SharedPtr msg)
+                        {
+                            PCBCallback(msg);
+                        }, options);
 
                     pcb_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
                     pcb_executor_->add_callback_group(pcb_callback_group_, this->get_node_base_interface());
 
-                    pcb_executor_thread_ = std::thread([this]() {
+                    pcb_executor_thread_ = std::thread([this]()
+                    {
                         pcb_executor_->spin();
                     });
 
@@ -63,170 +68,326 @@ namespace Modelec
         {
             RCLCPP_ERROR(this->get_logger(), "Service call failed");
         }
+
+        pcb_out_service_ = create_service<modelec_interface::srv::AlimOut>(
+            "alim/out",
+            [this](const std::shared_ptr<modelec_interface::srv::AlimOut::Request> request,
+                   std::shared_ptr<modelec_interface::srv::AlimOut::Response> response)
+            {
+                if (request->enable != -1 && request->type == modelec_interface::srv::AlimOut::Request::STATE)
+                {
+                    HandleSetPCBOutData(request, response);
+                }
+                else
+                {
+                    HandleGetPCBOutData(request, response);
+                }
+            });
+
+        pcb_in_service_ = create_service<modelec_interface::srv::AlimIn>(
+            "alim/in",
+            [this](const std::shared_ptr<modelec_interface::srv::AlimIn::Request> request,
+                   std::shared_ptr<modelec_interface::srv::AlimIn::Response> response)
+            {
+                HandleGetPCBInData(request, response);
+            });
+
+        pcb_bau_service_ = create_service<modelec_interface::srv::AlimBau>(
+            "alim/bau",
+            [this](const std::shared_ptr<modelec_interface::srv::AlimBau::Request> request,
+                   std::shared_ptr<modelec_interface::srv::AlimBau::Response> response)
+            {
+                HandleGetPCBBauData(request, response);
+            });
+
+        pcb_emg_service_ = create_service<modelec_interface::srv::AlimEmg>(
+            "alim/emg",
+            [this](const std::shared_ptr<modelec_interface::srv::AlimEmg::Request> request,
+                   std::shared_ptr<modelec_interface::srv::AlimEmg::Response> response)
+            {
+                HandleSetPCBEmgData(request, response);
+            });
+
+        pcb_temp_service_ = create_service<modelec_interface::srv::AlimTemp>(
+            "alim/temp",
+            [this](const std::shared_ptr<modelec_interface::srv::AlimTemp::Request> request,
+                   std::shared_ptr<modelec_interface::srv::AlimTemp::Response> response)
+            {
+                HandleGetPCBTempData(request, response);
+            });
     }
 
     PCBAlimInterface::~PCBAlimInterface()
     {
         pcb_executor_->cancel();
-        if (pcb_executor_thread_.joinable()) {
+        if (pcb_executor_thread_.joinable())
+        {
             pcb_executor_thread_.join();
         }
     }
 
     void PCBAlimInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg)
     {
-        RCLCPP_INFO(this->get_logger(), "Received message: '%s'", msg->data.c_str());
+        RCLCPP_DEBUG(this->get_logger(), "Received message: '%s'", msg->data.c_str());
+        std::vector<std::string> tokens = split(msg->data, ';');
+        if (tokens.size() < 2)
+        {
+            RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg->data.c_str());
+            return;
+        }
+
+        if (tokens[0] == "SET")
+        {
+            if (startsWith(tokens[1], "OUT"))
+            {
+                bool success = true;
+                int value = std::stoi(tokens[3]);
+                ResolveGetPCBOutRequest({success, value});
+            }
+            else if (startsWith(tokens[1], "IN"))
+            {
+                bool success = true;
+                int value = std::stoi(tokens[3]);
+                ResolveGetPCBInRequest({success, value});
+            }
+            else if (tokens[1] == "BAU")
+            {
+                bool success = true;
+                bool activate = (tokens[3] == "1");
+                ResolveGetPCBBauRequest({success, activate});
+            }
+            else if (tokens[1] == "TEMP")
+            {
+                bool success = true;
+                int value = std::stoi(tokens[3]);
+                ResolveGetPCBTempRequest({success, value});
+            }
+        }
+        else if (tokens[0] == "OK")
+        {
+            if (startsWith(tokens[1], "OUT"))
+            {
+                bool success = true;
+                int value = std::stoi(tokens[3]);
+
+                ResolveSetPCBOutRequest({success, value});
+            }
+            else if (tokens[1] == "EMG")
+            {
+                bool success = true;
+                bool value = (tokens[3] == "1");
+                ResolveSetPCBEmgRequest({success, value});
+            }
+        }
+        else if (tokens[0] == "KO")
+        {
+            if (startsWith(tokens[1], "OUT"))
+            {
+                bool success = false;
+                int value = -1;
+
+                ResolveSetPCBOutRequest({success, value});
+            }
+            else if (tokens[1] == "EMG")
+            {
+                bool success = false;
+                int value = -1;
+
+                ResolveSetPCBEmgRequest({success, value});
+            }
+        }
+        else
+        {
+            RCLCPP_ERROR(this->get_logger(), "Unknown message type: %s", tokens[0].c_str());
+        }
     }
 
-    void PCBAlimInterface::SendToPCB(const std::string& data)
+    void PCBAlimInterface::HandleGetPCBOutData(const std::shared_ptr<modelec_interface::srv::AlimOut::Request> request,
+                                               std::shared_ptr<modelec_interface::srv::AlimOut::Response> response)
+    {
+        std::promise<PCBData> promise;
+        auto future = promise.get_future();
+
+        {
+            std::lock_guard<std::mutex> lock(pcb_out_mutex_);
+            pcb_out_promises_.push(std::move(promise));
+        }
+
+        GetData(request->out, {request->type});
+
+        PCBData result = future.get();
+
+        response->success = result.success;
+        response->result = result.value;
+    }
+
+    void PCBAlimInterface::HandleSetPCBOutData(const std::shared_ptr<modelec_interface::srv::AlimOut::Request> request,
+                                               std::shared_ptr<modelec_interface::srv::AlimOut::Response> response)
+    {
+        std::promise<PCBData> promise;
+        auto future = promise.get_future();
+
+        {
+            std::lock_guard<std::mutex> lock(pcb_out_mutex_);
+            pcb_out_promises_.push(std::move(promise));
+        }
+
+        SendOrder(request->out, {request->type, request->enable == true ? "1" : "0"});
+
+        PCBData result = future.get();
+
+        response->success = result.success;
+        response->result = result.value;
+    }
+
+    void PCBAlimInterface::HandleGetPCBInData(const std::shared_ptr<modelec_interface::srv::AlimIn::Request> request,
+                                              std::shared_ptr<modelec_interface::srv::AlimIn::Response> response)
+    {
+        std::promise<PCBData> promise;
+        auto future = promise.get_future();
+
+        {
+            std::lock_guard<std::mutex> lock(pcb_out_mutex_);
+            pcb_out_promises_.push(std::move(promise));
+        }
+
+        GetData("IN" + request->input, {request->type});
+
+        PCBData result = future.get();
+
+        response->success = result.success;
+        response->result = result.value;
+    }
+
+    void PCBAlimInterface::HandleGetPCBBauData(const std::shared_ptr<modelec_interface::srv::AlimBau::Request> request,
+                                               std::shared_ptr<modelec_interface::srv::AlimBau::Response> response)
+    {
+        std::promise<PCBBau> promise;
+        auto future = promise.get_future();
+
+        {
+            std::lock_guard<std::mutex> lock(pcb_bau_mutex_);
+            pcb_bau_promises_.push(std::move(promise));
+        }
+
+        GetData("BAU", {"STATE"});
+
+        PCBBau result = future.get();
+        response->success = result.success;
+        response->activate = result.activate;
+    }
+
+    void PCBAlimInterface::HandleSetPCBEmgData(const std::shared_ptr<modelec_interface::srv::AlimEmg::Request> request,
+                                               std::shared_ptr<modelec_interface::srv::AlimEmg::Response> response)
+    {
+        std::promise<bool> promise;
+        auto future = promise.get_future();
+
+        {
+            std::lock_guard<std::mutex> lock(pcb_emg_mutex_);
+            pcb_emg_promises_.push(std::move(promise));
+        }
+
+        SendOrder("EMG", {"STATE", request->activate == true ? "1" : "0"});
+
+        response->success = future.get();
+    }
+
+    void PCBAlimInterface::HandleGetPCBTempData(
+        const std::shared_ptr<modelec_interface::srv::AlimTemp::Request> request,
+        std::shared_ptr<modelec_interface::srv::AlimTemp::Response> response)
+    {
+        std::promise<PCBData> promise;
+        auto future = promise.get_future();
+
+        {
+            std::lock_guard<std::mutex> lock(pcb_temp_mutex_);
+            pcb_temp_promises_.push(std::move(promise));
+        }
+
+        SendOrder("TEMP", {"CELS"});
+
+        PCBData result = future.get();
+        response->success = result.success;
+        response->value = result.value;
+    }
+
+    void PCBAlimInterface::ResolveGetPCBOutRequest(const PCBData& value)
+    {
+        std::lock_guard<std::mutex> lock(pcb_out_mutex_);
+        if (!pcb_out_promises_.empty())
+        {
+            auto promise = std::move(pcb_out_promises_.front());
+            pcb_out_promises_.pop();
+            promise.set_value(value);
+        }
+        else
+        {
+            RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB out data");
+        }
+    }
+
+    void PCBAlimInterface::ResolveSetPCBOutRequest(const PCBData& value)
+    {
+        std::lock_guard<std::mutex> lock(pcb_out_mutex_);
+        if (!pcb_out_promises_.empty())
+        {
+            auto promise = std::move(pcb_out_promises_.front());
+            pcb_out_promises_.pop();
+            promise.set_value(value);
+        }
+        else
+        {
+            RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB out data");
+        }
+    }
+
+    void PCBAlimInterface::ResolveGetPCBInRequest(const PCBData& value)
+    {
+        std::lock_guard<std::mutex> lock(pcb_in_mutex_);
+        if (!pcb_in_promises_.empty())
+        {
+            auto promise = std::move(pcb_in_promises_.front());
+            pcb_in_promises_.pop();
+            promise.set_value(value);
+        }
+        else
+        {
+            RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB in data");
+        }
+    }
+
+    void PCBAlimInterface::SendToPCB(const std::string& data) const
     {
         auto message = std_msgs::msg::String();
         message.data = data;
         pcb_publisher_->publish(message);
     }
 
-    void PCBAlimInterface::SendToPCB(const std::string& order, const std::string& elem, const std::string& data,
-                                     const std::string& value)
+    void PCBAlimInterface::SendToPCB(const std::string& order, const std::string& elem,
+                                    const std::vector<std::string>& data) const
     {
         std::string command = order + ";" + elem;
-        if (!data.empty())
+        for (const auto& d : data)
         {
-            command += ";" + data;
-        }
-        if (!value.empty())
-        {
-            command += ";" + value;
+            command += ";" + d;
         }
+
         SendToPCB(command);
     }
 
-    void PCBAlimInterface::GetData(const std::string& elem, const std::string& data, const std::string& value)
+    void PCBAlimInterface::GetData(const std::string& elem, const std::vector<std::string>& data) const
     {
-        SendToPCB("GET", elem, data, value);
+        SendToPCB("GET", elem, data);
     }
 
-    void PCBAlimInterface::SendOrder(const std::string& elem, const std::string& data, const std::string& value)
+    void PCBAlimInterface::SendOrder(const std::string& elem, const std::vector<std::string>& data) const
     {
-        SendToPCB("SET", elem, data, value);
-    }
-
-    void PCBAlimInterface::GetEmergencyStopButtonState()
-    {
-        GetData("BAU", "STATE");
-    }
-
-    void PCBAlimInterface::GetEntryVoltage(int entry)
-    {
-        GetData("IN" + std::to_string(entry), "VOLT");
-    }
-
-    void PCBAlimInterface::GetEntryCurrent(int entry)
-    {
-        GetData("IN" + std::to_string(entry), "AMP");
-    }
-
-    void PCBAlimInterface::GetEntryState(int entry)
-    {
-        GetData("IN" + std::to_string(entry), "STATE");
-    }
-
-    void PCBAlimInterface::GetEntryIsValide(int entry)
-    {
-        GetData("IN" + std::to_string(entry), "VALID");
-    }
-
-    void PCBAlimInterface::GetPCBTemperature()
-    {
-        GetData("TEMP", "CELS");
-    }
-
-
-    void PCBAlimInterface::GetOutput5VState()
-    {
-        GetData("OUT5V", "STATE");
-    }
-
-    void PCBAlimInterface::GetOutput5VVoltage()
-    {
-        GetData("OUT5V", "VOLT");
-    }
-
-    void PCBAlimInterface::GetOutput5VCurrent()
-    {
-        GetData("OUT5V", "AMP");
-    }
-
-
-    void PCBAlimInterface::GetOutput5V1State()
-    {
-        GetData("OUT5V1", "STATE");
-    }
-
-    void PCBAlimInterface::GetOutput5V1Voltage()
-    {
-        GetData("OUT5V1", "VOLT");
-    }
-
-    void PCBAlimInterface::GetOutput5V1Current()
-    {
-        GetData("OUT5V1", "AMP");
-    }
-
-
-    void PCBAlimInterface::GetOutput12VState()
-    {
-        GetData("OUT12V", "STATE");
-    }
-
-    void PCBAlimInterface::GetOutput12VVoltage()
-    {
-        GetData("OUT12V", "VOLT");
-    }
-
-    void PCBAlimInterface::GetOutput12VCurrent()
-    {
-        GetData("OUT12V", "AMP");
-    }
-
-
-    void PCBAlimInterface::GetOutput24VState()
-    {
-        GetData("OUT24V", "STATE");
-    }
-
-    void PCBAlimInterface::GetOutput24VVoltage()
-    {
-        GetData("OUT24V", "VOLT");
-    }
-
-    void PCBAlimInterface::GetOutput24VCurrent()
-    {
-        GetData("OUT24V", "AMP");
-    }
-
-
-    void PCBAlimInterface::SetSoftwareEmergencyStop(bool state)
-    {
-        SendOrder("EMG", "STATE", state ? "1" : "0");
-    }
-
-    void PCBAlimInterface::Set5VState(bool state)
-    {
-        SendOrder("OUT5V", "STATE", state ? "1" : "0");
-    }
-
-    void PCBAlimInterface::Set12VState(bool state)
-    {
-        SendOrder("OUT12V", "STATE", state ? "1" : "0");
-    }
-
-    void PCBAlimInterface::Set24VState(bool state)
-    {
-        SendOrder("OUT24V", "STATE", state ? "1" : "0");
+        SendToPCB("SET", elem, data);
     }
 } // namespace Modelec
 
-int main(int argc, char **argv)
+int main(int argc, char** argv)
 {
     rclcpp::init(argc, argv);
     auto node = std::make_shared<Modelec::PCBAlimInterface>();

src/modelec/src/pcb_odo_interface.cpp

@@ -272,7 +272,26 @@ namespace Modelec
         }
         else if (tokens[0] == "KO")
         {
-            RCLCPP_WARN(this->get_logger(), "PCB error: %s", msg->data.c_str());
+            if (tokens[1] == "START")
+            {
+                bool start = false;
+                ResolveStartRequest(start);
+            }
+            else if (tokens[1] == "WAYPOINT")
+            {
+                bool success = false;
+                ResolveAddWaypointRequest(success);
+            }
+
+            else if (tokens[1] == "PID")
+            {
+                bool success = false;
+                ResolveSetPIDRequest(success);
+            }
+            else
+            {
+                RCLCPP_WARN(this->get_logger(), "PCB error: %s", msg->data.c_str());
+            }
         }
     }
 

src/modelec_interface/CMakeLists.txt

@@ -22,6 +22,11 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/ServoMode.msg"
   "msg/Solenoid.msg"
   "msg/Button.msg"
+  "srv/Alim/AlimOut.srv"
+  "srv/Alim/AlimIn.srv"
+  "srv/Alim/AlimTemp.srv"
+  "srv/Alim/AlimBau.srv"
+  "srv/Alim/AlimEmg.srv"
   "srv/Odometry/OdometryPosition.srv"
   "srv/Odometry/OdometrySpeed.srv"
   "srv/Odometry/OdometryToF.srv"

src/modelec_interface/srv/Alim/AlimBau.srv

@@ -0,0 +1,3 @@
+---
+bool success
+bool activate

src/modelec_interface/srv/Alim/AlimEmg.srv

@@ -0,0 +1,3 @@
+bool activate
+---
+bool success

src/modelec_interface/srv/Alim/AlimIn.srv

@@ -0,0 +1,10 @@
+string VOLT="VOLT"
+string AMPS="AMPS"
+string STATE="STATE"
+string VALID="VALID"
+
+int16 input
+string type "STATE"
+---
+bool success
+int64 result

src/modelec_interface/srv/Alim/AlimOut.srv

@@ -0,0 +1,15 @@
+string OUT_5V="OUT5V"
+string OUT_5V1="OUT5V1"
+string OUT_12V="OUT12V"
+string OUT_24V="OUT24V"
+
+string STATE="STATE"
+string VOLT="VOLT"
+string AMPS="AMPS"
+
+string out
+string type "STATE"
+int8 enable -1
+---
+bool success
+int64 result

src/modelec_interface/srv/Alim/AlimTemp.srv

@@ -0,0 +1,3 @@
+---
+bool success
+int64 value