Example plot_map.py

#!/usr/bin/env python3

__copyright__ = """

  Slamcore Confidential
  ---------------------

  Slamcore Limited
  All Rights Reserved.
  (C) Copyright 2021

  NOTICE:

  All information contained herein is, and remains the property of Slamcore
  Limited and its suppliers, if any. The intellectual and technical concepts
  contained herein are proprietary to Slamcore Limited and its suppliers and
  may be covered by patents in process, and are protected by trade secret or
  copyright law. Dissemination of this information or reproduction of this
  material is strictly forbidden unless prior written permission is obtained
  from Slamcore Limited.
"""

__license__ = "Slamcore Confidential"

__doc__ = "Plot Slamcore occupancy maps"

from matplotlib.colors import ListedColormap
from pathlib import Path

import argparse
import matplotlib.patches as mpatches
import matplotlib.pyplot as plt
import numpy as np


COLORMAP = [(0x69 / 0xFF, 0x69 / 0xFF, 0x69 / 0xFF)] * 256  # Unknown
COLORMAP[0] = (0xA9 / 0xFF, 0xA9 / 0xFF, 0xA9 / 0xFF)  # Free
COLORMAP[100] = (0x00 / 0xFF, 0xBB / 0xFF, 0xF9 / 0xFF)  # Occupied


def valid_path(s: str) -> Path:
    p = Path(s)
    if not p.is_file():
        raise RuntimeError(f"Not a valid file: {p}")
    if p.suffix not in (".txt", ".TXT"):
        raise RuntimeError(f"Must be a TXT file: {p}")

    return p


def main():
    # Parse command line arguments. We expect a TXT map file.
    # This could be generated by the write_map example.
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument(
        "txt_map_file",
        type=valid_path,
        help="Slamcore map TXT file to plot",
    )
    args = parser.parse_args()

    map_ = np.loadtxt(args.txt_map_file, dtype=np.uint8)

    # Plot map.
    plt.figure("Slamcore Plot Map Example")
    ax = plt.axes()
    ax.set_xlabel("Cell X")
    ax.set_ylabel("Cell Y")

    cmap = ListedColormap(COLORMAP)
    plt.imshow(map_, interpolation="none", cmap=cmap)

    patches = (
        mpatches.Patch(color=COLORMAP[0], label="Free"),
        mpatches.Patch(color=COLORMAP[100], label="Occupied"),
        mpatches.Patch(color=COLORMAP[255], label="Unknown"),
    )
    ax.legend(handles=patches)
    ax.grid()
    plt.show()


if __name__ == "__main__":
    main()