Skip to content

Commit

Permalink
fix(vendor.3ir): Parse temporary maps
Browse files Browse the repository at this point in the history
  • Loading branch information
@Hypfer
Hypfer committed Nov 16, 2022
1 parent b351686 commit f43a7ed
Show file tree
Hide file tree
Showing 9 changed files with 3,705 additions and 46 deletions.
182 changes: 143 additions & 39 deletions backend/lib/robots/3irobotix/ThreeIRobotixMapParser.js
View file
Expand Up @@ -21,18 +21,89 @@ class ThreeIRobotixMapParser {
const uniqueMapIdBytes = mapBuf.subarray(4,8);
const uniqueMapId = uniqueMapIdBytes.readUInt32LE();

if (uniqueMapId === 0 || flagData.MAP_IMAGE !== true || flagData.ROBOT_STATUS !== true) {
if (flagData.MAP_IMAGE !== true || flagData.ROBOT_STATUS !== true) {
return null;
}

let blocks;

const blocks = ThreeIRobotixMapParser.BUILD_BLOCK_INDEX(mapBuf, uniqueMapIdBytes, flagData);
const processedBlocks = ThreeIRobotixMapParser.PROCESS_BLOCKS(blocks);
if (uniqueMapId > 0) {
blocks = ThreeIRobotixMapParser.BUILD_BLOCK_INDEX(mapBuf, uniqueMapIdBytes, flagData);
} else {
if (flagData.MAP_IMAGE === true && flagData.PATH === true) {
blocks = ThreeIRobotixMapParser.BUILD_FALLBACK_INDEX(mapBuf, flagData);
} else {
return null;
}
}

const processedBlocks = ThreeIRobotixMapParser.PROCESS_BLOCKS(blocks, uniqueMapId === 0);


return ThreeIRobotixMapParser.POST_PROCESS_BLOCKS(processedBlocks, uniqueMapId);
}

/**
* This is used for temporary maps (e.g., on initial cleanup when there are no segments yet)
*
* @param {Buffer} mapBuf
* @param {object} flagData
* @return {Array<Block>}
*/
static BUILD_FALLBACK_INDEX(mapBuf, flagData) {
const types = Object.entries(flagData).filter(([key, value]) => {
return value === true;
}).map(([key, value]) => {
return TYPE_FLAGS[key];
});
const foundChunks = [];
let offset = 0;

types.forEach((type) => {
switch (type) {
case TYPE_FLAGS.ROBOT_STATUS:
foundChunks.push({
type: TYPE_FLAGS.ROBOT_STATUS,
view: mapBuf.subarray(offset, offset + 48)
});

offset += 48;
break;
case TYPE_FLAGS.MAP_IMAGE: {
const header = ThreeIRobotixMapParser.PARSE_IMAGE_BLOCK_HEADER(mapBuf.subarray(offset));

if (
Number.isInteger(header.height) && Number.isInteger(header.width) &&
header.height > 0 && header.width > 0
) {
foundChunks.push({
type: TYPE_FLAGS.MAP_IMAGE,
view: mapBuf.subarray(offset, offset + header.blockLength)
});
}

offset += header.blockLength;

break;
}
case TYPE_FLAGS.PATH: {
const header = ThreeIRobotixMapParser.PARSE_PATH_BLOCK_HEADER(mapBuf.subarray(offset));

foundChunks.push({
type: TYPE_FLAGS.PATH,
view: mapBuf.subarray(offset, offset + header.blockLength)
});

offset += header.blockLength;

break;
}
}
});

return foundChunks;
}

/**
* Because each block conveniently starts with the uniqueMapId, we can use that
* to slice our map file into blocks that can then be processed further
Expand Down Expand Up @@ -94,24 +165,26 @@ class ThreeIRobotixMapParser {

/**
* @param {Block[]} blocks
* @param {boolean} isTemporaryMap
*/
static PROCESS_BLOCKS(blocks) {
static PROCESS_BLOCKS(blocks, isTemporaryMap) {
const result = {};

blocks.forEach(block => {
result[block.type] = ThreeIRobotixMapParser.PARSE_BLOCK(block);
result[block.type] = ThreeIRobotixMapParser.PARSE_BLOCK(block, isTemporaryMap);
});

return result;
}

/**
* @param {Block} block
* @param {boolean} isTemporaryMap
*/
static PARSE_BLOCK(block) {
static PARSE_BLOCK(block, isTemporaryMap) {
switch (block.type) {
case TYPE_FLAGS.MAP_IMAGE:
return ThreeIRobotixMapParser.PARSE_IMAGE_BLOCK(block);
return ThreeIRobotixMapParser.PARSE_IMAGE_BLOCK(block, isTemporaryMap);
case TYPE_FLAGS.SEGMENT_NAMES:
return ThreeIRobotixMapParser.PARSE_SEGMENT_NAMES_BLOCK(block);
case TYPE_FLAGS.PATH:
Expand All @@ -129,23 +202,32 @@ class ThreeIRobotixMapParser {
}
}

static PARSE_IMAGE_BLOCK_HEADER(buf) {
const headerData = {
mapId: buf.readUInt32LE(4),
valid: buf.readUInt32LE(8),

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

minX: buf.readFloatLE(20),
minY: buf.readFloatLE(24),
maxX: buf.readFloatLE(28),
maxY: buf.readFloatLE(32),
resolution: buf.readFloatLE(36),
};

headerData.blockLength = 40 + headerData.height * headerData.width;

return headerData;
}

/**
* @param {Block} block
* @param {boolean} isTemporaryMap
*/
static PARSE_IMAGE_BLOCK(block) {
const header = {
mapId: block.view.readUInt32LE(4),
valid: block.view.readUInt32LE(8),

height: block.view.readUInt32LE(12),
width: block.view.readUInt32LE(16),

minX: block.view.readFloatLE(20),
minY: block.view.readFloatLE(24),
maxX: block.view.readFloatLE(28),
maxY: block.view.readFloatLE(32),
resolution: block.view.readFloatLE(36)
};
static PARSE_IMAGE_BLOCK(block, isTemporaryMap) {
const header = ThreeIRobotixMapParser.PARSE_IMAGE_BLOCK_HEADER(block.view);

if (header.height * header.width !== block.view.length - 40) {
throw new Error("Image block does not contain the correct amount of pixels or invalid image header.");
Expand Down Expand Up @@ -178,21 +260,24 @@ class ThreeIRobotixMapParser {
pixels.floor.push([coords[0], coords[1]]);
break;
default: {
const isActive = val >= 60;
let segmentId = val;

if (isActive) {
segmentId = segmentId - 50; //TODO: this can't be right but it works?
activeSegments[segmentId] = true;
if (!isTemporaryMap) {
const isActive = val >= 60;
let segmentId = val;

if (isActive) {
segmentId = segmentId - 50; //TODO: this can't be right but it works?
activeSegments[segmentId] = true;
}

if (!Array.isArray(pixels.segments[segmentId])) {
pixels.segments[segmentId] = [];
}

pixels.segments[segmentId].push([coords[0], coords[1]]);
} else {
pixels.floor.push([coords[0], coords[1]]);
}

if (!Array.isArray(pixels.segments[segmentId])) {
pixels.segments[segmentId] = [];
}

pixels.segments[segmentId].push([coords[0], coords[1]]);
}

}
}
}
Expand Down Expand Up @@ -248,18 +333,26 @@ class ThreeIRobotixMapParser {
return segments;
}

static PARSE_PATH_BLOCK_HEADER(buf) {
const headerData = {
// At offset 4 there's a poseId. No idea what that does
pathLength: buf.readUInt32LE(8)
};

headerData.blockLength = 12 + (headerData.pathLength * 9);

return headerData;
}

/**
* @param {Block} block
*/
static PARSE_PATH_BLOCK(block) {
const points = [];

// At offset 4 there's a poseId. No idea what that does
const pathLength = block.view.readUInt32LE(8);
const header = ThreeIRobotixMapParser.PARSE_PATH_BLOCK_HEADER(block.view);
let offset = 12;


for (let i = 0; i < pathLength; i++) {
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(
block.view.readFloatLE(offset + 1),
Expand Down Expand Up @@ -426,6 +519,17 @@ class ThreeIRobotixMapParser {
},
type: Map.PointMapEntity.TYPE.ROBOT_POSITION
}));
} else if (uniqueMapId === 0 && blocks[TYPE_FLAGS.PATH]?.length >= 2) {
entities.push(new Map.PointMapEntity({
points: [
blocks[TYPE_FLAGS.PATH][blocks[TYPE_FLAGS.PATH].length - 2],
blocks[TYPE_FLAGS.PATH][blocks[TYPE_FLAGS.PATH].length - 1]
],
metaData: {
angle: calculatedRobotAngle ?? 0
},
type: Map.PointMapEntity.TYPE.ROBOT_POSITION
}));
}

if (blocks[TYPE_FLAGS.CHARGER_LOCATION]) {
Expand Down
26 changes: 23 additions & 3 deletions backend/lib/robots/viomi/ViomiValetudoRobot.js
View file
Expand Up @@ -46,7 +46,8 @@ class ViomiValetudoRobot extends MiioValetudoRobot {
carpetModeEnabled: undefined,
lastOperationType: null,
lastOperationAdditionalParams: [],
operationMode: undefined
operationMode: undefined,
vendorMapId: 0
};

this.registerCapability(new capabilities.ViomiBasicControlCapability({
Expand Down Expand Up @@ -474,8 +475,19 @@ class ViomiValetudoRobot extends MiioValetudoRobot {
const map = ThreeIRobotixMapParser.PARSE(data);

if (map !== null) {
this.state.map = map;
this.emitMapUpdated();
if (map.metaData.vendorMapId > 0) { // Regular map
this.ephemeralState.vendorMapId = map.metaData.vendorMapId;

this.state.map = map;

this.emitMapUpdated();
} else { // Temporary map
if (this.ephemeralState.vendorMapId === 0) {
this.state.map = map; // There is no previous full data so this is all we have

this.emitMapUpdated();
} // else: ignore since we already have a better map
}
}

return this.state.map;
Expand All @@ -494,6 +506,14 @@ class ViomiValetudoRobot extends MiioValetudoRobot {
}
}

clearValetudoMap() {
this.ephemeralState.vendorMapId = 0;

super.clearValetudoMap();

this.emitMapUpdated();
}

getManufacturer() {
return "Viomi";
}
Expand Down
4 changes: 2 additions & 2 deletions backend/lib/robots/viomi/capabilities/ViomiMapSegmentEditCapability.js
View file
Expand Up @@ -51,7 +51,7 @@ class ViomiMapSegmentEditCapability extends MapSegmentEditCapability {
try {
const result = await this.robot.sendCommand("arrange_room", {
lang: this.lang,
mapId: this.robot.state.map.metaData.vendorMapId,
mapId: this.robot.ephemeralState.vendorMapId,
roomArr: [[parseInt(segmentA.id), parseInt(segmentB.id)]],
type: this.mapActions.JOIN_SEGMENT_TYPE
}, {
Expand Down Expand Up @@ -83,7 +83,7 @@ class ViomiMapSegmentEditCapability extends MapSegmentEditCapability {
try {
const result = await this.robot.sendCommand("arrange_room", {
lang: this.lang,
mapId: this.robot.state.map.metaData.vendorMapId,
mapId: this.robot.ephemeralState.vendorMapId,
pointArr: [[
1,
this.pointToViomiString(ThreeIRobotixMapParser.CONVERT_TO_THREEIROBOTIX_COORDINATES(pA.x, pA.y)),
Expand Down
2 changes: 1 addition & 1 deletion backend/lib/robots/viomi/capabilities/ViomiMapSegmentRenameCapability.js
View file
Expand Up @@ -14,7 +14,7 @@ class ViomiMapSegmentRenameCapability extends MapSegmentRenameCapability {
}

await this.robot.sendCommand("rename_room", [
this.robot.state.map.metaData.vendorMapId,
this.robot.ephemeralState.vendorMapId,
1,
parseInt(segment.id),
name
Expand Down

0 comments on commit f43a7ed

Please sign in to comment.