IBA

Author: Michael Zechmair

hbp_nrp_cle/hbp_nrp_cle/cle/DeterministicClosedLoopEngine.py

@@ -60,6 +60,7 @@ def __init__(self,
                  brain_control_adapter,
                  brain_comm_adapter,
                  transfer_function_manager,
+                 external_module_array,
                  dt
                  ):
         """
@@ -78,6 +79,7 @@ def __init__(self,
         self.bca = brain_control_adapter
         self.bcm = brain_comm_adapter
         self.tfm = transfer_function_manager
+        self.ema = external_module_array
         # default timestep
         self.timestep = dt
 
@@ -130,6 +132,7 @@ def initialize(self, brain_file=None, **configuration):
         self.rca.initialize()
         self.bca.initialize()
         self.tfm.initialize('tfnode')
+        self.ema.initialize()
         cle.clock = 0.0
         self.start_time = 0.0
         self.elapsed_time = 0.0
@@ -229,6 +232,8 @@ def run_step(self, timestep):
         # self.tfm.run_neuron_to_robot(clk)
         self.tfm.run_tfs(clk)
 
+        self.ema.run_step()
+
         # update clock
         cle.clock += timestep
 
@@ -245,6 +250,7 @@ def shutdown(self):
         self.bcm.shutdown()
         self.rca.shutdown()
         self.bca.shutdown()
+        self.ema.shutdown()
 
     def start(self):
         """

hbp_nrp_cle/hbp_nrp_cle/externalsim/AsyncEmaCall.py

@@ -0,0 +1,27 @@
+from __future__ import print_function
+
+import rospy
+from concurrent.futures import ThreadPoolExecutor
+
+class AsyncServiceProxy(object):
+
+    def __init__(self, service_name, service_type, persistent=True,
+                 headers=None, callback=None):
+        """Create an asynchronous service proxy."""
+
+        self.executor = ThreadPoolExecutor(max_workers=1)
+        self.service_proxy = rospy.ServiceProxy(
+            service_name,
+            service_type,
+            persistent,
+            headers)
+        self.callback = callback
+
+    def __call__(self, *args, **kwargs):
+        """Get a Future corresponding to a call of this service."""
+
+        fut = self.executor.submit(self.service_proxy.call, *args, **kwargs)
+        if self.callback is not None:
+            fut.add_done_callback(self.callback)
+
+        return fut

hbp_nrp_cle/hbp_nrp_cle/externalsim/ExternalModule.py

@@ -0,0 +1,74 @@
+"""
+ExternalModule.py includes the corresponding CLE class for external ROS modules.
+"""
+
+__author__ = 'Omer Yilmaz'
+
+import os
+import threading
+from multiprocessing import Value
+import logging
+import rospy
+from hbp_nrp_cle.externalsim.AsyncEmaCall import AsyncServiceProxy
+from cle_ros_msgs.srv import Initialize, RunStep, RunStepRequest, Shutdown
+
+logger = logging.getLogger('hbp_nrp_cle')
+
+
+class ExternalModule(object):
+    """
+    External ROS modules have initialize, run_step and shutdown methods.
+    This class has the corresponding initialize, run_step and shutdown
+    methods which triggers the external ones through ROS service proxies.
+    Objects of this class is synchronized with the Deterministic Closed
+    Loop Engine via ExternalModuleManager.
+    """
+
+    def __init__(self, module_name):
+        self.service_name = 'emi/' + module_name + '_module/'
+        self.resp = None
+
+        rospy.wait_for_service(self.service_name + 'initialize')
+        self.initialize_proxy = AsyncServiceProxy(
+                self.service_name + 'initialize', Initialize, persistent=False)
+        
+        rospy.wait_for_service(self.service_name + 'run_step')
+        self.run_step_proxy = AsyncServiceProxy(
+                self.service_name + 'run_step', RunStep, persistent=True)
+        
+        rospy.wait_for_service(self.service_name + 'shutdown')
+        self.shutdown_proxy = AsyncServiceProxy(
+                self.service_name + 'shutdown', Shutdown, persistent=False)
+
+    def initialize(self):
+        """
+        This method triggers the initialize method served at the external module synchronously
+        with the CLE.
+        """
+        try:
+            self.resp = self.initialize_proxy()
+            return self.resp
+        except rospy.ServiceException as e:
+            logger.exception(self.service_name + 'initialize call failed: %s' % e)
+
+    def run_step(self):
+        """
+        This method triggers the run_step method served at the external module synchronously
+        with the CLE.
+        """
+        try:
+            fut = self.run_step_proxy()
+            return fut
+        except rospy.ServiceException as e:
+            logger.exception(self.service_name + 'run_step call failed: %s' % e)
+
+    def shutdown(self):
+        """
+        This method triggers the shutdown method served at the external module synchronously
+        with the CLE.
+        """
+        try:
+            fut = self.shutdown_proxy()
+            return fut
+        except rospy.ServiceException as e:
+            logger.exception(self.service_name + 'shutdown call failed: %s' % e)

