class Tripod:
"""Classe principale per la gestione del tripode.
Per la gestione del tripode, sono necessarie tre distinte attivita':
- Abilitazione dei segnali visivi e acustici in base allo stato
- Esecuzione del programma canopen_server per la comunicazione con i motori
- Avvio del server TCP per la comunicazione con il programma di controllo remoto
Il controllo dei segnali visivi ed acustici avviene tramite bus i2c, mentre il loro
stato dipende da quanto comunicato tramite PIPE dal canopen_server.
Il dialogo con il programma di controllo dei motori avviene tramite stdin, stdout e PIPE.
"""
def __init__(self):
logging.info("Initializing ALMA_Tripod class")
self.ini = ConfigParser.ConfigParser()
self.ini.read('/opt/spinitalia/service/config.ini')
# Istanzio le classi
self.canopen = Canopen(self)
self.alarm = Alarm(self)
self.kinematic = Kinematic(self)
self.posTime = 0.01
self.almaPositionProtocol = None
self.almaPositionProtocolMulticast = None
self.last_sim = ""
self.last_sim_file = None
self.last_sim_time = 0.0
self.last_sim_timestamp = 0.0
# All'avvio il sistema si trova nello stato 99, Non autenticato
self.canStatus = 99
self.oldStatus = 98 # Forzo il primo update
self.simHash = ''
self.userLoggedIn = False
self.isAsyncError = False
# Inizializza le strutture
self.motorsAddress = []
self.motorPos = {'120': 0L, '121': 0L, '122': 0L, '119': 0L}
self.OpProgress = 0
self.isCentered = False
self.isImporting = False
self.isReadyToStart = False
self.currentLine = 0
self.mex_counter = 0L
self.almaControlProtocol = None
self.isReading = False
self.antenna_weight = 50
# Per evitare che vengano eseguiti comandi errati prima che il canopen abbia comunicato lo stato tramite la
# pipe ( si sblocca con CT0 )
self.init_done = False
# Annulla l'importazione
self.stopAnalysis = False
self.password = "******"
# Indirizzi dei motori
self.motor_address_list = ['119', '120', '121', '122']
# Calcolo della velocità
self.old_roll = 0.0
self.old_pitch = 0.0
self.old_yaw = 0.0
# Compila i cercatori
self.find_motor_position = re.compile(
'@M([^ ]*) S([^ ]*) @M([^ ]*) S([^ ]*) @M([^ ]*) S([^ ]*) @M([^ ]*) S([^ ]*) AS([^ ]*) T([^ ]*) C([^ ]*)'
)
# Per prima cosa aggiorno lo stato
self.update_output()
def cleanup(self):
logging.info("De-Initializing ALMA_Tripod class")
# Invia CT6, spegnimento
#reactor.callFromThread(self.canopen.sendCommand, 'CT6', 'local')
# Chiude lo streamer
if 'almaPositionProtocolMulticast' in vars():
if self.almaPositionProtocolMulticast.posSender:
self.almaPositionProtocolMulticast.posSender.stop()
# Chiude lo stream_reader
if 'receiver_thread' in vars():
self.receiver_thread.stop()
# Chiude il processo di comunicazione con i motori se attivo
if self.canopen:
self.canopen.transport.closeStdin()
# Chiude il reattore se attivo
if reactor.running:
logging.info("Stopping reactor")
reactor.stop()
# Solo per rimuovere l'interrupt sul GPIO
self.alarm.cleanup()
def start_canopen(self):
# Avvio il server CANOPEN
# usePTY serve ad evitare l'ECHO
# INFO: uso stdbuf per evitare il buffering dell'output se non in terminale
if self.ini.getboolean("System", "canopen_fake"):
reactor.spawnProcess(
self.canopen,
"/usr/bin/stdbuf",
args=[
"stdbuf",
"--output=L",
"--input=0",
"{}alma3d_canopenshell".format(self.ini.get("Path", "base_path")),
"fake",
"load#libcanfestival_can_socket.so,0,1M,8"
],
env=os.environ,
usePTY=False
)
else:
reactor.spawnProcess(
self.canopen,
"/usr/bin/stdbuf",
args=[
"stdbuf",
"--output=L",
"--input=0",
"{}alma3d_canopenshell".format(self.ini.get("Path", "base_path")),
"load#libcanfestival_can_socket.so,0,1M,8"
],
env=os.environ,
usePTY=False
)
def update_import_progress(self, value, line_num):
self.OpProgress = value
self.currentLine = line_num
def update_import_end(self, filename):
if not self.stopAnalysis:
self.isImporting = False
self.last_sim = filename
if filename != "":
self.isReadyToStart = True
def stream_reader(self):
# TODO: Deve ripartire automaticamente in caso di errore ed in caso di assenza di pipe!
logging.info("Position reader thread started!")
is_pipe_open = False
motor_pos = {'120': 0L, '121': 0L, '122': 0L, '119': 0L}
is_async_error = False
can_status = 3
is_centered = False
op_progress = 0
mex_counter = 0L
pos_time = 0.0
while self.isReading:
if is_pipe_open:
try:
line = pipein.readline()[:-1]
# print 'Parent %d got "%s" at %s' % (os.getpid(), line, time.time( ))
# line: @M119 S0 @M120 S0 @M121 S0 @M122 S0 AS4 T9 C0
canopen_status = self.find_motor_position.search(line)
if canopen_status:
motor_pos[canopen_status.group(1)] = canopen_status.group(2)
motor_pos[canopen_status.group(3)] = canopen_status.group(4)
motor_pos[canopen_status.group(5)] = canopen_status.group(6)
motor_pos[canopen_status.group(7)] = canopen_status.group(8)
# Se lo stato e' zero, vuol dire che c'e' una segnalazione pendente
if canopen_status.group(9) == '0' and is_async_error is False:
is_async_error = True
elif canopen_status.group(9) != '0':
try:
can_status = int(canopen_status.group(9))
except ValueError:
can_status = 97
logging.error("Impossibile convertire lo stato {}".format(can_status))
if not is_centered:
if can_status == 6:
is_centered = True
# pos_time = float(canopen_status.group(10))
pos_time = 0.01
op_progress = canopen_status.group(11)
mex_counter += 1
reactor.callFromThread(
self.update_var_from_canopen, motor_pos, is_async_error, can_status, is_centered,
pos_time, op_progress, mex_counter
)
except Exception, e:
# Potrebbe essere semplicemente non disponibile un dato!
exc_type, exc_value, exc_traceback = sys.exc_info()
logging.error("Disabling Alarm, device not found! (%s)" % str(e))
traceback.print_exception(exc_type, exc_value, exc_traceback, limit=2, file=sys.stdout)
is_pipe_open = False
logging.info("Pipe closed!")
else:
# Questa devo analizzarla meglio
try:
if self.ini.get("System", "canopen_fake"):
if os.path.exists(self.ini.get("Path", "pipe_position_fake")):
mode = os.stat(self.ini.get("Path", "pipe_position_fake")).st_mode
if stat.S_ISFIFO(mode):
pipein = open(self.ini.get("Path", "pipe_position_fake"), 'r')
is_pipe_open = True
continue
else:
if os.path.exists(self.ini.get("Path", "pipe_position")):
mode = os.stat(self.ini.get("Path", "pipe_position")).st_mode
if stat.S_ISFIFO(mode):
pipein = open(self.ini.get("Path", "pipe_position"), 'r')
is_pipe_open = True
continue
sleep(0.5)
except:
exc_type, exc_value, exc_traceback = sys.exc_info()
logging.error("Impossibile inviare lo stato (%s)" % sys.exc_info()[0])
traceback.print_exception(exc_type, exc_value, exc_traceback, limit=2, file=sys.stdout)
