mirror rework to use checksum instead of outcome index on feedback

flexbe_core/src/flexbe_core/core/operatable_state.py

@@ -1,12 +1,13 @@
 #!/usr/bin/env python
 import rospy
+import zlib
 
 from flexbe_core.core.preemptable_state import PreemptableState
 from flexbe_core.core.operatable_state_machine import OperatableStateMachine
 
 from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached
 from flexbe_msgs.msg import Container, OutcomeRequest
-from std_msgs.msg import UInt8, String
+from std_msgs.msg import UInt8, String, Int32
 
 from flexbe_core.state_logger import StateLogger
 
@@ -26,8 +27,10 @@ def __init__(self, *args, **kwargs):
 
         self._mute = False  # is set to true when used in silent state machine (don't report transitions)
         self._sent_outcome_requests = []  # contains those outcomes that already requested a transition
+        self._sent_update = False
 
         self._outcome_topic = 'flexbe/mirror/outcome'
+        self._update_topic = 'flexbe/mirror/state_update'
         self._request_topic = 'flexbe/outcome_request'
         self._debug_topic = 'flexbe/debug/current_state'
         self._pub = ProxyPublisher()
@@ -73,6 +76,10 @@ def _build_msg(self, prefix, msg):
 
 
     def _operatable_execute(self, *args, **kwargs):
+        if self._is_controlled and not self._sent_update:
+            self._pub.publish(self._update_topic, Int32(zlib.adler32(self._get_path())))
+            self._sent_update = True
+
         outcome = self.__execute(*args, **kwargs)
 
         if self._is_controlled:
@@ -90,6 +97,7 @@ def _operatable_execute(self, *args, **kwargs):
             # autonomy level is high enough, report the executed transition
             elif outcome != OperatableState._loopback_name:
                 self._sent_outcome_requests = []
+                self._sent_update = False
                 rospy.loginfo("State result: %s > %s", self.name, outcome)
                 self._pub.publish(self._outcome_topic, UInt8(self._outcome_list.index(outcome)))
                 self._pub.publish(self._debug_topic, String("%s > %s" % (self._get_path(), outcome)))
@@ -106,6 +114,7 @@ def _notify_skipped(self):
     def _enable_ros_control(self):
         super(OperatableState, self)._enable_ros_control()
         self._pub.createPublisher(self._outcome_topic, UInt8)
+        self._pub.createPublisher(self._update_topic, Int32)
         self._pub.createPublisher(self._debug_topic, String)
         self._pub.createPublisher(self._request_topic, OutcomeRequest)
 

flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py

@@ -53,6 +53,7 @@ def __init__(self):
         self._outcome_topic = 'flexbe/mirror/outcome'
 
         self._struct_buffer = list()
+        self._last_update = None
 
         # listen for mirror message
         self._sub = ProxySubscriberCached()
@@ -63,6 +64,7 @@ def __init__(self):
         self._sub.subscribe('flexbe/status', BEStatus, self._status_callback)
         self._sub.subscribe('flexbe/mirror/sync', BehaviorSync, self._sync_callback)
         self._sub.subscribe('flexbe/mirror/preempt', Empty, self._preempt_callback)
+        self._sub.subscribe('flexbe/mirror/state_update', Int32, self._state_update_callback)
 
 
     def _mirror_callback(self, msg):
@@ -242,6 +244,10 @@ def _mirror_state_machine(self, msg):
         for con_msg in msg.containers:
             if con_msg.path.find('/') != -1:
                 self._state_checksums[zlib.adler32(con_msg.path)] = con_msg.path
+        # try to send a cached update if is known now
+        if self._last_update is not None and self._last_update in self._state_checksums:
+            current_state_path = self._state_checksums[self._last_update]
+            self._pub.publish('flexbe/behavior_update', current_state_path)
 
 
     def _add_node(self, msg, path):
@@ -298,7 +304,16 @@ def _preempt_callback(self, msg):
             rospy.logwarn('Explicit preempting is currently ignored, mirror should be preempted by onboard behavior.')
         else:
             rospy.logwarn('Could not preempt mirror because it seems not to be running!')
+
+
+    def _state_update_callback(self, msg):
+        self._last_update = msg.data
+        if msg.data not in self._state_checksums:
+            return # will try to apply last update when receiving new structure
 
+        current_state_path = self._state_checksums[self._last_update]
+        self._pub.publish('flexbe/behavior_update', current_state_path)
+        self._last_update = None # already sent update successfully
 
 
 

flexbe_mirror/src/flexbe_mirror/mirror_state.py

@@ -59,8 +59,8 @@ def execute(self, userdata):
 
     def on_enter(self, userdata):
         #rospy.loginfo("Mirror entering %s", self._target_path)
-        self._pub.publish('flexbe/behavior_update', String("/" + "/".join(self._target_path.split("/")[1:])))
-
+        #self._pub.publish('flexbe/behavior_update', String("/" + "/".join(self._target_path.split("/")[1:])))
+        pass