code in progress for manipulating vectors

L2_vector.py

# Handling distance vectors in the robot coordinate frame

# Import external libraries
import numpy as np
from numpy import exp, abs, angle
import time

# Import internal programs
import L1_lidar as lidar

def nearest(scan): # find the nearest point in the scan
    column_mins = np.argmin(scan, axis=0) # get index of min values along 0th axis (columns)
    row_index = column_mins[0] # index of the smallest distance
    vec = scan[row_index,:] # return the distance and angle of the nearest object in scan
    return vec

def polar2cart(r,theta):
    theta = np.pi/180*theta
    return r * exp( 1j * theta )
    
#--------------------------------------------------------------------------------------
# EXAMPLE MATRIX FOR MANIPULATING
# r12=np.array([2.701, 100.27]) # a 1x2 vector containing distance, angle
# print(r12)

# c12 = polar2cart(r12[0],r12[1])
# c12 = np.round(c12,3)
# print("x,y =  ", c12, "m")

# PRACTICE WITH MATRICES, MINIMUMS, INDEXING
#---------------------------------------------------------------------------------------
# r=np.array([(2.7, 100),(3.1,101),(2.2,102)]) # a 1x2 vector containing distance, angle
# r[1,:] = (3.3,101) # reassign the 2nd row
# print("\n whole matrix: \n", r)

# e23= r[1,1]  # assign the value from the 2nd row and 2nd column to e23
# e = r[1,:] # grab the whole 2nd row

# nearest = np.argmin(r, axis=0) # 0th axis = search along "among columns"
# index = nearest[0] # index of the smallest distance
# vec = r[index,:]
# print("argmin has returned:",nearest)
# print("the smallest vector is:\n", vec)
#---------------------------------------------------------------------------------------

while 1:
    scan = lidar.polarScan() # get values (54 vectors)
    obstacle = nearest(scan) # indicate the nearest obstacle
    print("shortest vector measured:", obstacle)
    time.sleep(1)