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 11b7f5b..c78268d 100644 --- a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp @@ -16,6 +16,11 @@ namespace Modelec ~PCBActionInterface() override; + protected: + std::map asc_v_; + std::map> servo_pos_v_; + std::map relay_v_; + private: rclcpp::Publisher::SharedPtr pcb_publisher_; rclcpp::Subscription::SharedPtr pcb_subscriber_; diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index a20d453..57b80bd 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -73,6 +73,19 @@ namespace Modelec { RCLCPP_ERROR(this->get_logger(), "Service call failed"); } + + + /** + * TODO : def + * action/x/y/... + * + * x -> asc | servo | relay + * y -> get | set | move + * + * + */ + + } PCBActionInterface::~PCBActionInterface() @@ -88,6 +101,29 @@ namespace Modelec { RCLCPP_DEBUG(this->get_logger(), "Received message: '%s'", msg->data.c_str()); std::vector tokens = split(msg->data, ';'); + + if (tokens.size() < 3) + { + RCLCPP_ERROR(this->get_logger(), "Invalid message format"); + return; + } + + if (tokens[0] == "SET") + { + if (tokens[1] == "ASC") + { + + } + else if (startsWith(tokens[2], "SERVO")) + { + int servo_id = std::stoi(tokens[2].substr(5)); + + } + } + else if (tokens[0] == "OK") + { + + } } void PCBActionInterface::SendToPCB(const std::string& data) const