Merge pull request #26 from jgdo/automatic_reload

Automatic reload

flexbe_core/src/flexbe_core/behavior.py

@@ -2,6 +2,9 @@
 import rospy
 import smach_ros
 import smach
+import importlib
+import sys
+import string
 
 from flexbe_core import OperatableStateMachine, LockableStateMachine
 from flexbe_core.core import PreemptableState
@@ -61,6 +64,24 @@ def add_parameter(self, name, default):
         """
         setattr(self, name, default)
 
+    def reload_class(self, class_obj):
+        """
+        reloads the module of a class
+        necessary since the class definition might have changed during execution
+        see http://stackoverflow.com/questions/9645388/dynamically-reload-a-class-definition-in-python
+        
+        @type class_obj: class
+        @param class_obj: The class whose module is to be reloaded
+        """
+
+        module_name = class_obj.__module__
+        module = sys.modules[module_name]
+        pycfile = module.__file__
+        modulepath = string.replace(pycfile, ".pyc", ".py")
+        code=open(modulepath, 'rU').read()
+        compile(code, module_name, "exec")
+        module = reload(module)
+        return getattr(module,class_obj.__name__)
 
     def add_behavior(self, behavior_class, behavior_id):
         """
@@ -75,11 +96,12 @@ def add_behavior(self, behavior_class, behavior_id):
         """
         if not hasattr(self, 'contains'):
             rospy.logerr('Behavior was not initialized! Please call superclass constructor.')
-
-        instance = behavior_class()
-        self.contains[behavior_id] = instance
 
+        behavior_class = self.reload_class(behavior_class)
 
+        instance = behavior_class()
+        self.contains[behavior_id] = instance
+
     def use_behavior(self, behavior_class, behavior_id):
         """
         Creates a state machine implementing the given behavior to use it in the behavior state machine.

flexbe_core/src/flexbe_core/reload_importer.py

@@ -0,0 +1,81 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import __builtin__
+import os
+import sys
+
+
+class ReloadImporter:
+
+    """Overrides the builtin import and automatically reloads all modules which are imported from on
+    of the reload paths after calling enable."""
+
+    def __init__(self):
+        self._excluded_modules = sys.modules.keys()
+        self._reload_paths = None
+        self._import_stack = []
+        self._reloaded_modules = set()
+        self._import = __builtin__.__import__
+
+    def enable(self):
+        __builtin__.__import__ = self._reimport
+
+    def disable(self):
+        __builtin__.__import__ = self._import
+
+    def add_reload_path(self, path):
+        if self._reload_paths is None:
+            self._reload_paths = tuple()
+        self._reload_paths += (os.path.abspath(path),)
+
+    def _reload(self, module):
+        if module.__name__ not in self._import_stack and module.__name__ in sys.modules:
+            if not self._import_stack:
+                self._reloaded_modules.clear()
+            self._import_stack.append(module.__name__)
+            # force reload
+            if module.__name__ not in self._reloaded_modules:
+                reload(module)
+                self._reloaded_modules.add(module.__name__)
+            self._import_stack.pop()
+
+    def _reimport(self, name, globals_=None, locals_=None, fromlist=None, level=-1):
+        module = self._import(name, globals_, locals_, fromlist if not None else [], level if not None else -1)
+
+        if module.__name__ not in self._excluded_modules and \
+            (self._reload_paths is None or \
+              (hasattr(module, '__file__') and len([p for p in self._reload_paths if module.__file__.startswith(p)]) > 0) \
+            ):
+            self._reload(module)
+
+        return module

flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py

@@ -14,6 +14,13 @@
 from flexbe_msgs.msg import ContainerStructure, BehaviorSync, BEStatus
 from std_msgs.msg import Empty, String, Int32, UInt8
 
