sensor.py 文件源码

def feature_inview_2d(f, x, theta, rmax, thmax):
    """Checks to see wheter features is in view of robot

    Parameters
    ----------
    f : np.array of size 2
        Feature position
    x : np.array of size 2
        Robot position
    theta : float
        Robot's current heading in radians
    rmax : float
        Max sensor range
    thmax : float
        Max sensor bearing

    Returns
    -------

        Boolean to denote whether feature is in view of robot

    """
    # Distance in x and y
    dx = f[0] - x[0]
    dy = f[1] - x[1]

    # Calculate range and bearing
    r = sqrt(dx**2 + dy**2)
    th = fmod(atan2(dy, dx) - theta, 2 * pi)
    if th > pi:
        th = th - 2 * pi

    # Check to see if range and bearing is within view
    if ((r < rmax) and (abs(th) < thmax)):
        return True
    else:
        return False