emergency topic alim pcb

src/modelec/include/modelec/pcb_alim_interface.hpp

@@ -7,6 +7,7 @@
 #include <modelec_interface/srv/alim_bau.hpp>
 #include <modelec_interface/srv/alim_emg.hpp>
 #include <modelec_interface/srv/alim_temp.hpp>
+#include <modelec_interface/msg/alim_emg.hpp>
 
 namespace Modelec
 {
@@ -38,6 +39,10 @@ private:
 
     void PCBCallback(const std_msgs::msg::String::SharedPtr msg);
 
+    rclcpp::Subscription<modelec_interface::msg::AlimEmg>::SharedPtr pcb_emg_subscriber_;
+
+    void PCBEmgCallback(const modelec_interface::msg::AlimEmg::SharedPtr msg) const;
+
     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_;
@@ -87,7 +92,7 @@ private:
     void ResolveSetPCBOutRequest(const PCBData& value);
     void ResolveGetPCBInRequest(const PCBData& value);
     void ResolveGetPCBBauRequest(const PCBBau& value);
-    void ResolveSetPCBEmgRequest(const PCBData& value);
+    void ResolveSetPCBEmgRequest(bool value);
     void ResolveGetPCBTempRequest(const PCBData& value);
 
 public:

src/modelec/launch/test.modelec.launch.yml

@@ -10,6 +10,11 @@ launch:
       exec: 'pcb_odo_interface'
       name: 'pcb_odo_interface'
 
+- node:
+      pkg: 'modelec'
+      exec: 'pcb_alim_interface'
+      name: 'pcb_alim_interface'
+
 - node:
       pkg: 'modelec_gui'
       exec: "modelec_gui"

src/modelec/src/pcb_alim_interface.cpp

@@ -10,7 +10,7 @@ namespace Modelec
         auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
         request->name = "pcb_alim";
         request->bauds = 115200;
-        request->serial_port = "/dev/serial0"; // TODO : check the real serial port
+        request->serial_port = "/dev/pts/12"; // TODO : check the real serial port
         auto client = this->create_client<modelec_interface::srv::AddSerialListener>("add_serial_listener");
         while (!client->wait_for_service(std::chrono::seconds(1)))
         {
@@ -38,7 +38,7 @@ namespace Modelec
                     options.callback_group = pcb_callback_group_;
 
                     pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
-                        result.get()->publisher, 10,
+                        res->publisher, 10,
                         [this](const std_msgs::msg::String::SharedPtr msg)
                         {
                             PCBCallback(msg);
@@ -52,7 +52,7 @@ namespace Modelec
                         pcb_executor_->spin();
                     });
 
-                    pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(result.get()->subscriber, 10);
+                    pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
                 }
                 else
                 {
@@ -115,6 +115,13 @@ namespace Modelec
             {
                 HandleGetPCBTempData(request, response);
             });
+
+        pcb_emg_subscriber_ = this->create_subscription<modelec_interface::msg::AlimEmg>(
+            "alim/emg", 10,
+            [this](const modelec_interface::msg::AlimEmg::SharedPtr msg)
+            {
+                PCBEmgCallback(msg);
+            });
     }
 
     PCBAlimInterface::~PCBAlimInterface()
@@ -175,8 +182,7 @@ namespace Modelec
             else if (tokens[1] == "EMG")
             {
                 bool success = true;
-                bool value = (tokens[3] == "1");
+                ResolveSetPCBEmgRequest(success);
-                ResolveSetPCBEmgRequest({success, value});
             }
         }
         else if (tokens[0] == "KO")
@@ -191,9 +197,8 @@ namespace Modelec
             else if (tokens[1] == "EMG")
             {
                 bool success = false;
-                int value = -1;
 
-                ResolveSetPCBEmgRequest({success, value});
+                ResolveSetPCBEmgRequest(success);
             }
         }
         else
