From 27fa0845197fa245f89e74a0bb76de24d13ac06a Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 14 May 2025 16:42:39 -0400 Subject: [PATCH] pcb action interface --- .../modelec_com/pcb_action_interface.hpp | 46 +++- src/modelec_com/src/pcb_action_interface.cpp | 231 +++++++++++++++++- src/modelec_interfaces/CMakeLists.txt | 3 + .../msg/Action/ActionAscPos.msg | 7 + .../msg/Action/ActionRelayState.msg | 3 + .../msg/Action/ActionServoPos.msg | 4 + 6 files changed, 286 insertions(+), 8 deletions(-) create mode 100644 src/modelec_interfaces/msg/Action/ActionAscPos.msg create mode 100644 src/modelec_interfaces/msg/Action/ActionRelayState.msg create mode 100644 src/modelec_interfaces/msg/Action/ActionServoPos.msg 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 e6ab089..1a1e61a 100644 --- a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp @@ -3,12 +3,23 @@ #include #include #include +#include +#include +#include namespace Modelec { class PCBActionInterface : public rclcpp::Node { public: + + enum ASCState + { + LOW = 0, + HIGH = 1, + MOVING = 2, + }; + PCBActionInterface(); rclcpp::CallbackGroup::SharedPtr pcb_callback_group_; std::shared_ptr pcb_executor_; @@ -17,9 +28,15 @@ namespace Modelec ~PCBActionInterface() override; protected: - std::map asc_v_; - std::map> servo_pos_v_; - std::map relay_v_; + std::map asc_value_mapper_ = { + {LOW, 0}, + {HIGH, 0}, + }; + std::map> servo_pos_mapper_; + + ASCState asc_state_ = LOW; + std::map servo_value_; + std::map relay_value_; private: rclcpp::Publisher::SharedPtr pcb_publisher_; @@ -27,6 +44,28 @@ namespace Modelec void PCBCallback(const std_msgs::msg::String::SharedPtr msg); + rclcpp::Subscription::SharedPtr asc_get_sub_; + rclcpp::Subscription::SharedPtr servo_get_sub_; + rclcpp::Subscription::SharedPtr relay_get_sub_; + + rclcpp::Publisher::SharedPtr asc_get_res_pub_; + rclcpp::Publisher::SharedPtr servo_get_res_pub_; + rclcpp::Publisher::SharedPtr relay_get_res_pub_; + + rclcpp::Subscription::SharedPtr asc_set_sub_; + rclcpp::Subscription::SharedPtr servo_set_sub_; + + rclcpp::Publisher::SharedPtr asc_set_res_pub_; + rclcpp::Publisher::SharedPtr servo_set_res_pub_; + + rclcpp::Subscription::SharedPtr asc_move_sub_; + rclcpp::Subscription::SharedPtr servo_move_sub_; + rclcpp::Subscription::SharedPtr relay_move_sub_; + + rclcpp::Publisher::SharedPtr asc_move_res_pub_; + rclcpp::Publisher::SharedPtr servo_move_res_pub_; + rclcpp::Publisher::SharedPtr relay_move_res_pub_; + public: void SendToPCB(const std::string& data) const; void SendToPCB(const std::string& order, const std::string& elem, @@ -34,5 +73,6 @@ namespace Modelec void GetData(const std::string& elem, const std::vector& data = {}) const; void SendOrder(const std::string& elem, const std::vector& data = {}) const; + void SendMove(const std::string& elem, const std::vector& data = {}) const; }; } \ No newline at end of file diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 27b3dbb..0d5815e 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -85,7 +85,85 @@ namespace Modelec * */ + asc_get_sub_ = this->create_subscription( + "action/asc/get", 10, + [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr) + { + GetData("ASC", {"POS"}); + }); + servo_get_sub_ = this->create_subscription( + "action/servo/get", 10, + [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg) + { + GetData("SERVO" + std::to_string(msg->id), {"POS"}); + }); + + relay_get_sub_ = this->create_subscription( + "action/relay/get", 10, + [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr msg) + { + GetData("RELAY" + std::to_string(msg->id), {"STATE"}); + }); + + asc_get_res_pub_ = this->create_publisher( + "action/asc/get/res", 10); + servo_get_res_pub_ = this->create_publisher( + "action/servo/get/res", 10); + relay_get_res_pub_ = this->create_publisher( + "action/relay/get/res", 10); + + asc_set_sub_ = this->create_subscription( + "action/asc/set", 10, + [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg) + { + SendOrder("ASC", {msg->pos == LOW ? "LOW" : "HIGH", std::to_string(msg->value)}); + }); + + servo_set_sub_ = this->create_subscription( + "action/servo/set", 10, + [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg) + { + SendOrder("SERVO" + std::to_string(msg->id), { + "POS" + std::to_string(msg->pos), std::to_string(msg->angle) + }); + }); + + asc_set_res_pub_ = this->create_publisher( + "action/asc/set/res", 10); + + servo_set_res_pub_ = this->create_publisher( + "action/servo/set/res", 10); + + asc_move_sub_ = this->create_subscription( + "action/asc/move", 10, + [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg) + { + SendMove("ASC", {msg->pos == LOW ? "LOW" : "HIGH"}); + }); + + servo_move_sub_ = this->create_subscription( + "action/servo/move", 10, + [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg) + { + SendMove("SERVO" + std::to_string(msg->id), {"POS" + std::to_string(msg->pos)}); + }); + + relay_move_sub_ = this->create_subscription( + "action/relay/move", 10, + [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr msg) + { + SendMove("RELAY" + std::to_string(msg->id), {std::to_string(msg->state)}); + }); + + asc_move_res_pub_ = this->create_publisher( + "action/asc/move/res", 10); + + servo_move_res_pub_ = this->create_publisher( + "action/servo/move/res", 10); + + relay_move_res_pub_ = this->create_publisher( + "action/relay/move/res", 10); } PCBActionInterface::~PCBActionInterface() @@ -112,25 +190,163 @@ namespace Modelec { if (tokens[1] == "ASC") { + if (tokens[3] == "DESC" || tokens[3] == "CLIMB") + { + asc_state_ = MOVING; + } + else if (tokens[3] == "LOW") + { + asc_state_ = LOW; + } + else if (tokens[3] == "HIGH") + { + asc_state_ = HIGH; + } + modelec_interfaces::msg::ActionAscPos asc_msg; + asc_msg.pos = asc_state_; + asc_msg.success = true; + asc_get_res_pub_->publish(asc_msg); } else if (startsWith(tokens[1], "SERVO")) { int servo_id = std::stoi(tokens[1].substr(5)); - int key = std::stoi(tokens[2]); int v = std::stoi(tokens[3]); - servo_pos_v_[servo_id][key] = v; + servo_value_[servo_id] = v; + + modelec_interfaces::msg::ActionServoPos servo_msg; + servo_msg.id = servo_id; + servo_msg.pos = v; + servo_msg.success = true; + servo_get_res_pub_->publish(servo_msg); } else if (startsWith(tokens[1], "RELAY")) { int relay_id = std::stoi(tokens[1].substr(5)); - bool state = (tokens[2] == "1"); - relay_v_[relay_id] = state; + bool state = (tokens[3] == "1"); + relay_value_[relay_id] = state; + + modelec_interfaces::msg::ActionRelayState relay_msg; + relay_msg.id = relay_id; + relay_msg.state = state; + relay_msg.success = true; + relay_get_res_pub_->publish(relay_msg); } } else if (tokens[0] == "OK") { + if (tokens.size() == 4) + { + if (tokens[1] == "ASC") + { + ASCState state = tokens[2] == "LOW" ? LOW : HIGH; + int v = std::stoi(tokens[3]); + asc_value_mapper_[state] = v; + modelec_interfaces::msg::ActionAscPos asc_msg; + asc_msg.pos = state; + asc_msg.value = v; + asc_msg.success = true; + asc_set_res_pub_->publish(asc_msg); + } + else if (startsWith(tokens[1], "SERVO")) + { + int servo_id = std::stoi(tokens[1].substr(5)); + int key = std::stoi(tokens[2].substr(3)); + int v = std::stoi(tokens[3]); + servo_pos_mapper_[servo_id][key] = v; + + modelec_interfaces::msg::ActionServoPos servo_msg; + servo_msg.id = servo_id; + servo_msg.pos = key; + servo_msg.angle = v; + servo_msg.success = true; + servo_set_res_pub_->publish(servo_msg); + } + } + else if (tokens.size() == 3) + { + if (tokens[1] == "ASC") + { + asc_state_ = tokens[2] == "LOW" ? LOW : HIGH; + + modelec_interfaces::msg::ActionAscPos asc_msg; + asc_msg.pos = asc_state_; + asc_msg.success = true; + asc_move_res_pub_->publish(asc_msg); + } + else if (startsWith(tokens[1], "SERVO")) + { + int servo_id = std::stoi(tokens[1].substr(5)); + int key = std::stoi(tokens[2].substr(3)); + servo_value_[servo_id] = key; + + modelec_interfaces::msg::ActionServoPos servo_msg; + servo_msg.id = servo_id; + servo_msg.pos = key; + servo_msg.success = true; + servo_move_res_pub_->publish(servo_msg); + } + else if (startsWith(tokens[1], "RELAY")) + { + int relay_id = std::stoi(tokens[1].substr(5)); + bool state = (tokens[2] == "1"); + relay_value_[relay_id] = state; + + modelec_interfaces::msg::ActionRelayState relay_msg; + relay_msg.id = relay_id; + relay_msg.state = state; + relay_msg.success = true; + relay_move_res_pub_->publish(relay_msg); + } + } + else + { + RCLCPP_WARN(this->get_logger(), "Unknown message format"); + } + } + else if (tokens[0] == "KO") + { + if (tokens.size() == 4) + { + if (tokens[1] == "ASC") + { + modelec_interfaces::msg::ActionAscPos asc_msg; + asc_msg.success = false; + asc_set_res_pub_->publish(asc_msg); + } + else if (startsWith(tokens[1], "SERVO")) + { + modelec_interfaces::msg::ActionServoPos servo_msg; + servo_msg.success = false; + servo_set_res_pub_->publish(servo_msg); + } + } + else if (tokens.size() == 3) + { + if (tokens[1] == "ASC") + { + modelec_interfaces::msg::ActionAscPos asc_msg; + asc_msg.success = false; + asc_move_res_pub_->publish(asc_msg); + } + else if (startsWith(tokens[1], "SERVO")) + { + modelec_interfaces::msg::ActionServoPos servo_msg; + servo_msg.success = false; + servo_move_res_pub_->publish(servo_msg); + } + else if (startsWith(tokens[1], "RELAY")) + { + modelec_interfaces::msg::ActionRelayState relay_msg; + relay_msg.success = false; + relay_move_res_pub_->publish(relay_msg); + } + } + else + { + RCLCPP_WARN(this->get_logger(), "Unknown message format"); + } } } @@ -142,7 +358,7 @@ namespace Modelec } void PCBActionInterface::SendToPCB(const std::string& order, const std::string& elem, - const std::vector& data) const + const std::vector& data) const { std::string command = order + ";" + elem; for (const auto& d : data) @@ -163,6 +379,11 @@ namespace Modelec { SendToPCB("SET", elem, data); } + + void PCBActionInterface::SendMove(const std::string& elem, const std::vector& data) const + { + SendToPCB("MOVE", elem, data); + } } int main(int argc, char** argv) diff --git a/src/modelec_interfaces/CMakeLists.txt b/src/modelec_interfaces/CMakeLists.txt index 0dfc066..6ef854f 100644 --- a/src/modelec_interfaces/CMakeLists.txt +++ b/src/modelec_interfaces/CMakeLists.txt @@ -27,6 +27,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ServoMode.msg" "msg/Solenoid.msg" "msg/Button.msg" + "msg/Action/ActionAscPos.msg" + "msg/Action/ActionRelayState.msg" + "msg/Action/ActionServoPos.msg" "srv/Alim/AlimOut.srv" "srv/Alim/AlimIn.srv" "srv/Alim/AlimTemp.srv" diff --git a/src/modelec_interfaces/msg/Action/ActionAscPos.msg b/src/modelec_interfaces/msg/Action/ActionAscPos.msg new file mode 100644 index 0000000..4c2ba2a --- /dev/null +++ b/src/modelec_interfaces/msg/Action/ActionAscPos.msg @@ -0,0 +1,7 @@ +int32 LOW=0 +int32 HIGH=1 +int32 MOVING=2 + +int32 pos +int32 value +bool success \ No newline at end of file diff --git a/src/modelec_interfaces/msg/Action/ActionRelayState.msg b/src/modelec_interfaces/msg/Action/ActionRelayState.msg new file mode 100644 index 0000000..88ad9ae --- /dev/null +++ b/src/modelec_interfaces/msg/Action/ActionRelayState.msg @@ -0,0 +1,3 @@ +int32 id +bool state +bool success \ No newline at end of file diff --git a/src/modelec_interfaces/msg/Action/ActionServoPos.msg b/src/modelec_interfaces/msg/Action/ActionServoPos.msg new file mode 100644 index 0000000..84b7339 --- /dev/null +++ b/src/modelec_interfaces/msg/Action/ActionServoPos.msg @@ -0,0 +1,4 @@ +int32 id +int32 pos +float32 angle +bool success \ No newline at end of file