Close

Logs and maps (part 3)

A project log for Old Roomba, new tricks

Add mapping & MQTT-interfacing with ESP8266 and an IMU

simon-jansenSimon Jansen 08/30/2022 at 18:010 Comments

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