when quit qt that quit everything

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")

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}
 )

src/modelec_com/include/modelec_com/pcb_action_interface.hpp

@@ -19,7 +19,7 @@ namespace Modelec
     protected:
         std::map<std::string, int> asc_v_;
         std::map<int, std::map<int, int>> servo_pos_v_;
-        std::map<std::string, bool> relay_v_;
+        std::map<int, bool> relay_v_;
 
     private:
         rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pcb_publisher_;

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")

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'
+        )
+    ])

src/modelec_core/launch/test.modelec.launch.yml

@@ -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'

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;
 }

src/modelec_strat/data/config.xml

@@ -53,7 +53,7 @@
             </pcb_alim>
             <pcb_action>
                 <name>pcb_action</name>
-                <port>/dev/pts/12</port>
+                <port>/dev/pts/16</port>
                 <baud_rate>115200</baud_rate>
             </pcb_action>
         </pcb>