---

import sys
import select
import termios
import tty
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

# Настройки скоростей
LINEAR_SPEED = 0.2   # Скорость вперед/назад (м/с)
ANGULAR_SPEED = 0.5  # Скорость поворота (рад/с)

INSTRUCTIONS = """
---------------------------
Управление роботом (WASD):
   w : вперед
   s : назад
   a : влево
   d : вправо
   пробел : СТОП
   q / Ctrl+C : выход
---------------------------
"""

def get_key(settings):
    """Считывает одну нажатую клавишу без нажатия Enter"""
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

def main():
    settings = termios.tcgetattr(sys.stdin)
    rclpy.init()
    node = rclpy.create_node('teleop_wasd')
    pub = node.create_publisher(Twist, '/cmd_vel', 10)

    print(INSTRUCTIONS)

    try:
        while True:
            key = get_key(settings)
            twist = Twist()

            if key == 'w':
                twist.linear.x = LINEAR_SPEED
                print("Вперед  ▲", end='\r')
            elif key == 's':
                twist.linear.x = -LINEAR_SPEED
                print("Назад   ▼", end='\r')
            elif key == 'a':
                twist.angular.z = ANGULAR_SPEED
                print("Влево   ◄", end='\r')
            elif key == 'd':
                twist.angular.z = -ANGULAR_SPEED
                print("Вправо  ►", end='\r')
            elif key == ' ':
                twist.linear.x = 0.0
                twist.angular.z = 0.0
                print("Стоп    ■", end='\r')
            elif key == 'q' or key == '\x03':  # 'q' или Ctrl+C
                print("\nВыход...")
                break
            else:
                # Если ничего не нажато, продолжаем крутить цикл
                continue

            pub.publish(twist)

    except Exception as e:
        print(f"\nОшибка: {e}")

    finally:
        # При выходе отправляем нулевую скорость для безопасности
        stop_twist = Twist()
        pub.publish(stop_twist)
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()