hbp_nrp_cle/hbp_nrp_cle/externalsim/ExternalModuleManager.py

@@ -0,0 +1,76 @@
+"""
+The manager for the external modules which extend the NRP through ROS launch
+mechanism.
+"""
+
+__author__ = 'Omer Yilmaz'
+
+import concurrent.futures
+from multiprocessing import Process, Pool, cpu_count
+import re
+import rosservice
+from hbp_nrp_cle.externalsim.ExternalModule import ExternalModule
+
+
+class ExternalModuleManager(object):
+    """
+    This class automatically detects the external modules searching the ROS
+    services available at the ROS server. It keeps and array of the external
+    modules and calls initialize, run_step ans shutdown methods for each
+    external module. One object of this class is used by the Deterministic
+    Closed Loop Engine and is synchronized with it making every external module
+    on the array also synchronized.
+    """
+
+    def __init__(self):
+        self.module_names = []
+        for service in rosservice.get_service_list():
+            m = re.match(r"/emi/.*/initialize", str(service))
+            if m:
+                module_name = m.group(0)[5:-18]
+                self.module_names.append(module_name)
+
+        self.ema = []
+        if len(self.module_names) is not 0:
+            with concurrent.futures.ThreadPoolExecutor(max_workers=max(len(self.module_names), 1)) as executor:
+                future_results = [executor.submit(ExternalModule, x) for x in self.module_names]
+                concurrent.futures.wait(future_results)
+                for future in future_results:
+                    self.ema.append(future.result())
+
+
+    def initialize(self):
+        """
+        This method is used to run all initialize methods served at each external models at once.
+        """
+        if len(self.module_names) is not 0:
+            with concurrent.futures.ThreadPoolExecutor(max_workers=len(self.ema)) as executor:
+                future_results = [executor.submit(x.initialize) for x in self.ema]
+                concurrent.futures.wait(future_results)
+                for future in future_results:
+                    while not future.result().done():
+                        pass
+
+    def run_step(self):
+        """
+        This method is used to run all run_step methods served at each external models at once.
+        """
+        if len(self.module_names) is not 0:
+            with concurrent.futures.ThreadPoolExecutor(max_workers=len(self.ema)) as executor:
+                future_results = [executor.submit(x.run_step) for x in self.ema]
+                concurrent.futures.wait(future_results)
+                for future in future_results:
+                    while not future.result().done():
+                        pass
+
+    def shutdown(self):
+        """
+        This method is used to run all shutdown methods served at each external models at once.
+        """
+        with concurrent.futures.ThreadPoolExecutor(max_workers=len(self.ema)) as executor:
+            future_results = [executor.submit(x.shutdown) for x in self.ema]
+            concurrent.futures.wait(future_results)
+            for future in future_results:
+                while not future.result().done():
+                    pass
+

hbp_nrp_cle/hbp_nrp_cle/externalsim/__init__.py

@@ -0,0 +1,5 @@
+"""
+CLE part of the External Ros Modules API version 2.0.0
+"""
+
+__author__ = 'Omer Yilmaz'