From b171ad4edd1a39a2b9699a848d0f3729c55b5a6b Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 11 Dec 2025 17:45:46 +0100 Subject: [PATCH] update action pcb listener --- src/modelec_com/CMakeLists.txt | 10 + .../modelec_com/pcb_action_interface.new.hpp | 76 +++ .../modelec_com/pcb_odo_interface.new.hpp | 2 - .../src/pcb_action_interface.new.cpp | 460 ++++++++++++++++++ src/modelec_com/src/pcb_odo_interface.new.cpp | 75 +-- src/modelec_core/launch/modelec.launch.py | 2 +- 6 files changed, 554 insertions(+), 71 deletions(-) create mode 100644 src/modelec_com/include/modelec_com/pcb_action_interface.new.hpp create mode 100644 src/modelec_com/src/pcb_action_interface.new.cpp diff --git a/src/modelec_com/CMakeLists.txt b/src/modelec_com/CMakeLists.txt index b9bef50..b6db509 100644 --- a/src/modelec_com/CMakeLists.txt +++ b/src/modelec_com/CMakeLists.txt @@ -44,6 +44,15 @@ target_include_directories(pcb_odo_interface_new PUBLIC $ ) +# PCB Action Node +add_executable(pcb_action_interface_new src/pcb_action_interface.new.cpp src/serial_listener.cpp) +ament_target_dependencies(pcb_action_interface_new rclcpp std_msgs modelec_interfaces ament_index_cpp) +target_link_libraries(pcb_action_interface_new modelec_utils::utils modelec_utils::config) +target_include_directories(pcb_action_interface_new PUBLIC + $ + $ +) + # PCB Alim Node add_executable(pcb_alim_interface src/pcb_alim_interface.cpp) ament_target_dependencies(pcb_alim_interface rclcpp std_msgs modelec_interfaces ament_index_cpp) @@ -78,6 +87,7 @@ install(TARGETS pcb_odo_interface pcb_odo_interface_new pcb_action_interface + pcb_action_interface_new DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/modelec_com/include/modelec_com/pcb_action_interface.new.hpp b/src/modelec_com/include/modelec_com/pcb_action_interface.new.hpp new file mode 100644 index 0000000..d3199d3 --- /dev/null +++ b/src/modelec_com/include/modelec_com/pcb_action_interface.new.hpp @@ -0,0 +1,76 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace Modelec +{ + class PCBActionInterface : public rclcpp::Node, public SerialListener + { + public: + + PCBActionInterface(); + + ~PCBActionInterface() override; + + protected: + std::map asc_value_mapper_; + std::map> servo_pos_mapper_; + + int asc_state_ = 0; + std::map servo_value_; + std::map relay_value_; + + private: + void read(const std::string& msg) override; + + 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_; + + rclcpp::Publisher::SharedPtr tir_start_pub_; + rclcpp::Publisher::SharedPtr tir_arm_pub_; + rclcpp::Publisher::SharedPtr tir_disarm_pub_; + + rclcpp::Subscription::SharedPtr tir_start_sub_; + rclcpp::Subscription::SharedPtr tir_arm_sub_; + rclcpp::Subscription::SharedPtr tir_disarm_sub_; + rclcpp::Subscription::SharedPtr tir_arm_set_sub_; + + public: + void SendToPCB(const std::string& data); + void SendToPCB(const std::string& order, const std::string& elem, + const std::vector& data = {}); + + void GetData(const std::string& elem, const std::vector& data = {}); + void SendOrder(const std::string& elem, const std::vector& data = {}); + void SendMove(const std::string& elem, const std::vector& data = {}); + void RespondEvent(const std::string& elem, const std::vector& data = {}); + }; +} \ No newline at end of file diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp index 8e496c8..213380d 100644 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp @@ -79,8 +79,6 @@ namespace Modelec int timeout_ms = 1000; int attempt = 5; - bool isOk = false; - public: void SendToPCB(const std::string& data); void SendToPCB(const std::string& order, const std::string& elem, diff --git a/src/modelec_com/src/pcb_action_interface.new.cpp b/src/modelec_com/src/pcb_action_interface.new.cpp new file mode 100644 index 0000000..78ed580 --- /dev/null +++ b/src/modelec_com/src/pcb_action_interface.new.cpp @@ -0,0 +1,460 @@ +#include +#include +#include +#include + +namespace Modelec +{ + PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface"), SerialListener() + { + // Service to create a new serial listener + declare_parameter("serial_port", "/dev/USB_ACTION"); + declare_parameter("baudrate", 115200); + declare_parameter("name", "pcb_action"); + + auto request = std::make_shared(); + request->name = get_parameter("name").as_string(); + request->bauds = get_parameter("baudrate").as_int(); + request->serial_port = get_parameter("serial_port").as_string(); + + this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); + + asc_get_sub_ = this->create_subscription( + "action/get/asc", 10, + [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr) + { + GetData("ASC", {"POS"}); + }); + + servo_get_sub_ = this->create_subscription( + "action/get/servo", 10, + [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg) + { + GetData("SERVO" + std::to_string(msg->id), {"POS"}); + }); + + relay_get_sub_ = this->create_subscription( + "action/get/relay", 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/get/asc/res", 10); + servo_get_res_pub_ = this->create_publisher( + "action/get/servo/res", 10); + relay_get_res_pub_ = this->create_publisher( + "action/get/relay/res", 10); + + asc_set_sub_ = this->create_subscription( + "action/set/asc", 10, + [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg) + { + SendOrder("ASC", {std::to_string(msg->pos), std::to_string(msg->value)}); + }); + + servo_set_sub_ = this->create_subscription( + "action/set/servo", 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(static_cast(msg->angle * 100)) + }); + }); + + asc_set_res_pub_ = this->create_publisher( + "action/set/asc/res", 10); + + servo_set_res_pub_ = this->create_publisher( + "action/set/servo/res", 10); + + asc_move_sub_ = this->create_subscription( + "action/move/asc", 10, + [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg) + { + SendMove("ASC", {std::to_string(msg->pos)}); + }); + + servo_move_sub_ = this->create_subscription( + "action/move/servo", 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/move/relay", 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/move/asc/res", 10); + + servo_move_res_pub_ = this->create_publisher( + "action/move/servo/res", 10); + + relay_move_res_pub_ = this->create_publisher( + "action/move/relay/res", 10); + + tir_start_pub_ = this->create_publisher( + "action/tir/start", 10); + + tir_start_sub_ = this->create_subscription( + "action/tir/start/res", 10, + [this](const std_msgs::msg::Empty::SharedPtr) + { + RespondEvent("TIR", {"START"}); + }); + + tir_arm_pub_ = this->create_publisher( + "action/tir/arm", 10); + + tir_arm_sub_ = this->create_subscription( + "action/tir/arm/res", 10, + [this](const std_msgs::msg::Empty::SharedPtr) + { + RespondEvent("TIR", {"ARM"}); + }); + + tir_disarm_pub_ = this->create_publisher( + "action/tir/disarm", 10); + + tir_disarm_sub_ = this->create_subscription( + "action/tir/disarm/res", 10, + [this](const std_msgs::msg::Empty::SharedPtr) + { + RespondEvent("TIR", {"DIS"}); + }); + + tir_arm_set_sub_ = this->create_subscription( + "action/tir/arm/set", 10, + [this](const std_msgs::msg::Bool::SharedPtr msg) + { + SendOrder("TIR", {"ARM", msg->data ? "1" : "0"}); + }); + + + // TODO : check for real value there + asc_value_mapper_ = { + {0, 0}, + {1, 100}, + {2, 200}, + {3, 300} + }; + /*for (auto & [id, v] : asc_value_mapper_) + { + SendOrder("ASC", {std::to_string(id), std::to_string(v)}); + }*/ + + asc_state_ = 3; + + // SendMove("ASC", {std::to_string(asc_state_)}); + + servo_pos_mapper_ = { + {0, {{0, 0.55}, {1, 0}}}, + {1, {{0, 0}, {1, 0.4}}}, + {2, {{0, M_PI_2}}}, + {3, {{0, M_PI_2}}}, + {4, {{0, 1.25}, {1, 0.45}}}, + {5, {{0, 0}, {1, M_PI}}}, + }; + + for (auto & [id, v] : servo_pos_mapper_) + { + if (id == 5) continue; + + for (auto & [key, angle] : v) + { + rclcpp::sleep_for(std::chrono::milliseconds(100)); + + SendOrder("SERVO" + std::to_string(id), {"POS" + std::to_string(key), std::to_string(static_cast(angle * 100))}); + } + } + + servo_value_ = { + {0, 1}, + {1, 1}, + {2, 0}, + {3, 0}, + {4, 1}, + {5, 0} + }; + + for (auto & [id, v] : servo_value_) + { + rclcpp::sleep_for(std::chrono::milliseconds(100)); + + SendMove("SERVO" + std::to_string(id), {"POS" + std::to_string(v)}); + } + + relay_value_ = { + {1, false}, + {2, false}, + {3, false}, + }; + + for (auto & [id, v] : relay_value_) + { + rclcpp::sleep_for(std::chrono::milliseconds(100)); + + SendMove("RELAY" + std::to_string(id), {std::to_string(v)}); + } + + rclcpp::sleep_for(std::chrono::milliseconds(100)); + + SendOrder("TIR", {"ARM", "1"}); + } + + PCBActionInterface::~PCBActionInterface() + { + } + + void PCBActionInterface::read(const std::string& msg) + { + RCLCPP_DEBUG(this->get_logger(), "Received message: '%s'", msg.c_str()); + std::vector tokens = split(trim(msg), ';'); + + if (tokens.size() < 2) + { + RCLCPP_ERROR(this->get_logger(), "Invalid message format"); + return; + } + + if (tokens[0] == "SET") + { + if (tokens[1] == "ASC") + { + asc_state_ = std::stoi(tokens[3]); + + modelec_interfaces::msg::ActionAscPos asc_msg; + asc_msg.pos = asc_state_; + asc_msg.value = asc_value_mapper_[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 v = std::stoi(tokens[3]); + servo_value_[servo_id] = v; + + modelec_interfaces::msg::ActionServoPos servo_msg; + servo_msg.id = servo_id; + servo_msg.pos = v; + servo_msg.angle = servo_pos_mapper_[servo_id][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[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") + { + int pos = std::stoi(tokens[2]); + int v = std::stoi(tokens[3]); + asc_value_mapper_[pos] = v; + + modelec_interfaces::msg::ActionAscPos asc_msg; + asc_msg.pos = pos; + 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_ = std::stoi(tokens[2]); + + 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 for OK response: '%s'", msg.c_str()); + } + } + 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: '%s'", msg.c_str()); + } + } + else if (tokens[0] == "EVENT") + { + if (tokens[1] == "TIR") + { + if (tokens[2] == "START") + { + tir_start_pub_->publish(std_msgs::msg::Empty()); + RespondEvent(tokens[1], {tokens[2]}); + } + else if (tokens[2] == "ARM") + { + tir_arm_pub_->publish(std_msgs::msg::Empty()); + RespondEvent(tokens[1], {tokens[2]}); + } + else if (tokens[2] == "DIS") + { + tir_disarm_pub_->publish(std_msgs::msg::Empty()); + RespondEvent(tokens[1], {tokens[2]}); + } + } + } + } + + void PCBActionInterface::SendToPCB(const std::string& data) + { + if (IsOk()) + { + RCLCPP_DEBUG(this->get_logger(), "Sending to PCB: %s", data.c_str()); + this->write(data); + } + } + + void PCBActionInterface::SendToPCB(const std::string& order, const std::string& elem, + const std::vector& data) + { + std::string command = order + ";" + elem; + for (const auto& d : data) + { + command += ";" + d; + } + command += "\n"; + + SendToPCB(command); + } + + void PCBActionInterface::GetData(const std::string& elem, const std::vector& data) + { + SendToPCB("GET", elem, data); + } + + void PCBActionInterface::SendOrder(const std::string& elem, const std::vector& data) + { + SendToPCB("SET", elem, data); + } + + void PCBActionInterface::SendMove(const std::string& elem, const std::vector& data) + { + SendToPCB("MOV", elem, data); + } + + void PCBActionInterface::RespondEvent(const std::string& elem, const std::vector& data) + { + SendToPCB("ACK", elem, data); + } +} + +#ifndef MODELEC_COM_TESTING +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + // Increase number of threads explicitly! + rclcpp::executors::MultiThreadedExecutor executor( + rclcpp::ExecutorOptions(), 2 /* or more threads! */); + + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} +#endif diff --git a/src/modelec_com/src/pcb_odo_interface.new.cpp b/src/modelec_com/src/pcb_odo_interface.new.cpp index 451cf95..7d29cac 100644 --- a/src/modelec_com/src/pcb_odo_interface.new.cpp +++ b/src/modelec_com/src/pcb_odo_interface.new.cpp @@ -1,7 +1,5 @@ #include -#include #include -#include #include #include @@ -21,78 +19,13 @@ namespace Modelec this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); - /*auto client = this->create_client("add_serial_listener"); - while (!client->wait_for_service(std::chrono::seconds(1))) - { - if (!rclcpp::ok()) - { - RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); - return; - } - RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); - } - auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == - rclcpp::FutureReturnCode::SUCCESS) - { - if (auto res = result.get()) - { - if (res->success) - { - pcb_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - rclcpp::SubscriptionOptions options; - options.callback_group = pcb_callback_group_; - - pcb_subscriber_ = this->create_subscription( - res->publisher, 10, - [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_->spin(); - }); - - pcb_publisher_ = this->create_publisher(res->subscriber, 10); - - isOk = true; - - SetStart(true); - - SetPID("THETA", 14, 0, 0); - SetPID("POS", 10, 0, 0); - SetPID("LEFT", 5, 0, 0); - SetPID("RIGHT", 5, 0, 0); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener"); - } - } - else - { - RCLCPP_ERROR(this->get_logger(), "Failed to ask for a serial listener"); - } - } - else - { - RCLCPP_ERROR(this->get_logger(), "Service call failed"); - }*/ - odo_pos_publisher_ = this->create_publisher( "odometry/position", 10); odo_get_pos_sub_ = this->create_subscription( "odometry/get/pos", 30, [this](const std_msgs::msg::Empty::SharedPtr) { - if (isOk) + if (IsOk()) { GetPos(); } @@ -148,6 +81,12 @@ namespace Modelec SendOrder("START", {std::to_string(msg->data)}); } }); + + + SetPID("THETA", 14, 0, 0); + SetPID("POS", 10, 0, 0); + SetPID("LEFT", 5, 0, 0); + SetPID("RIGHT", 5, 0, 0); } PCBOdoInterface::~PCBOdoInterface() diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 8022516..2670d0e 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -106,7 +106,7 @@ def generate_launch_description(): ), Node( package='modelec_com', - executable='pcb_action_interface', + executable='pcb_action_interface_new', name='pcb_action_interface', parameters=[{ 'serial_port': "/dev/USB_ACTION",