Image Projection & Walking People Tracking

print(__doc__)


#todo:
#todo : only take moving centroids in consideration 


# Cluster centroid detection
import numpy as np
from sklearn.cluster import KMeans

# Lidar 
from sweeppy import Sweep
import itertools
import sys
from math import cos, sin, degrees, radians 

# Servo & Projector Switch
#from Adafruit_PWM_Servo_Driver import PWM
import Adafruit_PCA9685
import time





# ===========================================================================
# Cluster centroids detection, trajectory, return the coordinates for the gibal
# ===========================================================================

def get_cluster_centroid (dataXY,plot_position,titre):
    result = []
    # kmeans = KMeans(n_clusters=10, random_state=0).fit(dataXY)
    kmeans = KMeans().fit(dataXY) 
    centroids = kmeans.cluster_centers_
    labels =  kmeans.labels_
    n_points_by_cluster = {i: np.where(kmeans.labels_ == i)[0] for i in range(kmeans.n_clusters)}
    #plt.subplot(plot_position)
    dataX, dataY = zip(*dataXY)
    #plt.scatter(dataX, dataY, c=labels.astype(np.float))
    #plt.title(titre)
    
    
    # for each centroids of clusters found we take only >=3 points and < 7 
    for index, (key, value) in enumerate(centroids):
        if len(n_points_by_cluster[index]) >= 3 and len(n_points_by_cluster[index]) < 30:
            #plt.scatter(centroids[index][0],centroids[index][1],color='red')  
            #plt.scatter(centroids[index][0]+30,centroids[index][1],color='yellow')
            result.append(centroids[index])
    return result





# ===========================================================================
# LIDAR data
# ===========================================================================
# on recpere la data du scan 
with Sweep('/dev/ttyUSB0') as sweep:    
    sweep.set_motor_speed(10) 
    sweep.set_sample_rate(1000) 
    speed = sweep.get_motor_speed()
    rate  = sweep.get_sample_rate()
    ready = sweep.get_motor_ready()
    print(speed)
    print(rate)
    print(ready)
    sweep.start_scanning()
    dataXY_temp = []
    dataXY = []
    centroids_history = []
    #for scan in sweep.get_scans():
    for scan in itertools.islice(sweep.get_scans(), 3):
        #print('{}\n'.format(scan))
        dataXY = []
        for data in scan[0]:
            X= data[1] * cos(radians(data[0])/1000)
            Y= data[1] * sin(radians(data[0])/1000)
            #print (X)
            #print (Y)
            dataXY_temp.append(X)
            dataXY_temp.append(Y)
            dataXY.append(dataXY_temp)
            dataXY_temp = []
        #print(dataXY)
        #print(' ')
        centroids_t0 = get_cluster_centroid(dataXY,221,'t0')
        print (centroids_t0)
        print(' ')
        # append  et pop 
        centroids_history.append(centroids_t0)
        #si centroids_history > 10 => centroids_history.pop(0)
        print (centroids_history)
        
        if (x-xc)**2+(y-yc)**2 < r**2:
            return 1
        else:
            return 0






# ===========================================================================
# Gimbal   /  https://github.com/adafruit/Adafruit_Python_PCA9685/blob/master/examples/simpletest.py
# ===========================================================================

# Initialise the PWM device using the default address
#pwm = PWM(0x40)
pwm = Adafruit_PCA9685.PCA9685()

# Note if you'd like more debug output you can instead run:
#pwm = PWM(0x40, debug=True)

servoMin = 200  # Min pulse length out of 4096
servoMax = 300  # Max pulse length out of 4096

def setServoPulse(channel, pulse):
    pulseLength = 1000000                   # 1,000,000 us per second
    pulseLength /= 40                       # 60 Hz
    #print "%d us per period" % pulseLength
    pulseLength /= 4096                     # 12 bits of resolution
    #print "%d us per bit" % pulseLength
    pulse *= 1000
    pulse /= pulseLength
    pwm.set_pwm(channel, 0, pulse)

pwm.set_pwm_freq(40)                        # Set frequency to 60 Hz
while (True):
    # Change speed of continuous servo on channel O
    pwm.set_pwm(0, 0, servoMin)
    pwm.set_pwm(1, 0, servoMin)
    pwm.set_pwm(2, 0, servoMin)
    pwm.set_pwm(3, 0, 0)
    time.sleep(1)
    pwm.set_pwm(0, 0, servoMax)
    pwm.set_pwm(1, 0, servoMax)
    pwm.set_pwm(2, 0, servoMax)
    pwm.set_pwm(3, 0, 300)
    time.sleep(1)

