New cfg scheme

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
 )

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"
+    }
+]

cfg/ZVPP_observatory.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<observatory>
+	<mount>
+		<name>Heq5</name>
+		<sname>EQmod</sname>
+		<driver>mount.EQmod</driver>
+		<telescope>
+			<name>Hlavni dalekohled</name>
+			<sname>main</sname>
+			<type>NWT</type>
+			<focal_lenght>1000</focal_lenght>
+			<diameter>130</diameter>
+			<camera>
+				<name>Hlavni>
+			</camera>
+		</telescope>
+		<telescope>
+			<name>Pointacni dalekohled</name>
+			<sname>pointing</sname>
+			<type>REF</type>
+			<focal_lenght>500</focal_lenght>
+			<diameter>100</diameter>
+
+		</telescope>
+	</mount>
+	<dome>
+		<camera></camera>
+		<camera></camera>
+		<weatherStation></weatherStation>
+		<
+	</dome>
+</observatory>

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"
+    }
+]

msg/PymlabServerStatusM.msg

@@ -0,0 +1,2 @@
+string name
+string data

msg/SensorValues.msg

@@ -0,0 +1,2 @@
+string name
+float32 value

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

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,14 +28,16 @@ def mountSlew(self, cor = [0,0]):
     def __init__(self):
         rospy.init_node('AROM_brain')
         self.mount = {}
+        self.weatherStation = {}
         self.camera = {}
         self.devices = {}
 
         from drivers import mount, camera, roof, weatherStation, focuser, rotator
         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__":