From e047f94ef29ee5b222acaf1b8a213e185cf8199d Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 8 Apr 2025 19:15:50 -0400 Subject: [PATCH] pcb alim --- .../include/modelec/pcb_alim_interface.hpp | 96 ++-- src/modelec/src/pcb_alim_interface.cpp | 427 ++++++++++++------ src/modelec/src/pcb_odo_interface.cpp | 21 +- src/modelec_interface/CMakeLists.txt | 5 + src/modelec_interface/srv/Alim/AlimBau.srv | 3 + src/modelec_interface/srv/Alim/AlimEmg.srv | 3 + src/modelec_interface/srv/Alim/AlimIn.srv | 10 + src/modelec_interface/srv/Alim/AlimOut.srv | 15 + src/modelec_interface/srv/Alim/AlimTemp.srv | 3 + 9 files changed, 421 insertions(+), 162 deletions(-) create mode 100644 src/modelec_interface/srv/Alim/AlimBau.srv create mode 100644 src/modelec_interface/srv/Alim/AlimEmg.srv create mode 100644 src/modelec_interface/srv/Alim/AlimIn.srv create mode 100644 src/modelec_interface/srv/Alim/AlimOut.srv create mode 100644 src/modelec_interface/srv/Alim/AlimTemp.srv diff --git a/src/modelec/include/modelec/pcb_alim_interface.hpp b/src/modelec/include/modelec/pcb_alim_interface.hpp index ba93388..d477db2 100644 --- a/src/modelec/include/modelec/pcb_alim_interface.hpp +++ b/src/modelec/include/modelec/pcb_alim_interface.hpp @@ -2,6 +2,11 @@ #include #include +#include +#include +#include +#include +#include namespace Modelec { @@ -15,6 +20,17 @@ public: ~PCBAlimInterface() override; + struct PCBData + { + bool success; + int value; + }; + + struct PCBBau + { + bool success; + bool activate; + }; private: rclcpp::Publisher::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::SharedPtr pcb_out_service_; + rclcpp::Service::SharedPtr pcb_in_service_; + rclcpp::Service::SharedPtr pcb_bau_service_; + rclcpp::Service::SharedPtr pcb_emg_service_; + rclcpp::Service::SharedPtr pcb_temp_service_; - void GetData(const std::string &elem, const std::string& data, const std::string& value = std::string()); + std::queue> 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> 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> pcb_bau_promises_; + std::mutex pcb_bau_mutex_; - void GetOutput5V1State(); - void GetOutput5V1Voltage(); - void GetOutput5V1Current(); + std::queue> pcb_emg_promises_; + std::mutex pcb_emg_mutex_; - void GetOutput12VState(); - void GetOutput12VVoltage(); - void GetOutput12VCurrent(); + std::queue> pcb_temp_promises_; + std::mutex pcb_temp_mutex_; - void GetOutput24VState(); - void GetOutput24VVoltage(); - void GetOutput24VCurrent(); + void HandleGetPCBOutData( + const std::shared_ptr request, + std::shared_ptr response); + + void HandleSetPCBOutData( + const std::shared_ptr request, + std::shared_ptr response); + + void HandleGetPCBInData( + const std::shared_ptr request, + std::shared_ptr response); + + void HandleGetPCBBauData( + const std::shared_ptr request, + std::shared_ptr response); + + void HandleSetPCBEmgData( + const std::shared_ptr request, + std::shared_ptr response); + + void HandleGetPCBTempData( + const std::shared_ptr request, + std::shared_ptr 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& data = {}) const; + + void GetData(const std::string& elem, const std::vector& data = {}) const; + void SendOrder(const std::string &elem, const std::vector &data = {}) const; - void SetSoftwareEmergencyStop(bool state); - void Set5VState(bool state); - void Set12VState(bool state); - void Set24VState(bool state); }; } // namespace Modelec diff --git a/src/modelec/src/pcb_alim_interface.cpp b/src/modelec/src/pcb_alim_interface.cpp index 8f198a6..e048b67 100644 --- a/src/modelec/src/pcb_alim_interface.cpp +++ b/src/modelec/src/pcb_alim_interface.cpp @@ -1,4 +1,5 @@ #include +#include #include namespace Modelec @@ -38,12 +39,16 @@ namespace Modelec pcb_subscriber_ = this->create_subscription( 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(); 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( + "alim/out", + [this](const std::shared_ptr request, + std::shared_ptr 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( + "alim/in", + [this](const std::shared_ptr request, + std::shared_ptr response) + { + HandleGetPCBInData(request, response); + }); + + pcb_bau_service_ = create_service( + "alim/bau", + [this](const std::shared_ptr request, + std::shared_ptr response) + { + HandleGetPCBBauData(request, response); + }); + + pcb_emg_service_ = create_service( + "alim/emg", + [this](const std::shared_ptr request, + std::shared_ptr response) + { + HandleSetPCBEmgData(request, response); + }); + + pcb_temp_service_ = create_service( + "alim/temp", + [this](const std::shared_ptr request, + std::shared_ptr 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 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 request, + std::shared_ptr response) + { + std::promise promise; + auto future = promise.get_future(); + + { + std::lock_guard 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 request, + std::shared_ptr response) + { + std::promise promise; + auto future = promise.get_future(); + + { + std::lock_guard 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 request, + std::shared_ptr response) + { + std::promise promise; + auto future = promise.get_future(); + + { + std::lock_guard 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 request, + std::shared_ptr response) + { + std::promise promise; + auto future = promise.get_future(); + + { + std::lock_guard 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 request, + std::shared_ptr response) + { + std::promise promise; + auto future = promise.get_future(); + + { + std::lock_guard 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 request, + std::shared_ptr response) + { + std::promise promise; + auto future = promise.get_future(); + + { + std::lock_guard 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 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 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 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& 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& 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& 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(); diff --git a/src/modelec/src/pcb_odo_interface.cpp b/src/modelec/src/pcb_odo_interface.cpp index 8a63c15..c440fe1 100644 --- a/src/modelec/src/pcb_odo_interface.cpp +++ b/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()); + } } } diff --git a/src/modelec_interface/CMakeLists.txt b/src/modelec_interface/CMakeLists.txt index db57708..51817fb 100644 --- a/src/modelec_interface/CMakeLists.txt +++ b/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" diff --git a/src/modelec_interface/srv/Alim/AlimBau.srv b/src/modelec_interface/srv/Alim/AlimBau.srv new file mode 100644 index 0000000..7ac5c3a --- /dev/null +++ b/src/modelec_interface/srv/Alim/AlimBau.srv @@ -0,0 +1,3 @@ +--- +bool success +bool activate diff --git a/src/modelec_interface/srv/Alim/AlimEmg.srv b/src/modelec_interface/srv/Alim/AlimEmg.srv new file mode 100644 index 0000000..3d6da4d --- /dev/null +++ b/src/modelec_interface/srv/Alim/AlimEmg.srv @@ -0,0 +1,3 @@ +bool activate +--- +bool success \ No newline at end of file diff --git a/src/modelec_interface/srv/Alim/AlimIn.srv b/src/modelec_interface/srv/Alim/AlimIn.srv new file mode 100644 index 0000000..d257bd5 --- /dev/null +++ b/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 diff --git a/src/modelec_interface/srv/Alim/AlimOut.srv b/src/modelec_interface/srv/Alim/AlimOut.srv new file mode 100644 index 0000000..97e6ecb --- /dev/null +++ b/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 diff --git a/src/modelec_interface/srv/Alim/AlimTemp.srv b/src/modelec_interface/srv/Alim/AlimTemp.srv new file mode 100644 index 0000000..b872ec3 --- /dev/null +++ b/src/modelec_interface/srv/Alim/AlimTemp.srv @@ -0,0 +1,3 @@ +--- +bool success +int64 value \ No newline at end of file