예제 #1
0
def rotateScanner(direction):
    # stop motors
    end()

    # validate input
    if type(direction) is str:
        direction = SLAM.dirToNum(direction)

    # Move scanner to face front
    if direction == 0:
        encoder_dif = BP.get_motor_encoder(BEACON_MOTOR) - SCAN_F_ENCODER
        print("Rotating scanner to face front")
        while (abs(BP.get_motor_encoder(BEACON_MOTOR)) > SCAN_TOLERANCE):
            if encoder_dif > 0:
                BP.set_motor_dps(BEACON_MOTOR, -SCAN_MOVE_SPEED)
            elif encoder_dif < 0:
                BP.set_motor_dps(BEACON_MOTOR, SCAN_MOVE_SPEED)

            encoder_dif = BP.get_motor_encoder(BEACON_MOTOR)

    # Move scanner to face right
    elif direction == 1:
        print("Rotating scanner to face right")
        encoder_dif = BP.get_motor_encoder(BEACON_MOTOR) - SCAN_R_ENCODER

        while (abs(encoder_dif) > SCAN_TOLERANCE):
            BP.set_motor_dps(BEACON_MOTOR, SCAN_MOVE_SPEED)
            encoder_dif = BP.get_motor_encoder(BEACON_MOTOR) - SCAN_R_ENCODER

    # Print error if trying to scan behind
    elif direction == 2:
        print("ERROR. CANNOT SCAN BEHIND ROBOT")

    # Move scanner to face left
    elif direction == 3:
        print("Rotating scanner to face left")
        encoder_dif = BP.get_motor_encoder(BEACON_MOTOR) - SCAN_L_ENCODER

        while (abs(encoder_dif) > SCAN_TOLERANCE):
            BP.set_motor_dps(BEACON_MOTOR, -SCAN_MOVE_SPEED)
            encoder_dif = BP.get_motor_encoder(BEACON_MOTOR) - SCAN_L_ENCODER

    # Update SLAM scanner status
    SLAM.scanPos = SLAM.numToDirScanner(direction)
    SLAM.updateScanDir()

    # stop moving
    stop()
예제 #2
0
def main():

    trackMap = TrackMap()
    carStatueUpdateQueue = queue.Queue()
    imuThread = threading.Thread(target = ImuLoop, args =(carStatueUpdateQueue,'data.csv',trackMap,), daemon=True)
    imuThread.start()
    car = Car()
    # oppMode = OppMode(type = 'offline',path = 'C:\\FinalProject\\TauPythonCar\\UTMPosition.csv')
    oppMode = OppMode("online")
    
    SLAM.RunSLAM(car,carStatueUpdateQueue,trackMap,oppMode)
예제 #3
0
파일: Room_Sim.py 프로젝트: ivanscode/sd
if __name__ == '__main__':
	# Initialize nodes and walls, scan
	#nodes = from_files()
	#walls = [Wall.Wall(Point(0,0), Point(0,0))]
#
	#Drawer.draw(nodes, walls)
#
	##for n in nodes:
	##	n.pred_x = 0
	##	n.pred_y = 0
	##	n.pred_angle = 0
	##	n.real_x = 0
	##	n.real_y = 0
	##	n.real_angle = 0
	#Drawer.draw(nodes, [])
	#s = SLAM.SLAM(nodes)
	#print(s.solve_multi())
	##s.gravitate()
	#Drawer.draw(nodes, walls)
	#Drawer.draw(nodes, [])

	for i in range(3):
		i = 'samapt/data%d.txt' % i
		for j in range(3):
			j = 'samapt/data%d.txt' % (j + 3)
			for k in range(3):
				k = 'samapt/data%d.txt' % (k + 6)
				nodes = from_files([i, j, k])
				s = SLAM.SLAM(nodes)
				print(s.solve_multi())
				Drawer.draw(nodes, [], quickclose=True)
예제 #4
0
sock_output.connect(server_address_out)

module_running = True

try:
    while module_running:

        ######################################################
        # WHEN YOU ARE READY, START READING INCOMING DATA
        # TO PROCESS IT. THIS FUNCTION CALL IS BLOCKING,
        # IT WILL WAIT UNTIL THERE'S DATA. THE DATA WILL
        # BE WAITING AND PILE UP UNTIL YOU READ IT.
        ######################################################
        cone_data = tcplib.receiveData(sock_input_perception)
        motion_data = tcplib.receiveData(sock_input)
        built_map = SLAM.main(cone_data, motion_data)

        ######################################################
        # PERFORM SOME CALCULATIONS.
        # WHEN YOU GET A RESULT YOU WISH TO SEND,
        # CONTINUE WITH THE CODE BELOW
        ######################################################
        tcplib.sendData(sock_output, built_map)

        ######################################################
        # IF YOU'RE DONE HANDLING THE DATA, THE LOOP
        # WILL TAKE YOU BACK TO WAITING ON INCOMING DATA.
        # IF YOUR MODULE ENCOUNTERS AN ERROR OR NEEDS TO
        # TERMINATE, CONTINUE WITH THE CODE BELOW
        ######################################################
        # Need to figure out the terminating condition, but for let's pretend it never stops
