We're closing the loop!

While I'm waiting on some stuff to make the Roomba run more stable (guess what, I'ts POWER-ISSUES!), I'm checking off the next thing on my to-do list. That's charting Roomba's travels post-process. And look at the results!:

Raw:

Post-Processed:

I already had a python script running as a daemon on a raspberry pi. This listens to published events and data on the MQTT-broker (running on Home Assistant).

There are two important events to listen to: start-run & end-run.

Then there are two data-streams. 1 pose-data calculated real-time by the Roomba. This has X,Y coordinates that are used for a live map.

2. Raw data containing motorencoder data, real-world yaw by the MPU (for now) and stuff like light-bumpers for wall detection. These values are recorded to a logfile, later used in post-process.

The positional data is used as a first approximation. The starting & ending position is both on the charging dock. Same position and same pose. This means a closed loop and therefore a loop closing error to correct.

The first few positions are used to get a starting angle. The last few positions as a ending angle (as a rolling average).

As long as the total accumulated drift is less than half a full circle, the difference between starting and ending angle can be linearly "smeared" out over the recorded yaw-values when post-processing.

The same can be done with starting and ending coordinate.

Python gives some output:

start run

Started mapping run
end run
Full run is complete. Save logfile and start postprocess
2558  lines recorded
Average opening Theta:  0.00093772
Average closing Theta:  -0.19936671270879255
Closing error:  0.20030443270879256
Error correction:  7.830509488224884e-05
dX: 487.2798  dY: -646.4786

 Using both angle and coordinate correction is a bit too much. 

So now I'm experimenting with full angle correction and half of the coordinate correction. We'll see after more runs :)

Full python script:

#!/usr/bin/env python
# (c) 2022-09-06 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
    # Log wall sensors
    # Render image from postprocess
    # Render walls in postprocess image

import time
import datetime
import MQTT_Config
import json
import paho.mqtt.client as paho
import csv
import matplotlib.pyplot as plt
import matplotlib.colors as col
import io
import numpy as np

#Roomba device info for Home Assistant autoconfig
DeviceName = 'Roomba632'
DeviceID = '01'
DeviceManufacturer = 'iRobot'
DeviceModel = '632'

DISTANCEPERCOUNTL = 0.4525 #(pi * 72.0 / 508.8) mm / count
DISTANCEPERCOUNTR = 0.4400 #(pi * 72.0 / 508.8) mm / count
WHEELDISTANCE = 247.0 #mm center wheel <-> center wheel

fieldnames = ['EncoderLeft','EncoderRight','Theta','AccelX','AccelY','GyroYaw','LightBumperLeft','LightBumperFrontLeft', 'LightBumperCenterLeft','LightBumperCenterRight','LightBumperFrontRight','LightBumperRight']
PosXdata, PosYdata = [], []
WallXdata, WallYdata = [], []
StartXdata, StartYdata = [], []
EndXdata, EndYdata = [], []
linecount = 0
sum_closing_theta = 0
sum_opening_theta = 0
closing_error = 0
error_correction = 0
PreviousEncoderLeft = 0
PreviousEncoderRight = 0
Theta = 0
PosXcoo = 0
PosYcoo = 0
WallXcoo = 0
WallYcoo = 0

poseLogFilename = './Roomba/Logs/poseDummy'
poseLogFile = open(poseLogFilename + ".csv", 'w')
poseLogWriter = csv.writer(poseLogFile)
rawLogFilename = './Roomba/Logs/rawDummy'
rawLogFile = open(rawLogFilename + ".csv", 'w')
rawLogWriter = csv.writer(rawLogFile)

liveFeedCounter = 0
Xdata, Ydata = [], []

