44 lines
1.3 KiB
Python
44 lines
1.3 KiB
Python
|
# rover.py
|
||
|
|
import time, json, struct
|
||
|
|
import paho.mqtt.client as mqtt
|
||
|
|
|
||
|
|
BROKER_IP = "192.168.99.120" # IP da Base
|
||
|
|
RID = "rover01"
|
||
|
|
TOP_TEL = f"agrobot/{RID}/telemetry"
|
||
|
|
TOP_CMD = f"agrobot/{RID}/cmd"
|
||
|
|
TOP_HEALTH = f"agrobot/{RID}/health"
|
||
|
|
TOP_LWT = f"agrobot/{RID}/lwt"
|
||
|
|
|
||
|
|
def on_connect(c, u, f, rc):
|
||
|
|
print("Rover conectado, rc=", rc)
|
||
|
|
c.subscribe(TOP_CMD, qos=1)
|
||
|
|
# Publica estado online (retido)
|
||
|
|
c.publish(TOP_HEALTH, json.dumps({"status":"online","t":time.time()}), qos=1, retain=True)
|
||
|
|
|
||
|
|
def on_message(c, u, msg):
|
||
|
|
if msg.topic == TOP_CMD:
|
||
|
|
# Exemplo de payload binário: vel (float32), ang (float32)
|
||
|
|
vel, ang = struct.unpack("<ff", msg.payload)
|
||
|
|
print(f"[CMD] vel={vel:.2f} m/s, ang={ang:.2f}°")
|
||
|
|
|
||
|
|
client = mqtt.Client(client_id=f"{RID}", clean_session=True)
|
||
|
|
client.will_set(TOP_LWT, payload="offline", qos=1, retain=True) # LWT
|
||
|
|
client.on_connect = on_connect
|
||
|
|
client.on_message = on_message
|
||
|
|
client.connect(BROKER_IP, 1883, keepalive=60)
|
||
|
|
client.loop_start()
|
||
|
|
|
||
|
|
try:
|
||
|
|
i = 0
|
||
|
|
while True:
|
||
|
|
# Telemetria json (poderia ser binário também)
|
||
|
|
tel = {"t": time.time(), "heading": 123.45+i*0.01, "vel": 1.6}
|
||
|
|
client.publish(TOP_TEL, json.dumps(tel), qos=0, retain=False)
|
||
|
|
i += 1
|
||
|
|
time.sleep(0.2)
|
||
|
|
except KeyboardInterrupt:
|
||
|
|
pass
|
||
|
|
finally:
|
||
|
|
client.loop_stop()
|
||
|
|
client.disconnect()
|