---

import socket
import json

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TwistStamped

OPERATOR_IP = '192.168.1.100'
CMD_PORT = 5001


def recv_exact(sock, n):
    buf = b''
    while len(buf) < n:
        chunk = sock.recv(n - len(buf))
        if not chunk:
            return None
        buf += chunk
    return buf


class CmdVelBridge(Node):

    def __init__(self):
        super().__init__('socket_cmd_bridge')
        self.pub = self.create_publisher(TwistStamped, '/cmd_vel', 10)

    def send(self, linear_x, angular_z):
        msg = TwistStamped()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'base_link'
        msg.twist.linear.x = float(linear_x)
        msg.twist.angular.z = float(angular_z)
        self.pub.publish(msg)

    def stop(self):
        self.send(0.0, 0.0)


def main():
    rclpy.init()
    node = CmdVelBridge()

    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.connect((OPERATOR_IP, CMD_PORT))
    print(f"подключено к оператору {OPERATOR_IP}:{CMD_PORT}")

    try:
        while True:
            size_bytes = recv_exact(sock, 4)
            if size_bytes is None:
                break
            msg_len = int.from_bytes(size_bytes, 'big')

            payload = recv_exact(sock, msg_len)
            if payload is None:
                break

            command = json.loads(payload.decode('utf-8'))
            node.send(command.get("linear_x", 0.0), command.get("angular_z", 0.0))
    except (ConnectionResetError, OSError) as e:
        print(f"соединение оборвано: {e}")
    except KeyboardInterrupt:
        pass
    finally:
        node.stop()
        sock.close()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()