+
+import inspect # FIXME remove
+
+def lineno():  # FIXME remove
+    """Returns the current line number in our program."""
+    return inspect.currentframe().f_back.f_lineno
+
 '''
 Created on 08.05.2013
 
@@ -65,26 +72,36 @@ def __init__(self):
 
 
     def _mirror_callback(self, msg):
+        #print(lineno())
         rate = rospy.Rate(10)
         while self._stopping:
             rate.sleep()
-
+
+        print(lineno())
         if self._running:
             rospy.logwarn('Received a new mirror structure while mirror is already running, adding to buffer (ID: %s).' % str(msg.behavior_id))
+            #print(lineno())
         elif self._active_id != 0 and msg.behavior_id != self._active_id:
             rospy.logwarn('ID of received mirror structure (%s) does not match expected ID (%s), will ignore.' % (str(msg.behavior_id), str(self._active_id)))
+            #print(lineno())
             return
         else:
             rospy.loginfo('Received a new mirror structure for ID %s' % str(msg.behavior_id))
+            #print(lineno())
 
         self._struct_buffer.append(msg)
-
-        if self._active_id == msg.behavior_id:
+
+        #print("self._active_id = " + str(self._active_id) + ", msg.behavior_id = " + str(msg.behavior_id));
+        # FIXME: recognise if this is a callback from own request or from UI
+        if False and self._active_id == msg.behavior_id:
+            print(lineno())
             self._struct_buffer = list()
             self._mirror_state_machine(msg)
             rospy.loginfo('Mirror built.')
 
             self._execute_mirror()
+        #else:
+        #    print(lineno())
 
 
     def _status_callback(self, msg):
@@ -118,6 +135,10 @@ def _start_mirror(self, msg):
             if struct.behavior_id == self._active_id:
                 self._mirror_state_machine(struct)
                 rospy.loginfo('Mirror built.')
+                #print("self._running = " + str(self._running))
+                #print("self._stopping = " + str(self._stopping))
+                #print("self._active_id = " + str(self._active_id))
+                #print("self._starting_path = " + str(self._starting_path))
             else:
                 rospy.logwarn('Discarded mismatching buffered structure for ID %s' % str(struct.behavior_id))
 
@@ -127,6 +148,8 @@ def _start_mirror(self, msg):
             self._pub.publish('flexbe/request_mirror_structure', Int32(msg.behavior_id))
             self._active_id = msg.behavior_id
             return
+        #else:
+        #    rospy.loginfo('Correct mirror structure given')
 
         self._execute_mirror()
 

flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py

@@ -15,6 +15,7 @@
 import xml.etree.ElementTree as ET
 
 from flexbe_core import Logger
+from flexbe_core.reload_importer import ReloadImporter
 
 from flexbe_msgs.msg import BehaviorSelection, BEStatus, ContainerStructure, CommandFeedback
 from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached
@@ -63,7 +64,7 @@ def __init__(self):
         if not os.path.exists(self._tmp_folder):
             os.makedirs(self._tmp_folder)
         sys.path.append(self._tmp_folder)
-
+        
         # prepare manifest folder access
         manifest_folder = os.path.join(rp.get_path(behaviors_package), 'behaviors/')
         rospy.loginfo("Parsing available behaviors...")
@@ -75,7 +76,12 @@ def __init__(self):
             e = m.find("executable")
             self._behavior_lib[i] = {"name": m.get("name"), "package": e.get("package_path").split(".")[0], "file": e.get("package_path").split(".")[1], "class": e.get("class")}
 #            rospy.loginfo("+++ " + self._behavior_lib[i]["name"])
-
+
+        # enable automatic reloading of all subsequent modules on reload
+        _reload_importer = ReloadImporter()
+        _reload_importer.add_reload_path(manifest_folder)
+        _reload_importer.enable()
+
         self._pub = ProxyPublisher()
         self._sub = ProxySubscriberCached()
 
@@ -97,14 +103,12 @@ def __init__(self):
         rospy.sleep(0.5) # wait for publishers etc to really be set up
         self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY))
         rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
-
 
     def _behavior_callback(self, msg):
         thread = threading.Thread(target=self._behavior_execution, args=[msg])
         thread.daemon = True
         thread.start()
 
-
     def _behavior_execution(self, msg):
         if self._running:
             Logger.loginfo('--> Initiating behavior switch...')

flexbe_widget/bin/flexbe_app

@@ -1,7 +1,7 @@
 #!/bin/bash
 if [ -z ${FLEXBE_ID} ]; then
   # use official flexbe app id
-  FLEXBE_ID=ofahdhifmkhpmlkhcpbhjkagkkoofeco
+  FLEXBE_ID=bainobklghincdjccndeffjmkgfkcgah
 else
   echo "Starting FLexBE App with local app id from environment FLEXBE_ID=${FLEXBE_ID}"
 fi