git.basschouten.com Git - openhab-addons.git/blob

   1 /**
   2  * Copyright (c) 2010-2020 Contributors to the openHAB project
   3  *
   4  * See the NOTICE file(s) distributed with this work for additional
   5  * information.
   6  *
   7  * This program and the accompanying materials are made available under the
   8  * terms of the Eclipse Public License 2.0 which is available at
   9  * http://www.eclipse.org/legal/epl-2.0
  10  *
  11  * SPDX-License-Identifier: EPL-2.0
  12  */
  13 package org.openhab.binding.miio.internal.robot;
  14
  15 import java.io.ByteArrayOutputStream;
  16 import java.io.File;
  17 import java.io.FileInputStream;
  18 import java.io.IOException;
  19 import java.io.InputStream;
  20 import java.io.PrintWriter;
  21 import java.io.StringWriter;
  22 import java.security.MessageDigest;
  23 import java.security.NoSuchAlgorithmException;
  24 import java.util.ArrayList;
  25 import java.util.Arrays;
  26 import java.util.HashMap;
  27 import java.util.Map;
  28 import java.util.zip.GZIPInputStream;
  29
  30 import org.eclipse.jdt.annotation.NonNullByDefault;
  31 import org.openhab.binding.miio.internal.Utils;
  32 import org.slf4j.Logger;
  33 import org.slf4j.LoggerFactory;
  34
  35 /**
  36  * The {@link RRMapFileParser} is used to parse the RR map file format created by Xiaomi / RockRobo vacuum
  37  *
  38  * @author Marcel Verpaalen - Initial contribution
  39  */
  40 @NonNullByDefault
  41 public class RRMapFileParser {
  42     public static final int CHARGER = 1;
  43     public static final int IMAGE = 2;
  44     public static final int PATH = 3;
  45     public static final int GOTO_PATH = 4;
  46     public static final int GOTO_PREDICTED_PATH = 5;
  47     public static final int CURRENTLY_CLEANED_ZONES = 6;
  48     public static final int GOTO_TARGET = 7;
  49     public static final int ROBOT_POSITION = 8;
  50     public static final int NO_GO_AREAS = 9;
  51     public static final int VIRTUAL_WALLS = 10;
  52     public static final int BLOCKS = 11;
  53     public static final int MFBZS_AREA = 12;
  54     public static final int OBSTACLES = 13;
  55     public static final int DIGEST = 1024;
  56     public static final int HEADER = 0x7272;
  57
  58     public static final String PATH_POINT_LENGTH = "pointLength";
  59     public static final String PATH_POINT_SIZE = "pointSize";
  60     public static final String PATH_ANGLE = "angle";
  61
  62     private byte[] image = new byte[] { 0 };
  63     private final int majorVersion;
  64     private final int minorVersion;
  65     private final int mapIndex;
  66     private final int mapSequence;
  67     private boolean isValid;
  68
  69     private int imgHeight;
  70     private int imgWidth;
  71     private int imageSize;
  72     private int top;
  73     private int left;
  74
  75     private int chargerX;
  76     private int chargerY;
  77     private int roboX;
  78     private int roboY;
  79     private int roboA;
  80     private float gotoX = 0;
  81     private float gotoY = 0;
  82     private Map<Integer, ArrayList<float[]>> paths = new HashMap<>();
  83     private Map<Integer, Map<String, Integer>> pathsDetails = new HashMap<>();
  84     private Map<Integer, ArrayList<float[]>> areas = new HashMap<>();
  85     private ArrayList<float[]> walls = new ArrayList<>();
  86     private ArrayList<float[]> zones = new ArrayList<>();
  87     private ArrayList<int[]> obstacles = new ArrayList<>();
  88     private byte[] blocks = new byte[0];
  89
  90     private final Logger logger = LoggerFactory.getLogger(RRMapFileParser.class);
  91
  92     public RRMapFileParser(byte[] raw) {
  93         boolean printBlockDetails = false;
  94         int mapHeaderLength = getUInt16(raw, 0x02);
  95         int mapDataLength = getUInt32LE(raw, 0x04);
  96         this.majorVersion = getUInt16(raw, 0x08);
  97         this.minorVersion = getUInt16(raw, 0x0A);
  98         this.mapIndex = getUInt32LE(raw, 0x0C);
  99         this.mapSequence = getUInt32LE(raw, 0x10);
 100
 101         int blockStartPos = getUInt16(raw, 0x02); // main header length
 102         while (blockStartPos < raw.length) {
 103             int blockHeaderLength = getUInt16(raw, blockStartPos + 0x02);
 104             byte[] header = getBytes(raw, blockStartPos, blockHeaderLength);
 105             int blocktype = getUInt16(header, 0x00);
 106             int blockDataLength = getUInt32LE(header, 0x04);
 107             int blockDataStart = blockStartPos + blockHeaderLength;
 108             byte[] data = getBytes(raw, blockDataStart, blockDataLength);
 109
 110             switch (blocktype) {
 111                 case CHARGER:
 112                     this.chargerX = getUInt32LE(raw, blockStartPos + 0x08);
 113                     this.chargerY = getUInt32LE(raw, blockStartPos + 0x0C);
 114                     break;
 115                 case IMAGE:
 116                     this.imageSize = blockDataLength;// (getUInt32LE(raw, blockStartPos + 0x04));
 117                     if (blockHeaderLength > 0x1C) {
 118                         logger.debug("block 2 unknown value @pos 8: {}", getUInt32LE(header, 0x08));
 119                     }
 120                     this.top = getUInt32LE(header, blockHeaderLength - 16);
 121                     this.left = getUInt32LE(header, blockHeaderLength - 12);
 122                     this.imgHeight = (getUInt32LE(header, blockHeaderLength - 8));
 123                     this.imgWidth = getUInt32LE(header, blockHeaderLength - 4);
 124                     this.image = data;
 125                     break;
 126                 case ROBOT_POSITION:
 127                     this.roboX = getUInt32LE(data, 0x00);
 128                     this.roboY = getUInt32LE(data, 0x04);
 129                     if (blockDataLength > 8) { // model S6
 130                         this.roboA = getUInt32LE(data, 0x08);
 131                     }
 132                     break;
 133                 case PATH:
 134                 case GOTO_PATH:
 135                 case GOTO_PREDICTED_PATH:
 136                     ArrayList<float[]> path = new ArrayList<float[]>();
 137                     Map<String, Integer> detail = new HashMap<String, Integer>();
 138                     int pairs = getUInt32LE(header, 0x04) / 4;
 139                     detail.put(PATH_POINT_LENGTH, getUInt32LE(header, 0x08));
 140                     detail.put(PATH_POINT_SIZE, getUInt32LE(header, 0x0C));
 141                     detail.put(PATH_ANGLE, getUInt32LE(header, 0x10));
 142                     for (int pathpair = 0; pathpair < pairs; pathpair++) {
 143                         float x = (getUInt16(getBytes(raw, blockDataStart + pathpair * 4, 2)));
 144                         float y = getUInt16(getBytes(raw, blockDataStart + pathpair * 4 + 2, 2));
 145                         path.add(new float[] { x, y });
 146                     }
 147                     paths.put(blocktype, path);
 148                     pathsDetails.put(blocktype, detail);
 149                     break;
 150                 case CURRENTLY_CLEANED_ZONES:
 151                     int zonePairs = getUInt16(header, 0x08);
 152                     for (int zonePair = 0; zonePair < zonePairs; zonePair++) {
 153                         float x0 = (getUInt16(raw, blockDataStart + zonePair * 8));
 154                         float y0 = getUInt16(raw, blockDataStart + zonePair * 8 + 2);
 155                         float x1 = (getUInt16(raw, blockDataStart + zonePair * 8 + 4));
 156                         float y1 = getUInt16(raw, blockDataStart + zonePair * 8 + 6);
 157                         zones.add(new float[] { x0, y0, x1, y1 });
 158                     }
 159                     break;
 160                 case GOTO_TARGET:
 161                     this.gotoX = getUInt16(data, 0x00);
 162                     this.gotoY = getUInt16(data, 0x02);
 163                     break;
 164                 case DIGEST:
 165                     isValid = Arrays.equals(data, sha1Hash(getBytes(raw, 0, mapHeaderLength + mapDataLength - 20)));
 166                     break;
 167                 case VIRTUAL_WALLS:
 168                     int wallPairs = getUInt16(header, 0x08);
 169                     for (int wallPair = 0; wallPair < wallPairs; wallPair++) {
 170                         float x0 = (getUInt16(raw, blockDataStart + wallPair * 8));
 171                         float y0 = getUInt16(raw, blockDataStart + wallPair * 8 + 2);
 172                         float x1 = (getUInt16(raw, blockDataStart + wallPair * 8 + 4));
 173                         float y1 = getUInt16(raw, blockDataStart + wallPair * 8 + 6);
 174                         walls.add(new float[] { x0, y0, x1, y1 });
 175                     }
 176                     break;
 177                 case NO_GO_AREAS:
 178                 case MFBZS_AREA:
 179                     int areaPairs = getUInt16(header, 0x08);
 180                     ArrayList<float[]> area = new ArrayList<float[]>();
 181                     for (int areaPair = 0; areaPair < areaPairs; areaPair++) {
 182                         float x0 = (getUInt16(raw, blockDataStart + areaPair * 16));
 183                         float y0 = getUInt16(raw, blockDataStart + areaPair * 16 + 2);
 184                         float x1 = (getUInt16(raw, blockDataStart + areaPair * 16 + 4));
 185                         float y1 = getUInt16(raw, blockDataStart + areaPair * 16 + 6);
 186                         float x2 = (getUInt16(raw, blockDataStart + areaPair * 16 + 8));
 187                         float y2 = getUInt16(raw, blockDataStart + areaPair * 16 + 10);
 188                         float x3 = (getUInt16(raw, blockDataStart + areaPair * 16 + 12));
 189                         float y3 = getUInt16(raw, blockDataStart + areaPair * 16 + 14);
 190                         area.add(new float[] { x0, y0, x1, y1, x2, y2, x3, y3 });
 191                     }
 192                     areas.put(Integer.valueOf(blocktype & 0xFF), area);
 193                     break;
 194                 case OBSTACLES:
 195                     int obstaclePairs = getUInt16(header, 0x08);
 196                     for (int obstaclePair = 0; obstaclePair < obstaclePairs; obstaclePair++) {
 197                         int x0 = getUInt16(data, obstaclePair * 5 + 0);
 198                         int y0 = getUInt16(data, obstaclePair * 5 + 2);
 199                         int u = data[obstaclePair * 5 + 0] & 0xFF;
 200                         obstacles.add(new int[] { x0, y0, u });
 201                     }
 202                     break;
 203                 case BLOCKS:
 204                     int blocksPairs = getUInt16(header, 0x08);
 205                     blocks = getBytes(data, 0, blocksPairs);
 206                     break;
 207                 default:
 208                     logger.info("Unknown blocktype (pls report to author)");
 209                     printBlockDetails = true;
 210             }
 211             if (logger.isTraceEnabled() || printBlockDetails) {
 212                 logger.debug("Blocktype: {}", Integer.toString(blocktype));
 213                 logger.debug("Header len: {}   data len: {} ", Integer.toString(blockHeaderLength),
 214                         Integer.toString(blockDataLength));
 215                 logger.debug("H: {}", Utils.getSpacedHex(header));
 216                 if (blockDataLength > 0) {
 217                     logger.debug("D: {}", (blockDataLength < 60 ? Utils.getSpacedHex(data)
 218                             : Utils.getSpacedHex(getBytes(data, 0, 60))));
 219                 }
 220                 printBlockDetails = false;
 221             }
 222             blockStartPos = blockStartPos + blockDataLength + (header[2] & 0xFF);
 223         }
 224     }
 225
 226     public static byte[] readRRMapFile(File file) throws IOException {
 227         return readRRMapFile(new FileInputStream(file));
 228     }
 229
 230     public static byte[] readRRMapFile(InputStream is) throws IOException {
 231         ByteArrayOutputStream baos = new ByteArrayOutputStream();
 232         try (GZIPInputStream in = new GZIPInputStream(is)) {
 233             int bufsize = 1024;
 234             byte[] buf = new byte[bufsize];
 235             int readbytes = 0;
 236             readbytes = in.read(buf);
 237             while (readbytes != -1) {
 238                 baos.write(buf, 0, readbytes);
 239                 readbytes = in.read(buf);
 240             }
 241             baos.flush();
 242             return baos.toByteArray();
 243         }
 244     }
 245
 246     private byte[] getBytes(byte[] raw, int pos, int len) {
 247         return java.util.Arrays.copyOfRange(raw, pos, pos + len);
 248     }
 249
 250     private int getUInt32LE(byte[] bytes, int pos) {
 251         int value = bytes[0 + pos] & 0xFF;
 252         value |= (bytes[1 + pos] << 8) & 0xFFFF;
 253         value |= (bytes[2 + pos] << 16) & 0xFFFFFF;
 254         value |= (bytes[3 + pos] << 24) & 0xFFFFFFFF;
 255         return value;
 256     }
 257
 258     private int getUInt16(byte[] bytes) {
 259         return getUInt16(bytes, 0);
 260     }
 261
 262     private int getUInt16(byte[] bytes, int pos) {
 263         int value = bytes[0 + pos] & 0xFF;
 264         value |= (bytes[1 + pos] << 8) & 0xFFFF;
 265         return value;
 266     }
 267
 268     @Override
 269     public String toString() {
 270         StringWriter sw = new StringWriter();
 271         PrintWriter pw = new PrintWriter(sw);
 272         pw.printf("RR Map:\tMajor Version: %d Minor version: %d Map Index: %d Map Sequence: %d\r\n", majorVersion,
 273                 minorVersion, mapIndex, mapSequence);
 274         pw.printf("Image:\tsize: %9d\ttop: %9d\tleft: %9d height: %9d width: %9d\r\n", imageSize, top, left, imgHeight,
 275                 imgWidth);
 276         pw.printf("Charger pos:\tX: %.0f\tY: %.0f\r\n", getChargerX(), getChargerY());
 277         pw.printf("Robo pos:\tX: %.0f\tY: %.0f\tAngle: %d\r\n", getRoboX(), getRoboY(), getRoboA());
 278         pw.printf("Goto:\tX: %.0f\tY: %.0f\r\n", getGotoX(), getGotoY());
 279         for (Integer area : areas.keySet()) {
 280             pw.print(area == NO_GO_AREAS ? "No Go zones:\t" : "MFBZS zones:\t");
 281             pw.printf("%d\r\n", areas.get(area).size());
 282             printAreaDetails(areas.get(area), pw);
 283         }
 284         pw.printf("Walls:\t%d\r\n", walls.size());
 285         printAreaDetails(walls, pw);
 286         pw.printf("Zones:\t%d\r\n", zones.size());
 287         printAreaDetails(zones, pw);
 288         pw.printf("Obstacles:\t%d\r\n", obstacles.size());
 289         pw.printf("Blocks:\t%d\r\n", blocks.length);
 290         pw.print("Paths:");
 291         for (Integer p : pathsDetails.keySet()) {
 292             pw.printf("\r\nPath type:\t%d", p);
 293             for (String detail : pathsDetails.get(p).keySet()) {
 294                 pw.printf("   %s: %d", detail, pathsDetails.get(p).get(detail));
 295             }
 296         }
 297         pw.println();
 298         pw.close();
 299         return sw.toString();
 300     }
 301
 302     private void printAreaDetails(ArrayList<float[]> areas, PrintWriter pw) {
 303         areas.forEach(area -> {
 304             pw.printf("\tArea coordinates:");
 305             for (int i = 0; i < area.length; i++) {
 306                 pw.printf("\t%.0f", area[i]);
 307             }
 308             pw.println();
 309         });
 310     }
 311
 312     /**
 313      * Compute SHA-1 hash value for the byte array
 314      *
 315      * @param inBytes ByteArray to be hashed
 316      * @return hash value
 317      */
 318     public static byte[] sha1Hash(byte[] inBytes) {
 319         try {
 320             MessageDigest md = MessageDigest.getInstance("SHA-1");
 321             return md.digest(inBytes);
 322         } catch (NoSuchAlgorithmException e) {
 323             return new byte[] { 0x00 };
 324         }
 325     }
 326
 327     public int getMajorVersion() {
 328         return majorVersion;
 329     }
 330
 331     public int getMinorVersion() {
 332         return minorVersion;
 333     }
 334
 335     public int getMapIndex() {
 336         return mapIndex;
 337     }
 338
 339     public int getMapSequence() {
 340         return mapSequence;
 341     }
 342
 343     public boolean isValid() {
 344         return isValid;
 345     }
 346
 347     public byte[] getImage() {
 348         return image;
 349     }
 350
 351     public int getImageSize() {
 352         return imageSize;
 353     }
 354
 355     public int getImgHeight() {
 356         return imgHeight;
 357     }
 358
 359     public int getImgWidth() {
 360         return imgWidth;
 361     }
 362
 363     public int getTop() {
 364         return top;
 365     }
 366
 367     public int getLeft() {
 368         return left;
 369     }
 370
 371     public ArrayList<float[]> getZones() {
 372         return zones;
 373     }
 374
 375     public float getRoboX() {
 376         return roboX;
 377     }
 378
 379     public float getRoboY() {
 380         return roboY;
 381     }
 382
 383     public float getChargerX() {
 384         return chargerX;
 385     }
 386
 387     public float getChargerY() {
 388         return chargerY;
 389     }
 390
 391     public float getGotoX() {
 392         return gotoX;
 393     }
 394
 395     public float getGotoY() {
 396         return gotoY;
 397     }
 398
 399     public int getRoboA() {
 400         return roboA;
 401     }
 402
 403     public Map<Integer, ArrayList<float[]>> getPaths() {
 404         return paths;
 405     }
 406
 407     public Map<Integer, Map<String, Integer>> getPathsDetails() {
 408         return pathsDetails;
 409     }
 410
 411     public ArrayList<float[]> getWalls() {
 412         return walls;
 413     }
 414
 415     public Map<Integer, ArrayList<float[]>> getAreas() {
 416         return areas;
 417     }
 418
 419     public ArrayList<int[]> getObstacles() {
 420         return obstacles;
 421     }
 422
 423     public byte[] getBlocks() {
 424         return blocks;
 425     }
 426 }