def euclid_of_laser2(scans, indices=None, angle_start = URG_ANGLE_START, angle_step = URG_ANGLE_STEP, num_scans=URG_NUM_SCANS): """ NOTE: USE THIS IN THE FUTURE NOT ABOVE By default this node uses URG parameters, where points are scanned from left to right. For SICK or other scanners supply the angle_start and angle_step arguments. Return points in native urg frame +x is forward +y is right """ if indices == None: indices = np.matrix(range(num_scans)) else: if indices.__class__ != np.matrix: raise Exception("Argument indices must be a matrix") angles = ((indices * angle_step) + angle_start) selected_scans = scans[0, indices] euclid = ut.cart_of_pol(np.vstack((selected_scans, angles))) return euclid
def euclid_of_laser(scans, indices=None, angle_start = URG_ANGLE_START, angle_step = URG_ANGLE_STEP, num_scans=URG_NUM_SCANS): """ Takes laser ranges and returns euclidean ego centric coordinates of points, where +x is forward +y is left consistent with Player/Stage driving coordinates. (Assume laser scanner is flipped upside down). By default this node uses URG parameters, where points are scanned from left to right. For SICK or other scanners supply the angle_start and angle_step arguments. """ if indices == None: indices = np.matrix(range(num_scans)) else: if indices.__class__ != np.matrix: raise Exception("Argument indices must be a matrix") angles = ((indices * angle_step) + angle_start) selected_scans = scans[0, indices] euclid = ut.cart_of_pol(np.vstack((selected_scans, angles))) euclid[1,:] = euclid[1,:] * -1.0 return euclid
def euclid_of_laser2(scans, indices=None, angle_start=URG_ANGLE_START, angle_step=URG_ANGLE_STEP, num_scans=URG_NUM_SCANS): """ NOTE: USE THIS IN THE FUTURE NOT ABOVE By default this node uses URG parameters, where points are scanned from left to right. For SICK or other scanners supply the angle_start and angle_step arguments. Return points in native urg frame +x is forward +y is right """ if indices == None: indices = np.matrix(range(num_scans)) else: if indices.__class__ != np.matrix: raise Exception("Argument indices must be a matrix") angles = ((indices * angle_step) + angle_start) selected_scans = scans[0, indices] euclid = ut.cart_of_pol(np.vstack((selected_scans, angles))) return euclid
def euclid_of_laser(scans, indices=None, angle_start=URG_ANGLE_START, angle_step=URG_ANGLE_STEP, num_scans=URG_NUM_SCANS): """ Takes laser ranges and returns euclidean ego centric coordinates of points, where +x is forward +y is left consistent with Player/Stage driving coordinates. (Assume laser scanner is flipped upside down). By default this node uses URG parameters, where points are scanned from left to right. For SICK or other scanners supply the angle_start and angle_step arguments. """ if indices == None: indices = np.matrix(range(num_scans)) else: if indices.__class__ != np.matrix: raise Exception("Argument indices must be a matrix") angles = ((indices * angle_step) + angle_start) selected_scans = scans[0, indices] euclid = ut.cart_of_pol(np.vstack((selected_scans, angles))) euclid[1, :] = euclid[1, :] * -1.0 return euclid
def v_repulse(euclid_pts, goal=None, zone_of_influence=.6, zone_of_safety=.2): """ Calculates a repulsive vector """ polar_pts = ut.pol_of_cart(euclid_pts) #print "v_repulse: In polar", polar_pts.T if goal != None: polar_goal = ut.pol_of_cart(goal) def determine_mag(pt): if pt[0, 0] < zone_of_safety: #print "DANGEr", pt[0,0] return 1.0 elif pt[0, 0] < zone_of_influence: return (zone_of_influence - pt[0, 0]) / (zone_of_influence - zone_of_safety) else: return 0.0 total = 0 repulsive_vec = np.matrix([0.0, 0.0]).T for i in range(polar_pts.shape[1]): pt = polar_pts[:, i] repulse = pt.copy() if goal != None: turn_dir = ut.best_turn_dir(polar_goal[1, 0], pt[1, 0]) #If it's in front if np.abs(turn_dir) < np.pi: #if it's to the right if turn_dir > 0: #print "RIGHT" repulse[1, 0] = pt[1, 0] + (np.pi / 2.0) else: #if it's to the left #print "LEFT" repulse[1, 0] = pt[1, 0] - (np.pi / 2.0) repulse[0, 0] = determine_mag(pt) repulsive_vec = repulsive_vec + ut.cart_of_pol(repulse) total = 1 + total if total == 0: return np.matrix([0.0, 0.0]).T else: return repulsive_vec / float(total)
def v_repulse(euclid_pts, goal=None, zone_of_influence=.6, zone_of_safety=.2): """ Calculates a repulsive vector """ polar_pts = ut.pol_of_cart(euclid_pts) #print "v_repulse: In polar", polar_pts.T if goal != None: polar_goal = ut.pol_of_cart(goal) def determine_mag(pt): if pt[0,0] < zone_of_safety: #print "DANGEr", pt[0,0] return 1.0 elif pt[0,0] < zone_of_influence: return (zone_of_influence - pt[0,0]) / (zone_of_influence - zone_of_safety) else: return 0.0 total = 0 repulsive_vec = np.matrix([0.0, 0.0]).T for i in range(polar_pts.shape[1]): pt = polar_pts[:,i] repulse = pt.copy() if goal != None: turn_dir = ut.best_turn_dir(polar_goal[1,0], pt[1,0]) #If it's in front if np.abs(turn_dir) < np.pi: #if it's to the right if turn_dir > 0: #print "RIGHT" repulse[1,0] = pt[1,0] + (np.pi/2.0) else: #if it's to the left #print "LEFT" repulse[1,0] = pt[1,0] - (np.pi/2.0) repulse[0,0] = determine_mag(pt) repulsive_vec = repulsive_vec + ut.cart_of_pol(repulse) total = 1 + total if total == 0: return np.matrix([0.0, 0.0]).T else: return repulsive_vec / float(total)