48 lines
1.3 KiB
Python
48 lines
1.3 KiB
Python
# base.py
|
|
import time, json, struct
|
|
import paho.mqtt.client as mqtt
|
|
|
|
BROKER_IP = "0.0.0.0" # se a base RODA o broker, tanto faz; para conectar, use "127.0.0.1"
|
|
RID = "rover01"
|
|
TOP_TEL = f"agrobot/{RID}/telemetry"
|
|
TOP_CMD = f"agrobot/{RID}/cmd"
|
|
|
|
last_t = None
|
|
|
|
def on_connect(c, u, f, rc):
|
|
print("Base conectada, rc=", rc)
|
|
c.subscribe(TOP_TEL, qos=0)
|
|
|
|
def on_message(c, u, msg):
|
|
global last_t
|
|
if msg.topic == TOP_TEL:
|
|
tel = json.loads(msg.payload.decode("utf-8"))
|
|
now = time.time()
|
|
if "t" in tel:
|
|
rtt_ms = (now - tel["t"]) * 1000
|
|
else:
|
|
rtt_ms = None
|
|
print(f"[TEL] vel={tel.get('vel')}, heading={tel.get('heading')}, rtt≈{rtt_ms:.1f} ms")
|
|
|
|
client = mqtt.Client(client_id="base01", clean_session=True)
|
|
client.on_connect = on_connect
|
|
client.on_message = on_message
|
|
client.connect("127.0.0.1", 1883, keepalive=60) # se a base é o broker; senão use o IP do broker
|
|
client.loop_start()
|
|
|
|
# Envia um comando binário exemplo (QoS 1)
|
|
time.sleep(1.0)
|
|
vel, ang = 0.5, 10.0
|
|
pkt = struct.pack("<ff", vel, ang)
|
|
client.publish(TOP_CMD, pkt, qos=1, retain=False)
|
|
print("[BASE] Comando enviado")
|
|
|
|
try:
|
|
while True:
|
|
time.sleep(0.5)
|
|
except KeyboardInterrupt:
|
|
pass
|
|
finally:
|
|
client.loop_stop()
|
|
client.disconnect()
|