Skip to content

Commit

Permalink
fix(vendor.3ir): Fix parsing of maps with different dimensions than 8…
Browse files Browse the repository at this point in the history 
…00x800px
  • Loading branch information
@Hypfer
Hypfer committed Apr 7, 2024
1 parent e7d2bc6 commit e45607b
Show file tree
Hide file tree
Showing 10 changed files with 10,636 additions and 10,609 deletions.
144 changes: 78 additions & 66 deletions backend/lib/robots/3irobotix/ThreeIRobotixMapParser.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -203,8 +203,8 @@ class ThreeIRobotixMapParser {
mapId: buf.readUInt32LE(4),
valid: buf.readUInt32LE(8),

height: buf.readUInt32LE(12),
width: buf.readUInt32LE(16),
width: buf.readUInt32LE(12),
height: buf.readUInt32LE(16),

minX: buf.readFloatLE(20),
minY: buf.readFloatLE(24),
Expand All @@ -229,7 +229,6 @@ class ThreeIRobotixMapParser {
throw new Error("Image block does not contain the correct amount of pixels or invalid image header.");
}


const pixelData = block.view.subarray(40);
const pixels = {
floor: [],
Expand All @@ -238,12 +237,13 @@ class ThreeIRobotixMapParser {
};
const activeSegments = {};

for (let i = 0; i < header.height * header.width; i++) {
const val = pixelData.readUInt8(i);
let coords;

if (val !== 0) {
coords = [i % header.width, header.height - 1 - Math.floor(i / header.width)];
for (let i = 0; i < header.height; i++) {
for (let j = 0; j < header.width; j++) {
const val = pixelData[(i * header.width) + j];
const coords = [
j,
i
];

switch (val) {
case 0:
Expand Down Expand Up @@ -289,7 +289,7 @@ class ThreeIRobotixMapParser {
height: header.height,
width: header.width,
},
pixelSize: Math.round(header.resolution * 100),
pixelSize: Math.round(header.resolution * FLOAT_TO_PIXEL_FACTOR),
activeSegments: activeSegments,
pixels: pixels,
};
Expand Down Expand Up @@ -354,11 +354,10 @@ class ThreeIRobotixMapParser {

for (let i = 0; i < header.pathLength; i++) {
// The first byte is the mode. 0: taxiing, 1: cleaning
const convertedCoordinates = ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_COORDINATES(
points.push(
block.view.readFloatLE(offset + 1),
block.view.readFloatLE(offset + 5)
);
points.push(convertedCoordinates.x, convertedCoordinates.y);

offset += 9;
}
Expand All @@ -371,15 +370,13 @@ class ThreeIRobotixMapParser {
*/
static PARSE_ROBOT_POSITION_BLOCK(block) {
// At offset 4 there's some ID and after that there's what seems to be some kind of flag byte?
const convertedCoordinates = ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_COORDINATES(
block.view.readFloatLE(9),
block.view.readFloatLE(13)
);
const x = block.view.readFloatLE(9);
const y = block.view.readFloatLE(13);
const angle = block.view.readFloatLE(17);

return {
x: convertedCoordinates.x,
y: convertedCoordinates.y,
x: x,
y: y,
angle: ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_ANGLE(angle)
};
}
Expand All @@ -388,15 +385,13 @@ class ThreeIRobotixMapParser {
* @param {Block} block
*/
static PARSE_CHARGER_LOCATION_BLOCK(block) {
const convertedCoordinates = ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_COORDINATES(
block.view.readFloatLE(4),
block.view.readFloatLE(8)
);
const x = block.view.readFloatLE(4);
const y = block.view.readFloatLE(8);
const angle = block.view.readFloatLE(12);

return {
x: convertedCoordinates.x,
y: convertedCoordinates.y,
x: x,
y: y,
angle: ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_ANGLE(angle)
};
}
Expand Down Expand Up @@ -430,15 +425,15 @@ class ThreeIRobotixMapParser {

if (canHaveWalls && x0 === x1 && y0 === y1 && x2 === x3 && y2 === y3) {
areaData.walls.push([
...Object.values(ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_COORDINATES(x0, y0)),
...Object.values(ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_COORDINATES(x2, y2)),
x0, y0,
x2, y2
]);
} else {
areaData.areas.push([
...Object.values(ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_COORDINATES(x0, y0)),
...Object.values(ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_COORDINATES(x1, y1)),
...Object.values(ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_COORDINATES(x2, y2)),
...Object.values(ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_COORDINATES(x3, y3)),
x0, y0,
x1, y1,
x2, y2,
x3, y3,
]);
}

Expand All @@ -457,8 +452,13 @@ class ThreeIRobotixMapParser {
*/
static POST_PROCESS_BLOCKS(blocks, uniqueMapId) {
if (blocks[TYPE_FLAGS.MAP_IMAGE]?.pixels) {
const mapWidth = blocks[TYPE_FLAGS.MAP_IMAGE].dimensions.width;
const pixelSize = blocks[TYPE_FLAGS.MAP_IMAGE].pixelSize;

const layers = [];
const entities = [];

let pathPoints = [];
let calculatedRobotAngle;

if (blocks[TYPE_FLAGS.MAP_IMAGE].pixels.floor.length > 0) {
Expand Down Expand Up @@ -491,39 +491,41 @@ class ThreeIRobotixMapParser {


if (blocks[TYPE_FLAGS.PATH]?.length > 0) {
pathPoints = ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_COORDINATES(blocks[TYPE_FLAGS.PATH], mapWidth, pixelSize);

entities.push(new mapEntities.PathMapEntity({
points: blocks[TYPE_FLAGS.PATH],
points: pathPoints,
type: mapEntities.PathMapEntity.TYPE.PATH
}));

// Calculate robot angle from path if possible - the robot-reported angle is inaccurate
if (blocks[TYPE_FLAGS.PATH].length >= 4) {
if (pathPoints.length >= 4) {
calculatedRobotAngle = (Math.round(Math.atan2(
blocks[TYPE_FLAGS.PATH][blocks[TYPE_FLAGS.PATH].length - 1] -
blocks[TYPE_FLAGS.PATH][blocks[TYPE_FLAGS.PATH].length - 3],
pathPoints[pathPoints.length - 1] -
pathPoints[pathPoints.length - 3],

blocks[TYPE_FLAGS.PATH][blocks[TYPE_FLAGS.PATH].length - 2] -
blocks[TYPE_FLAGS.PATH][blocks[TYPE_FLAGS.PATH].length - 4]
pathPoints[pathPoints.length - 2] -
pathPoints[pathPoints.length - 4]
) * 180 / Math.PI) + 90) % 360; //TODO: No idea why
}
}

if (blocks[TYPE_FLAGS.ROBOT_POSITION]) {
const x = blocks[TYPE_FLAGS.ROBOT_POSITION].x;
const y = blocks[TYPE_FLAGS.ROBOT_POSITION].y;

entities.push(new mapEntities.PointMapEntity({
points: [
blocks[TYPE_FLAGS.ROBOT_POSITION].x,
blocks[TYPE_FLAGS.ROBOT_POSITION].y
],
points: ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_COORDINATES([x, y], mapWidth, pixelSize),
metaData: {
angle: calculatedRobotAngle ?? blocks[TYPE_FLAGS.ROBOT_POSITION].angle
},
type: mapEntities.PointMapEntity.TYPE.ROBOT_POSITION
}));
} else if (uniqueMapId === 0 && blocks[TYPE_FLAGS.PATH]?.length >= 2) {
} else if (uniqueMapId === 0 && pathPoints.length >= 2) {
entities.push(new mapEntities.PointMapEntity({
points: [
blocks[TYPE_FLAGS.PATH][blocks[TYPE_FLAGS.PATH].length - 2],
blocks[TYPE_FLAGS.PATH][blocks[TYPE_FLAGS.PATH].length - 1]
pathPoints[pathPoints.length - 2],
pathPoints[pathPoints.length - 1]
],
metaData: {
angle: calculatedRobotAngle ?? 0
Expand All @@ -533,11 +535,11 @@ class ThreeIRobotixMapParser {
}

if (blocks[TYPE_FLAGS.CHARGER_LOCATION]) {
const x = blocks[TYPE_FLAGS.CHARGER_LOCATION].x;
const y = blocks[TYPE_FLAGS.CHARGER_LOCATION].y;

entities.push(new mapEntities.PointMapEntity({
points: [
blocks[TYPE_FLAGS.CHARGER_LOCATION].x,
blocks[TYPE_FLAGS.CHARGER_LOCATION].y
],
points: ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_COORDINATES([x, y], mapWidth, pixelSize),
metaData: {
angle: blocks[TYPE_FLAGS.CHARGER_LOCATION].angle
},
Expand All @@ -548,23 +550,23 @@ class ThreeIRobotixMapParser {
if (blocks[TYPE_FLAGS.VIRTUAL_RESTRICTIONS]) {
blocks[TYPE_FLAGS.VIRTUAL_RESTRICTIONS].walls.forEach((wall) => {
entities.push(new mapEntities.LineMapEntity({
points: wall,
points: ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_COORDINATES(wall, mapWidth, pixelSize),
type: mapEntities.LineMapEntity.TYPE.VIRTUAL_WALL
}));
});

blocks[TYPE_FLAGS.VIRTUAL_RESTRICTIONS].areas.forEach((wall) => {
blocks[TYPE_FLAGS.VIRTUAL_RESTRICTIONS].areas.forEach((area) => {
entities.push(new mapEntities.PolygonMapEntity({
points: wall,
points: ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_COORDINATES(area, mapWidth, pixelSize),
type: mapEntities.PolygonMapEntity.TYPE.NO_GO_AREA
}));
});
}

if (blocks[TYPE_FLAGS.ACTIVE_ZONES]) {
blocks[TYPE_FLAGS.ACTIVE_ZONES].areas.forEach((wall) => {
blocks[TYPE_FLAGS.ACTIVE_ZONES].areas.forEach((activeZone) => {
entities.push(new mapEntities.PolygonMapEntity({
points: wall,
points: ThreeIRobotixMapParser.CONVERT_TO_VALETUDO_COORDINATES(activeZone, mapWidth, pixelSize),
type: mapEntities.PolygonMapEntity.TYPE.ACTIVE_ZONE
}));
});
Expand Down Expand Up @@ -604,30 +606,41 @@ class ThreeIRobotixMapParser {
}

static CONVERT_FLOAT(value) {
return Math.ceil((20 + value) * 100);
return Math.ceil(value * FLOAT_TO_PIXEL_FACTOR);
}

/**
*
* @param {number} x
* @param {number} y
* @returns {{x: number, y: number}}
* @param {Array<number>} points
* @param {number} mapWidth
* @param {number} pixelSize
* @returns {Array<number>}
*/
static CONVERT_TO_VALETUDO_COORDINATES(x, y) {
return {
x: ThreeIRobotixMapParser.CONVERT_FLOAT(x),
y: ThreeIRobotixMapParser.MAX_MAP_HEIGHT - ThreeIRobotixMapParser.CONVERT_FLOAT(y)
};
static CONVERT_TO_VALETUDO_COORDINATES(points, mapWidth, pixelSize) {
const horizontalCenterOffset = ((mapWidth /2) * pixelSize);

const converted = [];
for (let i = 0; i < points.length; i = i + 2) {
converted.push(
horizontalCenterOffset + Math.ceil(points[i] * FLOAT_TO_PIXEL_FACTOR),
horizontalCenterOffset + Math.ceil(points[i+1] * FLOAT_TO_PIXEL_FACTOR),
);
}

return converted;
}

/**
*
* @param {number} x
* @param {number} y
* @param {number} mapWidth
* @param {number} pixelSize
*/
static CONVERT_TO_THREEIROBOTIX_COORDINATES(x, y) {
y = ThreeIRobotixMapParser.MAX_MAP_HEIGHT - y;
return {x: x / 100 - 20, y: y / 100 - 20};
static CONVERT_TO_THREEIROBOTIX_COORDINATES(x, y, mapWidth, pixelSize) {
const horizontalCenterOffset = ((mapWidth /2) * pixelSize) / FLOAT_TO_PIXEL_FACTOR;

return {x: x / FLOAT_TO_PIXEL_FACTOR - horizontalCenterOffset, y: y / FLOAT_TO_PIXEL_FACTOR - horizontalCenterOffset};
}

/**
Expand All @@ -640,8 +653,7 @@ class ThreeIRobotixMapParser {
}
}

// 4000 - Hardcoding this might break later. May need refactoring
ThreeIRobotixMapParser.MAX_MAP_HEIGHT = ThreeIRobotixMapParser.CONVERT_FLOAT(20);
const FLOAT_TO_PIXEL_FACTOR = 100;

const TYPE_FLAGS = {
ROBOT_STATUS: 0b0000000000000000000000000000001,
Expand Down
18 changes: 12 additions & 6 deletions backend/lib/robots/viomi/capabilities/ViomiCombinedVirtualRestrictionsCapability.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,11 +14,17 @@ class ViomiCombinedVirtualRestrictionsCapability extends CombinedVirtualRestrict
* @returns {Promise<void>}
*/
async setVirtualRestrictions(virtualRestrictions) {
if (this.robot.state.map?.metaData?.defaultMap === true) {
throw new Error("Can't set virtual restrictions because the map was not parsed yet");
}
const pixelSize = this.robot.state.map.pixelSize;
const mapWidth = this.robot.state.map.size.x / pixelSize;

const payload = [];

virtualRestrictions.virtualWalls.forEach(wall => {
const pA = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(wall.points.pA.x, wall.points.pA.y);
const pB = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(wall.points.pB.x, wall.points.pB.y);
const pA = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(wall.points.pA.x, wall.points.pA.y, mapWidth, pixelSize);
const pB = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(wall.points.pB.x, wall.points.pB.y, mapWidth, pixelSize);

payload.push(
[
Expand All @@ -33,10 +39,10 @@ class ViomiCombinedVirtualRestrictionsCapability extends CombinedVirtualRestrict
});

virtualRestrictions.restrictedZones.forEach(zone => {
const pA = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(zone.points.pA.x, zone.points.pA.y);
const pB = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(zone.points.pB.x, zone.points.pB.y);
const pC = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(zone.points.pC.x, zone.points.pC.y);
const pD = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(zone.points.pD.x, zone.points.pD.y);
const pA = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(zone.points.pA.x, zone.points.pA.y, mapWidth, pixelSize);
const pB = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(zone.points.pB.x, zone.points.pB.y, mapWidth, pixelSize);
const pC = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(zone.points.pC.x, zone.points.pC.y, mapWidth, pixelSize);
const pD = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(zone.points.pD.x, zone.points.pD.y, mapWidth, pixelSize);

payload.push(
[
Expand Down
7 changes: 5 additions & 2 deletions backend/lib/robots/viomi/capabilities/ViomiMapSegmentEditCapability.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,15 +79,18 @@ class ViomiMapSegmentEditCapability extends MapSegmentEditCapability {
if (this.robot.state.map?.metaData?.defaultMap === true) {
throw new Error("Can't split segment because the map was not parsed yet");
}
const pixelSize = this.robot.state.map.pixelSize;
const mapWidth = this.robot.state.map.size.x / pixelSize;


try {
const result = await this.robot.sendCommand("arrange_room", {
lang: this.lang,
mapId: this.robot.ephemeralState.vendorMapId,
pointArr: [[
1,
this.pointToViomiString(ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(pA.x, pA.y)),
this.pointToViomiString(ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(pB.x, pB.y))
this.pointToViomiString(ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(pA.x, pA.y, mapWidth, pixelSize)),
this.pointToViomiString(ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(pB.x, pB.y, mapWidth, pixelSize))
]],
roomId: parseInt(segment.id),
type: this.mapActions.SPLIT_SEGMENT_TYPE
Expand Down
10 changes: 8 additions & 2 deletions backend/lib/robots/viomi/capabilities/ViomiZoneCleaningCapability.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,12 @@ class ViomiZoneCleaningCapability extends ZoneCleaningCapability {
}

async start(options) {
if (this.robot.state.map?.metaData?.defaultMap === true) {
throw new Error("Can't start zone cleaning because the map was not parsed yet");
}
const pixelSize = this.robot.state.map.pixelSize;
const mapWidth = this.robot.state.map.size.x / pixelSize;

let areas = [];
const basicControlCap = this.getBasicControlCapability();

Expand All @@ -29,8 +35,8 @@ class ViomiZoneCleaningCapability extends ZoneCleaningCapability {
await this.robot.sendCommand("set_uploadmap", [1]);

options.zones.forEach(zone => {
const pA = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(zone.points.pA.x, zone.points.pA.y);
const pC = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(zone.points.pC.x, zone.points.pC.y);
const pA = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(zone.points.pA.x, zone.points.pA.y, mapWidth, pixelSize);
const pC = ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(zone.points.pC.x, zone.points.pC.y, mapWidth, pixelSize);

areas.push([areas.length,
attributes.ViomiArea.NORMAL,
Expand Down
Loading

0 comments on commit e45607b

Please sign in to comment.