node

wave_rover_controller/wave_rover_node.py

@@ -0,0 +1,122 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from serial import Serial
+from serial import SerialException
+import json
+import time
+from std_srvs.srv import Trigger
+from geometry_msgs.msg import Twist
+from wave_rover_controller.fan_gpio import GPIOController
+
+
+class WaveRoverNode(Node):
+
+    def __init__(self):
+        super().__init__('wave_rover_node')
+
+
+        self.declare_parameter('serial_port', '/dev/ttyUSB0')
+        self.declare_parameter('baudrate', 115200)
+
+        self.declare_parameter('fan_pin', 13)
+        self.declare_parameter('fan_time', 3.)
+
+        self._serial_port = self.get_parameter('serial_port').value
+        self._baudrate = self.get_parameter('baudrate').value
+
+        fan_pin = self._node.get_parameter('fan_pin').value
+        self._gpio_controller = GPIOController(fan_pin)
+        self._fan_time = self._node.get_parameter('fan_time').value
+        self._fe_burst_timer = None
+
+        wheel_max = 0.5 # popugais
+        self._linear_max = wheel_max
+        self._base = 0.2 # m
+        self._angular_max = 2 * wheel_max / self._base
+
+        # Serial подключение
+        self._serial_conn: Optional[Serial] = None
+        self._connect_serial()
+
+        self.create_service(Trigger, 'fe_burst', self.fe_burst_cb)
+        self.create_subscription(Twist, 'cmd_vel', self.twist_cb, 10)
+
+    def _connect_serial(self):
+        try:
+            self._serial_conn = Serial(
+                port=self._serial_port, baudrate=self._baudrate, timeout=1.0
+            )
+            time.sleep(0.5)
+            self._node.get_logger().info(f'Подключено к {self._serial_port}')
+        except SerialException as e:
+            self._node.get_logger().error(f'Ошибка serial: {e}')
+            self._serial_conn = None
+
+    def _send_json_command(self, command: dict):
+        if not self._serial_conn or not self._serial_conn.is_open:
+            self._connect_serial()
+            return
+        try:
+            json_str = json.dumps(command)
+            self._serial_conn.write(json_str.encode() + b'\n')
+            self._node.get_logger().debug(f'Отправлено: {json_str}')
+        except SerialException as e:
+            self._node.get_logger().error(f'Ошибка отправки: {e}')
+            self._serial_conn.close()
+
+    def fe_burst_cb(self, req, res):
+        if self._fe_burst_timer is None:
+            self._fe_burst_timer = self._node.create_timer(self._fan_time, self._fe_burst_timer_cb)
+            self._gpio_controller.enable()
+            msg = "Fire extingusher burst is performing"
+
+            res.success = True
+            self._node.get_logger().info(msg)
+        else:
+            msg = "Fire extingusher burst is already performing"
+            self._node.get_logger().error(msg)
+            res.success = False
+        res.message = msg
+        return res
+
+    def _fe_burst_timer_cb(self):
+        self._node.destroy_timer(self._fe_burst_timer)
+        self._fe_burst_timer = None
+        self._gpio_controller.disable()
+
+    def _twist_to_wheel_speeds(self, linear_x: float, angular_z: float):
+        """Twist -> скорости колес для дифференциального привода"""
+
+        left_speed = (linear_x - (self._base * angular_z) / 2) #/ self.max_linear_speed
+        right_speed = (linear_x + (self._base * angular_z) / 2) #/ self.max_linear_speed
+
+        left_speed = max(-1.0, min(1.0, left_speed))
+        right_speed = max(-1.0, min(1.0, right_speed))
+
+        return left_speed, right_speed
+
+    def twist_cb(self, msg):
+        left_speed, right_speed = self._twist_to_wheel_speeds(msg.linear.x, msg.angular.z)
+
+        command = {"T": 1, "L": left_speed, "R": right_speed}
+        self._send_json_command(command)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = WaveRoverNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
+
+
+
+
+