forked from zbw/yiliao2026
59 lines
1.4 KiB
Python
Executable File
59 lines
1.4 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
"""
|
|
UDP → /cmd_vel bridge for keyboard control.
|
|
Receives (float64 lin_x, float64 ang_z) from Windows client.
|
|
|
|
Deploy on RDKx5: /home/sunrise/yiliao_ws/udp_to_cmdvel.py
|
|
Run: python3 udp_to_cmdvel.py
|
|
"""
|
|
|
|
import rclpy
|
|
from rclpy.node import Node
|
|
from geometry_msgs.msg import Twist
|
|
import socket
|
|
import struct
|
|
import sys
|
|
|
|
|
|
class UDPToCmdVel(Node):
|
|
def __init__(self, port=9999):
|
|
super().__init__("udp_to_cmdvel")
|
|
self.pub = self.create_publisher(Twist, "/cmd_vel", 10)
|
|
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
self.sock.bind(("0.0.0.0", port))
|
|
self.sock.settimeout(0.1)
|
|
self.get_logger().info("Listening on UDP port %d → /cmd_vel" % port)
|
|
self.create_timer(0.05, self.loop)
|
|
|
|
def loop(self):
|
|
try:
|
|
data, addr = self.sock.recvfrom(16)
|
|
lin, ang = struct.unpack('dd', data)
|
|
msg = Twist()
|
|
msg.linear.x = lin
|
|
msg.angular.z = ang
|
|
self.pub.publish(msg)
|
|
except socket.timeout:
|
|
pass
|
|
|
|
def destroy_node(self):
|
|
self.sock.close()
|
|
super().destroy_node()
|
|
|
|
|
|
def main():
|
|
rclpy.init()
|
|
port = int(sys.argv[1]) if len(sys.argv) > 1 else 9999
|
|
node = UDPToCmdVel(port=port)
|
|
try:
|
|
rclpy.spin(node)
|
|
except KeyboardInterrupt:
|
|
pass
|
|
finally:
|
|
node.destroy_node()
|
|
rclpy.shutdown()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|