diff --git a/wave_rover_controller/wave_rover_node.py b/wave_rover_controller/wave_rover_node.py new file mode 100644 index 0000000..1e905ad --- /dev/null +++ b/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() + + + + +