예제 #5
0
 def main_control(self):
     self.slam = SLAM(GridMap(), LSLAMGUI())
     rospy.Subscriber('scan_odom', scan_odom, self.Call, queue_size=1, buff_size=52428800)
     rospy.spin()
예제 #6
0
class Controller:
    def __init__(self, k_rho=0.5*3, k_alpha=0.8*3, k_beta=-0.15*3, threshold=0.05):
        assert k_rho >= 0 and k_beta <=0 and k_alpha >= k_rho, 'Bad parameters.'
        rospy.init_node('test_control', anonymous=True)
        self.pub = rospy.Publisher('/cmd_vel_mux/input/teleop', Twist, queue_size=5)
        self.threshold = threshold
        self.is_navigating = False
        # self.K = np.array([
        #     [-k_rho, 0, 0],
        #     [0, -(k_alpha-k_rho), k_beta],
        #     [0, -k_rho, 0]
        # ])
        self.k_rho = k_rho
        self.k_alpha = k_alpha
        self.k_beta = k_beta

        # self.kp = [0.1, 0.1, 0.1]
        # self.ki = [0.1, 1, 0.1]


    def get_path(self, path):
        self.path = []
        for i in range(len(path)):
            if i == len(path) - 1:
                phi = self.path[i-1][2]
            else:
                phi = atan2(path[i+1, 1] - path[i, 1], path[i+1, 0] - path[i, 0])
            self.path.append([path[i, 0], path[i, 1], phi])
        

        self.path = np.array(self.path)

        np.savetxt('path.txt', self.path)
        
        return self.path

    def get_input(self, cur_pose_in_goal):
        ## stupid pi controller, which works like shit!!!
        # x, y, theta = e

        # try:
        #     last_v = self.last_v
        #     last_w = self.last_w
        #     last_e = self.last_e
        # except:
        #     self.last_v = 0
        #     self.last_w = 0
        #     self.last_e = e
        #     return np.array([0., 0.])
        
        # v = 0.*last_v + self.kp[0]*(e[0] - last_e[0]) + self.ki[0]*e[0]
        # w = 0.*last_w + self.kp[1]*(e[1] - last_e[1]) + self.ki[1]*e[1] + \
        #             self.kp[2]*(e[2] - last_e[2]) + self.ki[2]*e[2]

        # print e
        
        # return np.array([v, w])

        x, y = -cur_pose_in_goal[:2]
        theta = cur_pose_in_goal[2]

        # print x, y, theta

        while theta < -pi:
            theta += 2*pi
        while theta > pi:
            theta -= 2*pi

        rho = sqrt(x**2 + y**2)
        beta = -atan2(y, x)

        try:
            last_beta = self.last_beta
            last_theta = self.last_theta
            # print self.last_alpha, self.last_beta
            while beta - last_beta > (2 - 0.2)*pi:
                beta -= 2*pi
            while beta - last_beta < -(2 - 0.2)*pi:
                beta += 2*pi
            while theta - last_theta > (2 - 0.2)*pi:
                theta -= 2*pi
            while theta - last_theta < -(2 - 0.2)*pi:
                theta += 2*pi
        except:
            pass

        alpha = -theta - beta
        # beta = max(beta, -2)
        # beta = min(beta, 2)
        print rho, alpha, beta

        self.last_beta = beta
        self.last_theta = theta

        # if abs(y/x) < 0.05 and x < 0:
        #     beta = pi

        # if abs(y/x) > 1e3:
        #     if y >= 0:
        #         beta = pi / 2.
        #     else:
        #         beta = - pi / 2.



        
        v = self.k_rho*rho
        # w = self.k_alpha*alpha + self.k_beta*beta

        if rho < 0.15:
            w = (rho / 0.15)**2 * self.k_alpha*alpha + self.k_beta*beta
        else:
            w = self.k_alpha*alpha + (0.15 / rho)**2 * self.k_beta*beta

        return np.array([v, w])


    def forward(self):
        pose = self.slam.pose
        while True:
            k = self.k
            pk = self.path[k]
            pose_mat = self.slam.pose2mat(pk)
            cur_pose_in_goal = self.slam.world2robot(pose[:2], pose_mat)
            

            theta = 1.*pose[2] - pk[2]
            if theta >= pi:
                theta = -2*pi + theta
            elif theta < -pi:
                theta = 2*pi + theta
            cur_pose_in_goal = np.insert(cur_pose_in_goal, 2, values=theta, axis=0)
            print self.k, cur_pose_in_goal
            if self.k < len(self.path) and np.sum(np.square(cur_pose_in_goal)) < self.threshold:
                self.k += 1
            else:
                break
 
            if self.k == len(self.path):
                print 'done.'
                return    

        v, w = self.get_input(cur_pose_in_goal)
        self.publish(v, w)
    
    def publish(self, v, w):

        k = 1.
        if abs(v) > max_speed:
            k = max_speed / abs(v)
        if abs(w) > max_turn:
            k = min(k, max_turn / abs(w))

        v *= k
        w *= k

        print 'v =',v , ', w =', w

        twist = Twist()
        twist.linear.x = v; twist.linear.y = 0; twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = w
        self.pub.publish(twist)

    def main_control(self):
        self.slam = SLAM(GridMap(), LSLAMGUI())
        rospy.Subscriber('scan_odom', scan_odom, self.Call, queue_size=1, buff_size=52428800)
        rospy.spin()

    def Call(self, msg):
        if self.slam.gui and self.slam.gui.state == -1:
            return 
        elif self.slam.gui and self.slam.gui.state == 0:
            self.slam.gui.start()
        elif self.slam.gui and self.slam.gui.state == 1:
            pass
        else:
            pose = self.slam.pose
            if self.is_navigating is False:
                self.is_navigating = True
                self.k = 0
                p_end = self.slam.init_pose[:2]
                p_start = self.slam.pose[:2]
                path = self.slam.gridmap.path_planning_map(p_start.copy(), p_end.copy())
                self.get_path(path)
            else:
                if self.k == len(self.path):
                    pass
                else:
                    self.forward()
                    

        self.slam.forward(msg)
