From f1fe274b49d2bc8443b3056519a7c25be80465d9 Mon Sep 17 00:00:00 2001 From: ODROID Date: Wed, 16 Mar 2016 23:18:11 +0100 Subject: [PATCH] New cfg scheme --- CMakeLists.txt | 4 ++ cfg/ZVPP_observatory.json | 37 ++++++++++ cfg/ZVPP_observatory.xml | 32 +++++++++ cfg/test.json | 37 ++++++++++ msg/PymlabServerStatusM.msg | 2 + msg/SensorValues.msg | 2 + src/ROSpymlabServer.py | 123 ++++++++++++++++++++++++++++++++ src/aromBrain.py | 58 +++------------ src/drivers/mount.py | 98 ++++++-------------------- src/drivers/userInterface.py | 128 ++++++++++++++++++++++++++++++++++ src/drivers/weatherStation.py | 123 ++++++++++++++++++++++++++++++++ src/initPymlab.py | 42 +++++++++++ src/move_mount.py | 26 ++++++- srv/PymlabDrive.srv | 5 ++ srv/PymlabInit.srv | 5 ++ srv/PymlabServerStatus.srv | 4 ++ srv/PymlabSetValue.srv | 5 ++ 17 files changed, 605 insertions(+), 126 deletions(-) create mode 100644 cfg/ZVPP_observatory.json create mode 100644 cfg/ZVPP_observatory.xml create mode 100644 cfg/test.json create mode 100644 msg/PymlabServerStatusM.msg create mode 100644 msg/SensorValues.msg create mode 100755 src/ROSpymlabServer.py create mode 100755 src/drivers/userInterface.py create mode 100755 src/initPymlab.py create mode 100644 srv/PymlabDrive.srv create mode 100644 srv/PymlabInit.srv create mode 100644 srv/PymlabServerStatus.srv create mode 100644 srv/PymlabSetValue.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index c8ff293..7844249 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS add_message_files( FILES DriverControlm.msg + PymlabServerStatusM.msg # pro inicializaci PYMLAB serveru # Message1.msg # Message2.msg ) @@ -28,6 +29,9 @@ add_service_files( MountParameter.srv RegisterDriver.srv DriverControl.srv + PymlabInit.srv # Pro inicializaci PYMLABU + PymlabServerStatus.srv # Pro + PymlabDrive.srv # Pro # Service1.srv # Service2.srv ) diff --git a/cfg/ZVPP_observatory.json b/cfg/ZVPP_observatory.json new file mode 100644 index 0000000..f8d4d7e --- /dev/null +++ b/cfg/ZVPP_observatory.json @@ -0,0 +1,37 @@ +[ + { + "type": "dome", + "name": "dome_robozor", + "driver": "dome.robozor" + }, + { + "type": "mount", + "name": "mount_montaz", + "driver": "mount.EQmod", + "parent": "dome_robozor" + }, + { + "type": "telescope", + "name": "dalekohled", + "parent": "mount_montaz", + "driver": "telescope.telescope", + "t_type": "telescope.telescope" + }, + { + "type": "camera", + "name": "cam_camera", + "parent": "dalekohled", + "driver": "camera.gphoto" + }, + { + "type": "focuser", + "name": "cam_camera", + "parent": "cam_camera", + "driver": "acs.tefo" + }, + { + "type": "weatherStation", + "name": "ws_AWS", + "driver": "ws.AWS" + } +] \ No newline at end of file diff --git a/cfg/ZVPP_observatory.xml b/cfg/ZVPP_observatory.xml new file mode 100644 index 0000000..e6b0bbd --- /dev/null +++ b/cfg/ZVPP_observatory.xml @@ -0,0 +1,32 @@ + + + + Heq5 + EQmod + mount.EQmod + + Hlavni dalekohled + main + NWT + 1000 + 130 + + Hlavni> + + + + Pointacni dalekohled + pointing + REF + 500 + 100 + + + + + + + + < + + \ No newline at end of file diff --git a/cfg/test.json b/cfg/test.json new file mode 100644 index 0000000..5f1cfb9 --- /dev/null +++ b/cfg/test.json @@ -0,0 +1,37 @@ +[ + + { + "type": "mount", + "name": "mount_montaz", + "driver": "EQmod" + }, + { + "type": "telescope", + "name": "dalekohled", + "parent": "mount_montaz", + "driver": "telescope.telescope", + "t_type": "telescope.telescope" + }, + { + "type": "camera", + "name": "cam_camera", + "parent": "dalekohled", + "driver": "camera.gphoto" + }, + { + "type": "focuser", + "name": "cam_camera", + "parent": "cam_camera", + "driver": "acs.tefo" + }, + { + "type": "weatherStation", + "name": "ws_AWS", + "driver": "AWS01B" + }, + { + "type": "userInterface", + "name": "lcd", + "driver": "i2clcd" + } +] \ No newline at end of file diff --git a/msg/PymlabServerStatusM.msg b/msg/PymlabServerStatusM.msg new file mode 100644 index 0000000..589dd1d --- /dev/null +++ b/msg/PymlabServerStatusM.msg @@ -0,0 +1,2 @@ +string name +string data \ No newline at end of file diff --git a/msg/SensorValues.msg b/msg/SensorValues.msg new file mode 100644 index 0000000..df12c6d --- /dev/null +++ b/msg/SensorValues.msg @@ -0,0 +1,2 @@ +string name +float32 value \ No newline at end of file diff --git a/src/ROSpymlabServer.py b/src/ROSpymlabServer.py new file mode 100755 index 0000000..22650ae --- /dev/null +++ b/src/ROSpymlabServer.py @@ -0,0 +1,123 @@ +#!/usr/bin/env python + +import rospy +import pymlab +from pymlab import config +import sys +import sensor_server +from std_msgs.msg import String +from std_msgs.msg import Float32 +import std_msgs +from sensor_server.srv import * +from sensor_server.msg import * + + +def server(req): + print req + print "Returning [%s + %s]"%(req.name, req.data) + return GetSensValResponse( 10 ) + +class pymlab_server(): + def __init__(self): + self.pymlab_read = False # slouzi k ochrane pymlabu pred pokusem o dve cteni zaroven ... + + def init(self, cfg=None): + self.status = False + self.init = cfg + self.cfg_i2c = eval(cfg.i2c) + self.cfg_bus = eval(cfg.bus) + self.devices = {} + Local_devices = {} + rospy.loginfo("configuracni soubor: %s" %str(cfg)) + i2c = { + "port": 1, + } + bus = [ + { + "name": "lts01", + "type": "lts01", + }, + ] + self.pymlab_config = config.Config(i2c = self.cfg_i2c, bus = self.cfg_bus) + self.pymlab_config.initialize() + for x in self.cfg_bus: + self.devices[x['name']] = self.pymlab_config.get_device(x['name']) + rospy.set_param("devices", str(self.devices)) + rospy.loginfo("self.device: %s" %str(self.devices)) + + return True + + def getvalue(self, cfg=None): + val = int(float(self.lts_sen.get_temp())) + return GetSensValResponse(val) + + def status(self, cfg=None): + self.rate = 1 + rospy.loginfo("prijaty cfg: %s" %(str(cfg))) + try: # pokud je servis s GetSensVal, tak se pouzije toto, + ecfg = eval(cfg.data) + except Exception, e: # toto je pro zpravu 'pymlab_server' + ecfg = eval(cfg) + print ecfg + if 'rate' in ecfg: + self.rate = ecfg['rate'] + #print "Vlastni frekvence", self.rate + rospy.set_param("rate", float(self.rate)) + if 'AutoInputs' in ecfg: + self.AutoInputs = ecfg['AutoInputs'] + rospy.set_param("AutoInputs", str(self.AutoInputs)) + if "start" in ecfg: + self.status = True + rate = rospy.Rate(self.rate) + AutoInputs = self.AutoInputs + devices = self.devices + sender = rospy.Publisher('pymlab_data', SensorValues, queue_size=20) + values = {} + for x in AutoInputs: + for y in AutoInputs[x]: + #print "AutoInputs >>", x, y, + #print str(x)+"/"+str(y), str(x)+"/"+str(y) + values[str(x)+"/"+str(y)] = str(x)+"/"+str(y) + rospy.set_param("values", values) + # print "\n run \n\n" + while True: + print "\r", + for x in AutoInputs: + for y in AutoInputs[x]: + while self.pymlab_read: pass + self.pymlab_read = True + data = getattr(self.devices[devices[x].name], y)() + self.pymlab_read = False + print x, "%.3f"%data, "||", + sender.publish(name=str(devices[x].name)+"/"+str(y), value=data) + #senderTest.publish(data) + print "\r", + rate.sleep() + return True + + def drive(self, cfg): + #print cfg + parameters = cfg.parameters + method = cfg.method + device = cfg.device + if parameters == "" or parameters == None: + reval = getattr(self.devices[cfg.device], cfg.method)() + else: + reval = getattr(self.devices[cfg.device], cfg.method)(*eval(parameters)) + return str(reval) + + +def main(): + ps = pymlab_server() + rospy.init_node('pymlab_node') + + rospy.Subscriber("pymlab_server", PymlabServerStatusM, ps.status) + s1 = rospy.Service('pymlab_init', PymlabInit, ps.init) + s2 = rospy.Service('pymlab_server', PymlabServerStatus, ps.status) + s3 = rospy.Service('pymlab_drive', PymlabDrive, ps.drive) + + rospy.loginfo("Ready to get work.") + rospy.spin() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/aromBrain.py b/src/aromBrain.py index 4d83e78..732b860 100755 --- a/src/aromBrain.py +++ b/src/aromBrain.py @@ -15,7 +15,7 @@ #from drivers import devices #from drivers import camera #from drivers import focuser -#from drivers import weatherStation +from drivers import weatherStation #from drivers import rotator #from drivers import roof #import drivers @@ -28,6 +28,7 @@ def mountSlew(self, cor = [0,0]): def __init__(self): rospy.init_node('AROM_brain') self.mount = {} + self.weatherStation = {} self.camera = {} self.devices = {} @@ -35,7 +36,8 @@ def __init__(self): self.drivers = { 'EQmod': mount.EQmod, 'SynScan': mount.SynScan, - 'mount': drivers.mount.EQmod, + 'mount': mount.EQmod, + 'AWS01A': weatherStation.AWS01B, } s_RegisterDriver = rospy.Service('arom/RegisterDriver', arom.srv.RegisterDriver, self.RegisterDriver) @@ -44,49 +46,20 @@ def __init__(self): with open(self.config_file) as data_file: self.config = json.load(data_file) rospy.set_param("ObservatoryConfig/file", str(self.config_file)) - - ''' - rospy.wait_for_service('arom/mount/parameter') - mount = rospy.ServiceProxy('arom/mount/parameter', arom.srv.MountParameter) - registred = mount(name = 'getPosition', value = '') - registred = mount(name = 'getTime', value = '') - rospy.loginfo("%s: >> position %s" %("brain", registred)) - Actmount = rospy.ServiceProxy('arom/mount/parameter', arom.srv.MountParameter) - ''' - - ''' - client = actionlib.SimpleActionClient('fibonacci', actionlib_tutorials.msg.FibonacciAction) - # Waits until the action server has started up and started - # listening for goals. - client.wait_for_server() - # Creates a goal to send to the action server. - goal = actionlib_tutorials.msg.FibonacciGoal(order=20) - # Sends the goal to the action server. - client.send_goal(goal) - # Waits for the server to finish performing the action. - client.wait_for_result() - # Prints out the result of executing the action - return client.get_result() - ''' - - ''' - for x in xrange(1,5): - registred = mount(name = 'getPosition', value = '') - time.sleep(5) - ''' - + rospy.loginfo(str(self.config_file)) + + rospy.loginfo("AROMbrain started") rospy.spin() def RegisterDriver(self, srv): - #rospy.loginfo("NewDevice>> type: %s, name %s (%s). With driver %s" %(srv.device, srv.name, srv.sname, srv.driver)) - dev_driver = self.drivers[srv.driver] - eval('self.'+srv.device)[srv.sname] = {'name':srv.name, 'sname':srv.sname, 'driver':srv.driver, 'object':dev_driver} - self.devices[srv.sname] = {'name':srv.name, 'sname':srv.sname, 'driver':srv.driver, 'object':dev_driver} + #dev_driver = self.drivers[srv.driver] + #eval('self.'+srv.device)[srv.sname] = {'name':srv.name, 'sname':srv.sname, 'driver':srv.driver, 'object':dev_driver} + #self.devices[srv.sname] = {'name':srv.name, 'sname':srv.sname, 'driver':srv.driver, 'object':dev_driver} + rospy.loginfo("NewDevice>> type: %s, name %s (%s). With driver %s >> %s" %(srv.device, srv.name, srv.sname, srv.driver, str(self.devices[srv.sname]))) return 1 def loadDriver(self, deviceType = None, driverName = 'mount'): - try: driver = None if deviceType: @@ -102,15 +75,6 @@ def mount_move(self, target = [10,10]): def main(): - - #rospy.Subscriber("pymlab_server", PymlabServerStatusM, ps.status) - #s1 = rospy.Service('pymlab_init', PymlabInit, ps.init) - #s2 = rospy.Service('pymlab_server', PymlabServerStatus, ps.status) - #s3 = rospy.Service('pymlab_drive', PymlabDrive, ps.drive) - - #rospy.loginfo("Ready to get work.") - #rospy.spin() - ab = AromBrain() if __name__ == "__main__": diff --git a/src/drivers/mount.py b/src/drivers/mount.py index 69ffc19..e89f744 100755 --- a/src/drivers/mount.py +++ b/src/drivers/mount.py @@ -11,6 +11,7 @@ from arom.msg import * import arom import serial +import json from astropy import units as u from astropy.coordinates import SkyCoord from astropy.coordinates import Angle @@ -19,7 +20,8 @@ class mount(): - def __init__(self, parent = None, name = "EQmod", port="/dev/ttyUSB0", connect = True): + def __init__(self, parent = None, arg = None, name = "EQmod", port="/dev/ttyUSB0", connect = True): + self.arg = arg self.newTarget = 0 self.init() self.slewing = False @@ -400,41 +402,25 @@ def setPosition(self, param = None): ## if change[0].degree < -180: - dirRA = '20' + dirRA = '21' change[0] = Angle(360 + change[0].degree, unit = 'deg') elif change[0] <= 0: - dirRA = '21' + dirRA = '20' change[0] = Angle(abs(change[0].degree), unit = 'deg') elif change[0].degree > 180: - dirRA = '21' + dirRA = '20' change[0] = Angle(360 - change[0].degree, unit = 'deg') elif change[0].degree > 0: - dirRA = '20' + dirRA = '21' else: - dirRA = '20' + dirRA = '21' self.logwarn("Problem in 'setPosition': Ra is out_of_range") - ''' - - if change[0].degree < 0 and change[0].degree > -180: - dirRA = '21' - change[0] = Angle( abs(change[0].degree), unit = 'deg') - elif change[0].degree > 180: - dirRA = '21' - change[0] = Angle(360 - change[0].degree, unit = 'deg') - elif change[0].degree < -180: - dirRA = '20' - change[0] = Angle(360 - change[0].degree, unit = 'deg') - else: - dirRA = '20' - ''' - if change[1].degree >= 0: - dirDEC = '21' - elif change[1].degree < 0: dirDEC = '20' - + elif change[1].degree < 0: + dirDEC = '21' stepsRA = self.mountUnit[0] * abs(change[0].degree) @@ -443,13 +429,6 @@ def setPosition(self, param = None): rospy.loginfo("SLEW TO %s (%s) with difference %s" %(self.coordinates, self.getCoordinates, change)) - - #stepsRA = self._degRealToAxis(change[0], self.AxRa) - #stepsDEC = self._degRealToAxis(change[1], self.AxDec) - - #ra = self._GetData('K', self.Axis1, "") - #ra = self._GetData(self.NotInstantAxisStop, self.Axis2, "") - ra = self._GetData(self.NotInstantAxisStop, self.Axis1,) ra = self._GetData(self.GetAxisStatus, self.Axis1,) ra = self._GetData(self.SetMotionMode, self.Axis1, dirRA) @@ -473,7 +452,6 @@ def setPosition(self, param = None): while True: ax0 = self._GetData(self.GetAxisStatus, self.Axis1) ax1 = self._GetData(self.GetAxisStatus, self.Axis2) - #print "osy", ax1, ax0 if ax0[2] == "0" and ax1[2] == "0": self.coordinates = self.coordinates_target self.time_data = time.time() @@ -482,7 +460,7 @@ def setPosition(self, param = None): sp = self.long2Revu24str( int(self.stepsPerRev[1]/self.DayLenght['sidreal'])*6) ra = self._GetData(self.SetStepPeriod, self.Axis1, sp) self.newTarget -= 1 - #ra = self._GetData(self.StartMotion, self.Axis1) + ra = self._GetData(self.StartMotion, self.Axis1) break time.sleep(0.1) @@ -502,42 +480,17 @@ def getPosition(self, param = None): raw = self._GetData(self.GetAxisPosition, self.Axis1) decw = self._GetData(self.GetAxisPosition, self.Axis2) - ra = self.Revu24str2long(raw) - dec = self.Revu24str2long(decw) - self.stepsPerRev[1]/2 - ra_old = self.coordinates.ra - + dec = self.Revu24str2long(decw) ra = Angle((ra ) / self.mountUnit[0], unit='deg') - Angle( 380*60*60*24 / (time.time() - self.driverSyncTime), unit='deg') - #ra = Angle((ra ) / self.mountUnit[0], unit='deg') - #dec = Angle((dec) / self.mountUnit[1] - 90 - self.syncOffSet[1], unit='deg') dec = Angle(0, unit='deg') - ''' - if dec.degree > 0 and dec.degree < 90*1: - pass - elif dec.degree < 90*2: - dec.degree -= 90 - elif dec.degree < 90*3: - dec.degree -= 90*2 - elif dec.degree < 90*4: - dec.degree -= 90*3 - else: - dec.degree = 0 - rospy.loginfo("Err: DEC over limit") - ''' - - #time_data - #ra += ra_old - #dec+=dec_old - - print "\t \t \t \t \t \t \t \t\t \t \t \t \t \t \t \t ",ra, dec, self.coordinates_target.to_string('dms'), self.coordinates.position_angle(self.coordinates_target).degree > 0 try: self.coordinates_instrumental = SkyCoord(ra = ra, dec = dec) except Exception, e: print e - #self.coordinates = SkyCoord(ra = ra * u.deg, dec = ra * u.deg) - #self.coordinates = SkyCoord(ra = 21.1901329787, dec = 21.1901329787, unit = 'deg') + return self.coordinates except Exception, e: @@ -714,20 +667,11 @@ def getTime(self, param = None): if __name__ == '__main__': - mount = EQmod() - #mount.connect() - mount.getPosition() - #for x in xrange(1,3): - # time.sleep(0.5) - # pos = mount.getPosition() - #ra = 0 - #dec = 90 - #mount.setPosition([ra, dec]) - #print "-----------------------------" - #ra = ra - 10 - #dec = dec - 10 - #mount.setPosition([ra, dec]) - - #while 1: - # time.sleep(1) - # # mount.getPosition() + cfg = rospy.get_param("ObservatoryConfig/file") + with open(cfg) as data_file: + config = json.load(data_file) + for x in config: + if x['name'] == sys.argv[1]: + break + mount = locals()[x['driver']](arg = x) + #mount = EQmod() diff --git a/src/drivers/userInterface.py b/src/drivers/userInterface.py new file mode 100755 index 0000000..16d5ed3 --- /dev/null +++ b/src/drivers/userInterface.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python + +import math +import time +import rospy +import std_msgs +import actionlib +import json +from std_msgs.msg import String +from std_msgs.msg import Float32 +from arom.srv import * +from arom.msg import * +import serial +from astropy.time import Time + + +class UserInteface(object): + def __init__(self, parent = None, arg = None, name = "I2CLCD", port="", connect = True, var = {}): + self.arg = arg + self.Autoconnect = connect + self.port = port + self.parent = parent + self.name = name + self.variables = var + + ## + ## Inicializace vlastniho ovladace + ## + + self.init() + + s_RegisterDriver = rospy.Service('driver/userInteface/%s' %self.name, arom.srv.DriverControl, self.reset) + + ## + ## Ceka to na spusteni AROMbrain nodu + ## + + rospy.init_node('AROM_userInterface') + rospy.loginfo("%s: wait_for_service: 'arom/RegisterDriver'" % self.name) + rospy.wait_for_service('arom/RegisterDriver') + rospy.loginfo("%s: >> brain found" % self.name) + + ## + ## Registrace zarizeni + ## >Arom returns 1 - OK, 0 - False + ## + + RegisterDriver = rospy.ServiceProxy('arom/RegisterDriver', arom.srv.RegisterDriver) + registred = RegisterDriver(name = self.name, sname= self.name, driver = 'I2CLCD', device = 'userInteface', status = 1) + rospy.loginfo("%s: >> register %s driver: %s" %(self.name, 'I2CLCD', registred)) + + + ## + ## Ovladac se pripoji k montazi + ## + + if self.Autoconnect: + self.connect() + + ## + ## Ovladac pujde ukoncit + ## + + rare = rospy.Rate(0.5) + while not rospy.is_shutdown(): + self.run() + rare.sleep() + + def run(self): + pass + + def reset(self, val=None): + pass + +############################################################################################################################################################################ +############################################################################################################################################################################ +############################################################################################################################################################################ +############################################################################################################################################################################ +############################################################################################################################################################################ + +###################################################################################### +###################################################################################### +## ## +## Driver for --AWS01A-- MLAB weather station ## +## ============================================ ## +## ## +## ## +###################################################################################### + +class i2clcd(UserInteface): + def init(self): + + rospy.loginfo("I2CLCD user inteface requires 'pymlab_drive' service from 'ROSpymlabServer' node") + rospy.loginfo("run>> 'rosrun arom initPymlab.py'") + rospy.wait_for_service('pymlab_drive') + self.pymlab = rospy.ServiceProxy('pymlab_drive', PymlabDrive) + rospy.loginfo("%s: >> 'ROSpymlabServer' found" % self.name) + + def run(self): + weather = rospy.get_param("weatherStation") + print weather + + + self.pymlab(device="StatusLCD", method="set_clear", parameters=None) + time.sleep(0.05) + self.pymlab(device="StatusLCD", method="set_home", parameters=None) + time.sleep(0.05) + self.pymlab(device="StatusLCD", method="puts", parameters="Tem:%.2fC SHT25" % (i, temperature)) + time.sleep(0.05) + self.pymlab(device="StatusLCD", method="set_row2", parameters=None) + time.sleep(0.05) + self.pymlab(device="StatusLCD", method="puts", parameters="%dTem:%.2fC SHT25" % (i, temperature)) + + + def connect(self): + pass + + + +if __name__ == '__main__': + cfg = rospy.get_param("ObservatoryConfig/file") + with open(cfg) as data_file: + config = json.load(data_file) + for x in config: + if x['name'] == sys.argv[1]: + break + weatherStation = locals()[x['driver']](arg = x) + #weatherStation = AWS01B() \ No newline at end of file diff --git a/src/drivers/weatherStation.py b/src/drivers/weatherStation.py index e69de29..811e20a 100755 --- a/src/drivers/weatherStation.py +++ b/src/drivers/weatherStation.py @@ -0,0 +1,123 @@ +#!/usr/bin/env python + +import math +import time +import rospy +import std_msgs +import actionlib +import json +from std_msgs.msg import String +from std_msgs.msg import Float32 +from arom.srv import * +from arom.msg import * +import serial +from astropy.time import Time + + +class weatherStation(object): + def __init__(self, parent = None, arg = None, name = "AWS01A", port="", connect = True, var = {}): + self.arg = arg + self.Autoconnect = connect + self.port = port + self.parent = parent + self.name = name + self.variables = var + + ## + ## Inicializace vlastniho ovladace + ## + + self.init() + + s_RegisterDriver = rospy.Service('driver/weatherStation/%s' %self.name, arom.srv.DriverControl, self.reset) + + ## + ## Ceka to na spusteni AROMbrain nodu + ## + + rospy.init_node('AROM_weatherStation') + rospy.loginfo("%s: wait_for_service: 'arom/RegisterDriver'" % self.name) + rospy.wait_for_service('arom/RegisterDriver') + rospy.loginfo("%s: >> brain found" % self.name) + + ## + ## Registrace zarizeni + ## >Arom returns 1 - OK, 0 - False + ## + + RegisterDriver = rospy.ServiceProxy('arom/RegisterDriver', arom.srv.RegisterDriver) + registred = RegisterDriver(name = self.name, sname= self.name, driver = 'AWS01A', device = 'weatherStation', status = 1) + rospy.loginfo("%s: >> register %s driver: %s" %(self.name, 'AWS01A', registred)) + + + ## + ## Ovladac se pripoji k montazi + ## + + if self.Autoconnect: + self.connect() + + ## + ## Ovladac pujde ukoncit + ## + + rare = rospy.Rate(0.5) + while not rospy.is_shutdown(): + self.mesure() + rare.sleep() + + + def reset(self, val=None): + pass + +############################################################################################################################################################################ +############################################################################################################################################################################ +############################################################################################################################################################################ +############################################################################################################################################################################ +############################################################################################################################################################################ + +###################################################################################### +###################################################################################### +## ## +## Driver for --AWS01A-- MLAB weather station ## +## ============================================ ## +## ## +## ## +###################################################################################### + +class AWS01B(weatherStation): + def init(self): + self.variables = { + 'AWS_LTS_temp': 0, + 'AWS_SHT_temp': 0, + 'AWS_SHT_humi': 0 + } + rospy.loginfo("AWS01A weather station requires 'pymlab_drive' service from 'ROSpymlabServer' node") + rospy.loginfo("run>> 'rosrun arom initPymlab.py'") + rospy.wait_for_service('pymlab_drive') + self.pymlab = rospy.ServiceProxy('pymlab_drive', PymlabDrive) + rospy.loginfo("%s: >> 'ROSpymlabServer' found" % self.name) + + + def mesure(self): + self.variables['AWS_LTS_temp'] = self.pymlab(device="AWS_temp", method="get_temp", parameters=None).value + self.variables['AWS_SHT_temp'] = self.pymlab(device="AWS_humi", method="get_temp_8bit", parameters=None).value + self.variables['AWS_SHT_humi'] = self.pymlab(device="AWS_humi", method="get_hum_8bit", parameters=None).value + print self.variables + rospy.set_param("weatherStation", str(self.variables)) + + + def connect(self): + pass + + + +if __name__ == '__main__': + cfg = rospy.get_param("ObservatoryConfig/file") + with open(cfg) as data_file: + config = json.load(data_file) + for x in config: + if x['name'] == sys.argv[1]: + break + weatherStation = locals()[x['driver']](arg = x) + #weatherStation = AWS01B() \ No newline at end of file diff --git a/src/initPymlab.py b/src/initPymlab.py new file mode 100755 index 0000000..f23e090 --- /dev/null +++ b/src/initPymlab.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python + +import sys +import pymlab +import rospy +import time +from pymlab import config +import sensor_server +from std_msgs.msg import String +from sensor_server.srv import * +from sensor_server.msg import * + + + +if __name__ == "__main__": + i2c = str({ + "port": 1, + }) + bus = str([ + { + "name": "AWS_temp", + "type": "lts01", + }, + { + "name": "AWS_humi", + "type": "sht25", + },{ + "name": "StatusLCD", + "type": "i2clcd", + }, + + ]) + + + msg_pymlab = rospy.Publisher('pymlab_server', PymlabServerStatusM, queue_size=10) + rospy.init_node('pymlab_client', anonymous=True) + + pymlab = rospy.ServiceProxy('pymlab_init', PymlabInit) + print pymlab(i2c=i2c, bus=bus) + + msg_pymlab.publish(name = "", data="{'rate': 0.01, 'start': True, 'AutoInputs': {}}") + diff --git a/src/move_mount.py b/src/move_mount.py index 22b967b..67abc9f 100755 --- a/src/move_mount.py +++ b/src/move_mount.py @@ -2,14 +2,36 @@ # license removed for brevity import rospy import sys +import time from arom.srv import * from arom.msg import * +import astropy.units as u +from astropy.coordinates import SkyCoord +from astropy.coordinates import EarthLocation +from astropy.time import Time + +observatory = EarthLocation(lat=49*u.deg, lon=14*u.deg, height=300*u.m) + def talker(): pub = rospy.Publisher('arom/mount/', arom.msg.DriverControlm, queue_size=10) rospy.init_node('controller', anonymous=True) - - pub.publish(name = 'mount', type = 'Slew', data = "{'ra':%s, 'dec':%s}" %(str(sys.argv[1]), str(sys.argv[2])), validate = '', check = '') + while True: + var = raw_input("Enter: ") + var = var.split(' ') + # print var + + if var[0] == 'radec': + pub.publish(name = 'mount', type = 'Slew', data = "{'ra':%s, 'dec':%s}" %(str(var[1]), str(var[2])), validate = '', check = '') + + elif var[0] == 'altaz': + coord = SkyCoord(alt = float(var[1]) * u.deg, az = float(var[2]) * u.deg, obstime = Time.now(), frame = 'altaz', location = observatory) + print coord.icrs.to_string('hmsdms') + pub.publish(name = 'mount', type = 'Slew', data = "{'ra':%s, 'dec':%s}" %(str(coord.icrs.ra.degree), str(coord.icrs.dec.degree)), validate = '', check = '') + elif var[0] == 'q': + break + else: + print "no value" if __name__ == '__main__': try: diff --git a/srv/PymlabDrive.srv b/srv/PymlabDrive.srv new file mode 100644 index 0000000..f75f96f --- /dev/null +++ b/srv/PymlabDrive.srv @@ -0,0 +1,5 @@ +string device +string method +string parameters +--- +string value diff --git a/srv/PymlabInit.srv b/srv/PymlabInit.srv new file mode 100644 index 0000000..84f97e3 --- /dev/null +++ b/srv/PymlabInit.srv @@ -0,0 +1,5 @@ +string i2c +string bus +string devices +--- +bool status diff --git a/srv/PymlabServerStatus.srv b/srv/PymlabServerStatus.srv new file mode 100644 index 0000000..5b57840 --- /dev/null +++ b/srv/PymlabServerStatus.srv @@ -0,0 +1,4 @@ +string name +string data +--- +int64 val diff --git a/srv/PymlabSetValue.srv b/srv/PymlabSetValue.srv new file mode 100644 index 0000000..f75f96f --- /dev/null +++ b/srv/PymlabSetValue.srv @@ -0,0 +1,5 @@ +string device +string method +string parameters +--- +string value