dataXY = [[	187.997248275713	,	1.01717292530373	],
[	185.641079755076	,	11.5494375347605	],
[	183.667855166122	,	22.161204359795	],
[	154.371020487541	,	28.6109775022672	],
[	152.500704960682	,	37.3166850952092	],
[	153.649489504421	,	48.0919366945302	],
[	114.176772396524	,	42.9844698130887	],
[	94.1827180243109	,	41.6967100087423	],
[	87.2174676641165	,	44.6890739919591	],
[	84.5436450647286	,	49.5618006045906	],
[	73.7843439573987	,	49.7681684088965	],
[	70.8441782255814	,	53.8711649358176	],
[	66.5352134279979	,	57.5939699455661	],
[	60.6262881781054	,	59.5772874654873	],
[	53.3954194663634	,	59.5728896395928	],
[	67.4084549667496	,	84.3806861728186	],
[	0.573576436351046	,	0.819152044288992	],
[	0.52621392365187	,	0.850352224995563	],
[	0.476238203667939	,	0.879316310190556	],
[	0.419610513115511	,	0.907704256508108	],
[	0.361461843374802	,	0.932386902409129	],
[	0.308020881064551	,	0.951379596600756	],
[	0.253589120650365	,	0.967312027159683	],
[	0.19183684814863	,	0.981426830534197	],
[	0.135888488485093	,	0.990724138545759	],
[	0.0730641317999801	,	0.997327244511206	],
[	0.0228618212283992	,	0.999738634409074	],
[	-0.927042237544253	,	22.9813096382649	],
[	-0.0968880762377993	,	0.995295283161203	],
[	-0.159708897477666	,	0.987164154569273	],
[	-0.215417120660776	,	0.976522126798067	],
[	-0.276643835391984	,	0.96097252215639	],
[	-0.331831864834764	,	0.943338546588807	],
[	-0.395705795671223	,	0.918377331641088	],
[	-0.447134741355167	,	0.89446661372756	],
[	-0.502869094560208	,	0.86436258233232	],
[	-55.1936985312058	,	83.3885822067168	],
[	-21.0927857758671	,	27.9301698565078	],
[	-32.3927017282938	,	38.08822488305	],
[	-37.7211700839198	,	38.6408246224119	],
[	-34.7239902545554	,	31.6740351202928	],
[	-35.0652865529653	,	28.2032919879654	],
[	-39.8916603994596	,	28.4544448298361	],
[	-36.5176752189725	,	22.7037308960805	],
[	-37.7460805593282	,	20.5969270137247	],
[	-37.949509875331	,	17.9954077537063	],
[	-39.0100012206208	,	15.5634123754131	],
[	-36.9846225530418	,	12.3748815997986	],
[	-46.3925079005807	,	12.318084700738	],
[	-46.0367350228463	,	9.46673272234066	],
[	-36.6399185434381	,	5.14940473552242	],
[	-36.8729235924172	,	3.06390367795156	],
[	-58.9887280522401	,	1.15324012194445	],
[	-57.9447968517478	,	-2.52992446718948	],
[	-52.7333314672983	,	-5.30996726544003	],
[	-154.900770743559	,	-25.5881070626823	],
[	-194.184679318028	,	-43.5121858581533	],
[	-177.609223061397	,	-51.7683675957335	],
[	-180.044524798606	,	-63.7571101136503	],
[	-184.35910066384	,	-77.5352952043133	],
[	-167.988550073695	,	-82.1513666601944	],
[	-169.400005136399	,	-96.5848759371161	],
[	-174.562014151695	,	-113.102180417989	],
[	-169.482273353333	,	-125.685158308315	],
[	-168.823816206826	,	-142.616685844833	],
[	-161.919144051757	,	-153.333593154098	],
[	-161.197160255508	,	-170.99846644213	],
[	-157.742479991378	,	-190.069224247824	],
[	-152.059049938224	,	-205.946705076543	],
[	-147.384535133608	,	-228.608396178816	],
[	-134.68037973088	,	-237.466198258923	],
[	-138.66521226032	,	-285.06307882432	],
[	-121.509682300469	,	-290.620710045308	],
[	-143.445223689407	,	-406.428920969825	],
[	-120.422096273276	,	-425.279341996704	],
[	-92.4197151515699	,	-416.878395040212	],
[	-70.7746193777145	,	-453.510698056771	],
[	-38.061717265659	,	-440.358156139737	],
[	-9.99061587681071	,	-436.885783236765	],
[	14.5229018783059	,	-428.75410822642	],
[	39.1626927879364	,	-426.204509001955	],
[	23.1269051638283	,	-149.218451464768	],
[	30.8891835370348	,	-143.717981966125	],
[	36.3730202819199	,	-128.969001684789	],
[	137.710223906427	,	-391.485496834355	],
[	166.430493625763	,	-393.230073610202	],
[	189.741848103951	,	-384.758406117523	],
[	218.205076138697	,	-379.776967110047	],
[	221.348908425243	,	-339.160228710492	],
[	240.813983144473	,	-326.870961576726	],
[	276.179239148375	,	-333.487672730829	],
[	264.020467251534	,	-284.318822578248	],
[	252.319927034153	,	-242.558558747119	],
[	248.050552679322	,	-209.990293383963	],
[	237.993381994093	,	-179.34087689931	],
[	233.002938378875	,	-153.462147472301	],
[	224.164844535532	,	-129.734815968424	],
[	221.943721677302	,	-110.657057651233	],
[	216.93216313712	,	-92.9324302740964	],
[	212.451360598038	,	-77.0741161483062	],
[	207.307359023411	,	-60.6601920103998	],
[	205.54226606153	,	-47.6799419283537	],
[	203.210327021534	,	-33.7870240151658	],
[	198.833959654781	,	-21.5651683972295	],
[	192.807385167232	,	-8.62045387289923	]]


