Last active
August 29, 2024 16:01
-
-
Save okets/b2bdf3ba2ab96c27ad58274372298261 to your computer and use it in GitHub Desktop.
figure out the room Valetudo robot is right now.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
const mapData = msg.payload; | |
const _pixelSize = mapData.pixelSize; | |
let _robotXY = null; | |
for (let i = 0; i < mapData.entities.length; i++) { | |
if (mapData.entities[i].type == "robot_position") { | |
_robotXY = { | |
x: Math.floor(mapData.entities[i].points[0] / _pixelSize), | |
y: Math.floor(mapData.entities[i].points[1] / _pixelSize) | |
}; | |
break; | |
} | |
} | |
// Test Data | |
// _robotXY = { | |
// x: 1000, | |
// y: 1000 | |
// }; | |
const _nearestPoint = { | |
x: mapData.size.x, | |
y: mapData.size.y, | |
distance: Math.pow(mapData.size.x,2) + Math.pow(mapData.size.y,2), //Just a placeholder with the biggest distance possible | |
foundRoom:{ | |
segmentId: null, | |
name: null | |
} | |
} | |
if (mapData.metaData?.version === 2 && Array.isArray(mapData.layers)) { | |
mapData.layers.forEach(layer => { | |
if (layer.pixels.length === 0 && layer.compressedPixels.length !== 0 && layer.type == "segment") { | |
for (let i = 0; i < layer.compressedPixels.length; i = i + 3) { | |
const xStart = layer.compressedPixels[i]; | |
const y = layer.compressedPixels[i + 1] | |
const count = layer.compressedPixels[i + 2] | |
for (let j = 0; j < count; j++) { | |
let x = xStart + j; | |
let _distanceX = x - _robotXY.x; | |
let _distanceY = y - _robotXY.y; | |
//const _pointToRobotDistance = Math.sqrt(Math.pow(_distanceX, 2) + Math.pow(_distanceY, 2)); | |
const _pointToRobotDistance = Math.pow(_distanceX, 2) + Math.pow(_distanceY, 2);//sqrt is the "correct" trig function, but I only need to compare relative to other points, not exact distance, so I dropped the "sqrt" function for efficiancy | |
if (_nearestPoint.distance > _pointToRobotDistance){ | |
_nearestPoint.distance = _pointToRobotDistance; | |
_nearestPoint.x = x; | |
_nearestPoint.y = y; | |
_nearestPoint.foundRoom= { | |
segmentId: layer.metaData.segmentId, | |
name: layer.metaData.name | |
}; | |
} | |
if (_nearestPoint.distance==0) { | |
return; | |
} | |
} | |
} | |
} | |
}) | |
} | |
return _nearestPoint.foundRoom; |
Thanks for the script! With the help of NodeRed I made a small automation to disable obstacle recognition in a certain segment, that the robot can't pass with it enabled.
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
This is so simple and brilliant thank you so much for this!