Skip to content

Commit

Permalink
[miio] improve viewer with coordinates (openhab#8168)
Browse files Browse the repository at this point in the history 
Signed-off-by: Marcel Verpaalen <marcel@verpaalen.com>
  • Loading branch information
@marcelrv @andrewfg
marcelrv authored and andrewfg committed Aug 31, 2020
1 parent 794466b commit d861580
Show file tree
Hide file tree
Showing 3 changed files with 277 additions and 75 deletions.
66 changes: 42 additions & 24 deletions ...openhab.binding.miio/src/main/java/org/openhab/binding/miio/internal/robot/RRMapDraw.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,8 @@
@NonNullByDefault
public class RRMapDraw {

private static final float MM = 50.0f;

private static final int MAP_OUTSIDE = 0x00;
private static final int MAP_WALL = 0x01;
private static final int MAP_INSIDE = 0xFF;
Expand Down Expand Up @@ -101,6 +103,10 @@ public int getHeight() {
return rmfp.getImgHeight();
}

public RRMapFileParser getMapParseDetails() {
return this.rmfp;
}

/**
* load Gzipped RR inputstream
*
Expand Down Expand Up @@ -198,8 +204,8 @@ private void drawPath(Graphics2D g2d, float scale) {
float prvX = 0;
float prvY = 0;
for (float[] point : rmfp.getPaths().get(pathType)) {
float x = point[0] * scale;
float y = point[1] * scale;
float x = toXCoord(point[0]) * scale;
float y = toYCoord(point[1]) * scale;
if (prvX > 1) {
g2d.draw(new Line2D.Float(prvX, prvY, x, y));
}
Expand All @@ -211,10 +217,10 @@ private void drawPath(Graphics2D g2d, float scale) {

private void drawZones(Graphics2D g2d, float scale) {
for (float[] point : rmfp.getZones()) {
float x = point[0] * scale;
float y = point[1] * scale;
float x1 = point[2] * scale;
float y1 = point[3] * scale;
float x = toXCoord(point[0]) * scale;
float y = toYCoord(point[1]) * scale;
float x1 = toXCoord(point[2]) * scale;
float y1 = toYCoord(point[3]) * scale;
float sx = Math.min(x, x1);
float w = Math.max(x, x1) - sx;
float sy = Math.min(y, y1);
Expand All @@ -227,14 +233,14 @@ private void drawZones(Graphics2D g2d, float scale) {
private void drawNoGo(Graphics2D g2d, float scale) {
for (Integer area : rmfp.getAreas().keySet()) {
for (float[] point : rmfp.getAreas().get(area)) {
float x = point[0] * scale;
float y = point[1] * scale;
float x1 = point[2] * scale;
float y1 = point[3] * scale;
float x2 = point[4] * scale;
float y2 = point[5] * scale;
float x3 = point[6] * scale;
float y3 = point[7] * scale;
float x = toXCoord(point[0]) * scale;
float y = toYCoord(point[1]) * scale;
float x1 = toXCoord(point[2]) * scale;
float y1 = toYCoord(point[3]) * scale;
float x2 = toXCoord(point[4]) * scale;
float y2 = toYCoord(point[5]) * scale;
float x3 = toXCoord(point[6]) * scale;
float y3 = toYCoord(point[7]) * scale;
Path2D noGo = new Path2D.Float();
noGo.moveTo(x, y);
noGo.lineTo(x1, y1);
Expand All @@ -253,10 +259,10 @@ private void drawWalls(Graphics2D g2d, float scale) {
Stroke stroke = new BasicStroke(3 * scale);
g2d.setStroke(stroke);
for (float[] point : rmfp.getWalls()) {
float x = point[0] * scale;
float y = point[1] * scale;
float x1 = point[2] * scale;
float y1 = point[3] * scale;
float x = toXCoord(point[0]) * scale;
float y = toYCoord(point[1]) * scale;
float x1 = toXCoord(point[2]) * scale;
float y1 = toYCoord(point[3]) * scale;
g2d.setColor(Color.RED);
g2d.draw(new Line2D.Float(x, y, x1, y1));
}
Expand All @@ -267,13 +273,17 @@ private void drawRobo(Graphics2D g2d, float scale) {
Stroke stroke = new BasicStroke(2 * scale);
g2d.setStroke(stroke);
g2d.setColor(COLOR_CHARGER_HALO);
drawCircle(g2d, rmfp.getChargerX() * scale, rmfp.getChargerY() * scale, radius);
drawCenteredImg(g2d, scale / 8, "charger.png", rmfp.getChargerX() * scale, rmfp.getChargerY() * scale);
final float chargerX = toXCoord(rmfp.getChargerX()) * scale;
final float chargerY = toYCoord(rmfp.getChargerY()) * scale;
drawCircle(g2d, chargerX, chargerY, radius);
drawCenteredImg(g2d, scale / 8, "charger.png", chargerX, chargerY);
radius = 3 * scale;
g2d.setColor(COLOR_ROBO);
drawCircle(g2d, rmfp.getRoboX() * scale, rmfp.getRoboY() * scale, radius);
final float roboX = toXCoord(rmfp.getRoboX()) * scale;
final float roboY = toYCoord(rmfp.getRoboY()) * scale;
drawCircle(g2d, roboX, roboY, radius);
if (scale > 1.5) {
drawCenteredImg(g2d, scale / 15, "robo.png", rmfp.getRoboX() * scale, rmfp.getRoboY() * scale);
drawCenteredImg(g2d, scale / 15, "robo.png", roboX, roboY);
}
}

Expand Down Expand Up @@ -301,8 +311,8 @@ private void drawCenteredImg(Graphics2D g2d, float scale, String imgFile, float
}

private void drawGoTo(Graphics2D g2d, float scale) {
float x = rmfp.getGotoX() * scale;
float y = rmfp.getGotoY() * scale;
float x = toXCoord(rmfp.getGotoX()) * scale;
float y = toYCoord(rmfp.getGotoY()) * scale;
if (!(x == 0 && y == 0)) {
g2d.setStroke(new BasicStroke());
g2d.setColor(Color.YELLOW);
Expand Down Expand Up @@ -409,6 +419,14 @@ public boolean writePic(String filename, String formatName, float scale) throws
return ImageIO.write(getImage(scale), formatName, new File(filename));
}

private float toXCoord(float x) {
return rmfp.getImgWidth() + rmfp.getLeft() - (x / MM);
}

private float toYCoord(float y) {
return y / MM - rmfp.getTop();
}

@Override
public String toString() {
return rmfp.toString();
Expand Down
73 changes: 42 additions & 31 deletions ...b.binding.miio/src/main/java/org/openhab/binding/miio/internal/robot/RRMapFileParser.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,8 +59,6 @@ public class RRMapFileParser {
public static final String PATH_POINT_SIZE = "pointSize";
public static final String PATH_ANGLE = "angle";

private static final float MM = 50.0f;

private byte[] image = new byte[] { 0 };
private final int majorVersion;
private final int minorVersion;
Expand All @@ -73,7 +71,6 @@ public class RRMapFileParser {
private int imageSize;
private int top;
private int left;
private int offset;

private int chargerX;
private int chargerY;
Expand Down Expand Up @@ -124,7 +121,6 @@ public RRMapFileParser(byte[] raw) {
this.left = getUInt32LE(header, blockHeaderLength - 12);
this.imgHeight = (getUInt32LE(header, blockHeaderLength - 8));
this.imgWidth = getUInt32LE(header, blockHeaderLength - 4);
this.offset = imgWidth + left;
this.image = data;
break;
case ROBOT_POSITION:
Expand All @@ -144,8 +140,8 @@ public RRMapFileParser(byte[] raw) {
detail.put(PATH_POINT_SIZE, getUInt32LE(header, 0x0C));
detail.put(PATH_ANGLE, getUInt32LE(header, 0x10));
for (int pathpair = 0; pathpair < pairs; pathpair++) {
float x = offset - (getUInt16(getBytes(raw, blockDataStart + pathpair * 4, 2))) / MM;
float y = getUInt16(getBytes(raw, blockDataStart + pathpair * 4 + 2, 2)) / MM - top;
float x = (getUInt16(getBytes(raw, blockDataStart + pathpair * 4, 2)));
float y = getUInt16(getBytes(raw, blockDataStart + pathpair * 4 + 2, 2));
path.add(new float[] { x, y });
}
paths.put(blocktype, path);
Expand All @@ -154,27 +150,27 @@ public RRMapFileParser(byte[] raw) {
case CURRENTLY_CLEANED_ZONES:
int zonePairs = getUInt16(header, 0x08);
for (int zonePair = 0; zonePair < zonePairs; zonePair++) {
float x0 = offset - (getUInt16(raw, blockDataStart + zonePair * 8)) / MM;
float y0 = getUInt16(raw, blockDataStart + zonePair * 8 + 2) / MM - top;
float x1 = offset - (getUInt16(raw, blockDataStart + zonePair * 8 + 4)) / MM;
float y1 = getUInt16(raw, blockDataStart + zonePair * 8 + 6) / MM - top;
float x0 = (getUInt16(raw, blockDataStart + zonePair * 8));
float y0 = getUInt16(raw, blockDataStart + zonePair * 8 + 2);
float x1 = (getUInt16(raw, blockDataStart + zonePair * 8 + 4));
float y1 = getUInt16(raw, blockDataStart + zonePair * 8 + 6);
zones.add(new float[] { x0, y0, x1, y1 });
}
break;
case GOTO_TARGET:
this.gotoX = offset - getUInt16(data, 0x00) / MM;
this.gotoY = getUInt16(data, 0x02) / MM - top;
this.gotoX = getUInt16(data, 0x00);
this.gotoY = getUInt16(data, 0x02);
break;
case DIGEST:
isValid = Arrays.equals(data, sha1Hash(getBytes(raw, 0, mapHeaderLength + mapDataLength - 20)));
break;
case VIRTUAL_WALLS:
int wallPairs = getUInt16(header, 0x08);
for (int wallPair = 0; wallPair < wallPairs; wallPair++) {
float x0 = offset - (getUInt16(raw, blockDataStart + wallPair * 8)) / MM;
float y0 = getUInt16(raw, blockDataStart + wallPair * 8 + 2) / MM - top;
float x1 = offset - (getUInt16(raw, blockDataStart + wallPair * 8 + 4)) / MM;
float y1 = getUInt16(raw, blockDataStart + wallPair * 8 + 6) / MM - top;
float x0 = (getUInt16(raw, blockDataStart + wallPair * 8));
float y0 = getUInt16(raw, blockDataStart + wallPair * 8 + 2);
float x1 = (getUInt16(raw, blockDataStart + wallPair * 8 + 4));
float y1 = getUInt16(raw, blockDataStart + wallPair * 8 + 6);
walls.add(new float[] { x0, y0, x1, y1 });
}
break;
Expand All @@ -183,14 +179,14 @@ public RRMapFileParser(byte[] raw) {
int areaPairs = getUInt16(header, 0x08);
ArrayList<float[]> area = new ArrayList<float[]>();
for (int areaPair = 0; areaPair < areaPairs; areaPair++) {
float x0 = offset - (getUInt16(raw, blockDataStart + areaPair * 16)) / MM;
float y0 = getUInt16(raw, blockDataStart + areaPair * 16 + 2) / MM - top;
float x1 = offset - (getUInt16(raw, blockDataStart + areaPair * 16 + 4)) / MM;
float y1 = getUInt16(raw, blockDataStart + areaPair * 16 + 6) / MM - top;
float x2 = offset - (getUInt16(raw, blockDataStart + areaPair * 16 + 8)) / MM;
float y2 = getUInt16(raw, blockDataStart + areaPair * 16 + 10) / MM - top;
float x3 = offset - (getUInt16(raw, blockDataStart + areaPair * 16 + 12)) / MM;
float y3 = getUInt16(raw, blockDataStart + areaPair * 16 + 14) / MM - top;
float x0 = (getUInt16(raw, blockDataStart + areaPair * 16));
float y0 = getUInt16(raw, blockDataStart + areaPair * 16 + 2);
float x1 = (getUInt16(raw, blockDataStart + areaPair * 16 + 4));
float y1 = getUInt16(raw, blockDataStart + areaPair * 16 + 6);
float x2 = (getUInt16(raw, blockDataStart + areaPair * 16 + 8));
float y2 = getUInt16(raw, blockDataStart + areaPair * 16 + 10);
float x3 = (getUInt16(raw, blockDataStart + areaPair * 16 + 12));
float y3 = getUInt16(raw, blockDataStart + areaPair * 16 + 14);
area.add(new float[] { x0, y0, x1, y1, x2, y2, x3, y3 });
}
areas.put(Integer.valueOf(blocktype & 0xFF), area);
Expand Down Expand Up @@ -277,14 +273,18 @@ public String toString() {
minorVersion, mapIndex, mapSequence);
pw.printf("Image:\tsize: %9d\ttop: %9d\tleft: %9d height: %9d width: %9d\r\n", imageSize, top, left, imgHeight,
imgWidth);
pw.printf("Charger pos:\tX: %.1f\tY: %.1f\r\n", getChargerX(), getChargerY());
pw.printf("Robo pos:\tX: %.1f\tY: %.1f\tAngle: %d\r\n", getRoboX(), getRoboY(), getRoboA());
pw.printf("Goto:\tX: %.1f\tY: %.1f\r\n", getGotoX(), getGotoY());
pw.printf("Charger pos:\tX: %.0f\tY: %.0f\r\n", getChargerX(), getChargerY());
pw.printf("Robo pos:\tX: %.0f\tY: %.0f\tAngle: %d\r\n", getRoboX(), getRoboY(), getRoboA());
pw.printf("Goto:\tX: %.0f\tY: %.0f\r\n", getGotoX(), getGotoY());
for (Integer area : areas.keySet()) {
pw.print(area == NO_GO_AREAS ? "No Go zones:\t" : "MFBZS zones:\t");
pw.printf("%d\r\n", areas.get(area).size());
printAreaDetails(areas.get(area), pw);
}
pw.printf("Walls:\t%d\r\n", walls.size());
printAreaDetails(walls, pw);
pw.printf("Zones:\t%d\r\n", zones.size());
printAreaDetails(zones, pw);
pw.printf("Obstacles:\t%d\r\n", obstacles.size());
pw.printf("Blocks:\t%d\r\n", blocks.length);
pw.print("Paths:");
Expand All @@ -299,6 +299,16 @@ public String toString() {
return sw.toString();
}

private void printAreaDetails(ArrayList<float[]> areas, PrintWriter pw) {
areas.forEach(area -> {
pw.printf("\tArea coordinates:");
for (int i = 0; i < area.length; i++) {
pw.printf("\t%.0f", area[i]);
}
pw.println();
});
}

/**
* Compute SHA-1 hash value for the byte array
*
Expand Down Expand Up @@ -363,19 +373,19 @@ public ArrayList<float[]> getZones() {
}

public float getRoboX() {
return offset - (roboX / MM);
return roboX;
}

public float getRoboY() {
return roboY / MM - top;
return roboY;
}

public float getChargerX() {
return offset - (chargerX / MM);
return chargerX;
}

public float getChargerY() {
return chargerY / MM - top;
return chargerY;
}

public float getGotoX() {
Expand Down Expand Up @@ -413,4 +423,5 @@ public ArrayList<int[]> getObstacles() {
public byte[] getBlocks() {
return blocks;
}

}
Loading

0 comments on commit d861580

Please sign in to comment.