dataXY2 = [[	187.697634064782	,	10.658244061915	],
[	184.660654383236	,	22.2809946536317	],
[	155.536973269077	,	27.7893854968856	],
[	156.386073239935	,	38.2674286645139	],
[	155.877675221975	,	47.6565878698061	],
[	119.792023497993	,	45.0984601317652	],
[	98.7546946274328	,	43.720822145089	],
[	91.6673384633061	,	46.9691287874673	],
[	81.955574297441	,	48.044602626899	],
[	77.434717541194	,	51.5059658613981	],
[	71.640180228116	,	54.4764589238605	],
[	65.3363885606202	,	55.9209829121025	],
[	61.3395386272596	,	60.2781967297872	],
[	55.397747696352	,	61.8068730010775	],
[	0.619093949309834	,	0.785316930880745	],
[	0.573576436351046	,	0.819152044288992	],
[	0.52056264228976	,	0.853823480265273	],
[	2.35659886441059	,	4.4098119905795	],
[	0.414693242656239	,	0.909961270876543	],
[	0.35624880212219	,	0.93439113383342	],
[	0.302702598633306	,	0.953085062720345	],
[	0.241921895599668	,	0.970295726275996	],
[	0.185495114272864	,	0.982645186514897	],
[	0.122908668292287	,	0.992417986162391	],
[	0.0664480485987362	,	0.997789886117022	],
[	0.00331611950105911	,	0.999994501660611	],
[	-0.0533816897587604	,	0.99857418111951	],
[	-0.11649739434453	,	0.99319099729656	],
[	-0.172616794374807	,	0.98498905694417	],
[	-1.17231749534281	,	4.86062461933784	],
[	-0.289198876313672	,	0.957269037386517	],
[	-0.343003999423668	,	0.939333942950732	],
[	-0.395705795671223	,	0.918377331641088	],
[	-0.447134741355167	,	0.89446661372756	],
[	-0.502869094560208	,	0.86436258233232	],
[	-31.4604081627873	,	47.5314918578286	],
[	-28.9272490640463	,	38.3042329460679	],
[	-28.5055775208985	,	33.517637897084	],
[	-31.2596266706549	,	32.3702910152393	],
[	-36.7161254717843	,	33.9400372766471	],
[	-38.581229169386	,	31.8039110138882	],
[	-46.773787047514	,	34.2959596050878	],
[	-42.1132139601797	,	26.953241176974	],
[	-42.8563656498304	,	23.7556713920705	],
[	-36.0279295729035	,	17.3778102961767	],
[	-29.6447119163136	,	12.0495251109235	],
[	-28.3876449568808	,	9.70266014050176	],
[	-35.6335395835955	,	9.96247242125823	],
[	-27.3499682288921	,	5.99826957368462	],
[	-44.4385791574057	,	7.08609077496097	],
[	-58.6969203054644	,	5.97256616991032	],
[	-58.9569064665257	,	2.25459084920899	],
[	-59.981051356998	,	-1.50780572660025	],
[	-55.8124936659308	,	-4.57881543528891	],
[	-65.3070382197899	,	-9.53890763970886	],
[	-191.044500999785	,	-39.0768299346724	],
[	-179.501576717811	,	-48.735859034387	],
[	-181.589887070539	,	-59.2124388427712	],
[	-188.188818447304	,	-73.4095948184403	],
[	-172.282472416891	,	-80.1170999095898	],
[	-171.271805523518	,	-93.2253647496975	],
[	-170.949803854201	,	-107.611173036122	],
[	-171.923882999651	,	-124.041035364602	],
[	-173.156492329219	,	-140.519853274337	],
[	-164.172023833429	,	-149.437433698642	],
[	-162.030828312154	,	-167.436587030665	],
[	-152.745782076277	,	-177.270206345868	],
[	-159.320399119854	,	-210.506556725178	],
[	-71.8930466476214	,	-107.109242569084	],
[	-58.4377813372811	,	-100.20491860371	],
[	-47.4955450018206	,	-94.7637758058428	],
[	-46.4100742191361	,	-107.401606184332	],
[	-39.8741727243926	,	-111.058769800252	],
[	-36.637979466806	,	-123.687745797997	],
[	-96.4462908204473	,	-422.122154105867	],
[	-77.2061663114974	,	-455.503246841865	],
[	-46.6196946173734	,	-443.55676533425	],
[	-21.1583546779126	,	-440.492138439863	],
[	6.68085770510441	,	-434.948693687341	],
[	30.8244287241746	,	-426.888573979005	],
[	57.0419435057849	,	-419.136274594651	],
[	80.3305870535255	,	-413.265044231467	],
[	107.092230361543	,	-410.252671162773	],
[	135.449138755242	,	-401.782939919633	],
[	159.127478428576	,	-396.241650522712	],
[	183.828238337142	,	-385.403916417391	],
[	208.268136942095	,	-379.622685221351	],
[	227.601115425467	,	-358.918837980237	],
[	236.514743335268	,	-329.995115395747	],
[	274.266889820625	,	-344.06056610446	],
[	269.979696288426	,	-297.850572589696	],
[	257.93081652183	,	-253.99939741736	],
[	245.625700123061	,	-215.863418482743	],
[	238.981736260764	,	-184.639458767054	],
[	232.794880675278	,	-157.377074351327	],
[	230.177298156513	,	-137.267663392977	],
[	221.631701222871	,	-115.669308863917	],
[	218.439042775501	,	-96.9813620822218	],
[	215.166313203491	,	-81.2616617010579	],
[	209.36357530054	,	-64.2486835458519	],
[	204.900545600245	,	-50.3663222076206	],
[	200.544960502095	,	-37.3860778527663	],
[	200.519992191683	,	-24.4076367444127	],
[	195.608930921474	,	-12.3752229781182	]]




