Beispiel #1
0
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)
Beispiel #5
0
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
Beispiel #10
0
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)