#MQTT callback functions
def on_message(client, userdata, message):
    data = message.payload
    global poseLogFilename
    global poseLogFile
    global poseLogWriter
    global rawLogFilename
    global rawLogFile
    global rawLogWriter
    global liveFeedCounter
    global Xdata,Ydata
    global PosXdata, PosYdata
    global WallXdata, WallYdata
    global StartXdata, StartYdata
    global EndXdata, EndYdata
    global linecount
    global sum_closing_theta
    global sum_opening_theta
    global PreviousEncoderLeft
    global PreviousEncoderRight
    global Theta
    global PosXcoo
    global PosYcoo
    global WallXcoo
    global WallYcoo

    if message.topic == MQTT_Config.HA_name + "/device_automation/" + DeviceName + "_" + DeviceID + "/event_StartRun":
        event=data.decode("utf-8")
        print(event)
        # start logging
        if  event == "start run":
            #reset values
            linecount = 0
            sum_closing_theta = 0
            sum_opening_theta = 0
            PosXdata, PosYdata = [], []
            WallXdata, WallYdata = [], []
            StartXdata, StartYdata = [], []
            EndXdata, EndYdata = [], []
            PreviousEncoderLeft = 0
            PreviousEncoderRight = 0
            Theta = 0
            PosXcoo = 0
            PosYcoo = 0
            WallXcoo = 0
            WallYcoo = 0
            liveFeedCounter = 0
            Xdata, Ydata = [], []
            # open logfile
            print("Started mapping run")
            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_EndRun":
        event=data.decode("utf-8")
        print(event)
        # Stop logging
        if event == "end run":
            # close log
            print("Full run is complete. Save logfile and start postprocess")
            print(linecount, " lines recorded")
            poseLogFile.close()
            #poseLogFile = open('poseDummy.csv', 'w')
            #poseLogWriter = csv.writer(poseLogFile)
            rawLogFile.close()
            #rawLogFile = open('rawDummy.csv', 'w')
            #rawLogWriter = csv.writer(rawLogFile)

            avg_opening_theta = sum_opening_theta / 50
            print("Average opening Theta: ", avg_opening_theta)
            avg_closing_theta = sum_closing_theta / 100
            print("Average closing Theta: ", avg_closing_theta)
            closing_error = avg_opening_theta - avg_closing_theta
            if((0-np.pi) < closing_error < np.pi):
                print("Closing error: ", closing_error)
                error_correction = closing_error / linecount
                print("Error correction: ", error_correction)
                print("dX:", Xdata[-1], " dY:", Ydata[-1])
                eX = (Xdata[-1] / linecount) /2
                eY = (Ydata[-1] / linecount) /2
            else:
                print("Closing error out of range: ", closing_error)
                error_correction = 0

            with open(rawLogFilename + ".csv", newline='') as rawLogFile:
                DictReader = csv.DictReader(rawLogFile, fieldnames)
                #Apply error correction and plot to map:
                for row in DictReader:
                    if(DictReader.line_num == 1):
                        PreviousEncoderLeft = np.int16(row['EncoderLeft'])
                        PreviousEncoderRight= np.int16(row['EncoderRight'])
                    EncoderLeft = np.int16(row['EncoderLeft'])
                    EncoderRight = np.int16(row['EncoderRight'])
                    DeltaEncoderLeft = EncoderLeft - PreviousEncoderLeft
                    DeltaEncoderRight =  EncoderRight - PreviousEncoderRight
                    DistanceLeft = DISTANCEPERCOUNTL * DeltaEncoderLeft
                    DistanceRight = DISTANCEPERCOUNTR * DeltaEncoderRight
                    Distance = (DistanceLeft + DistanceRight) / 2
                    Theta += ((DistanceRight-DistanceLeft)/WHEELDISTANCE)
                    if(Theta > 6.28):
                        Theta -= 6.28
                    if(Theta < -6.28):
                        Theta += 6.28
                    PreviousEncoderLeft = EncoderLeft
                    PreviousEncoderRight = EncoderRight
                    #GyroYaw = Theta
                    GyroYaw = float(row['GyroYaw'])
                    GyroYaw += error_correction * DictReader.line_num
                    PosXcoo += Distance * np.cos(GyroYaw)
                    PosYcoo += Distance * np.sin(GyroYaw)
                    PosXcoo -= eX
                    PosYcoo -= eY
                    PosXdata.append(PosXcoo)
                    PosYdata.append(PosYcoo)
                    if (DictReader.line_num < 100):
                        StartXdata.append(PosXcoo)
                        StartYdata.append(PosYcoo)
                    if (DictReader.line_num > (linecount-100)):
                        EndXdata.append(PosXcoo)
                        EndYdata.append(PosYcoo)
                    if(row['LightBumperLeft'] == "True"):
                        WallXcoo = PosXcoo + (174.25 * np.cos(GyroYaw + 1.57))
                        WallYcoo = PosYcoo + (174.25 * np.sin(GyroYaw + 1.57))
                        WallXdata.append(WallXcoo)
                        WallYdata.append(WallYcoo)
                        #print("WallX:", WallXcoo, " WallY:", WallYcoo)
                    if(row['LightBumperFrontLeft'] == "True"):
                        WallXcoo = PosXcoo + (174.25 * np.cos(GyroYaw + 1.04))
                        WallYcoo = PosYcoo + (174.25 * np.sin(GyroYaw + 1.04))
                        WallXdata.append(WallXcoo)
                        WallYdata.append(WallYcoo)
                        #print("WallX:", WallXcoo, " WallY:", WallYcoo)
                    if(row['LightBumperCenterLeft'] == "True"):
                        WallXcoo = PosXcoo + (174.25 * np.cos(GyroYaw + 0.52))
                        WallYcoo = PosYcoo + (174.25 * np.sin(GyroYaw + 0.52))
                        WallXdata.append(WallXcoo)
                        WallYdata.append(WallYcoo)
                        #print("WallX:", WallXcoo, " WallY:", WallYcoo)
                    if(row['LightBumperCenterRight'] == "True"):
                        WallXcoo = PosXcoo + (174.25 * np.cos(GyroYaw - 0.52))
                        WallYcoo = PosYcoo + (174.25 * np.sin(GyroYaw - 0.52))
                        WallXdata.append(WallXcoo)
                        WallYdata.append(WallYcoo)
                        #print("WallX:", WallXcoo, " WallY:", WallYcoo)
                    if(row['LightBumperFrontRight'] == "True"):
                        WallXcoo = PosXcoo + (174.25 * np.cos(GyroYaw - 1.04))
                        WallYcoo = PosYcoo + (174.25 * np.sin(GyroYaw - 1.04))
                        WallXdata.append(WallXcoo)
                        WallYdata.append(WallYcoo)
                        #print("WallX:", WallXcoo, " WallY:", WallYcoo)
                    if(row['LightBumperRight'] == "True"):
                        WallXcoo = PosXcoo + (174.25 * np.cos(GyroYaw - 1.57))
                        WallYcoo = PosYcoo + (174.25 * np.sin(GyroYaw - 1.57))
                        WallXdata.append(WallXcoo)
                        WallYdata.append(WallYcoo)
                        #print("WallX:", WallXcoo, " WallY:", WallYcoo)
                    #print("Theta:", Theta, " Roomba:", row['Theta'], " GyroYaw:", row['GyroYaw'], " X:", PosXcoo, "Y:", PosYcoo)
            # plot image from data
            plt.savefig(poseLogFilename + "_raw.png", dpi=600,format='png')
            plt.clf()
            plt.axis('equal')
            plt.grid(True)
            plt.plot(PosXdata, PosYdata, linestyle='-', color='b')
            plt.plot(StartXdata, StartYdata, linestyle='-', color='m', zorder=2.5)
            plt.plot(EndXdata, EndYdata, linestyle='-', color='r', zorder=2.5)
            plt.scatter(WallXdata,WallYdata, color='g', marker ='o', s=3, zorder=2.5)
            plt.scatter(PosXdata[0],PosYdata[0], color='g', marker = 'o',s=120, zorder=2.5)
            plt.scatter(PosXdata[-1],PosYdata[-1], color='r', marker = 'o',s=120, zorder=2.5)
            plt.hist2d(PosXdata, PosYdata, bins=25, cmin=1, norm=col.LogNorm())
            plt.savefig(poseLogFilename + ".png", dpi=600,format='png')
            plt.close
            # - 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,True)
    if message.topic == MQTT_Config.HA_name + "/device/roomba/raw":
        rawJson=json.loads(data.decode("utf-8"))
        # start logging
        if not rawLogFile.closed:
            rawLogData = [rawJson['EL'],rawJson['ER'],rawJson['Th'],rawJson['Xa'],rawJson['Ya'],rawJson['Yw'],rawJson['LL'],rawJson['FL'],rawJson['CL'],rawJson['CR'],rawJson['FR'],rawJson['RR']]
            rawLogWriter.writerow(rawLogData)
            linecount += 1
            #First 50 values
            if (linecount <51):
                sum_opening_theta += float(rawJson['Yw'])
            #Last 100 values rolling average
            sum_closing_theta -= (sum_closing_theta/100) + float(rawJson['Yw'])
        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
        if not poseLogFile.closed:
            #poseLogData = [poseJson['X'],poseJson['Y'],poseJson['rTh']]
            poseLogData = [poseJson['X'],poseJson['Y']]
            poseLogWriter.writerow(poseLogData)         
            Xdata.append(poseJson['X'])
            Ydata.append(poseJson['Y']) 
            #-every 10 seconds records?
            liveFeedCounter=liveFeedCounter+1
            if liveFeedCounter == 10:
                liveFeedCounter = 0

                plt.close()
                plt.axis('equal')
                plt.grid(True)
                plt.plot(Xdata,Ydata,linestyle='-',color='b')
                plt.scatter(poseJson['X'],poseJson['Y'], color='r', marker = 'o',s=120, zorder=2.5)
                plt.scatter(Xdata[0],Ydata[0], color='g', marker = 'o',s=120, zorder=2.5)

                img_buf = io.BytesIO()
                plt.savefig(img_buf, dpi=50, format='png')
                img_buf.seek(0)
                poseLogImageString = img_buf.read()                          
                poseLogImageByteArray = bytes(poseLogImageString)
                client.publish(MQTT_Config.HA_name + "/camera/" + DeviceName + "_" + DeviceID + "/map",poseLogImageByteArray,0,False)
                img_buf.close()

        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_EndRun",0)
    # - Event for ending of clean cylce:
    client.subscribe(MQTT_Config.HA_name + "/device_automation/" + DeviceName + "_" + DeviceID + "/event_StartRun",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()

Also: as a bonus, I'm trying to render the walls!