@@ -202,6 +207,11 @@ namespace Modelec
         }
     }
 
+    void PCBAlimInterface::PCBEmgCallback(const modelec_interface::msg::AlimEmg::SharedPtr msg) const
+    {
+        SendOrder("EMG", {"STATE", msg->activate == true ? "1" : "0"});
+    }
+
     void PCBAlimInterface::HandleGetPCBOutData(const std::shared_ptr<modelec_interface::srv::AlimOut::Request> request,
                                                std::shared_ptr<modelec_interface::srv::AlimOut::Response> response)
     {
@@ -259,7 +269,7 @@ namespace Modelec
         response->result = result.value;
     }
 
-    void PCBAlimInterface::HandleGetPCBBauData(const std::shared_ptr<modelec_interface::srv::AlimBau::Request> request,
+    void PCBAlimInterface::HandleGetPCBBauData(const std::shared_ptr<modelec_interface::srv::AlimBau::Request>,
                                                std::shared_ptr<modelec_interface::srv::AlimBau::Response> response)
     {
         std::promise<PCBBau> promise;
@@ -294,7 +304,7 @@ namespace Modelec
     }
 
     void PCBAlimInterface::HandleGetPCBTempData(
-        const std::shared_ptr<modelec_interface::srv::AlimTemp::Request> request,
+        const std::shared_ptr<modelec_interface::srv::AlimTemp::Request>,
         std::shared_ptr<modelec_interface::srv::AlimTemp::Response> response)
     {
         std::promise<PCBData> promise;
@@ -357,6 +367,51 @@ namespace Modelec
         }
     }
 
+    void PCBAlimInterface::ResolveGetPCBBauRequest(const PCBBau& value)
+    {
+        std::lock_guard<std::mutex> lock(pcb_bau_mutex_);
+        if (!pcb_bau_promises_.empty())
+        {
+            auto promise = std::move(pcb_bau_promises_.front());
+            pcb_bau_promises_.pop();
+            promise.set_value(value);
+        }
+        else
+        {
+            RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB bau data");
+        }
+    }
+
+    void PCBAlimInterface::ResolveSetPCBEmgRequest(bool value)
+    {
+        std::lock_guard<std::mutex> lock(pcb_emg_mutex_);
+        if (!pcb_emg_promises_.empty())
+        {
+            auto promise = std::move(pcb_emg_promises_.front());
+            pcb_emg_promises_.pop();
+            promise.set_value(value);
+        }
+        else
+        {
+            RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB emg data");
+        }
+    }
+
+    void PCBAlimInterface::ResolveGetPCBTempRequest(const PCBData& value)
+    {
+        std::lock_guard<std::mutex> lock(pcb_temp_mutex_);
+        if (!pcb_temp_promises_.empty())
+        {
+            auto promise = std::move(pcb_temp_promises_.front());
+            pcb_temp_promises_.pop();
+            promise.set_value(value);
+        }
+        else
+        {
+            RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB temp data");
+        }
+    }
+
     void PCBAlimInterface::SendToPCB(const std::string& data) const
     {
         auto message = std_msgs::msg::String();

src/modelec/src/pcb_odo_interface.cpp

@@ -10,7 +10,7 @@ namespace Modelec
         auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
         request->name = "pcb_odo";
         request->bauds = 115200;
-        request->serial_port = "/dev/pts/4"; // TODO : check the real serial port
+        request->serial_port = "/dev/pts/7"; // TODO : check the real serial port
         auto client = this->create_client<modelec_interface::srv::AddSerialListener>("add_serial_listener");
         while (!client->wait_for_service(std::chrono::seconds(1)))
         {

src/modelec_interface/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(std_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Alim/AlimEmg.msg"
   "msg/Odometry/OdometryPos.msg"
   "msg/Odometry/OdometrySpeed.msg"
   "msg/Odometry/OdometryToF.msg"

src/modelec_interface/msg/Alim/AlimEmg.msg

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