Source code for roboticstoolbox.mobile.landmarkmap

from abc import ABC
import numpy as np
import scipy as sp
from math import pi, sin, cos
import matplotlib.pyplot as plt
from spatialmath import base
import roboticstoolbox as rtb


[docs]class LandmarkMap: """ Map of planar point landmarks :param map: map or number of landmarks :type map: ndarray(2, N) or int :param workspace: workspace or map bounds, defaults to 10 :type workspace: scalar, array_like(2), array_like(4), optional :param verbose: display debug information, defaults to True :type verbose: bool, optional :param seed: random number seed, defaults to 0 :type seed: int, optional :return: a landmark map object :rtype: LandmarkMap A LandmarkMap object represents a rectangular 2D environment with a number of point landmarks. The landmarks can be specified explicitly or be uniform randomly positioned inside a region defined by the workspace. The workspace can be numeric: ============== ======= ======= ``workspace`` x-range y-range ============== ======= ======= A (scalar) -A:A -A:A [A, B] A:B A:B [A, B, C, D] A:B C:D ============== ======= ======= or any object that has a ``workspace`` attribute. Example: .. runblock:: pycon >>> from roboticstoolbox import LandmarkMap >>> map = LandmarkMap(20) >>> print(map) >>> print(map[3]) # coordinates of landmark 3 The object is an iterator that returns consecutive landmark coordinates. :Reference: Robotics, Vision & Control, Chap 6, Peter Corke, Springer 2011 See also :class:`~roboticstoolbox.mobile.sensors.RangeBearingSensor` :class:`~roboticstoolbox.mobile.EKF` """
[docs] def __init__(self, map, workspace=10, verbose=True, seed=0): try: self._workspace = workspace.workspace except: self._workspace = base.expand_dims(workspace) if base.ismatrix(map, (2, None)): self._map = map self._nlandmarks = map.shape[1] elif isinstance(map, int): self._nlandmarks = map random = np.random.default_rng(seed) x = random.uniform(self._workspace[0], self._workspace[1], self._nlandmarks) y = random.uniform(self._workspace[2], self._workspace[3], self._nlandmarks) self._map = np.c_[x, y].T else: raise ValueError("bad type for map") self._verbose = verbose
def __str__(self): # s = M.char() is a string showing map parameters in # a compact human readable format. ws = self._workspace s = f"LandmarkMap object with {self._nlandmarks} landmarks, workspace=" s += f"({ws[0]}: {ws[1]}, {ws[2]}: {ws[3]})" return s def __repr__(self): return str(self)
[docs] def __len__(self): """ Number of landmarks in map :return: number of landmarks in the map :rtype: int """ return self._nlandmarks
@property def landmarks(self): """ xy-coordinates of all landmarks :return: xy-coordinates for landmark points :rtype: ndarray(2, n) """ return self._map @property def workspace(self): """ Size of map workspace :return: workspace bounds [xmin, xmax, ymin, ymax] :rtype: ndarray(4) Returns the bounds of the workspace as specified by constructor option ``workspace``. """ return self._workspace
[docs] def __getitem__(self, k): """ Get landmark coordinates from map :param k: landmark index :type k: int :return: coordinate :math:`(x,y)` of k'th landmark :rtype: ndarray(2) """ return self._map[:, k]
[docs] def plot(self, labels=False, block=None, **kwargs): """ Plot landmark map :param labels: number the points on the plot, defaults to False :type labels: bool, optional :param block: block until figure is closed, defaults to False :type block: bool, optional :param kwargs: :meth:`~matplotlib.axes.Axes.plot` options Plot landmark points using Matplotlib options. Default style is black crosses. """ ax = base.plotvol2(self._workspace) ax.set_aspect("equal") ax.set_xlabel("x") ax.set_ylabel("y") if len(kwargs) == 0: kwargs = { "linewidth": 0, "marker": "x", "color": "black", "linestyle": "None", } if "label" not in kwargs: kwargs["label"] = "landmark" # plt.plot(self._map[0,:], self._map[1,:], , **kwargs) if labels: labels = "#{}" else: labels = None base.plot_point(self._map, text=labels, **kwargs) plt.grid(True) if block is not None: plt.show(block=block)
if __name__ == "__main__": import unittest