Update 'wave_rover_controller/wave_rover_controller.py'

wave_rover_controller/wave_rover_controller.py

@@ -7,6 +7,7 @@ from serial import Serial
 from serial import SerialException
 import json
 import time
+from wave_rover_controller.fan_gpio import GPIOController
 
 class WaveRoverController(RobotController):
 
@@ -18,8 +19,15 @@ class WaveRoverController(RobotController):
         self._node.declare_parameter('serial_port', '/dev/ttyUSB0')
         self._node.declare_parameter('baudrate', 115200)
 
+        self._node.declare_parameter('fan_pin', 13)
+        self._node.declare_parameter('fan_time', 5.)
+
         self._serial_port = self._node.get_parameter('serial_port').value
         self._baudrate = self._node.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
@@ -47,7 +55,6 @@ class WaveRoverController(RobotController):
         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')
@@ -69,6 +76,23 @@ class WaveRoverController(RobotController):
         self._send_json_command(command)
 
 
+    def send_fire_ext_burst_cmd(self):
+        result = super(WaveRoverController, self).send_fire_ext_burst_cmd()
+        if not result:
+            self._node.get_logger().error("Fire extinguisher is out of fuel!")
+            return False
+
+        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()
+            self._node.get_logger().info("Fire extingusher burst is performing")
+        else:
+            self._node.get_logger().error("Fire extingusher burst is already performing")
+
+    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 -> скорости колес для дифференциального привода"""