From da79f5d871884b52a770ae1734b8a69a3ee07de5 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 14 May 2025 12:57:17 -0400 Subject: [PATCH] when quit qt that quit everything --- simulated_pcb/action.py | 139 ++++++++++++++++++ src/modelec_com/CMakeLists.txt | 1 + .../modelec_com/pcb_action_interface.hpp | 2 +- src/modelec_com/src/pcb_action_interface.cpp | 14 +- .../launch/test.modelec.launch.py | 60 ++++++++ .../launch/test.modelec.launch.yml | 41 ------ src/modelec_gui/src/main.cpp | 18 +-- src/modelec_strat/data/config.xml | 2 +- 8 files changed, 218 insertions(+), 59 deletions(-) create mode 100644 simulated_pcb/action.py create mode 100644 src/modelec_core/launch/test.modelec.launch.py delete mode 100644 src/modelec_core/launch/test.modelec.launch.yml diff --git a/simulated_pcb/action.py b/simulated_pcb/action.py new file mode 100644 index 0000000..5b5b1ae --- /dev/null +++ b/simulated_pcb/action.py @@ -0,0 +1,139 @@ +import argparse + +import serial +import time +import math +import threading + +class SimulatedPCB: + def __init__(self, port='/dev/pts/6', baud=115200): + self.ser = serial.Serial(port, baud, timeout=1) + self.running = True + self.last_update = time.time() + + # État simulé + self.servos = {} # {id: pos_index} + self.servo_angles = {} # {id: {pos_index: angle}} + self.relais = {1: 0, 2: 0, 3: 0} + self.ascenseur_pos = "low" # 'low', 'climb', 'high', 'desc' + self.fin_course = {} # à implémenter si besoin + self.tirette_arm = False + + self.thread = threading.Thread(target=self.run) + self.thread.start() + + def send_response(self, msg): + print(f"[TX] {msg}") + self.ser.write((msg + "\n").encode()) + + def process_command(self, line): + parts = line.strip().split(';') + if not parts: + return + + cmd = parts[0] + + if cmd == "GET": + self.handle_get(parts) + elif cmd == "SET": + self.handle_set(parts) + elif cmd == "MOV": + self.handle_mov(parts) + + def handle_get(self, parts): + if len(parts) != 3: + return + + category, data = parts[1], parts[2] + + if category.startswith("SERVO") and data == "POS": + sid = int(category[5:]) + pos = self.servos.get(sid, 0) + self.send_response(f"SET;{category};{data};{pos}") + elif category == "ASC" and data == "POS": + self.send_response(f"SET;ASC;POS;{self.ascenseur_pos}") + elif category.startswith("RELAY") and data == "STATE": + rid = int(category[5:]) + state = self.relais.get(rid, 0) + self.send_response(f"SET;{category};{data};{state}") + + def handle_set(self, parts): + if len(parts) != 4: + return + + category, data, val = parts[1], parts[2], parts[3] + + if category.startswith("SERVO") and data.startswith("POS"): + sid = int(category[5:]) + pos_index = int(data[3:]) + angle = float(val) + + if sid not in self.servo_angles: + self.servo_angles[sid] = {} + self.servo_angles[sid][pos_index] = angle + + self.send_response(f"OK;{category};{data};{val}") + elif category == "ASC" and data in ("HIGH", "LOW"): + self.send_response(f"OK;ASC;{data};{val}") + else: + self.send_response(f"KO;{category};{data};{val}") + + def handle_mov(self, parts): + if len(parts) != 3: + return + + category, data = parts[1], parts[2] + + if category == "ASC": + if data in ("HIGH", "LOW"): + self.ascenseur_pos = data.lower() + self.send_response(f"OK;{category};{data}") + else: + self.send_response(f"KO;{category};{data}") + elif category.startswith("SERVO") and data.startswith("POS"): + sid = int(category[5:]) + pos_index = int(data[3:]) + if sid in self.servo_angles and pos_index in self.servo_angles[sid]: + self.servos[sid] = pos_index + self.send_response(f"OK;{category};{data}") + else: + self.send_response(f"KO;{category};{data}") + elif category == "RELAY": + if data == "1": + for r in self.relais: + self.relais[r] = 1 + self.send_response(f"OK;RELAY;1") + elif data == "0": + for r in self.relais: + self.relais[r] = 0 + self.send_response(f"OK;RELAY;0") + else: + self.send_response(f"KO;RELAY;{data}") + else: + self.send_response(f"KO;{category};{data}") + + def run(self): + while self.running: + if self.ser.in_waiting: + msg = self.ser.readline().decode().strip() + print(f"[RX] {msg}") + self.process_command(msg) + + def stop(self): + self.running = False + self.thread.join() + self.ser.close() + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description="Simulated PCB") + parser.add_argument('--port', type=str, default='/dev/pts/6', help='Serial port to use') + args = parser.parse_args() + + sim = None + try: + sim = SimulatedPCB(port=args.port) + except KeyboardInterrupt: + if sim: + sim.stop() + print("Simulation stopped") diff --git a/src/modelec_com/CMakeLists.txt b/src/modelec_com/CMakeLists.txt index f6f6d7d..feb415e 100644 --- a/src/modelec_com/CMakeLists.txt +++ b/src/modelec_com/CMakeLists.txt @@ -77,6 +77,7 @@ install(TARGETS serial_listener pcb_alim_interface pcb_odo_interface + pcb_action_interface pca9685_controller DESTINATION lib/${PROJECT_NAME} ) 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 c78268d..e6ab089 100644 --- a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp @@ -19,7 +19,7 @@ namespace Modelec protected: std::map asc_v_; std::map> servo_pos_v_; - std::map relay_v_; + std::map relay_v_; private: rclcpp::Publisher::SharedPtr pcb_publisher_; diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 57b80bd..27b3dbb 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -114,10 +114,18 @@ namespace Modelec { } - else if (startsWith(tokens[2], "SERVO")) + else if (startsWith(tokens[1], "SERVO")) { - int servo_id = std::stoi(tokens[2].substr(5)); - + 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; + } + 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; } } else if (tokens[0] == "OK") diff --git a/src/modelec_core/launch/test.modelec.launch.py b/src/modelec_core/launch/test.modelec.launch.py new file mode 100644 index 0000000..2686fe2 --- /dev/null +++ b/src/modelec_core/launch/test.modelec.launch.py @@ -0,0 +1,60 @@ +from launch import LaunchDescription +from launch.actions import RegisterEventHandler, Shutdown +from launch.event_handlers import OnProcessExit +from launch_ros.actions import Node + +def generate_launch_description(): + # Qt GUI node + gui_node = Node( + package='modelec_gui', + executable='modelec_gui', + name='modelec_gui' + ) + + # Shut down all nodes when GUI exits + shutdown_on_gui_exit = RegisterEventHandler( + OnProcessExit( + target_action=gui_node, + on_exit=[Shutdown()] + ) + ) + + return LaunchDescription([ + Node( + package='modelec_com', + executable='serial_listener', + name='serial_listener' + ), + Node( + package='modelec_com', + executable='pcb_odo_interface', + name='pcb_odo_interface' + ), + Node( + package='modelec_com', + executable='pcb_alim_interface', + name='pcb_alim_interface' + ), + Node( + package='modelec_com', + executable='pcb_action_interface', + name='pcb_action_interface' + ), + gui_node, + shutdown_on_gui_exit, + Node( + package='modelec_core', + executable='speed_result', + name='speed_result' + ), + Node( + package='modelec_strat', + executable='strat_fsm', + name='strat_fsm' + ), + Node( + package='modelec_strat', + executable='pami_manager', + name='pami_manager' + ) + ]) diff --git a/src/modelec_core/launch/test.modelec.launch.yml b/src/modelec_core/launch/test.modelec.launch.yml deleted file mode 100644 index 660b776..0000000 --- a/src/modelec_core/launch/test.modelec.launch.yml +++ /dev/null @@ -1,41 +0,0 @@ -launch: - - - node: - pkg: 'modelec_com' - exec: "serial_listener" - name: "serial_listener" - - - node: - pkg: 'modelec_com' - exec: 'pcb_odo_interface' - name: 'pcb_odo_interface' - - - node: - pkg: 'modelec_com' - exec: 'pcb_alim_interface' - name: 'pcb_alim_interface' - - - node: - pkg: 'modelec_com' - exec: 'pcb_action_interface' - name: 'pcb_action_interface' - - - node: - pkg: 'modelec_gui' - exec: "modelec_gui" - name: "modelec_gui" - - - node: - pkg: 'modelec_core' - exec: 'speed_result' - name: 'speed_result' - - - node: - pkg: 'modelec_strat' - exec: 'strat_fsm' - name: 'strat_fsm' - - - node: - pkg: 'modelec_strat' - exec: 'pami_manager' - name: 'pami_manager' diff --git a/src/modelec_gui/src/main.cpp b/src/modelec_gui/src/main.cpp index 17129d7..2e45856 100644 --- a/src/modelec_gui/src/main.cpp +++ b/src/modelec_gui/src/main.cpp @@ -8,10 +8,8 @@ int main(int argc, char **argv) { - // Initialize the Qt application QApplication app(argc, argv); - // Initialize ROS 2 rclcpp::init(argc, argv); std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"; @@ -20,30 +18,24 @@ int main(int argc, char **argv) RCLCPP_ERROR(rclcpp::get_logger("modelec_gui"), "Failed to load configuration file: %s", config_path.c_str()); } - // Create the node only once auto node = rclcpp::Node::make_shared("qt_gui_node"); - // Create the GUI and pass the node to it - ModelecGUI::ROS2QtGUI window(node); // Pass the node to the GUI component + ModelecGUI::ROS2QtGUI window(node); window.show(); - // Create an executor for ROS 2 to manage the node rclcpp::executors::MultiThreadedExecutor executor; - // Add the node to the executor once executor.add_node(node); - // Run ROS 2 in a separate thread std::thread ros_thread([&executor]() { - executor.spin(); // Execute the node's callbacks + executor.spin(); }); - // Start the Qt application event loop int ret = app.exec(); - // Ensure the ROS 2 executor thread ends correctly + executor.cancel(); + rclcpp::shutdown(); ros_thread.join(); - rclcpp::shutdown(); // Shutdown ROS 2 - return ret; // Return the application result + return ret; } diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 03b57b1..6f8ed1e 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -53,7 +53,7 @@ pcb_action - /dev/pts/12 + /dev/pts/16 115200