import numpy as np
import matplotlib.pyplot as plt
from sklearn.cluster import KMeans




# ===========================================================================
# Cluster centroids detection, trajectory, return the coordinates for the gibal
# ===========================================================================

def get_cluster_centroid (dataXY,plot_position,titre):
    result = []
    # kmeans = KMeans(n_clusters=10, random_state=0).fit(dataXY)
    kmeans = KMeans().fit(dataXY) 
    centroids = kmeans.cluster_centers_
    labels =  kmeans.labels_
    n_points_by_cluster = {i: np.where(kmeans.labels_ == i)[0] for i in range(kmeans.n_clusters)}
    plt.subplot(plot_position)
    dataX, dataY = zip(*dataXY)
    plt.scatter(dataX, dataY, c=labels.astype(np.float))
    plt.title(titre)
    
    # /// on parcourt les centroids des clusters trouves et on ne prends que ceux >=3 points et < 7 
    for index, (key, value) in enumerate(centroids):
        if len(n_points_by_cluster[index]) >= 3 and len(n_points_by_cluster[index]) < 30:
            plt.scatter(centroids[index][0],centroids[index][1],color='red')    	 
            plt.scatter(centroids[index][0]+30,centroids[index][1],color='yellow')
            result.append(centroids[index])
    return result


plt.figure(figsize=(12, 12))

# /// on recup les centroid en t0 et t1
centroids_t0 = get_cluster_centroid(dataXY,221,'t0')
print (centroids_t0)
#centroids_t1 = get_cluster_centroid(dataXY2,222,'t1')
#print (centroids_t1)

plt.show()

