class ManuallyTransitionableState(MonitoringState): """ A state for that a desired outcome can be declared. If any outcome is declared, this outcome is forced. """ def __init__(self, *args, **kwargs): super(ManuallyTransitionableState, self).__init__(*args, **kwargs) self._force_transition = False self.__execute = self.execute self.execute = self._manually_transitionable_execute self._feedback_topic = '/flexbe/command_feedback' self._transition_topic = '/flexbe/command/transition' self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() def _manually_transitionable_execute(self, *args, **kwargs): if self._is_controlled and self._sub.has_buffered( self._transition_topic): command_msg = self._sub.get_from_buffer(self._transition_topic) self._pub.publish( self._feedback_topic, CommandFeedback(command="transition", args=[command_msg.target, self.name])) if command_msg.target != self.name: rospy.logwarn("--> Requested outcome for state " + command_msg.target + " but active state is " + self.name) else: self._force_transition = True outcome = self._outcome_list[command_msg.outcome] rospy.loginfo("--> Manually triggered outcome " + outcome + " of state " + self.name) return outcome # return the normal outcome self._force_transition = False return self.__execute(*args, **kwargs) def _enable_ros_control(self): super(ManuallyTransitionableState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._transition_topic, OutcomeRequest) self._sub.enable_buffer(self._transition_topic) def _disable_ros_control(self): super(ManuallyTransitionableState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._transition_topic)
class GetJointValuesDynState(EventState): ''' Retrieves current values of specified joints. ># joint_names string[] List of desired joint names. #> joint_values float[] List of current joint values. <= retrieved Joint values are available. ''' def __init__(self): ''' Constructor ''' super(GetJointValuesDynState, self).__init__( outcomes=['retrieved'], output_keys=['joint_values'], input_keys=['joint_names']) self._topic = '/joint_states' self._sub = ProxySubscriberCached({self._topic: JointState}) self._joints = None self._joint_values = list() def execute(self, userdata): while self._sub.has_buffered(self._topic): msg = self._sub.get_from_buffer(self._topic) for i in range(len(msg.name)): if msg.name[i] in self._joints \ and self._joint_values[self._joints.index(msg.name[i])] is None: self._joint_values[self._joints.index(msg.name[i])] = msg.position[i] if all(v is not None for v in self._joint_values): userdata.joint_values = self._joint_values return 'retrieved' def on_enter(self, userdata): self._sub.enable_buffer(self._topic) self._joint_values = [None] * len(self._joints) self._joints = userdata.joint_names def on_exit(self, userdata): self._sub.disable_buffer(self._topic)
class ManuallyTransitionableState(MonitoringState): """ A state for that a desired outcome can be declared. If any outcome is declared, this outcome is forced. """ def __init__(self, *args, **kwargs): super(ManuallyTransitionableState, self).__init__(*args, **kwargs) self._force_transition = False self.__execute = self.execute self.execute = self._manually_transitionable_execute self._feedback_topic = '/flexbe/command_feedback' self._transition_topic = '/flexbe/command/transition' self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() def _manually_transitionable_execute(self, *args, **kwargs): if self._is_controlled and self._sub.has_buffered(self._transition_topic): command_msg = self._sub.get_from_buffer(self._transition_topic) self._pub.publish(self._feedback_topic, CommandFeedback(command="transition", args=[command_msg.target, self.name])) if command_msg.target != self.name: rospy.logwarn("--> Requested outcome for state " + command_msg.target + " but active state is " + self.name) else: self._force_transition = True outcome = self._outcome_list[command_msg.outcome] rospy.loginfo("--> Manually triggered outcome " + outcome + " of state " + self.name) return outcome # return the normal outcome self._force_transition = False return self.__execute(*args, **kwargs) def _enable_ros_control(self): super(ManuallyTransitionableState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._transition_topic, OutcomeRequest) self._sub.enable_buffer(self._transition_topic) def _disable_ros_control(self): super(ManuallyTransitionableState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._transition_topic)
class WaitForOpenDoorState(EventState): """ Implements a state that waits for the way to be clear. <= done Indicates completion. """ LASERS_TO_IGNORE = 5 ERR_THRESHOLD = 0.15 old_values = None def __init__(self): """ Constructor """ super(WaitForOpenDoorState, self).__init__(outcomes=['done']) self._topic = '/laser/srd_front/scan' self._sub = ProxySubscriberCached({self._topic: LaserScan}) def execute(self, userdata): """Execute this state""" while self._sub.has_buffered(self._topic): msg = self._sub.get_from_buffer(self._topic) values = msg.ranges if self.old_values is None: self.old_values = values else: err = 0.0 for i in range(self.LASERS_TO_IGNORE, len(values)-1-self.LASERS_TO_IGNORE): err += self.old_values[i] - values[i] err /= len(values) if err < 0 and abs(err) > self.ERR_THRESHOLD: Logger.loginfo('THRESHOLD REACHED! (%.3f) - Door should be open!' % err) return 'done' self.old_values = values def on_enter(self, userdata): self._sub.enable_buffer(self._topic) Logger.loginfo('Waiting for open door...') def on_stop(self): self._sub.unsubscribe_topic(self._topic) def on_exit(self): self._sub.unsubscribe_topic(self._topic)
class GetJointValuesDynState(EventState): ''' Retrieves current values of specified joints. ># joint_names string[] List of desired joint names. #> joint_values float[] List of current joint values. <= retrieved Joint values are available. ''' def __init__(self): ''' Constructor ''' super(GetJointValuesDynState, self).__init__(outcomes=['retrieved'], output_keys=['joint_values'], input_keys=['joint_names']) self._topic = '/joint_states' self._sub = ProxySubscriberCached({self._topic: JointState}) self._joints = None self._joint_values = list() def execute(self, userdata): while self._sub.has_buffered(self._topic): msg = self._sub.get_from_buffer(self._topic) for i in range(len(msg.name)): if msg.name[i] in self._joints \ and self._joint_values[self._joints.index(msg.name[i])] is None: self._joint_values[self._joints.index( msg.name[i])] = msg.position[i] if all(v is not None for v in self._joint_values): userdata.joint_values = self._joint_values return 'retrieved' def on_enter(self, userdata): self._sub.enable_buffer(self._topic) self._joint_values = [None] * len(self._joints) self._joints = userdata.joint_names def on_exit(self, userdata): self._sub.disable_buffer(self._topic)
def test_subscribe_buffer(self): t1 = '/buffered_1' pub = ProxyPublisher({t1: String}) sub = ProxySubscriberCached({t1: String}) sub.enable_buffer(t1) self.assertTrue(pub.wait_for_any(t1)) pub.publish(t1, String('1')) pub.publish(t1, String('2')) rospy.sleep(.5) # make sure messages can be received self.assertTrue(sub.has_msg(t1)) self.assertTrue(sub.has_buffered(t1)) self.assertEqual(sub.get_from_buffer(t1).data, '1') pub.publish(t1, String('3')) rospy.sleep(.5) # make sure messages can be received self.assertEqual(sub.get_from_buffer(t1).data, '2') self.assertEqual(sub.get_from_buffer(t1).data, '3') self.assertIsNone(sub.get_from_buffer(t1)) self.assertFalse(sub.has_buffered(t1))
class GetCurrentJointValuesListState(EventState): ''' Retrieves current values of specified joints. Joints are specified by parameter list. -- joint_names string[] List of desired joint names. -- timeout double Timeout value (optional) -- joint_states_topic string Optional name of joint states topic (default: /joint_states) #> joint_names string[] List of current joint names. #> joint_values float[] List of current joint values. <= retrieved Joint values are available. <= timeout Joint values are not available. ''' def __init__(self, joint_names, timeout=None, joint_states_topic='/joint_states'): ''' Constructor ''' super(GetCurrentJointValuesListState, self).__init__(outcomes=['retrieved', 'timeout'], output_keys=['joint_names', 'joint_values']) self.topic = joint_states_topic self.sub = ProxySubscriberCached({self.topic: JointState}) self.joint_names = joint_names self.joint_values = list() if (timeout is not None) and (timeout > 0.0): self.timeout_duration = rospy.Duration(timeout) else: self.timeout_duration = None def execute(self, userdata): if (self.return_code is not None): # Handle blocked transition or error during on_enter userdata.joint_names = self.joint_names userdata.joint_values = self.joint_values return self.return_code while self.sub.has_buffered(self.topic): msg = self.sub.get_from_buffer(self.topic) for i in range(len(msg.name)): if msg.name[i] in self.joint_names \ and self.joint_values[self.joint_names.index(msg.name[i])] is None: self.joint_values[self.joint_names.index( msg.name[i])] = msg.position[i] if all(v is not None for v in self.joint_values): userdata.joint_names = self.joint_names userdata.joint_values = self.joint_values self.return_code = 'retrieved' return 'retrieved' if (self.timeout_duration is not None and \ (rospy.Time.now()-self.start_time) > self.timeout_duration ): Logger.logerr( 'Timeout %s - found %s of %s' % (self.name, str(self.joint_values), str(self.joint_names))) self.return_code = 'timeout' return 'timeout' def on_enter(self, userdata): self.sub.enable_buffer(self.topic) self.sub.remove_last_msg(self.topic, True) self.joint_values = [None] * len(self.joint_names) self.return_code = None self.start_time = rospy.Time.now() userdata.joint_names = self.joint_names # Clear user data until we have valid data userdata.joint_values = None def on_exit(self, userdata): self.sub.disable_buffer(self.topic)
class MonitoringState(smach.State): """ A state to monitor a custom set of conditions. For each conditions, an outcome is added or mapped which will be returned if the condition is not met. """ def __init__(self, *args, **kwargs): super(MonitoringState, self).__init__(*args, **kwargs) outcomes = kwargs.get('outcomes', []) self._outcome_list = list(outcomes) self._outcome_list.remove('loopback') self.name = None self._parent = None self._is_controlled = False self._force_monitoring = False self._monitored_keys = dict() self._sent_keys = list() self._current_status = None self.__execute = self.execute self.execute = self._monitoring_execute self._diagnostics_topic = 'diagnostics_agg' self._sub = ProxySubscriberCached() def _monitoring_execute(self, *args, **kwargs): new_status = None had_warning = False if (self._force_monitoring or self._is_controlled) and self._sub.has_buffered( self._diagnostics_topic): new_status = "" diag_msg = self._sub.get_from_buffer(self._diagnostics_topic) for status in diag_msg.status: if not status.name in self._monitored_keys.keys(): continue if status.level == DiagnosticStatus.WARN: had_warning = True if not status.name + "_warn" in self._sent_keys: self._sent_keys.append(status.name + "_warn") Logger.logwarn("%s: %s" % (status.name, status.message)) if status.level == DiagnosticStatus.ERROR: if not status.name + "_err" in self._sent_keys: self._sent_keys.append(status.name + "_err") Logger.logerr("%s: %s" % (status.name, status.message)) new_status = status.name if new_status == "": self._current_status = None new_status = None if not had_warning: self._sent_keys = list() if new_status is None or self._current_status == new_status: return self.__execute(*args, **kwargs) self._current_status = new_status return self._monitored_keys[self._current_status] def monitor(self, key, outcome=None): oc = outcome if not outcome is None else key self._monitored_keys[key] = oc if not oc in self._outcomes: self.register_outcomes([oc]) self._outcome_list.append(oc) def force_monitoring(self): self._force_monitoring = True if not self._is_controlled: self._sub.subscribe(self._diagnostics_topic, DiagnosticArray) self._sub.enable_buffer(self._diagnostics_topic) def _enable_ros_control(self): self._is_controlled = True self._sub.subscribe(self._diagnostics_topic, DiagnosticArray) self._sub.enable_buffer(self._diagnostics_topic) def _disable_ros_control(self): self._is_controlled = False self._sub.unsubscribe_topic(self._diagnostics_topic) def _get_path(self): return self._parent._get_path() + "/" + self.name
class MonitoringState(smach.State): """ A state to monitor a custom set of conditions. For each conditions, an outcome is added or mapped which will be returned if the condition is not met. """ def __init__(self, *args, **kwargs): super(MonitoringState, self).__init__(*args, **kwargs) outcomes = kwargs.get('outcomes', []) self._outcome_list = list(outcomes) self._outcome_list.remove('loopback') self.name = None self._parent = None self._is_controlled = False self._force_monitoring = False self._monitored_keys = dict() self._sent_keys = list() self._current_status = None self.__execute = self.execute self.execute = self._monitoring_execute self._diagnostics_topic = 'diagnostics_agg' self._sub = ProxySubscriberCached() def _monitoring_execute(self, *args, **kwargs): new_status = None had_warning = False if (self._force_monitoring or self._is_controlled) and self._sub.has_buffered(self._diagnostics_topic): new_status = "" diag_msg = self._sub.get_from_buffer(self._diagnostics_topic) for status in diag_msg.status: if not status.name in self._monitored_keys.keys(): continue if status.level == DiagnosticStatus.WARN: had_warning = True if not status.name + "_warn" in self._sent_keys: self._sent_keys.append(status.name + "_warn") Logger.logwarn("%s: %s" % (status.name, status.message)) if status.level == DiagnosticStatus.ERROR: if not status.name + "_err" in self._sent_keys: self._sent_keys.append(status.name + "_err") Logger.logerr("%s: %s" % (status.name, status.message)) new_status = status.name if new_status == "": self._current_status = None new_status = None if not had_warning: self._sent_keys = list() if new_status is None or self._current_status == new_status: return self.__execute(*args, **kwargs) self._current_status = new_status return self._monitored_keys[self._current_status] def monitor(self, key, outcome = None): oc = outcome if not outcome is None else key self._monitored_keys[key] = oc if not oc in self._outcomes: self.register_outcomes([oc]) self._outcome_list.append(oc) def force_monitoring(self): self._force_monitoring = True if not self._is_controlled: self._sub.subscribe(self._diagnostics_topic, DiagnosticArray) self._sub.enable_buffer(self._diagnostics_topic) def _enable_ros_control(self): self._is_controlled = True self._sub.subscribe(self._diagnostics_topic, DiagnosticArray) self._sub.enable_buffer(self._diagnostics_topic) def _disable_ros_control(self): self._is_controlled = False self._sub.unsubscribe_topic(self._diagnostics_topic) def _get_path(self): return self._parent._get_path() + "/" + self.name
class ObjectDetectionMonitor(EventState): ''' Receive list of detected objects as cob_object_detection_msgs/DetectionArray messages and check if there is an object which matches given (label, type) pair. Based on present of this object classify situation in `no_detections` state, `have_detections` (there is objects but none of them matches give (label, type)), `detection_matches` (desired object found). In the last case its pose bublished on given topic. -- detection_topic string DetectionArray message topic. -- label string Object label. Use '*' to match all labals. -- type string Object type. Use '*' to match all types. -- detection_period float Parameteres: detection_period (s), position_tolerance (m), orientation_tolerance (rad). -- exit_states string[] Stop monitoring and return outcome if detected situation in this list. -- pose_topic string Topic for publishing matched object pose (may be Empty). -- pose_frame_id string Frame in which pose should be published. #> pose object Object pose before exit. <= no_detections Nothing have been detected for detection_period. <= have_detections Objects were detected, but not of them matches (label, type) <= detecton_matches Object presents and it is moving. `pose` key contains last pose. <= failure Runtime error. ''' def __init__(self, detection_topic='detection', label='*', type='*', exit_states=['no_detections'], pose_topic='', pose_frame_id='odom_combined', detection_period=1.0): super(ObjectDetectionMonitor, self).__init__(outcomes=[ 'no_detections', 'have_detections', 'detection_matches', 'failure' ], output_keys=['pose']) # store state parameter for later use. self._detection_topic = detection_topic self._label = label self._type = type self._pose_topic = pose_topic self._frame_id = pose_frame_id self._exit_states = exit_states self._detection_period = detection_period # setup proxies self._detection_subscriber = ProxySubscriberCached( {self._detection_topic: DetectionArray}) self._detection_subscriber.enable_buffer(self._detection_topic) if self._pose_topic: self._pose_publisher = ProxyPublisher( {self._pose_topic: PoseStamped}) else: self._pose_publisher = None self._tf_listener = ProxyTransformListener().listener() # check parameters if not isinstance( self._frame_id, str): # or not self._tf_listener.frameExists(self._frame_id): raise ValueError( 'ObjectDetectionMonitor: %s is not a valid frame' % self._frame_id) if not isinstance(self._label, str) or not isinstance(self._type, str): raise ValueError( 'ObjectDetectionMonitor: label and type parameters must be string.' ) if not isinstance(self._detection_period, float) or self._detection_period < 0.0: raise ValueError( 'ObjectDetectionMonitor: detection_period must be float.') if not isinstance(self._exit_states, list) or not all([ state in ['no_detections', 'have_detections', 'detection_matches'] for state in self._exit_states ]): raise ValueError( 'ObjectDetectionMonitor: incorrect list of exit states.') # pose buffers self._pose = None self._last_detection_stamp = None self._last_match_stamp = None self._last_match_id = None def on_enter(self, userdata): # reset pose buffers self._pose = None stamp = rospy.Time.now() self._last_detection_stamp = stamp self._last_match_stamp = stamp self._last_match_id = None Logger.loginfo('ObjectDetectionMonitor: start monitoring.') def execute(self, userdata): # list of found matches have_detections = False best_match = None match = None # check if new message is available while self._detection_subscriber.has_buffered(self._detection_topic): # get detections msg detections_msg = self._detection_subscriber.get_from_buffer( self._detection_topic) # check if have detections if len(detections_msg.detections) > 0: have_detections = True # process detections for detection in detections_msg.detections: # check match is_match = True if self._label != '*' and detection.label != self._label: is_match = False if self._type != '*' and detection.type != self._type: is_match = False # register match if is_match: match = detection if match.id == self._last_match_id: best_match = match if best_match: match = best_match # result stamp = rospy.Time.now() if not have_detections: # no object detected # check if detection preiod exceeded if (stamp - self._last_detection_stamp ).to_sec() > self._detection_period: # no detections state if 'no_detections' in self._exit_states: Logger.loginfo('ObjectDetectionMonitor: no detections.') return 'no_detections' elif not match: # object detected but it is not match self._last_detection_stamp = stamp # check if detection perod exceeded if (stamp - self._last_detection_stamp ).to_sec() > self._detection_period: # have_detections state if 'have_detections' in self._exit_states: Logger.loginfo('ObjectDetectionMonitor: detection.') return 'have_detections' else: # appropriate match is found self._last_match_stamp = stamp self._last_match_id = match.id # publish if self._pose_publisher: # transform to frame_id try: # transform pose_stamped = PoseStamped( pose=match.pose, header=Header(frame_id=match.header.frame_id)) self._pose = self._tf_listener.transformPose( self._frame_id, pose_stamped) except tf.Exception as e: Logger.logwarn( 'Unable to transform from %s to %s' % (match.pose.header.frame_id, self._frame_id)) return 'failure' # publish self._pose_publisher.publish(self._pose_topic, self._pose) # match_detection state if 'detection_matches' in self._exit_states: Logger.loginfo('ObjectDetectionMonitor: match.') return 'detection_matches' return None def on_exit(self, userdata): userdata.pose = self._pose
class VigirBehaviorMirror(object): ''' classdocs ''' def __init__(self): ''' Constructor ''' self._sm = None smach.set_loggers( rospy.logdebug, # hide SMACH transition log spamming rospy.logwarn, rospy.logdebug, rospy.logerr) # set up proxys for sm <--> GUI communication # publish topics self._pub = ProxyPublisher({ 'flexbe/behavior_update': String, 'flexbe/request_mirror_structure': Int32 }) self._running = False self._stopping = False self._active_id = 0 self._starting_path = None self._current_struct = None self._sync_lock = threading.Lock() self._outcome_topic = 'flexbe/mirror/outcome' self._struct_buffer = list() # listen for mirror message self._sub = ProxySubscriberCached() self._sub.subscribe(self._outcome_topic, UInt8) self._sub.enable_buffer(self._outcome_topic) self._sub.subscribe('flexbe/mirror/structure', ContainerStructure, self._mirror_callback) 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) def _mirror_callback(self, msg): rate = rospy.Rate(10) while self._stopping: rate.sleep() if self._running: rospy.logwarn( 'Received a new mirror structure while mirror is already running, adding to buffer (checksum: %s).' % str(msg.behavior_id)) elif self._active_id != 0 and msg.behavior_id != self._active_id: rospy.logwarn( 'checksum of received mirror structure (%s) does not match expected checksum (%s), will ignore.' % (str(msg.behavior_id), str(self._active_id))) return else: rospy.loginfo('Received a new mirror structure for checksum %s' % str(msg.behavior_id)) self._struct_buffer.append(msg) if self._active_id == msg.behavior_id: self._struct_buffer = list() self._mirror_state_machine(msg) rospy.loginfo('Mirror built.') self._execute_mirror() def _status_callback(self, msg): if msg.code == BEStatus.STARTED: thread = threading.Thread(target=self._start_mirror, args=[msg]) thread.daemon = True thread.start() else: thread = threading.Thread(target=self._stop_mirror, args=[msg]) thread.daemon = True thread.start() def _start_mirror(self, msg): with self._sync_lock: 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" self._active_id = msg.behavior_id 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): 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) if msg is not None and msg.code != BEStatus.SWITCHING: rospy.loginfo('\033[92m--- Behavior Mirror ready! ---\033[0m') self._stopping = False def _sync_callback(self, msg): if msg.behavior_id == self._active_id: thread = threading.Thread(target=self._restart_mirror, args=[msg]) thread.daemon = True thread.start() else: rospy.logerr( 'Cannot synchronize! Different behavior is running onboard, please stop execution!' ) thread = threading.Thread(target=self._stop_mirror, args=[None]) thread.daemon = True thread.start() def _restart_mirror(self, msg): 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() def _execute_mirror(self): self._running = True rospy.loginfo("Executing mirror...") if self._starting_path is not None: LockableStateMachine.path_for_switch = self._starting_path rospy.loginfo("Starting mirror in state " + self._starting_path) self._starting_path = None result = 'preempted' try: result = self._sm.execute() except Exception as e: rospy.loginfo('Caught exception on preempt:\n%s' % str(e)) result = 'preempted' if self._sm is not None: self._sm.destroy() self._running = False rospy.loginfo('Mirror finished with result ' + result) def _mirror_state_machine(self, msg): self._current_struct = msg self._state_checksums = dict() root = None for con_msg in msg.containers: if con_msg.path.find('/') == -1: root = con_msg.path break self._add_node(msg, root) # calculate checksums of all states for con_msg in msg.containers: if con_msg.path.find('/') != -1: self._state_checksums[zlib.adler32( con_msg.path)] = con_msg.path def _add_node(self, msg, path): #rospy.loginfo('Add node: '+path) container = None for con_msg in msg.containers: if con_msg.path == path: container = con_msg break transitions = None if container.transitions is not None: transitions = {} for i in range(len(container.transitions)): transitions[container. outcomes[i]] = container.transitions[i] + '_mirror' path_frags = path.split('/') container_name = path_frags[len(path_frags) - 1] if len(container.children) > 0: sm_outcomes = [] for outcome in container.outcomes: sm_outcomes.append(outcome + '_mirror') sm = JumpableStateMachine(outcomes=sm_outcomes) with sm: for child in container.children: self._add_node(msg, path + '/' + child) if len(transitions) > 0: container_transitions = {} for i in range(len(container.transitions)): container_transitions[sm_outcomes[i]] = transitions[ container.outcomes[i]] JumpableStateMachine.add(container_name + '_mirror', sm, transitions=container_transitions) else: self._sm = sm else: JumpableStateMachine.add(container_name + '_mirror', MirrorState(container_name, path, container.outcomes, container.autonomy), transitions=transitions) def _jump_callback(self, msg): start_time = rospy.Time.now() current_elapsed = 0 jumpable = True while self._sm is None or not self._sm.is_running(): current_elapsed = rospy.Time.now() - start_time if current_elapsed > rospy.Duration.from_sec(10): jumpable = False break rospy.sleep(0.05) if jumpable: self._sm.jump_to(msg.stateName) else: rospy.logwarn( 'Could not jump in mirror: Mirror does not exist or is not running!' ) def _preempt_callback(self, msg): if self._sm is not None and self._sm.is_running(): 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!')
class VigirBehaviorMirror(object): ''' classdocs ''' def __init__(self): ''' Constructor ''' self._sm = None smach.set_loggers ( rospy.logdebug, # hide SMACH transition log spamming rospy.logwarn, rospy.logdebug, rospy.logerr ) # set up proxys for sm <--> GUI communication # publish topics self._pub = ProxyPublisher({'flexbe/behavior_update': String, 'flexbe/request_mirror_structure': Int32}) self._running = False self._stopping = False self._active_id = 0 self._starting_path = None self._current_struct = None self._outcome_topic = 'flexbe/mirror/outcome' self._struct_buffer = list() # listen for mirror message self._sub = ProxySubscriberCached() self._sub.subscribe(self._outcome_topic, UInt8) self._sub.enable_buffer(self._outcome_topic) self._sub.subscribe('flexbe/mirror/structure', ContainerStructure, self._mirror_callback) 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) def _mirror_callback(self, msg): rate = rospy.Rate(10) while self._stopping: rate.sleep() if self._running: rospy.logwarn('Received a new mirror structure while mirror is already running, adding to buffer (ID: %s).' % str(msg.behavior_id)) 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))) return else: rospy.loginfo('Received a new mirror structure for ID %s' % str(msg.behavior_id)) self._struct_buffer.append(msg) if self._active_id == msg.behavior_id: self._struct_buffer = list() self._mirror_state_machine(msg) rospy.loginfo('Mirror built.') self._execute_mirror() def _status_callback(self, msg): if msg.code == BEStatus.STARTED: thread = threading.Thread(target=self._start_mirror, args=[msg]) thread.daemon = True thread.start() else: thread = threading.Thread(target=self._stop_mirror, args=[msg]) thread.daemon = True thread.start() def _start_mirror(self, msg): 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" self._active_id = msg.behavior_id 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 ID %s' % str(struct.behavior_id)) if self._sm is None: rospy.logwarn('Missing correct mirror structure, requesting...') rospy.sleep(0.2) # no clean way 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._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) if msg is not None and msg.code != BEStatus.SWITCHING: rospy.loginfo('\033[92m--- Behavior Mirror ready! ---\033[0m') self._stopping = False def _sync_callback(self, msg): if msg.behavior_id == self._active_id: thread = threading.Thread(target=self._restart_mirror, args=[msg]) thread.daemon = True thread.start() else: rospy.logerr('Cannot synchronize! Different behavior is running onboard, please stop execution!') thread = threading.Thread(target=self._stop_mirror, args=[None]) thread.daemon = True thread.start() def _restart_mirror(self, msg): 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.') self._execute_mirror() except (AttributeError, smach.InvalidStateError): rospy.loginfo('Stopping synchronization because behavior has stopped.') def _execute_mirror(self): self._running = True rospy.loginfo("Executing mirror...") if self._starting_path is not None: LockableStateMachine.path_for_switch = self._starting_path rospy.loginfo("Starting mirror in state " + self._starting_path) self._starting_path = None result = 'preempted' try: result = self._sm.execute() except Exception as e: rospy.loginfo('Catched exception on preempt:\n%s' % str(e)) if self._sm is not None: self._sm.destroy() self._running = False rospy.loginfo('Mirror finished with result ' + result) def _mirror_state_machine(self, msg): self._current_struct = msg self._state_checksums = dict() root = None for con_msg in msg.containers: if con_msg.path.find('/') == -1: root = con_msg.path break self._add_node(msg, root) # calculate checksums of all states for con_msg in msg.containers: if con_msg.path.find('/') != -1: self._state_checksums[zlib.adler32(con_msg.path)] = con_msg.path def _add_node(self, msg, path): #rospy.loginfo('Add node: '+path) container = None for con_msg in msg.containers: if con_msg.path == path: container = con_msg break transitions = None if container.transitions is not None: transitions = {} for i in range(len(container.transitions)): transitions[container.outcomes[i]] = container.transitions[i] + '_mirror' path_frags = path.split('/') container_name = path_frags[len(path_frags)-1] if len(container.children) > 0: sm_outcomes = [] for outcome in container.outcomes: sm_outcomes.append(outcome + '_mirror') sm = JumpableStateMachine(outcomes=sm_outcomes) with sm: for child in container.children: self._add_node(msg, path+'/'+child) if len(transitions) > 0: container_transitions = {} for i in range(len(container.transitions)): container_transitions[sm_outcomes[i]] = transitions[container.outcomes[i]] JumpableStateMachine.add(container_name + '_mirror', sm, transitions=container_transitions) else: self._sm = sm else: JumpableStateMachine.add(container_name + '_mirror', MirrorState(container_name, path, container.outcomes, container.autonomy), transitions=transitions) def _jump_callback(self, msg): start_time = rospy.Time.now() current_elapsed = 0 jumpable = True while self._sm is None or not self._sm.is_running(): current_elapsed = rospy.Time.now() - start_time if current_elapsed > rospy.Duration.from_sec(10): jumpable = False break rospy.sleep(0.05) if jumpable: self._sm.jump_to(msg.stateName) else: rospy.logwarn('Could not jump in mirror: Mirror does not exist or is not running!') def _preempt_callback(self, msg): if self._sm is not None and self._sm.is_running(): 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!')
class TestOnboard(unittest.TestCase): def __init__(self, name): super(TestOnboard, self).__init__(name) self.sub = ProxySubscriberCached({ 'flexbe/status': BEStatus, 'flexbe/log': BehaviorLog }) self.rate = rospy.Rate(100) # make sure that behaviors can be imported data_folder = os.path.dirname(os.path.realpath(__file__)) sys.path.insert(0, data_folder) # run onboard and add custom test behaviors to onboard lib self.onboard = FlexbeOnboard() self.lib = self.onboard._behavior_lib self.lib._add_behavior_manifests(data_folder) def assertStatus(self, expected, timeout): """ Assert that the expected onboard status is received before the timeout. """ for i in range(int(timeout * 100)): self.rate.sleep() if self.sub.has_msg('flexbe/status'): break else: raise AssertionError('Did not receive a status as required.') msg = self.sub.get_last_msg('flexbe/status') self.sub.remove_last_msg('flexbe/status') self.assertEqual(msg.code, expected) return msg def test_onboard_behaviors(self): behavior_pub = rospy.Publisher('flexbe/start_behavior', BehaviorSelection, queue_size=1) rospy.sleep(0.5) # wait for publisher # wait for the initial status message self.assertStatus(BEStatus.READY, 1) # send simple behavior request without checksum be_id, _ = self.lib.find_behavior("Test Behavior Log") request = BehaviorSelection() request.behavior_id = be_id request.autonomy_level = 255 behavior_pub.publish(request) self.assertStatus(BEStatus.ERROR, 2) # send valid simple behavior request with open(self.lib.get_sourcecode_filepath(be_id)) as f: request.behavior_checksum = zlib.adler32( f.read().encode()) & 0x7fffffff self.sub.enable_buffer('flexbe/log') behavior_pub.publish(request) self.assertStatus(BEStatus.STARTED, 1) self.assertStatus(BEStatus.FINISHED, 3) behavior_logs = [] while self.sub.has_buffered('flexbe/log'): behavior_logs.append(self.sub.get_from_buffer('flexbe/log').text) self.assertIn('Test data', behavior_logs) # send valid complex behavior request be_id, _ = self.lib.find_behavior("Test Behavior Complex") request = BehaviorSelection() request.behavior_id = be_id request.autonomy_level = 255 request.arg_keys = ['param'] request.arg_values = ['value_2'] request.input_keys = ['data'] request.input_values = ['2'] with open(self.lib.get_sourcecode_filepath(be_id)) as f: content = f.read() modifications = [('flexbe_INVALID', 'flexbe_core'), ('raise ValueError("TODO: Remove!")', '')] for replace, by in modifications: index = content.index(replace) request.modifications.append( BehaviorModification(index, index + len(replace), by)) for replace, by in modifications: content = content.replace(replace, by) request.behavior_checksum = zlib.adler32(content.encode()) & 0x7fffffff behavior_pub.publish(request) self.assertStatus(BEStatus.STARTED, 1) result = self.assertStatus(BEStatus.FINISHED, 3) self.assertEqual(result.args[0], 'finished') behavior_logs = [] while self.sub.has_buffered('flexbe/log'): behavior_logs.append(self.sub.get_from_buffer('flexbe/log').text) self.assertIn('value_2', behavior_logs) # send the same behavior with different parameters request.arg_keys = ['param', 'invalid'] request.arg_values = ['value_1', 'should be ignored'] request.input_keys = [] request.input_values = [] behavior_pub.publish(request) self.assertStatus(BEStatus.STARTED, 1) result = self.assertStatus(BEStatus.FINISHED, 3) self.assertEqual(result.args[0], 'failed') behavior_logs = [] while self.sub.has_buffered('flexbe/log'): behavior_logs.append(self.sub.get_from_buffer('flexbe/log').text) self.assertIn('value_1', behavior_logs)