From f51bab9788dbce429a95d4260187c8a2d6a27bb2 Mon Sep 17 00:00:00 2001 From: Nils Berg Date: Fri, 5 Dec 2014 21:39:43 +0100 Subject: [PATCH] change MonitorState and document its behavior --- smach_ros/src/smach_ros/monitor_state.py | 24 +++++++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/smach_ros/src/smach_ros/monitor_state.py b/smach_ros/src/smach_ros/monitor_state.py index 38825ea..355156d 100644 --- a/smach_ros/src/smach_ros/monitor_state.py +++ b/smach_ros/src/smach_ros/monitor_state.py @@ -10,9 +10,23 @@ __all__ = ['MonitorState'] class MonitorState(smach.State): - """A state that will check a given ROS topic with a condition function. + """ + A state that will check a given ROS topic with a condition function. """ def __init__(self, topic, msg_type, cond_cb, max_checks=-1,input_keys = [],output_keys=[]): + """State constructor + @type topic string + @param topic the topic to monitor + + @type msg_type a ROS message type + @param msg_type determines the type of the monitored topic + + @type max_checks int + @param max_checks the number of messages to receive and evaluate. If cond_cb returns False for any + of them, the state will finish with outcome 'invalid'. If cond_cb returns True for + all of them, the outcome will be 'valid' + + """ smach.State.__init__( self, outcomes=['valid','invalid','preempted'], @@ -52,12 +66,16 @@ def execute(self, ud): def _cb(self,msg,ud) : try: - if (self._max_checks > 0 and self._n_checks >= self._max_checks) or not self._cond_cb(ud, msg): + if self._cond_cb(ud, msg): + self._n_checks +=1 + else: self._trigger_event.set() except Exception as e: rospy.logerr("Error thrown while executing condition callback %s: %s" % (str(self._cond_cb), e)) self._trigger_event.set() - self._n_checks += 1 + + if (self._max_checks > 0 and self._n_checks >= self._max_checks): + self._trigger_event.set() def request_preempt(self): smach.State.request_preempt(self)