From 8948ad1324da7f49746670be408304bb6252e883 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 13 Dec 2019 17:11:08 +0100 Subject: [PATCH] [flexbe_mirror] Fix mirror sync lock (see FlexBE/flexbe_app#47) --- .../src/flexbe_mirror/flexbe_mirror.py | 147 +++++++++--------- 1 file changed, 72 insertions(+), 75 deletions(-) diff --git a/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py index 7f6b9770..d3d2aa6e 100644 --- a/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py +++ b/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py @@ -100,74 +100,72 @@ def _status_callback(self, msg): def _start_mirror(self, msg): - self._sync_lock.acquire() - rate = rospy.Rate(10) - while self._stopping: - rate.sleep() - - if self._running: - rospy.logwarn('Tried to start mirror while it is already running, will ignore.') - return - - if len(msg.args) > 0: - self._starting_path = "/" + msg.args[0][1:].replace("/", "_mirror/") + "_mirror" + with self._sync_lock: + rate = rospy.Rate(10) + while self._stopping: + rate.sleep() - self._active_id = msg.behavior_id + if self._running: + rospy.logwarn('Tried to start mirror while it is already running, will ignore.') + return - while self._sm is None and len(self._struct_buffer) > 0: - struct = self._struct_buffer[0] - self._struct_buffer = self._struct_buffer[1:] - if struct.behavior_id == self._active_id: - self._mirror_state_machine(struct) - rospy.loginfo('Mirror built.') - else: - rospy.logwarn('Discarded mismatching buffered structure for checksum %s' % str(struct.behavior_id)) + if len(msg.args) > 0: + self._starting_path = "/" + msg.args[0][1:].replace("/", "_mirror/") + "_mirror" - if self._sm is None: - rospy.logwarn('Missing correct mirror structure, requesting...') - rospy.sleep(0.2) # no clean wayacquire to wait for publisher to be ready... - self._pub.publish('flexbe/request_mirror_structure', Int32(msg.behavior_id)) self._active_id = msg.behavior_id - return - self._sync_lock.release() + + while self._sm is None and len(self._struct_buffer) > 0: + struct = self._struct_buffer[0] + self._struct_buffer = self._struct_buffer[1:] + if struct.behavior_id == self._active_id: + self._mirror_state_machine(struct) + rospy.loginfo('Mirror built.') + else: + rospy.logwarn('Discarded mismatching buffered structure for checksum %s' % str(struct.behavior_id)) + + if self._sm is None: + rospy.logwarn('Missing correct mirror structure, requesting...') + rospy.sleep(0.2) # no clean wayacquire to wait for publisher to be ready... + self._pub.publish('flexbe/request_mirror_structure', Int32(msg.behavior_id)) + self._active_id = msg.behavior_id + return self._execute_mirror() def _stop_mirror(self, msg): - self._sync_lock.acquire() - self._stopping = True - if self._sm is not None and self._running: - if msg is not None and msg.code == BEStatus.FINISHED: - rospy.loginfo('Onboard behavior finished successfully.') - self._pub.publish('flexbe/behavior_update', String()) - elif msg is not None and msg.code == BEStatus.SWITCHING: - self._starting_path = None - rospy.loginfo('Onboard performing behavior switch.') - elif msg is not None and msg.code == BEStatus.READY: - rospy.loginfo('Onboard engine just started, stopping currently running mirror.') - self._pub.publish('flexbe/behavior_update', String()) - elif msg is not None: - rospy.logwarn('Onboard behavior failed!') - self._pub.publish('flexbe/behavior_update', String()) - - PreemptableState.preempt = True - rate = rospy.Rate(10) - while self._running: - rate.sleep() - else: - rospy.loginfo('No onboard behavior is active.') + with self._sync_lock: + self._stopping = True + if self._sm is not None and self._running: + if msg is not None and msg.code == BEStatus.FINISHED: + rospy.loginfo('Onboard behavior finished successfully.') + self._pub.publish('flexbe/behavior_update', String()) + elif msg is not None and msg.code == BEStatus.SWITCHING: + self._starting_path = None + rospy.loginfo('Onboard performing behavior switch.') + elif msg is not None and msg.code == BEStatus.READY: + rospy.loginfo('Onboard engine just started, stopping currently running mirror.') + self._pub.publish('flexbe/behavior_update', String()) + elif msg is not None: + rospy.logwarn('Onboard behavior failed!') + self._pub.publish('flexbe/behavior_update', String()) + + PreemptableState.preempt = True + rate = rospy.Rate(10) + while self._running: + rate.sleep() + else: + rospy.loginfo('No onboard behavior is active.') - self._active_id = 0 - self._sm = None - self._current_struct = None - self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True) + self._active_id = 0 + self._sm = None + self._current_struct = None + self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True) - if msg is not None and msg.code != BEStatus.SWITCHING: - rospy.loginfo('\033[92m--- Behavior Mirror ready! ---\033[0m') + if msg is not None and msg.code != BEStatus.SWITCHING: + rospy.loginfo('\033[92m--- Behavior Mirror ready! ---\033[0m') - self._stopping = False - self._sync_lock.release() + self._stopping = False def _sync_callback(self, msg): @@ -183,25 +181,24 @@ def _sync_callback(self, msg): def _restart_mirror(self, msg): - self._sync_lock.acquire() - rospy.loginfo('Restarting mirror for synchronization...') - self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True) - if self._sm is not None and self._running: - PreemptableState.preempt = True - rate = rospy.Rate(10) - while self._running: - rate.sleep() - self._sm = None - if msg.current_state_checksum in self._state_checksums: - current_state_path = self._state_checksums[msg.current_state_checksum] - self._starting_path = "/" + current_state_path[1:].replace("/", "_mirror/") + "_mirror" - rospy.loginfo('Current state: %s' % current_state_path) - try: - self._mirror_state_machine(self._current_struct) - rospy.loginfo('Mirror built.') - except (AttributeError, smach.InvalidStateError): - rospy.loginfo('Stopping synchronization because behavior has stopped.') - self._sync_lock.release() + with self._sync_lock: + rospy.loginfo('Restarting mirror for synchronization...') + self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True) + if self._sm is not None and self._running: + PreemptableState.preempt = True + rate = rospy.Rate(10) + while self._running: + rate.sleep() + self._sm = None + if msg.current_state_checksum in self._state_checksums: + current_state_path = self._state_checksums[msg.current_state_checksum] + self._starting_path = "/" + current_state_path[1:].replace("/", "_mirror/") + "_mirror" + rospy.loginfo('Current state: %s' % current_state_path) + try: + self._mirror_state_machine(self._current_struct) + rospy.loginfo('Mirror built.') + except (AttributeError, smach.InvalidStateError): + rospy.loginfo('Stopping synchronization because behavior has stopped.') self._execute_mirror()