예제 #7
0
import DataVisual
import SLAM
import cv2
import argparse

parser = argparse.ArgumentParser()
parser.add_argument('-video', nargs='?', type=str, default='drive.mp4')
parser.add_argument('-fov', nargs='?', type=int, default=60)
args = parser.parse_args()

cap = cv2.VideoCapture(args.video)
count = 0
slam = SLAM.Slam(args.fov)

# Read until video is completed
while (cap.isOpened()):
    # Capture frame-by-frame
    ret, frame = cap.read()
    if ret == True:

        count = count + 1
        # Display the resulting frame
        frame = cv2.resize(frame, (800, 600))

        DataVisual.featureExtraction(frame)

        if count >= 10:
            slam.runSlam(frame)
            count = 0

        # Press Q on keyboard to exit
예제 #8
0
#
# ELECTRONIC SIGNATURE
# Aaron Thielmeyer
# Kat Li
# Evelyn Shi
# Zubin Kane
#
# The electronic signatures above indicate that the program
# submitted for evaluation is the combined effort of all
# team members and that each member of the team was an
# equal participant in its creation. In addition, each
# member of the team has a general understanding of
# all aspects of the program development and execution.
#
# Main file used for final robot demo

import driveLibrary
import SLAM

# Create empty map the size of the demo map area
SLAM.emptyMap(SLAM.map_max_x, SLAM.map_max_y)

# Move forward and perform initial scan
SLAM.moveForward()
SLAM.scanSurroundings()

while (1 == 1):  # Run indefinitely
    SLAM.completeMap()  # Output map
    SLAM.drivePath(SLAM.pathToClosestPt())  # Drive to closest point
    SLAM.scanSurroundings()  # Scan surroundings
예제 #9
0

def getFrame():
    for i in FRAME_GENERATOR:
        return i

    return -1, -1


#####################
# READ FRAMES END
#####################

# GLOBAL VARIABLES
# poseFig, poseAxis = plt.subplots()
slamAlgorithm = SLAM()

frameA, depthA = getFrame()
frameB, depthB = getFrame()
images = [frameA, frameB]
depths = [depthA, depthB]

i = 2
while True:
    # SLAMMING
    slamAlgorithm.process(images, depths, i)
    i += 1

    # plt.cla()
    # trajectory = slamAlgorithm.get_trajectory()
    # visualize_trajectory(trajectory)
예제 #10
0
	return

def replot():
	plt.clf()
	plotLandmarks()
	plotRobotPositionAfterSlam()
	plt.axis([chartMin,chartMax,chartMin,chartMax])
	plt.pause(0.001)
	return

######################################
# MAIN START #########################
######################################
print "Starting position:", globalPosition, globalHeading

slam = SLAM.Slam(motionNoise, measurementNoise)	# try and tidy file/class name
needNewUserInput = True

while True:
	if needNewUserInput:
		userInput = raw_input("Enter next command, Enter for default, q to quit\n")
		if userInput == '':
			# default path
			robotReadyForNewCommand = True
			needNewUserInput = False
		if userInput == 'q':
			break
		if userInput != '':
			print "Not valid command"
			# add possibility to give custom commands