Aaaand, it's done!
Using numpy and matplotlib. It's not pretty, but it works! Actually I wanted to use a heatmap. But this will work for now.
Full python code:
#!/usr/bin/env python
# (c) 2022-08-30 S.E.Jansen.
# MQTT-layer for Roomba logging
# Listen for the start and stop of cleaning event
# Log raw and positional data to file
# Loop closing and error correction on positional data
# Render map from positional data as a imagefile
# Post imagefile to camera entity for Home Assistant
import time
import datetime
import MQTT_Config
import json
import paho.mqtt.client as paho
import csv
import matplotlib.pyplot as plt
import numpy as np
#Roomba device info for Home Assistant autoconfig
DeviceName = 'Roomba632'
DeviceID = '01'
DeviceManufacturer = 'iRobot'
DeviceModel = '632'
poseLogFilename = 'poseDummy'
poseLogFile = open(poseLogFilename + ".csv", 'w')
poseLogWriter = csv.writer(poseLogFile)
rawLogFilename = 'rawDummy'
rawLogFile = open(rawLogFilename + ".csv", 'w')
rawLogWriter = csv.writer(rawLogFile)
#MQTT callback functions
def on_message(client, userdata, message):
data = message.payload
global poseLogFilename
global poseLogFile
global poseLogWriter
global rawLogFilename
global rawLogFile
global rawLogWriter
if message.topic == MQTT_Config.HA_name + "/button/" + DeviceName + "_" + DeviceID + "/set":
command=data.decode("utf-8")
# start logging
if command == "Clean":
# open logfile
print("Started cleaning cycle")
poseLogFile.close()
poseLogFilename = "./Roomba/Logs/" + datetime.datetime.now().isoformat('_', 'seconds') + "_pose"
poseLogFile = open(poseLogFilename + ".csv", 'w')
poseLogWriter = csv.writer(poseLogFile)
rawLogFile.close()
rawLogFilename = "./Roomba/Logs/" + datetime.datetime.now().isoformat('_', 'seconds')+ "_raw"
rawLogFile = open(rawLogFilename + ".csv", 'w')
rawLogWriter = csv.writer(rawLogFile)
if message.topic == MQTT_Config.HA_name + "/device_automation/" + DeviceName + "_" + DeviceID + "/event_DoneCleaning":
event=data.decode("utf-8")
# start logging
if event == "done cleaning":
# close log
print("Cleaning cycle is done. Save logfile and start postprocess")
poseLogFile.close()
poseLogFile = open('poseDummy.csv', 'w')
poseLogWriter = csv.writer(poseLogFile)
rawLogFile.close()
rawLogFile = open('rawDummy.csv', 'w')
rawLogWriter = csv.writer(rawLogFile)
# plot image from CSV file.
with open(poseLogFilename + ".csv", 'r') as plotDataFile:
plotData = list(csv.reader(plotDataFile, delimiter=",",quoting=csv.QUOTE_NONNUMERIC))
plotData = np.array(plotData)
x,y = plotData.T
plt.scatter(x,y)
#plt.imshow(plotData, cmap='hot', interpolation='nearest')
plt.savefig(poseLogFilename + ".png", format='png')
# - publish map
poseLogImageFile = open(poseLogFilename+".png", "rb")
poseLogImageString = poseLogImageFile.read()
poseLogImageByteArray = bytes(poseLogImageString)
client.publish(MQTT_Config.HA_name + "/camera/" + DeviceName + "_" + DeviceID + "/map",poseLogImageByteArray,0,False)
if message.topic == MQTT_Config.HA_name + "/device/roomba/raw":
rawJson=json.loads(data.decode("utf-8"))
# start logging
#print(rawJson['mEL'])
#print(rawJson['mER'])
#print(rawJson['rTh'])
#print(rawJson['Xacc'])
#print(rawJson['Yacc'])
#print(rawJson['Yaw'])
if not rawLogFile.closed:
rawLogData = [rawJson['mEL'],rawJson['mER'],rawJson['rTh'],rawJson['Xacc'],rawJson['Yacc'],rawJson['Yaw']]
rawLogWriter.writerow(rawLogData)
else:
print("Raw logfile was closed")
if message.topic == MQTT_Config.HA_name + "/device/roomba/pose":
poseJson=json.loads(data.decode("utf-8"))
# start logging
#print(poseJson['X'])
#print(poseJson['Y'])
#print(poseJson['rTh'])
if not poseLogFile.closed:
#poseLogData = [poseJson['X'],poseJson['Y'],poseJson['rTh']]
poseLogData = [poseJson['X'],poseJson['Y']]
poseLogWriter.writerow(poseLogData)
else:
print("Pose logfile was closed")
#call back function for MQTT connection
def on_connect(client, userdata, flags, rc):
if rc==0:
client.connected_flag=True #set flag
print("connected OK")
# Send autoconfig messages and subscribe to command topics
init()
else:
print("Bad connection Returned code=",rc)
client.bad_connection_flag=True
#call back function for disconnect MQTT connection to reconnect
#def on_disconnect
# MQTT Home Assistant autodiscover config and subscriptions
def init():
# MQTT autoconfig
device = {}
device['identifiers'] = DeviceName + "_" + DeviceID
device['name'] = DeviceName + "_" + DeviceID
device['manufacturer'] = DeviceManufacturer
#device['sw_version'] = ""
device['model'] = DeviceModel
# - Camera entity for maps
data = {}
data['name'] = "Map of last clean"
data['topic'] = MQTT_Config.HA_name + "/camera/" + DeviceName + "_" + DeviceID + "/map"
#data['availability_topic'] = MQTT_Config.HA_name + "/button/heos/" + HeosName + "_play/available"
#data['payload_available'] = 'yes'
#data['payload_available'] = 'no'
data['unique_id'] = DeviceName + "_" + DeviceID + "_Map"
data['icon'] = "mdi:map-outline"
data['device'] = device
client.publish(MQTT_Config.HA_name + "/camera/" + DeviceName + "_" + DeviceID + "/config",json.dumps(data),0,True)
# MQTT subscribe to command topics to receive commands
# - Start clean cycle:
client.subscribe(MQTT_Config.HA_name + "/device_automation/" + DeviceName + "_" + DeviceID + "/event_DoneCleaning",0)
# - Event for ending of clean cylce:
client.subscribe(MQTT_Config.HA_name + "/button/" + DeviceName + "_" + DeviceID + "/set",0)
# - Positional data calculated on roomba:
client.subscribe(MQTT_Config.HA_name + "/device/roomba/pose",0)
# - Raw sensordata for postprocess calculation of position
client.subscribe(MQTT_Config.HA_name + "/device/roomba/raw",0)
# MQTT details
paho.Client.connected_flag=False #Create flag in class
paho.Client.bad_connection_flag=False #another flag
client= paho.Client() #create client object
client.username_pw_set(MQTT_Config.user,MQTT_Config.password)
client.on_connect=on_connect #bind call back function
#on disconnect?
client.on_message= on_message #attach function to callback
client.connect(MQTT_Config.broker,MQTT_Config.port) #establish connection
client.loop_start() #start network loop
while not client.connected_flag and not client.bad_connection_flag: #wait in loop
time.sleep(0.1)
if client.bad_connection_flag:
client.loop_stop() #Stop loop
sys.exit()
# Send autoconfig messages and subscribe to command topics
init()
# Main loop
while True:
try:
#print("still running")
# publish availability?
# publishAvailability(Heos1Name)
# publishAvailability(Heos2Name)
# publishAvailability(Heos3Name)
# publish 5x in expiration time
time.sleep(60)
except KeyboardInterrupt:
client.loop_stop() #stop connection loop
client.disconnect() #gracefully disconnect
client.connected_flag=False #reset flag
poseLogFile.close()
rawLogFile.close()
print("Ending program")
exit()
This runs as a service. Once again, I'm not responsible for any spilled beer with the use of this code. Still quite proud though :)
Discussions
Become a Hackaday.io Member
Create an account to leave a comment. Already have an account? Log In.