new node for AWS - "awsAdvance", updated "ROSpymlabServer" and new "p…

…ymlab_bridgi", some new services and toppics, integration of webUI

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
 )

msg/CameraParam.msg

@@ -0,0 +1,4 @@
+# tato zprava je vyuzita v setCameraParam.srv - NEMAZAT :) samostatnj
+Header header
+string param
+string value

msg/msg_aws.msg

@@ -0,0 +1,2 @@
+#Header header
+float32 value

src/ROSpymlabServer.py

@@ -11,14 +11,20 @@
 import time
 from arom.srv import *
 from arom.msg import *
+from drivers.__init__ import AromNode
 
 
 def server(req):
     print 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()

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()

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')

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()
+    m = mount()

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()

srv/PymlabInit.srv

@@ -1,5 +1,7 @@
 string i2c
 string bus
 string devices
+string config
+byte default
 ---
 bool status