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()
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)
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)
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
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()
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)
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
#
# 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
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)
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