From db5a234630389ec15afeea17fcfea329be6eecaf Mon Sep 17 00:00:00 2001 From: "David C. Conner" Date: Sat, 8 Jun 2019 14:04:22 -0400 Subject: [PATCH] mirror rework to use checksum instead of outcome index on feedback --- .../src/flexbe_core/core/operatable_state.py | 11 ++++++++++- flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py | 15 +++++++++++++++ flexbe_mirror/src/flexbe_mirror/mirror_state.py | 4 ++-- 3 files changed, 27 insertions(+), 3 deletions(-) diff --git a/flexbe_core/src/flexbe_core/core/operatable_state.py b/flexbe_core/src/flexbe_core/core/operatable_state.py index 68f4ad32..062cfc6a 100644 --- a/flexbe_core/src/flexbe_core/core/operatable_state.py +++ b/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) diff --git a/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py index 7f6b9770..4653837d 100644 --- a/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py +++ b/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 diff --git a/flexbe_mirror/src/flexbe_mirror/mirror_state.py b/flexbe_mirror/src/flexbe_mirror/mirror_state.py index 378b2b3d..969b51ed 100644 --- a/flexbe_mirror/src/flexbe_mirror/mirror_state.py +++ b/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