-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathDemoCentral.py
155 lines (98 loc) · 5.26 KB
/
DemoCentral.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
# -*- coding: utf-8 -*-
#!/usr/bin/env python
"""
@author: Quentin
"""
import socket
import SQLTools
import SELECTTools
import InsertTools
import time
import subprocess
import rospy
import PubForHeron
from geometry_msgs.msg import Twist
from std_msgs.msg import Bool
from heron.msg import Motion
HERON_ID='Heron01'
rospy.init_node('centralheron', anonymous=True)
velocity_publisher = rospy.Publisher('/'+HERON_ID+'/cmd_vel', Twist, queue_size=10)
zone_publisher = rospy.Publisher('/'+HERON_ID+'/move_to', Motion, queue_size=10)
rospy.sleep(1)
import PubForHeron
import rospy
from std_msgs.msg import String
HERON_ID='Heron01'
ListFunction=[['DICTIONNARY','DICTIONNARY',''],
['TEMPERATURE','TEMPERATURE(nZone)','Return in Comm the TEMPERATURE of the zone : BCPU-therm / MCPU-therm / GPU-therm / PLL-therm / Tboard_tegra / Tdiode_tegra / PMIC-Die / thermal-fan-est'],
['CIRCLE','CIRCLE',''],['ZONE','ZONE(ZoneName)','Send Robot to the zone named "BANC", "BASE", "STOCK"']]
def RecovPass():#Récupère la phrase de passe à l'intérieur du fichier et stock la chaîne de caractère dans une variable (retournée)
f= open("../Guess.txt","r")
data=f.read()
f.close()
return(data)
def InitConnection(name):
Server = '192.168.0.105' #IP du Server
passwd=RecovPass() #Récupération de la Phrase de passe
port = 22322
s = socket.socket(socket.AF_INET,socket.SOCK_STREAM)#Préparation de la socket
s.connect((Server,port))#Connection
s.send(name.encode('ascii'))#On envoi la demande
data = int(s.recv(1024).decode("ascii"))#On récupère le mot de passe demandé
s.send(passwd[data:data+24].encode('ascii'))#Et on l'envoi
data2 = s.recv(1024).decode('ascii')#On récupère le message confirmant l'autorisation de communication
#print(data2)
s.close()#Fermeture de la connection
return data2
ID=InitConnection("central")
print("----------------------")
#InsertTools.InsertCOMMANDS('fortheTest',ID,'TEMPERATURE(4)','fortheTest',com=None)
now=time.time()
itDemo=0
while(time.time()-now<300):
commands=SELECTTools.CommandsFor(ID)
for elt in commands:
LineOrder,OrderID,Function,Target,Status,Source,ComOrder=elt[0],elt[1],elt[2],elt[3],elt[4],elt[5],elt[6]
if(Status=='waiting'):
if(Status=='waiting'):
if(Function=='DEMOSPRINT'):
InsertTools.ChangeCOMMANDS("accepted",LineOrder)
itDemo+=1
print("---------PHASE 1-----------")
ref1,ref2="DemoPhase1-"+str(itDemo)+"-Heron","DemoPhase1-"+str(itDemo)+"-Niryo"
InsertTools.InsertCOMMANDS(ref1,"heron-1","ZONE(STOCK)","Central",com=None)
InsertTools.InsertCOMMANDS(ref2,"niryo-1","TAKE(0,0)","Central",com=None)
print("avant")
status1=SELECTTools.SELECT("COMMANDS","OrderID='"+ref1+"'")[0][4]
status2=SELECTTools.SELECT("COMMANDS","OrderID='"+ref2+"'")[0][4]
print("after")
while(status1!='done' or status2!='done'):
status1=SELECTTools.SELECT("COMMANDS","OrderID='"+ref1+"'")[0][4]
status2=SELECTTools.SELECT("COMMANDS","OrderID='"+ref2+"'")[0][4]
print("status1 : ",status1)
print("status2 : ",status2)
print("---------PHASE 2-----------")
ref1="DemoPhase1b-"+str(itDemo)+"-Niryo"
InsertTools.InsertCOMMANDS(ref1,"niryo-1","PREPARE(1)","Central",com=None)
status1=SELECTTools.SELECT("COMMANDS","OrderID='"+ref1+"'")[0][4]
while(status1!='done'):
status1=SELECTTools.SELECT("COMMANDS","OrderID='"+ref1+"'")[0][4]
print("---------PHASE 3-----------")
ref1="DemoPhase3-"+str(itDemo)+"-Heron"
InsertTools.InsertCOMMANDS(ref1,"heron-1","ZONE(BANC)","Central",com=None)
status1=SELECTTools.SELECT("COMMANDS","OrderID='"+ref1+"'")[0][4]
while(status1!='done'):
status1=SELECTTools.SELECT("COMMANDS","OrderID='"+ref1+"'")[0][4]
print("---------PHASE 4-----------")
ref1="DemoPhase1b-"+str(itDemo)+"-Niryo"
InsertTools.InsertCOMMANDS(ref1,"niryo-2","PREPARE(1)","Central",com=None)
status1=SELECTTools.SELECT("COMMANDS","OrderID='"+ref1+"'")[0][4]
while(status1!='done'):
status1=SELECTTools.SELECT("COMMANDS","OrderID='"+ref1+"'")[0][4]
print("---------PHASE 5-----------")
ref1="DemoPhase5-"+str(itDemo)+"-Heron"
InsertTools.InsertCOMMANDS(ref1,"heron-1","ZONE(BASE)","Central",com=None)
status1=SELECTTools.SELECT("COMMANDS","OrderID='"+ref1+"'")[0][4]
while(status1!='done'):
status1=SELECTTools.SELECT("COMMANDS","OrderID='"+ref1+"'")[0][4]
InsertTools.ChangeCOMMANDS("done",LineOrder)