def process_movie(mov, op, index, results):
print(f"performing shot detection for {mov}")
shot_detection(mov, op)
print(f"Saving shots for {mov}")
save_shots(mov, op)
print(f"procesing shot colors for {mov}")
shot_colors(mov, op)
print(f"processing shot colors average for {mov}")
shot_colors_avg(mov, op)
print(f"processing movie colors for {mov}")
movie_colors(mov, op)
print(f"Calculating motion for {mov}")
motion(mov, op)
print(f"Sorting motion spectrum for {mov}")
sort_motion_spectrum(mov, op)
results[index] = quantize(mov, op)
return
def take_pic(sigt, strt, turn, flag):
import cv2
import numpy as np
import time
import motion
import mail
capture = cv2.VideoCapture(
0
) #'GenYoutube.net_Cars_moving_on_road_Stock_Footage_-_Free_Download.mp4.mp4')
fgmk = cv2.createBackgroundSubtractorMOG2()
def get_image():
ret, frame = capture.read()
frame1 = fgmk.apply(frame)
frame2 = cv2.bitwise_and(frame, frame, mask=frame1)
return frame2, frame1, frame
for i in range(30):
temp = get_image()
print('Taking image...')
t = round(time.time())
nm = "/home/rahul/Project/images/" + str(t)
camera_capture, mask, orig = get_image()
cv2.imwrite(nm + '.png', camera_capture)
#cv2.imwrite(nm+'_mask.png',mask)
cv2.imwrite(nm + '_orig.png', orig)
#cv2.imshow(nm+'_orig.png',orig)
#time.sleep(5)
del (capture)
cv2.destroyAllWindows()
mail.find_plate(
nm) # image extesion is missing added later in the called function
motion.motion(sigt, strt, turn, flag)
'''
img=cv2.imread('first.png',cv2.IMREAD_GRAYSCALE)
threshold = cv2.adaptiveThreshold(img,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,cv2.THRESH_BINARY,115,1)
cv2.imwrite('second.png',threshold)
NOT THE RIGHT IDEA
'''
'''To remove noise
img=cv2.imread('first.png')
kernel = np.ones((115,15),np.float32)/255
smooth =cv2.filter2D(img,-1,kernel)
cv2.imshow('smoother',smooth)
cv2.waitKey(0)
cv2.destroyAllWindows()
NO USE
'''
'''kernel = np.ones((5,5),np.int8)
def mov_analysis(statsqueue, imagequeue, imagennqueue,
brightanalysis_targetarea, brightanalysis_drawarea,
motionareas_resize, motionareas_crop, motionareas_targetarea):
t = motion(brightanalysis_targetarea, brightanalysis_drawarea,
motionareas_resize, motionareas_crop, motionareas_targetarea)
start_time = time.time()
cpt = 0
while True:
#if imagequeue.qsize() > 30:
# print("Warning : imagequeue : " + str(imagequeue.qsize()))
image = imagequeue.get() # Added
if not (image is None):
result, color_image_src, grey_image, stats = t.run(image)
if (result > 0):
# Ajout filtrage 1 image sur 2 pour ne pas surcharger le nn
if (cpt >= 1):
cpt = 0
else:
imagennqueue.put([time.time(), image])
cpt += 1
print("Exiting process motion")
def __parseMotion(self, fin):
fin.readline()
blockChainNum = int(re.match(r'BlockNum: (\d+)', fin.readline().strip('\n')).group(1))
hashCode = re.match(r'Hash: ([\w\d]+)', fin.readline().strip('\n')).group(1)
innerRadius = int(re.match(r'Inner radius: (\d+)', fin.readline().strip('\n')).group(1))
outerRadius = int(re.match(r'Outer radius: (\d+)', fin.readline().strip('\n')).group(1))
motionAngle = []
clockwiseDirect = []
pattern = re.compile(r'Motion: Angle: (\d+) Clockwise: (\w+)')
while True:
tmpStr = fin.readline()
if tmpStr == '\n':
break
else:
tmpStr = tmpStr.strip('\n')
result = pattern.match(tmpStr)
motionAngle.append(int(result.group(1)))
if result.group(2)=='true':
clockwiseDirect.append(True)
else:
clockwiseDirect.append(False)
# WARNING: droneAngle here is hardcoded, and is not read from the metadata! This will be fixed in the parser
# for the new metadata format. Hard to fix in the old format.
return motion(blockChainNum, hashCode, innerRadius, outerRadius, motionAngle, clockwiseDirect, droneAngle=50)
def __parseMotion(self, fin):
# extract init info
blockChainNum = re.match(r'[\d]+ BlockNum ([\w-]+)',
fin.readline().strip('\n')).group(1)
hashCode = re.match(r'[\d]+ Hash ([\w-]+)',
fin.readline().strip('\n')).group(1)
innerRadius = int(
re.match(r'[\d]+ Inner radius: (\d+)',
fin.readline().strip('\n')).group(1))
outerRadius = int(
re.match(r'[\d]+ Outer radius: (\d+)',
fin.readline().strip('\n')).group(1))
startingPoint = re.match(r'[\d]+ STARTING_POINT ([\w]+)',
fin.readline().strip('\n')).group(1)
centerPoint_pattern = re.match(
r'[\d]+ CENTER_POINT {"longitude":([-\d.]+),"latitude":([-\d.]+)}',
fin.readline().strip('\n'))
centerPoint = float(centerPoint_pattern.group(1)), float(
centerPoint_pattern.group(2))
droneAngle = abs(
int(
re.match(r'[\d]+ RESET_GIMBAL ([-\d]+)',
fin.readline().strip('\n')).group(1)))
startMotionAngle = int(
re.match(r'[\d]+ Motion: Angle: (\d+)',
fin.readline().strip('\n')).group(1))
yaw = int(
re.match(r'[\d]+ YAW ([-\d]+)',
fin.readline().strip('\n')).group(1))
assert re.match(
r'[\d]+ (\w+)',
fin.readline().strip('\n')).group(1) == 'START_RECORDING'
motionAngle_pattern = re.compile(r'[\d]+ Motion: Angle: (\d+) (\w+)')
motionAngle = [startMotionAngle]
clockwiseDirect = [False]
while True:
tmpStr = fin.readline().strip('\n')
cmd = re.match('[\d]+ ([:\w]+).*', tmpStr).group(1)
if cmd == 'IN_AND_OUT':
pass
elif cmd == 'Motion:':
result = motionAngle_pattern.match(tmpStr)
motionAngle.append(int(result.group(1)))
if result.group(2) == 'true':
clockwiseDirect.append(True)
else:
clockwiseDirect.append(False)
elif cmd == 'STOP_RECORDING':
break
# print(re.match(r'[\d]+ ([\w ]+)', fin.readline().strip('\n')).group(1) == 'TIMELINE STARTED')
return motion(blockChainNum, hashCode, innerRadius, outerRadius,
motionAngle, clockwiseDirect, droneAngle)
from picamera import PiCamera
from time import sleep
from motion import motion
#from predict import Predict
#from servo import Servo
camera = PiCamera()
motion_obj = motion()
rows = 4
cols = 2
j = 0
i = 0
while (j < cols):
i = 0
while (i < rows):
if (j % 2 == 0):
motion_obj.forward()
motion_obj.stop()
else:
motion_obj.reverse()
motion_obj.stop()
camera.start_preview()
sleep(1)
camera.capture('captured_img/' + str(j + 1) + '_' + str(i + 1) +
'.jpg')
camera.stop_preview()
sleep(1)
i += 1
def seif():
filename1 = 'z.mat'
filename2 = 'aa3_dr.mat'
filename3 = 'aa3_lsr2.mat'
filename4 = 'c_5000.mat'
include_dir = './data/'
z_contents = sio.loadmat(include_dir + filename1)
z = z_contents['z']
z_contents = sio.loadmat(include_dir + filename2)
speed = z_contents['speed'].ravel()
steering = z_contents['steering'].ravel()
time = z_contents['time'].ravel()
z_contents = sio.loadmat(include_dir + filename3)
timeLsr = z_contents['TLsr'].ravel()
L = size(timeLsr, 0)
z_contents = sio.loadmat(include_dir + filename4)
corresp = z_contents['corresp']
Lc = size(corresp, 0)
del z_contents
dt = 25e-3
G = csr_matrix((1, 1), dtype=bool)
Q = diag([5.0, 0.02])
m = zeros(3)
xi = zeros(3)
omega = 10e4 * eye(3)
m0 = zeros(0)
#max active landmarks
N = 20
plt.ion()
fig, ax = plt.subplots(1, 1)
ax.set_aspect('equal')
ax.set_xlim(-100, 300)
ax.set_ylim(-50, 350)
ax.hold(True)
plt.show(False)
plt.draw()
#background = fig.canvas.copy_from_bbox(ax.bbox)
line1 = ax.plot(0, 0, 'b-')[0]
line2 = ax.plot(-1000, 1000, 'ro', markersize=2)[0]
poses = zeros([3, 5000])
stindex = 0
j = searchsorted(timeLsr, time[stindex])
timechr.sleep(3)
t1 = timechr.time()
for i in range(stindex, time.shape[0]):
#for i in range(stindex, 10000):
t3 = timechr.time()
if i > stindex and i % 5000 == 0:
save(include_dir + 'poses_' + str(i), poses)
save(include_dir + 'landmarks_' + str(i), m)
save(include_dir + 'xi_' + str(i), xi)
save(include_dir + 'omega_' + str(i), omega)
save(include_dir + 'm0_' + str(i), m0)
save_npz(include_dir + 'G_', G)
xi, omega, m, G = motion(speed[i], steering[i], dt, m0, xi, omega, m,
G)
m = estimate(m0, xi, omega, m)
while j < L and timeLsr[j] < time[i] + dt * 1000:
z1 = z[0:2, nonzero(z[0, :, j]), j].transpose((0, 2, 1))[:, :, 0]
if z1.size > 0:
co = correspondence(z1, m0, omega, m, G)
xi, omega, m, G = measurement(z1, co, xi, omega, m, G)
j += 1
if co.size > 0:
n = (xi.size - 3) // 2
m0a = setdiff1d(m0, co)
#returns float when m0 empty
tq = hstack((m0a, unique(co))).astype(int)
tq = tq[max(0, tq.size - N):]
m1 = setdiff1d(m0, tq)
m1 = union1d(m1, setdiff1d(co, tq))
m0 = tq
if m1.size > 0:
xi, omega, G = sparsification(m0, m1, xi, omega, m, G)
print("\x1b[2J\x1b[H")
t2 = timechr.time()
print('iter: ' + str(i))
print('iter time: {0:.5f}'.format(t2 - t3))
print('avg: {0:.5f}'.format((t2 - t1) / (i + 1)))
poses[:, i % 5000] = m[0:3]
if i % 5000 == 4999:
line2.set_data(m[3::2], m[4::2])
line1.set_xdata(hstack((line1.get_xdata(), poses[0, :])))
line1.set_ydata(hstack((line1.get_ydata(), poses[1, :])))
#fig.canvas.restore_region(background)
#ax.draw_artist(line1)
#ax.draw_artist(line2)
fig.canvas.blit(ax.bbox)
fig.canvas.draw()
if name == 'nt':
CST_LSS_Port = "COM3"
# for mac and linux(here, os.name is 'posix')
else:
CST_LSS_Port = "/dev/ttyUSB0"
# Constants
#CST_LSS_Port = "/dev/ttyUSB0" # For Linux/Unix platforms
# CST_LSS_Port = "COM1" # For windows platforms
CST_LSS_Baud = lssc.LSS_DefaultBaud
# Create and open a serial port
lss.initBus(CST_LSS_Port, CST_LSS_Baud)
# Create LSS objects
mt = motion(lss)
# define our clear function
def clear():
# for windows
if name == 'nt':
_ = system('cls')
# for mac and linux(here, os.name is 'posix')
else:
_ = system('clear')
# Setup GPIO input reading
def mainListen():
while True:
x=listen().lower()
if "news" in x:
if "india" in x:
playNewsIndia()
doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3"
os.system("play "+doyou+" tempo 1.1")
# speak("Do you want me to do anything else?")
else:
playNews()
doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3"
os.system("play "+doyou+" tempo 1.1")
# speak("Do you want me to do anything else?")
elif "exit" in x or "no" in x or "no exit" in x:
exit="Sounds/Alright-Thank-You-for-using-t1619638216.mp3"
os.system("play "+exit+" tempo 1.1")
# speak("Alright, Thank You for using this Voice Assistant, have a nice day.")
break
elif "songs" in x or "play songs" in x or "play some songs" in x or "music" in x:
music=Music()
music.main()
doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3"
os.system("play "+doyou+" tempo 1.1")
elif "email" in x or "send email" in x or "send" in x or "send an email" in x:
alri="Sounds/Okay1620499210.mp3"
os.system("play "+alri+" tempo 1.1")
emailer()
doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3"
os.system("play "+doyou+" tempo 1.1")
elif "radio" in x or "play radio" in x:
radi="Sounds/OkayPlaying-RadioEnjoy1621184618.mp3"
os.system("play "+radi+" tempo 1.1")
os.system("python3 radio.py")
it_was_so_good="Sounds/It-was-so-good1621184764.mp3"
os.system("play "+it_was_so_good+" tempo 1.1")
doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3"
os.system("play "+doyou+" tempo 1.1")
elif "time" in x:
if "date" in x:
getDate_Time()
doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3"
os.system("play "+doyou+" tempo 1.1")
else:
getTime()
doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3"
os.system("play "+doyou+" tempo 1.1")
elif "date" in x:
getDate()
doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3"
os.system("play "+doyou+" tempo 1.1")
elif "age" in x or "old" in x:
age()
doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3"
os.system("play "+doyou+" tempo 1.1")
elif "describe yourself" in x:
alina="Sounds/Myself-Eleena-Alexas-little-1619638384.mp3"
os.system("play "+alina+" tempo 1.1")
# speak("Myself Alina, Alexa's little sister.")
hm="greetings/How-can-I-help-you1619639087.mp3"
os.system("play "+hm+" tempo 1.1")
# speak("How may I help you?")
elif "where" in x or "where am i" in x or "location" in x or "locate me" in x:
# speak("You are currently in "+getLocation()[0]+getLocation()[1])
# located="Sounds/You-are-currently-in1619697928.mp3"
# os.system("play "+located+" tempo 1.1")
# speak(getLocation()[0]+getLocation()[1])
located="Sounds/You-are-currently-in-Valsad-Gu1619698254.mp3"
os.system("play "+located+" tempo 1.1")
doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3"
os.system("play "+doyou+" tempo 1.1")
elif "monitor" in x or "home" in x or "monitor my home" in x or "house" in x:
mt="Sounds/AlrightI-am-now-monitoring-you1620383303.mp3"
os.system("play "+mt+" tempo 1.1")
motion()
break
elif "vaccinations" in x or "vaccines" in x or "vaccinations have started" in x or "have vaccinations" in x or "vaccination" in x:
ok_let_me_check="Sounds/OKLet-me-check1620390574.mp3"
os.system("play "+ok_let_me_check+" tempo 1.1")
time.sleep(0.5)
vaccination()
doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3"
os.system("play "+doyou+" tempo 1.1")
else:
icouldnot="Sounds/Sorry-I-could-not-do-that-at-1619638005.mp3"
os.system("play "+icouldnot+" tempo 1.1")
# speak("Sorry, I could not do that at this point of time, but I may get future updates")
doyou="Sounds/Do-you-want-me-to-do-anything-1619637731.mp3"
os.system("play "+doyou+" tempo 1.1")
# speak("Do you want me to do anything else?")
try:
video_path = dl.download(arg)
except Exception, e:
print_error(str(e))
if video_path is None:
print_error('Cannot download video from YouTube')
else:
video_path = arg
try:
with open(log_file, 'a') as f, stderr_redirector(f):
if verbose: print_info('Normalizing video...')
audio, video = dl.normalize(video_path, remove=False)
if verbose: print_info('Extracting motion features...')
motion_f = mo.motion(video)
if verbose: print_info('Extracting audio features...')
audio_f = au.extractAudioFeatures(audio)
if verbose: print_info('Extracting image features...')
image_f = im.extract_image_feature(video)
features = [np.array(list(motion_f) + list(audio_f) + list(image_f))]
label = clf.predict(features)[0]
if verbose: print_info('Removing temporary files...')
os.unlink(video)
os.unlink(audio)
return label
if __name__ == '__main__':
try:
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
#buzzer = Buzzer()
led = LED()
sp = SensorProcess(led)
database = Database()
buttons = Buttons(database, sp, led)
buttons_thread = threading.Thread(target=buttons.start,)
buttons_thread.start()
receive = Rec(database, sp);
receive_thread = threading.Thread(target=receive.start,)
receive_thread.start()
motion = motion()
motion_thread = threading.Thread(target=motion.start,)
motion_thread.start()
while True:
pass
except KeyboardInterrupt:
GPIO.cleanup()
database.cleanup()
@author: Arnov
"""
import video
from mosaicBuilder import mosaic
from motion import motion
import imutils
import time
import cv2
leftVideo = video.video(src=0).start() #The webcam attached to the laptop
rightVideo = video.video(src=1).start() #The inbuilt camera of the laptop
panImage = mosaic()
motion = motion(area=500)
time.sleep(2.0)
while True:
l = leftVideo.read()
r = rightVideo.read()
left = imutils.resize(l, width=400)
right = imutils.resize(r, width=400)
result = panImage.pack(left, right)
if result is None:
print("Something went wrong, no results :(")
break
from configuration import configuration as config
from log import logging
from signal import pause
from time import sleep
from audiomoth import audiomoth
from motion import motion
from camera import camera
from diskio import diskio
from threading import Thread, ThreadError
m = motion()
c = camera()
am = audiomoth(config.am_swdio_pin, config.am_rst_pin, config.am_swo_pin,
config.am_swclk_pin)
d = diskio()
def on_motion():
logging.info("on_motion")
print("Recording starting")
x = Thread(target=am.unmountMoth, args=())
y = Thread(target=c.click,
args=(config.photo_count_on_motion,
config.photo_count_delay_sec))
x.start()
y.start()
x.join()
y.join()
print("Recording started")
sleep(30)
def extract_feature(data, **kwargs):
try:
num = kwargs.pop('num_video', NUM_VIDEO)
count = 0
file = open(data, "r")
data_file = data[5:-4]
audio_file = open("csv/%s_audio.csv" % data_file, "w")
image_file = open("csv/%s_image.csv" % data_file, "w")
image_file.close(
) # Close immediately to clear contents in file; file will be write in subprocess
motion_file = open("csv/%s_motion.csv" % data_file, "w")
contents = file.readlines()
for content in contents:
try:
content = content[:-1]
print_process('\r#%s %-30s:\tDownloading' % (count, content))
path = dl.download(content)
if path is None:
print '#%s %-30s:\tDOWNLOAD FAIL' % (count, content)
continue
print_process('\r#%s %-30s:\tNormalizing' % (count, content))
audio_path, video_path = dl.normalize(path)
print_process('\r#%s %-30s:\tExtracting audio feature ' %
(count, content))
audio_features = au.extractAudioFeatures(audio_path)
print_process('\r#%s %-30s:\tExtracting motion feature ' %
(count, content))
motion_features = mo.motion(video_path)
print_process('\r#%s %-30s:\tExtracting image feature ' %
(count, content))
print_process('\r')
# No longer use extract_image_feature
# image_features = im.extract_image_feature(video_path)
# Use subprocess to extracting image features
command = "python src/process_image_csv.py videos/%s.avi csv/%s_image.csv" % (
content, data_file)
process_image_log = open("process_image.log", 'w')
subprocess.call(command,
shell=True,
stdout=process_image_log,
stderr=subprocess.STDOUT)
print_process('\r#%s %-30s:\tExporting csv file' %
(count, content))
export_csv(audio_features, content, audio_file)
export_csv(motion_features, content, motion_file)
# export_csv(image_features,content,image_file)
print_process('\r#%s %-30s:\tRemoving files' %
(count, content))
os.remove(video_path)
os.remove(audio_path)
except Exception:
print '#%s %-30s:\tEXPORT FAIL' % (content, count)
else:
print '\r#%s %-30s:\tSUCCESS ' % (
count, content)
count += 1
if str(count) == num:
break
finally:
print_process('\rClosing file')
file.close()
# image_file.close()
motion_file.close()
audio_file.close()
data_file_number = '2'
f = 100
T = 1 / f
# 1. read data
measure_data = readData.readData(file_dir, data_file_number)
print(measure_data.frames)
# 2. calculate the norm of acc and gyro and detect the contact
contact_flag = Cal_Acc_Gyro.contac_detection(measure_data.acc_data,
measure_data.gyro_data,
measure_data.frames)
# 3. calculate the attitude and position
calculate_data = motion.motion(measure_data.acc_data, measure_data.gyro_data,
contact_flag)
# 4. plot the signal of vel and position
n = measure_data.frames
t = np.linspace(0, n * T, n + 1)
plt.figure(1)
ax1 = plt.subplot(2, 1, 1)
#ax1.plot(t,calculate_data.vel.x_data,t,calculate_data.vel.y_data,t,calculate_data.vel.z_data,label='z')
ax1.plot(t, calculate_data.vel.x_data, label='x')
ax1.plot(t, calculate_data.vel.y_data, label='y')
ax1.plot(t, calculate_data.vel.z_data, label='z')
ax1.legend()
ax2 = plt.subplot(2, 1, 2)
#ax2.plot(t,calculate_data.ss.x_data,t,calculate_data.ss.y_data,t,calculate_data.ss.z_data)
ax2.plot(t, calculate_data.ss.x_data, label='x')
ax2.plot(t, calculate_data.ss.y_data, label='y')
def gps(record, tripid, xFlat, yFlat): # communicate with gps module
# dictionary to store values from gps, loaded with default values
gpsvalues = {
'speed': 10,
'time': '2019-09-23T22:53:41.000Z',
'lat': '40.1',
'lng': '80.1',
'alt': 0,
'climb': 0
}
distance = 0
try:
gpsd.connect() # connect to gps module
gpsData = gpsd.get_current() # get data packet from gps
if gpsData.mode > 1: # if gps mode is greater than 1 gps has a fix
gpsvalues['time'] = gpsData.time # time with date
gpsvalues['speed'] = gpsData.hspeed # horizontal speed
gpsvalues['lat'] = gpsData.lat # latitude
gpsvalues['lng'] = gpsData.lon # longitude
gpsvalues['alt'] = gpsData.alt # altitude
try:
xrotate, yrotate = motion.motion(
) # motion function to for current angle
gpsvalues['climb'] = xrotate
if xrotate > xFlat: # to determine if travelling uphill/downhill/flat
gpsvalues['climb'] = 1
elif xrotate < xFlat:
gpsvalues['climb'] = -1
else:
gpsvalues['climb'] = 0
except:
logging.error('Motion Read Error')
if gpsvalues['speed'] > 0.25: # filter speed
gpsvalues['speed'] = round(
gpsvalues['speed'] * 3.6,
1) # convert speed m/s to km/h, round 1 decimal
distance = gpsvalues[
'speed'] / 3600 # calculate distance travelled in last second
else:
gpsvalues['speed'] = 0.0
if record is True: # if recording trip save values
interface.entryid += 1 # increment entry_id, entry_id must be unique for entry into database
interface.gpslist.append(
(interface.entryid, gpsvalues['time'],
float(gpsvalues['speed']), float(gpsvalues['lat']),
float(gpsvalues['lng']), gpsvalues['alt'],
gpsvalues['climb'], tripid))
except:
logging.error('GPS Module Error')
pass
return gpsvalues, distance
xes = np.linspace(xMin, xMax, xSteps)
print(xes)
logging.info(xes)
initialY = 80
yMin = -24
yMax = 16
ySteps = 21
yes = np.linspace(yMin, yMax, ySteps)
print(yes)
logging.info(yes)
if options.nevents < 1:
sys.exit("Arguments to -n must be positive")
motors = motion.motion(port='COM3')
time.sleep(1)
motors.moveFor(-500, -500)
motors.moveTo(initialX, initialY)
motors.setHome()
f = open(options.outfile, "w")
f.write('x\ty\trates\n')
try:
for x in xes:
for y in yes:
logging.info(f'Moving to {x},{y}')
print((f'Moving to {x},{y}'))
motors.moveTo(x=x, y=y)
def mcl(state, u, z):
# Variavel de Normalizacao
wsum = 0.0
# Variaveis de medida de confianca
wavg = 0.0
wfast = param.wfast
wslow = param.wslow
# transforma u nas varias componentes separadas
u_x = [u[0], u[3]]
u_y = [u[1], u[4]]
u_t = [u[2], u[5]]
# em cada ciclo variavel reiniciada
x = []
y = []
t = []
w = []
# Prediction e Update
for i in range(param.M):
# funcao com o motion model
xaux, yaux, taux = motion(u_x, u_y, u_t, state.x[i], state.y[i],
state.t[i])
# Inicia Waux
waux = 1.0
waux = waux * maps.mapa(xaux, yaux)
# funcao com o perception model
waux = perception(z, [xaux, yaux, taux]) * waux
# adicionar as novas particulas
x[len(x):] = [xaux]
y[len(y):] = [yaux]
t[len(t):] = [taux]
w[len(w):] = [waux * state.w[i]]
wsum = wsum + waux * state.w[i]
wavg = wavg + waux * state.w[i] / param.M
# normalizacao dos pesos
for i in range(param.M):
if wsum > 0:
w[i] = float(w[i]) / wsum
#atribuicao ao estado dos novos valores
state.x = x
state.y = y
state.t = t
state.w = w
# Determinacao de confianca das particulas
wslow = wslow + param.alphaslow * (wavg - wslow)
wfast = wfast + param.alphafast * (wavg - wfast)
# Probabilidade de falha ou kidnap
kidnap_prob = max([0.0, 1 - wfast / wslow])
# Calculo da pose media
mean_pose = state.rosbustmean()
print kidnap_prob, wfast, wslow, wavg
# Resampling
neff = resample.eff_particles(state.w)
# Verificar numero efectivo de particulas
if neff < 1 * param.M / 3:
state = resample.kidnap_resample(state, kidnap_prob, z)
# Guarda wfast
param.wfast = wfast
param.wslow = wslow
return state, mean_pose