From 575fbfee791ea7867d063d2dfdd9c8909bdc86d9 Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 21 Apr 2025 23:54:35 -0400 Subject: [PATCH] emergency topic alim pcb --- .../include/modelec/pcb_alim_interface.hpp | 7 +- src/modelec/launch/test.modelec.launch.yml | 5 ++ src/modelec/src/pcb_alim_interface.cpp | 73 ++++++++++++++++--- src/modelec/src/pcb_odo_interface.cpp | 2 +- src/modelec_interface/CMakeLists.txt | 1 + src/modelec_interface/msg/Alim/AlimEmg.msg | 1 + 6 files changed, 78 insertions(+), 11 deletions(-) create mode 100644 src/modelec_interface/msg/Alim/AlimEmg.msg diff --git a/src/modelec/include/modelec/pcb_alim_interface.hpp b/src/modelec/include/modelec/pcb_alim_interface.hpp index d477db2..f5f7c9f 100644 --- a/src/modelec/include/modelec/pcb_alim_interface.hpp +++ b/src/modelec/include/modelec/pcb_alim_interface.hpp @@ -7,6 +7,7 @@ #include #include #include +#include namespace Modelec { @@ -38,6 +39,10 @@ private: void PCBCallback(const std_msgs::msg::String::SharedPtr msg); + rclcpp::Subscription::SharedPtr pcb_emg_subscriber_; + + void PCBEmgCallback(const modelec_interface::msg::AlimEmg::SharedPtr msg) const; + rclcpp::Service::SharedPtr pcb_out_service_; rclcpp::Service::SharedPtr pcb_in_service_; rclcpp::Service::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: diff --git a/src/modelec/launch/test.modelec.launch.yml b/src/modelec/launch/test.modelec.launch.yml index 634b02c..139ac4a 100644 --- a/src/modelec/launch/test.modelec.launch.yml +++ b/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" diff --git a/src/modelec/src/pcb_alim_interface.cpp b/src/modelec/src/pcb_alim_interface.cpp index e048b67..c54bfca 100644 --- a/src/modelec/src/pcb_alim_interface.cpp +++ b/src/modelec/src/pcb_alim_interface.cpp @@ -10,7 +10,7 @@ namespace Modelec auto request = std::make_shared(); 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("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( - 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(result.get()->subscriber, 10); + pcb_publisher_ = this->create_publisher(res->subscriber, 10); } else { @@ -115,6 +115,13 @@ namespace Modelec { HandleGetPCBTempData(request, response); }); + + pcb_emg_subscriber_ = this->create_subscription( + "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, value}); + ResolveSetPCBEmgRequest(success); } } 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 request, std::shared_ptr response) { @@ -259,7 +269,7 @@ namespace Modelec response->result = result.value; } - void PCBAlimInterface::HandleGetPCBBauData(const std::shared_ptr request, + void PCBAlimInterface::HandleGetPCBBauData(const std::shared_ptr, std::shared_ptr response) { std::promise promise; @@ -294,7 +304,7 @@ namespace Modelec } void PCBAlimInterface::HandleGetPCBTempData( - const std::shared_ptr request, + const std::shared_ptr, std::shared_ptr response) { std::promise promise; @@ -357,6 +367,51 @@ namespace Modelec } } + void PCBAlimInterface::ResolveGetPCBBauRequest(const PCBBau& value) + { + std::lock_guard 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 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 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(); diff --git a/src/modelec/src/pcb_odo_interface.cpp b/src/modelec/src/pcb_odo_interface.cpp index c440fe1..7248848 100644 --- a/src/modelec/src/pcb_odo_interface.cpp +++ b/src/modelec/src/pcb_odo_interface.cpp @@ -10,7 +10,7 @@ namespace Modelec auto request = std::make_shared(); 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("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) { diff --git a/src/modelec_interface/CMakeLists.txt b/src/modelec_interface/CMakeLists.txt index 51817fb..edbec23 100644 --- a/src/modelec_interface/CMakeLists.txt +++ b/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" diff --git a/src/modelec_interface/msg/Alim/AlimEmg.msg b/src/modelec_interface/msg/Alim/AlimEmg.msg new file mode 100644 index 0000000..402f094 --- /dev/null +++ b/src/modelec_interface/msg/Alim/AlimEmg.msg @@ -0,0 +1 @@ +bool activate