pymlab_bridge

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import sys
import json
from pymlab import config
import arom
import std_msgs
import time
from drivers.__init__ import AromNode


#uncomment for debbug purposes
import logging
logging.basicConfig(level=logging.DEBUG) 


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.hub_devices = [] # seznam modulu, ktere vyuzivaji HUB
        self.i2c = []
        self.bus = []

        bus_config = rospy.get_param('~config', "/home/odroid/robozor/station/pymlab_presets/telescope.json")
        
        try: bus_config = sys.argv[1]
        except: pass
        

        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': bus_config})
        self.init()


        rate = rospy.Rate(0.2)
        devices = self.devices
        while True:
            if self.err_count > 10:
                rospy.logwarn("restarting pymlab")
                self.init()
                self.err_count = 0

            rate.sleep()


    def init(self, cfg=None):
        print cfg
        try:
            #if cfg == None or cfg.default == True:
            print "Inicializace pymlabu (nova verze)"

            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 = []

            if cfg and cfg.config != '':
                rospy.set_param('/arom/node{}/feature/pymlab_structure/cfg'.format(rospy.get_name()), cfg.config)

            rospy.set_param('/arom/node%s/devices'%(rospy.get_name()), {})
            print "Pouzivana konfigurace:", rospy.get_param('/arom/node%s/feature/pymlab_structure/cfg'%(rospy.get_name()))

            with open(rospy.get_param('/arom/node%s/feature/pymlab_structure/cfg'%(rospy.get_name()))) 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%s/devices/'%(rospy.get_name())+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("#init"+repr(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:
                if x['type'] == "i2chub":
                    for y in x['children']:
                        print "NewDevice: [HUB] ", y['name'],
                        self.devices[y['name']] = self.pymlab_config.get_device(y['name'])
                        self.hub_devices.append(y['name'])
                        print "      [OK]"
                else:
                    print "NewDevice: ", x['name'],
                    self.devices[x['name']] = self.pymlab_config.get_device(x['name'])
                    print "      [OK]"
            except Exception, e:
                print "      [FAIL]"
                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 cfg.device in self.hub_devices:
                print "[HUB]",
                self.devices[cfg.device].route()
            #print self.devices
            #print getattr(self.devices[cfg.device], "route")()
        except Exception, e:
            rospy.logerr("#hub:"+repr(e))

        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
            print repr(reval)
            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.")
    while True:
        time.sleep(0.3)

    #rospy.spin()

if __name__ == "__main__":
    main()