#sys.exit("Error message")

print(__doc__)

# Cluster centroid detection
import numpy as np
import matplotlib.pyplot as plt
from sklearn.cluster import KMeans

# Lidar 
from sweeppy import Sweep
import itertools
import sys
from math import cos, sin

# Servo & Projector Switch
#from Adafruit_PWM_Servo_Driver import PWM
import Adafruit_PCA9685
import time








# ===========================================================================
# LIDAR data
# ===========================================================================
# on recpere la data du scan 
with Sweep('/dev/ttyUSB0') as sweep:    
    speed = sweep.get_motor_speed()
    rate = sweep.get_sample_rate()
    print(speed)
    print(rate)
    sweep.start_scanning()
    dataXY_temp = []
    dataXY = []
    #for scan in sweep.get_scans():
    for scan in itertools.islice(sweep.get_scans(), 1):
        print('{}\n'.format(scan))
        dataXY = []
        for data in scan[0]:
            X= data[1] * cos(radians(data[0])/1000)
            Y= data[1] * sin(radians(data[0])/1000)
            dataXY_temp.append(X)
            dataXY_temp.append(Y)
            dataXY.append(dataXY_temp)
            dataXY_temp = []
        print(dataXY)
        print(' ')


#with Sweep('/dev/ttyUSB0') as sweep:
#    sweep.start_scanning()
#    for scan in sweep.get_scans():
#        print('{}\n'.format(scan))





# ===========================================================================
# Cluster centroids detection, trajectory, return the coordinates for the gibal
# ===========================================================================

def get_cluster_centroid (dataXY,plot_position,titre):
    result = []
    # kmeans = KMeans(n_clusters=10, random_state=0).fit(dataXY)
    kmeans = KMeans().fit(dataXY) 
    centroids = kmeans.cluster_centers_
    labels =  kmeans.labels_
    n_points_by_cluster = {i: np.where(kmeans.labels_ == i)[0] for i in range(kmeans.n_clusters)}
    plt.subplot(plot_position)
    dataX, dataY = zip(*dataXY)
    plt.scatter(dataX, dataY, c=labels.astype(np.float))
    plt.title(titre)
    
    # /// on parcourt les centroids des clusters trouves et on ne prends que ceux >=3 points et < 7 
    for index, (key, value) in enumerate(centroids):
        if len(n_points_by_cluster[index]) >= 3 and len(n_points_by_cluster[index]) < 30:
            plt.scatter(centroids[index][0],centroids[index][1],color='red')    	 
            plt.scatter(centroids[index][0]+30,centroids[index][1],color='yellow')
            result.append(centroids[index])
    return result


plt.figure(figsize=(12, 12))

# /// on recup les centroid en t0 et t1
centroids_t0 = get_cluster_centroid(dataXY,221,'t0')
print (centroids_t0)
#centroids_t1 = get_cluster_centroid(dataXY2,222,'t1')
#print (centroids_t1)

plt.show()

#sys.exit("Error message")









# ===========================================================================
# Gimbal   /  https://github.com/adafruit/Adafruit_Python_PCA9685/blob/master/examples/simpletest.py
# ===========================================================================

# Initialise the PWM device using the default address
#pwm = PWM(0x40)
pwm = Adafruit_PCA9685.PCA9685()

# Note if you'd like more debug output you can instead run:
#pwm = PWM(0x40, debug=True)

servoMin = 200  # Min pulse length out of 4096
servoMax = 300  # Max pulse length out of 4096

def setServoPulse(channel, pulse):
    pulseLength = 1000000                   # 1,000,000 us per second
    pulseLength /= 40                       # 60 Hz
    #print "%d us per period" % pulseLength
    pulseLength /= 4096                     # 12 bits of resolution
    #print "%d us per bit" % pulseLength
    pulse *= 1000
    pulse /= pulseLength
    pwm.set_pwm(channel, 0, pulse)

pwm.set_pwm_freq(40)                        # Set frequency to 60 Hz
while (True):
    # Change speed of continuous servo on channel O
    pwm.set_pwm(0, 0, servoMin)
    pwm.set_pwm(1, 0, servoMin)
    pwm.set_pwm(2, 0, servoMin)
    pwm.set_pwm(3, 0, 0)
    time.sleep(1)
    pwm.set_pwm(0, 0, servoMax)
    pwm.set_pwm(1, 0, servoMax)
    pwm.set_pwm(2, 0, servoMax)
    pwm.set_pwm(3, 0, 300)
    time.sleep(1)