diff --git a/CMakeLists.txt b/CMakeLists.txt index 6aa84de..e51eddc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,10 @@ add_message_files( DriverControlm.msg msg_pymlabInit.msg # pro inicializaci PYMLAB serveru msg_WeatherStation.msg + msg_aws.msg + + CameraParam.msg + # Message1.msg # Message2.msg ) @@ -34,6 +38,10 @@ add_service_files( PymlabInit.srv # Pro inicializaci PYMLABU PymlabServerStatus.srv # Pro PymlabDrive.srv # Pro + + setCameraParam.srv + + # Service1.srv # Service2.srv ) diff --git a/DOC/scheme_mount.dia b/DOC/scheme_mount.dia new file mode 100644 index 0000000..fc11e8b Binary files /dev/null and b/DOC/scheme_mount.dia differ diff --git a/msg/CameraParam.msg b/msg/CameraParam.msg new file mode 100644 index 0000000..779582a --- /dev/null +++ b/msg/CameraParam.msg @@ -0,0 +1,4 @@ +# tato zprava je vyuzita v setCameraParam.srv - NEMAZAT :) samostatnj +Header header +string param +string value \ No newline at end of file diff --git a/msg/msg_aws.msg b/msg/msg_aws.msg new file mode 100644 index 0000000..fd6afc9 --- /dev/null +++ b/msg/msg_aws.msg @@ -0,0 +1,2 @@ +#Header header +float32 value \ No newline at end of file diff --git a/src/ROSpymlabServer.py b/src/ROSpymlabServer.py index 5fa06d7..f977712 100755 --- a/src/ROSpymlabServer.py +++ b/src/ROSpymlabServer.py @@ -11,6 +11,7 @@ import time from arom.srv import * from arom.msg import * +from drivers.__init__ import AromNode def server(req): @@ -18,7 +19,12 @@ def server(req): print "Returning [%s + %s]"%(req.name, req.data) return GetSensValResponse( 10 ) -class pymlab_server(): +class pymlab_server(AromNode): + node_name = "pymlab_node" + node_type = "pymlab" + node_pymlab = True + + def __init__(self): self.err_count = 0 self.pymlab_read = False # slouzi k ochrane pymlabu pred pokusem o dve cteni zaroven ... @@ -27,6 +33,16 @@ def __init__(self): self.bus = [] + rospy.Subscriber("pymlab_server", msg_pymlabInit, self.status) + + s1 = rospy.Service('pymlab_init', PymlabInit, self.init) + s2 = rospy.Service('pymlab_server', PymlabServerStatus, self.status) + s3 = rospy.Service('pymlab_drive', PymlabDrive, self.drive) + + AromNode.__init__(self) + self.set_feature('pymlab_structure',{'i2c': "", 'bus': ""}) + + def init(self, cfg=None): try: self.status = False @@ -128,13 +144,8 @@ def drive(self, cfg): # zpracovava requesty ostatatich nodu def main(): ps = pymlab_server() - rospy.init_node('pymlab_node') - - rospy.Subscriber("pymlab_server", msg_pymlabInit, 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) # slouzi k prijmuti requestu + #rospy.init_node('pymlab_node') + # slouzi k prijmuti requestu rospy.loginfo("Ready to get work.") rospy.spin() diff --git a/src/drivers/awsAdvance b/src/drivers/awsAdvance new file mode 100755 index 0000000..6ba6a4b --- /dev/null +++ b/src/drivers/awsAdvance @@ -0,0 +1,76 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import math +import time +import rospy +import std_msgs +import json +from std_msgs.msg import String +from arom.srv import * +from arom.msg import * +import numpy as np +from __init__ import AromNode + + + +class awsAdvance(AromNode): + node_name = "awsAdvance" + node_type = "awsAdvance" + node_pymlab = True + + def __init__(self): + self.dataValues = {} + self.dataPublishers={} + + #rospy.Subscriber("/mount/laser", String, callback_btn) + AromNode.__init__(self) + self.set_feature('aws_setting', {'cfg': "/home/odroid/robozor/station/aws_presets/ZVPP.json"}) + self.pymlab = rospy.ServiceProxy('pymlab_drive', PymlabDrive) + + with open(rospy.get_param('/arom/node/awsAdvance/feature/aws_setting/cfg')) as data_file: #TODO: retezec k paramserveru podle nazvu nodu atd... + self.cfg_json = json.load(data_file) + + print json.dumps(self.cfg_json, indent=4, sort_keys=False) + self.awsPublisher = rospy.Publisher('/arom/node/aws', std_msgs.msg.String, queue_size=10) + for device in self.cfg_json['sensors'] + self.cfg_json['calculation']: + self.dataPublishers[device['name']] = rospy.Publisher('/arom/node/aws/'+device['name'], arom.msg.msg_aws, queue_size=10) + + + rate = rospy.Rate(0.25) + while not rospy.is_shutdown(): + try: + print ">>>", self.dataValues + for i, device in enumerate(self.cfg_json['sensors']): + try: + value = eval(self.pymlab(device=device['device'], method=device['method']).value) + self.dataPublishers[device['name']].publish(value) + self.dataValues[device['name']] = value + self.cfg_json['sensors'][i]['value'] = value + #print self.dataValues[device['name']], device['device'], device['method'] + except Exception, e: + print e + + for i, device in enumerate(self.cfg_json['calculation']): + try: + value = self.calc(values=device['values']) + self.dataPublishers[device['name']].publish(value) + self.dataValues[device['name']] = value + self.cfg_json['calculation'][i]['value'] = value + #print self.dataValues[device['name']], device['device'], device['method'] + except Exception, e: + print e + + self.awsPublisher.publish(json.dumps(self.cfg_json)) + + except Exception, e: + print e + rate.sleep() + + def calc(self, values): + pass + + + +if __name__ == '__main__': + m = awsAdvance() \ No newline at end of file diff --git a/src/drivers/lcdInfo.py b/src/drivers/lcdInfo.py index 61c6db4..b5df547 100755 --- a/src/drivers/lcdInfo.py +++ b/src/drivers/lcdInfo.py @@ -12,7 +12,6 @@ from arom.srv import * from arom.msg import * import numpy as np -import pylirc from __init__ import AromNode try: @@ -277,5 +276,5 @@ def getWeather(self): if __name__ == '__main__': - m = lcdInfo16x2(file='/home/odroid/rosws/src/AROM/cfg/lcdAROM.xml') + m = lcdInfo16x2(file='/home/odroid/ros_ws/src/AROM/cfg/lcdAROM.xml') #m.loadCFG('/home/odroid/rosws/src/AROM/cfg/lcdAROM.xml') diff --git a/src/drivers/mount.py b/src/drivers/mount.py index 1ff3a0c..e031381 100755 --- a/src/drivers/mount.py +++ b/src/drivers/mount.py @@ -29,7 +29,7 @@ from astropy.coordinates import ICRS, Galactic, FK4, FK5, AltAz # Low-level frames from astropy.coordinates import Angle, Latitude, Longitude # Angles from astropy.coordinates import EarthLocation -from astropy.coordinates import get_sun, get_body +from astropy.coordinates import get_sun #, get_body from astropy.coordinates import solar_system_ephemeris #from astroquery.simbad import Simbad @@ -99,9 +99,9 @@ def __init__(self, parent = None, arg = None, name = "mount", port="", connect = split = lastBtn.split(" ") self.mount.Slew(SkyCoord.from_name(split[1])) - elif "solar" in lastBtn: - split = lastBtn.split(" ") - self.mount.Slew(get_body(split[1], time = Time.now(), location = self.mount.getObs()).icrs) + #elif "solar" in lastBtn: + # split = lastBtn.split(" ") + # self.mount.Slew(get_body(split[1], time = Time.now(), location = self.mount.getObs()).icrs) elif "sun" in lastBtn: print get_sun(Time.now()).icrs @@ -204,4 +204,4 @@ def send_position(self, object): if __name__ == '__main__': - m = mount() \ No newline at end of file + m = mount() diff --git a/src/pymlab_bridge b/src/pymlab_bridge new file mode 100755 index 0000000..98c5b4b --- /dev/null +++ b/src/pymlab_bridge @@ -0,0 +1,179 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import rospy +import json +from pymlab import config +import arom +import std_msgs +import time +from drivers.__init__ import AromNode + + +def server(req): + print req + print "Returning [%s + %s]"%(req.name, req.data) + return GetSensValResponse( 10 ) + +class pymlab_server(AromNode): + node_name = "pymlab_node" + node_type = "pymlab" + node_pymlab = True + + + def __init__(self): + self.err_count = 0 + self.pymlab_read = False # slouzi k ochrane pymlabu pred pokusem o dve cteni zaroven ... + self.devices = {} + self.i2c = [] + self.bus = [] + + + rospy.Subscriber("pymlab_server", arom.msg.msg_pymlabInit, self.status) + + s1 = rospy.Service('pymlab_init', arom.srv.PymlabInit, self.init) + s2 = rospy.Service('pymlab_server', arom.srv.PymlabServerStatus, self.status) + s3 = rospy.Service('pymlab_drive', arom.srv.PymlabDrive, self.drive) + + AromNode.__init__(self) + self.set_feature('pymlab_structure',{'cfg': "/home/odroid/robozor/station/pymlab_presets/ZVPP.json"}) + + + def init(self, cfg=None): + print cfg + try: + if cfg.default == True: + + self.i2c = [] #TODO: moznost pridavat dalsi smernice za behu - tzn. pridavat jen cast... To by bylo v zavislosti na nejake promene v srv filu + self.bus = [] + rospy.set_param('/arom/node/pymlab_node/devices', {}) + + with open(rospy.get_param('/arom/node/pymlab_node/feature/pymlab_structure/cfg')) as data_file: #TODO: retezec k paramserveru podle nazvu nodu atd... + data = json.load(data_file)['children'] + + for bus in data: + try: + self.cfg_i2c = bus + #self.cfg_i2c.pop('children', None); #TODO: zde by bylo dobre z JSONu odebrat 'children' index + + self.cfg_bus = bus['children'] + + print json.dumps(self.cfg_i2c, indent=4, sort_keys=False) + print json.dumps(self.cfg_bus, indent=4, sort_keys=False) + rospy.set_param('/arom/node/pymlab_node/devices/'+bus['name'], self.cfg_bus) + + self.status = False + + self.i2c.append(self.cfg_i2c) + self.bus.append(self.cfg_bus) + + rospy.loginfo("konfiguracni data: \n\r %s" %str(cfg)) + self.init_bus(i2c = self.cfg_i2c, bus = self.cfg_bus) + except Exception, e: + rospy.logerr(e) + + rospy.loginfo("Inicializace pymlabu problehla uspesne") + return True + + else: #TODO: Tato cast by mela byt v budoucnu odstranena.... + self.status = False + + self.cfg_i2c = eval(cfg.i2c) + self.i2c.append(self.cfg_i2c) + self.cfg_bus = eval(cfg.bus) + self.bus.append(self.cfg_bus) + + rospy.loginfo("konfiguracni data: \n\r %s" %str(cfg)) + self.init_bus(i2c = self.cfg_i2c, bus = self.cfg_bus) + + return True + except Exception, e: + rospy.logerr("#03:"+repr(e)) + return False + + def init_bus(self, i2c, bus): + # + # Tato funkce provede zkonfigurovani pymlabu (config.Config()) + # na jedno sbernici a pak si vytvori objekt kazdeho senzoru + # zapojeneho do odpovidajici sbernice + # + + self.pymlab_config = config.Config(i2c = i2c, bus = bus) + self.pymlab_config.initialize() + for x in bus: + try: + self.devices[x['name']] = self.pymlab_config.get_device(x['name']) + print x, self.devices[x['name']] + except Exception, e: + rospy.logerr("#02:"+repr(e)) + + + 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 "start" in ecfg: + rospy.loginfo("Starting PyMLAB ROS server with: %s" %(repr(self.devices))) + self.status = True + rate = rospy.Rate(0.1) + devices = self.devices + while True: + if self.err_count > 100: + rospy.logwarn("restarting pymlab") + self.err_count = 0 + for i, bus in enumerate(self.bus): + try: + rospy.loginfo("reloading pymlab bus:" + repr(bus)) + self.init_bus(i2c = self.i2c[i], bus = bus) + except Exception, e: + rospy.logerr(e) + + rate.sleep() + return True + + def drive(self, cfg): # zpracovava requesty ostatatich nodu + parameters = cfg.parameters + method = cfg.method + device = cfg.device + print "parameters '%s', method '%s', device '%s'" %(cfg.parameters, cfg.method, cfg.device) + while self.pymlab_read: time.sleep(0.01) + self.pymlab_read = True + try: + if parameters == "" or parameters == None: + reval = getattr(self.devices[cfg.device], cfg.method)() + elif isinstance(eval(parameters), tuple): + print "#Tuple:", eval(parameters) + reval = getattr(self.devices[cfg.device], cfg.method)(*eval(parameters)) + elif isinstance(eval(parameters), dict): + print "#Dictionary:", eval(parameters) + reval = getattr(self.devices[cfg.device], cfg.method)(**eval(parameters)) + else: + reval = getattr(self.devices[cfg.device], cfg.method)(eval(parameters)) + print "#None" + self.pymlab_read = False + return str(reval) + except Exception, e: + self.err_count += 1 + rospy.logerr("#01:"+repr(e)) + self.pymlab_read = False + return str(False) + +def main(): + ps = pymlab_server() + #rospy.init_node('pymlab_node') + # slouzi k prijmuti requestu + + rospy.loginfo("Ready to get work.") + rospy.spin() + +if __name__ == "__main__": + main() diff --git a/srv/PymlabInit.srv b/srv/PymlabInit.srv index 84f97e3..736fb97 100644 --- a/srv/PymlabInit.srv +++ b/srv/PymlabInit.srv @@ -1,5 +1,7 @@ string i2c string bus string devices +string config +byte default --- bool status diff --git a/srv/setCameraParam.srv b/srv/setCameraParam.srv new file mode 100644 index 0000000..23670b7 --- /dev/null +++ b/srv/setCameraParam.srv @@ -0,0 +1,5 @@ +Header header +#arom/CameraParam parameter +--- +bool success +string status_message \ No newline at end of file