From 11d63dfd96d8b9bc128a4fe78ac7bdb7521644e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Roman=20Dvo=C5=99=C3=A1k?= Date: Sun, 26 Aug 2018 19:30:57 -0400 Subject: [PATCH] expa 18 modifications --- src/drivers/gpio.py | 4 +- src/drivers/io/io_keyboard | 71 ++++++++-------- src/drivers/lcd_client | 10 ++- src/drivers/lcd_client.save | 138 ++++++++++++++++++++++++++++++++ src/drivers/mount_stellarium.py | 72 ++++------------- src/lcd_test.py | 28 +++++++ src/pymlab_bridge | 2 +- 7 files changed, 227 insertions(+), 98 deletions(-) create mode 100755 src/drivers/lcd_client.save create mode 100755 src/lcd_test.py diff --git a/src/drivers/gpio.py b/src/drivers/gpio.py index da9716d..ae0c972 100755 --- a/src/drivers/gpio.py +++ b/src/drivers/gpio.py @@ -34,12 +34,12 @@ def __init__(self): self.config1 = 0x00 self.port0 = 0b00000000 self.port1 = 0b00000000 - self.devices0 = ['mount_pwr', 'laser_pwr', '12V_3', '12V_4', 'laser_status', None, None, None] + self.devices0 = ['mount_pwr', 'laser_pwr', '12V_3', 'laser_status', 'Empty', None, None, None] #self.devices0 = ['a', 'b', 'c', 'd', 'e', 'f', 'g', 'h'] self.devices1 = [None, None, None, None, None, None, None, None] rospy.Subscriber("/gpio/gpio_mount", String, callback_btn) - self.pub_status = rospy.Publisher('/gpio/gpio_status', String, queue_size=10) + self.pub_status = rospy.Publisher('/gpio/gpio_status', String, queue_size=2, latch=True) AromNode.__init__(self) self.set_feature('gpio_set_port',{'ports': 8, 'devices': str(self.devices0) ,'subscrib': '/gpio/gpio_mount'}) diff --git a/src/drivers/io/io_keyboard b/src/drivers/io/io_keyboard index 3fccfe0..32074ba 100755 --- a/src/drivers/io/io_keyboard +++ b/src/drivers/io/io_keyboard @@ -38,10 +38,10 @@ class user_input(arom_helper.AromNode): self.led_status = 0 self.layer_count = 2 - self.toggle_button = 172 + self.toggle_button = 69 self.setLayer() - self.device1 = evdev.InputDevice('/dev/input/event2') - self.device2 = evdev.InputDevice('/dev/input/event3') + self.device1 = evdev.InputDevice('/dev/input/event6') + self.device2 = evdev.InputDevice('/dev/input/event5') self.device1.grab() self.device2.grab() @@ -100,46 +100,48 @@ class user_input(arom_helper.AromNode): }] keymap_zvpp = [{ - 172: 'shift', - 15: 'dome_a', - 155: 'dome_b', - 140: 'tracking_toggle', - 69: 'telescopeA_close', - 98: 'telescopeB_close', - 55: 'telescopeC_close', - 14: None, - 71: 'telescopeA_irisM', - 72: 'telescopeB_irisM', - 73: 'telescopeC_irisM', - 74: None, - 75: 'telescopeA_irisP', - 76: 'telescopeB_irisP', - 77: 'telescopeC_irisP', - 78: None, - 79: 'telescopeA_focusM', - 80: 'telescopeB_focusM', - 81: 'telescopeC_focusM', - 82: 'telescopeA_focusP', - 11: 'telescopeB_focusP', - 83: 'telescopeC_focusP', + 69: 'shift', + 15: None, + 155: None, + 140: None, + 690: None, + 98: None, + 51: 'A', + 55: None, + 14: 'gpio_3_toggle', + 71: None, + 72: 'DEC_P', + 73: None, + 74: 'B', + 75: 'RA_M', + 76: 'STOP', + 77: 'RA_P', + 78: 'C', + 79: None, + 80: 'DEC_M', + 81: None, + 82: None, + 11: None, + 83: None, 96: None, },{ - 172: 'shift', - 15: 'dome_a', - 155: 'dome_b', - 140: 'tracking_toggle', - 69: 'RA_center', - 98: 'DEC_center', + 69: 'shift', + 15: None, + 155: None, + 140: None, + 690: None, + 98: None, + 51: 'A', 55: None, - 14: None, + 14: 'gpio_3_toggle', 71: None, 72: 'DEC_P', 73: None, - 74: None, + 74: 'B', 75: 'RA_P', 76: 'STOP', 77: 'RA_M', - 78: None, + 78: 'C', 79: None, 80: 'DEC_M', 81: None, @@ -161,6 +163,7 @@ class user_input(arom_helper.AromNode): while True: dev_out = dev.next() if dev_out.type == ecodes.EV_KEY and dev_out.value in [0,1]: + print(dev_out) data.append(self.GetKey(dev_out)) except Exception as e: pass diff --git a/src/drivers/lcd_client b/src/drivers/lcd_client index 9dd2ea9..f5739ff 100755 --- a/src/drivers/lcd_client +++ b/src/drivers/lcd_client @@ -40,7 +40,7 @@ class lcdClient(AromNode): rospy.Subscriber("/arom/UI/buttons", std_msgs.msg.String, self.callback_btn) rospy.Subscriber("/arom/node/lcdText", std_msgs.msg.String, self.callback_btn) - rospy.Subscriber("/arom/node/lcdText", std_msgs.msg.String, self.callback_btn) + #rospy.Subscriber("/arom/node/lcdText", std_msgs.msg.String, self.callback_btn) self.lcdText_pub = rospy.Publisher('/arom/node/lcdText', std_msgs.msg.String, queue_size=5) @@ -72,7 +72,7 @@ class lcdClient(AromNode): self.pymlab(device="StatusLCD", method="home") time.sleep(0.5) - self.pymlab(device="StatusLCD", method="putsFull", parameters=str(dict(lineA="Welcome...", lineB="AROM MLAB.cz"))) + self.pymlab(device="StatusLCD", method="putsFull", parameters=str(dict(lineA="Welcome ...", lineB="AROM MLAB.cz"))) print "-------------" @@ -97,6 +97,9 @@ class lcdClient(AromNode): elif last_data[0] == 'KEY_POWER': print "lightOff" self.pymlab(device="StatusLCD", method="light") + else: + #self.setLCD("TEST", "test") + print "Data not sended to LCD display" except Exception, e: @@ -104,8 +107,9 @@ class lcdClient(AromNode): time.sleep(0.1) def setLCD(self, lineA, lineB): + self.pymlab(device="StatusLCD", method="clear") self.pymlab(device="StatusLCD", method="putsFull", parameters=str(dict(lineA=str(lineA), lineB=str(lineB)))) - self.lcdText_pub.publish(str(lineA)+"\n"+str(lineB)) + #self.lcdText_pub.publish(str(lineA)+"\n"+str(lineB)) def callback_btn(self,recive): diff --git a/src/drivers/lcd_client.save b/src/drivers/lcd_client.save new file mode 100755 index 0000000..8b48e01 --- /dev/null +++ b/src/drivers/lcd_client.save @@ -0,0 +1,138 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import math +import time +import rospy +import std_msgs +import json +import os +from std_msgs.msg import String +from std_msgs.msg import Float32 +from arom.srv import * +from arom.msg import * +import numpy as np +from __init__ import AromNode + + +aws_data = {} +btn_data = [] + +def callback_aws(recive): + global aws_data + for i, type in enumerate(recive.type): + aws_data[type] = recive.value[i] + #print aws_data + +def callback_btn(recive): + global btn_data + btn_data.append(recive.data) + #print recive, btn_data + +class lcdClient(AromNode): + node_name = "lcd_client" + node_type = "lcd_client" + node_pymlab = True + btn_data = [] + + def __init__(self): + print os.path.abspath(__file__) + + rospy.Subscriber("/arom/UI/buttons", std_msgs.msg.String, self.callback_btn) + rospy.Subscriber("/arom/node/lcdText", std_msgs.msg.String, self.callback_btn) + rospy.Subscriber("/arom/node/lcdText", std_msgs.msg.String, self.callback_btn) + self.lcdText_pub = rospy.Publisher('/arom/node/lcdText', std_msgs.msg.String, queue_size=5) + + + AromNode.__init__(self) + self.set_feature('display_show',{'rows': 2, 'cols': 16, 'publish': '/arom/node/lcdText'}) + #self.set_feature('display_show',{'rows': rows, 'cols': cols, 'publish': '/arom/node/lcdText'}) + #self.set_feature('display_control', {'up': 'KEY_VOLUMEUP', 'down': 'KEY_VOLUMEDOWN', 'back': 'KEY_F2', 'enter': 'KEY_F3', 'subscrib': '/arom/UI/buttons'}) + + + rospy.Subscriber(name = "/arom/node%s/rows" %(rospy.get_name()), data_class = std_msgs.msg.String, callback = self.RowData, callback_args=None) + rospy.Subscriber(name = "/arom/node%s/row1" %(rospy.get_name()), data_class = std_msgs.msg.String, callback = self.RowData, callback_args=1) + rospy.Subscriber(name = "/arom/node%s/row2" %(rospy.get_name()), data_class = std_msgs.msg.String, callback = self.RowData, callback_args=2) + rospy.Subscriber(name = "/arom/node%s/row3" %(rospy.get_name()), data_class = std_msgs.msg.String, callback = self.RowData, callback_args=3) + rospy.Subscriber(name = "/arom/node%s/row4" %(rospy.get_name()), data_class = std_msgs.msg.String, callback = self.RowData, callback_args=4) + + + ## + ## Konec zakladni inicializace + ## + + self.rows = 2 + self.cols = 16 + + print "zinicializovano" + + self.pymlab(device="StatusLCD", method="reset") + self.pymlab(device="StatusLCD", method="init") + self.pymlab(device="StatusLCD", method="clear") + self.pymlab(device="StatusLCD", method="home") + + time.sleep(0.5) + self.pymlab(device="StatusLCD", method="putsFull", parameters=str(dict(lineA="Welcome...", lineB="AROM MLAB.cz"))) + + print "-------------" + + + rate = rospy.Rate(10) + ra = 0 + dec = 90 + while not rospy.is_shutdown(): + try: + if len(self.btn_data) > 0: + print self.btn_data[0], len(self.btn_data) + lastBtn = self.btn_data[0] + last_data = lastBtn.split(" ") + self.btn_data.pop(0) + + print "incomming:", last_data + + if last_data[0] == 'KEY_MENU': + print "light" + self.pymlab(device="StatusLCD", method="lightToggle") + + elif last_data[0] == 'KEY_POWER': + print "lightOff" + self.pymlab(device="StatusLCD", method="light") + + + except Exception, e: + print e + time.sleep(0.1) + + def setLCD(self, lineA, lineB): + self.pymlab(device="StatusLCD", method="putsFull", parameters=str(dict(lineA=str(lineA), lineB=str(lineB)))) + self.lcdText_pub.publish(str(lineA)+"\n"+str(lineB)) + + + def callback_btn(self,recive): + self.btn_data.append(recive.data) + + def RowData(self, recive, arg = None): + print "RowData", arg, recive + recive = str(recive.data) + + if not arg: + recive += "\n " # tohle je ochrana pred tim, kdyz retezec nebude obsahovat EOL retezec + recive = recive.splitlines() + self.pymlab(device="StatusLCD", method="putsFull", parameters=str(dict(lineA=str(recive[0]), lineB=str(recive[1])))) + self.lcdText_pub.publish(str(recive[0])+"\n"+str(recive[1])) + elif arg == 1: + self.pymlab(device="StatusLCD", method="putsFull", parameters=str(dict(lineA=str(recive), lineB = None) )) + self.lcdText_pub.publish(str(recive)+"\n"+str("")) + elif arg == 2: + self.pymlab(device="StatusLCD", method="putsFull", parameters=str(dict(lineA=None, lineB = str(recive) ))) + self.lcdText_pub.publish(str("")+"\n"+str(recive)) + elif arg == 3: + self.pymlab(device="StatusLCD", method="putsFull", parameters=str(dict(lineA=None, lineB = None ))) + elif arg == 4: + self.pymlab(device="StatusLCD", method="putsFull", parameters=str(dict(lineA=None, lineB = None ))) + + + +if __name__ == '__main__': + m = lcdClient() + diff --git a/src/drivers/mount_stellarium.py b/src/drivers/mount_stellarium.py index e6d1524..b652295 100755 --- a/src/drivers/mount_stellarium.py +++ b/src/drivers/mount_stellarium.py @@ -4,6 +4,7 @@ # +# Protokol pro posilani a prijimani dat ze Stellaria # http://yoestuve.es/blog/communications-between-python-and-stellarium-stellarium-telescope-protocol/ # @@ -38,8 +39,8 @@ class StellariumRemote(asyncore.dispatcher): def __init__(self): asyncore.dispatcher.__init__(self) - TCP_IP = '' - TCP_PORT = 1234 + TCP_IP = '0.0.0.0' + TCP_PORT = rospy.get_param('port', 1234) BUFFER_SIZE = 1024 self.dataRaDec = {'ra': 0, 'dec':0} @@ -47,6 +48,7 @@ def __init__(self): self.NewPos = False self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + print("Adresa {}:{}".format(TCP_IP, TCP_PORT)) self.s.bind((TCP_IP, TCP_PORT)) self.s.listen(1) print "cekam na spojeni" @@ -54,7 +56,7 @@ def __init__(self): self.conn.setblocking(False) print('Connected by', addr) #self.s.setblocking(0) - self.s.settimeout(1) + self.s.settimeout(0.5) #elf.clients = [self.s, conn] #print TCP_IP, ":", TCP_PORT self.run() @@ -72,17 +74,16 @@ def handle_accept(self): if pair is not None: sock, addr = pair self.sock = sock - print 'Incoming connection from %s' % repr(addr) + print('Incoming connection from %s' % repr(addr)) handler = EchoHandler(sock) def run(self): - try: - print "spostim smycku" + print("spostim smycku") rospy.Subscriber("/mount/status/coordinates/RaDec", Float32MultiArray, self.callback_RaDec) self.sendCoord = rospy.Publisher('/mount/controll', String, queue_size=5) rospy.init_node('AROM_stellarium') - print "node inicializovan" + print("node inicializovan") data_rec = None while not rospy.is_shutdown(): @@ -92,7 +93,7 @@ def run(self): if data_rec: self.GetPosition(data_rec) data_rec = None - print "Diky" + print ("Diky") except Exception as e: if e.args[0] == 11: # nejsou nova data @@ -103,51 +104,10 @@ def run(self): self.s.close() except Exception as e: - print e, e.args - - finally: - self.conn.close() - - - - ''' + self.s.close() + print(e, e.args) + self.__init__() - try: - while not rospy.is_shutdown(): - if not dataSocket: - print "received data:", dataSocket - - try: - array = bytearray(dataSocket) - lenght = array[0] | array[1] << 8 - type = array[2] | array[3] << 8 - timed = array[4] | array[5]<< 8 | array[6]<< 8*2 | array[7] << 8*3 | array[8] << 8*4| array[9] << 8*5| array[10] << 8*6| array[11] << 8*7 - ra = array[12] | array[13]<< 8 | array[14]<< 8*2 | array[15] << 8*3 - dec = array[16] | array[17]<< 8 | array[18]<< 8*2 | array[19] << 8*3 - - if dec > 0x40000000: - dec = dec-0xffffffff - if ra > 0xff000000: - ra = ra-0xffffffff - - ra = ra/11930465 - dec = dec/11930465 - - print "Slew - RaDec", ra, dec - - #self.SendPosition() - - - except Exception, e: - print e - dataSocket = None - - self.SendPosition() - - - except Exception, e: - print e - ''' def SendPosition(self): try: @@ -168,11 +128,10 @@ def SendPosition(self): chr((ra) & 0xFF), chr((ra >> 8) & 0xFF), chr((ra >> 16) & 0xFF), chr((ra >> 24) & 0xFF), chr((dec) & 0xFF), chr((dec >> 8) & 0xFF), chr((dec >> 16) & 0xFF), chr((dec >> 24) & 0xFF), chr((status) & 0xFF), chr((status >> 8) & 0xFF), chr((status >> 16) & 0xFF), chr((status >> 24) & 0xFF) ]) - #print "posilam", array return str(bytes(array)) - except Exception, e: - print "ERROR1:", repr(e) + except Exception as e: + print("ERROR1:", repr(e)) def GetPosition(self, string): try: @@ -187,7 +146,6 @@ def GetPosition(self, string): print (hex(ra), hex(dec)) if ra < 0xffffffff/2: ra = (ra*360.0/0xffffffff) else: ra = -((0xffffffff-ra)*360.0/0xffffffff) - #ra = (ra*360.0/0xffffffff) if dec < 0xffffffff/2: dec = (dec*360.0/0xffffffff) else: dec = -((0xffffffff-dec)*360.0/0xffffffff) print ra, dec @@ -199,8 +157,6 @@ def GetPosition(self, string): 'TIME': time } self.sendCoord.publish("radec %f %f"%(ra, dec)) - #print data & 0xffff - #print int(data & 0xffff) except Exception as e: raise e diff --git a/src/lcd_test.py b/src/lcd_test.py new file mode 100755 index 0000000..1e6289d --- /dev/null +++ b/src/lcd_test.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +import math +import time +import rospy +import rosnode +import std_msgs +import sensor_msgs +import geometry_msgs +from geometry_msgs.msg import Point +import arom +import axis +import json +import os +import arom_helper + +class LCD(arom_helper.AromNode): + node_name = "LCD" + node_type = "LCD" + def __init__(self): + arom_helper.AromNode.__init__(self) + lcd = rospy.Publisher('/arom/node/lcdText', std_msgs.msg.String, queue_size=10) + lcd.publish("LCD test") + print "Test done" + while not rospy.is_shutdown(): + pass + +lcd = LCD() \ No newline at end of file diff --git a/src/pymlab_bridge b/src/pymlab_bridge index 31d9ef2..fce7098 100755 --- a/src/pymlab_bridge +++ b/src/pymlab_bridge @@ -35,7 +35,7 @@ class pymlab_server(AromNode): self.i2c = [] self.bus = [] - bus_config = rospy.get_param('~config', "/home/odroid/robozor/station/telescope.json") + bus_config = rospy.get_param('~config', "/home/odroid/robozor/station/pymlab_presets/telescope.json") try: bus_config = sys.argv[1] except: pass