示例#1
0
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
示例#2
0
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
示例#3
0
文件: nav.py 项目: janfrs/kwc-ros-pkg
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)