The WaitForMsgState TemplateΒΆ

WaitForMsgState

smacha/src/smacha_ros/templates/WaitForMsgState.tpl.py

{% block meta %}
name: WaitForMsgState
description: >
  SMACH template that provides a WaitForMsgState helper class for
  ReadTopicState.

  The class acts as a generic message listener with blocking, timeout, latch
  and flexible usage.

  It is meant to be extended with a case specific class that initializes this
  one appropriately and contains the msg_cb (or overrides execute if really
  needed).

  Its waitForMsg method implements the core functionality: waiting for the
  message, returning the message itself or None on timeout.

  Its execute method wraps the waitForMsg and returns succeeded or aborted,
  depending on the returned message being existent or None. Additionally, in
  the successful case, the msg_cb, if given, will be called with the message
  and the userdata, so that a self defined method can convert message data to
  smach userdata. Those userdata fields have to be passed via 'output_keys'.

  Based on the original code described here:
      http://wiki.ros.org/executive_smach/AdditionalStateTypes
      https://github.com/felix-kolbe/uashh-rvl-ros-pkg/blob/master/uashh_smach/src/uashh_smach/util.py
language: Python
framework: SMACH
type: None
tags: [core]
includes: []
extends:
- State
variables:
- topic:
    description: The name of the topic from which the data should be read.
    type: str
- msg_type:
    description: The ROS message type of the topic.
    type: str
- - msg_cb:
      description: >
        If the state outcome should depend on the message content, the msg_cb
        can dictate the outcome:
        If the state outcome should depend on the message content, the msg_cb can dictate the outcome:
        If msg_cb returns True, execute() will return "succeeded".
        If msg_cb returns False, execute() will return "aborted".
        If msg_cb has no return statement, execute() will act as described above.
      type: str
- - latch:
      description:
        If True waitForMsg will return the last received message, so one
        message might be returned indefinite times.
      type: bool
- - timeout:
      description:
        Seconds to wait for a message, defaults to None, disabling timeout.
      type: int
- - output_keys:
      description:
        Userdata keys that the message callback needs to write to.
      type: list of str
input_keys: []
output_keys: []
outcomes:
- succeeded
- aborted
{% endblock meta %}

{% from "Utils.tpl.py" import import_module, render_transitions, render_remapping, render_output_keys %}

{% extends "State.tpl.py" %}

{% block imports %}
{{ super() }}
{{ import_module(defined_headers, 'threading') }}
{% endblock imports %}

{% block class_defs %}
{{ super() }}
{% if 'class_WaitForMsgState' not in defined_headers %}
class WaitForMsgState(smach.State):
    """This class acts as a generic message listener with blocking, timeout, latch and flexible usage.

    It is meant to be extended with a case specific class that initializes this one appropriately
    and contains the msg_cb (or overrides execute if really needed).

    Its waitForMsg method implements the core functionality: waiting for the message, returning
    the message itself or None on timeout.

    Its execute method wraps the waitForMsg and returns succeeded or aborted, depending on the returned
    message beeing existent or None. Additionally, in the successfull case, the msg_cb, if given, will
    be called with the message and the userdata, so that a self defined method can convert message data to
    smach userdata.
    Those userdata fields have to be passed via 'output_keys'.

    If the state outcome should depend on the message content, the msg_cb can dictate the outcome:
    If msg_cb returns True, execute() will return "succeeded".
    If msg_cb returns False, execute() will return "aborted".
    If msg_cb has no return statement, execute() will act as described above.

    If thats still not enough, execute() might be overridden.

    latch: If True waitForMsg will return the last received message, so one message might be returned indefinite times.
    timeout: Seconds to wait for a message, defaults to None, disabling timeout
    output_keys: Userdata keys that the message callback needs to write to.
    """

    def __init__(self, topic, msg_type, msg_cb=None, output_keys=None, latch=False, timeout=None):
        if output_keys is None:
            output_keys = []
        smach.State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'], output_keys=output_keys)
        self.latch = latch
        self.timeout = timeout
        self.mutex = threading.Lock()
        self.msg = None
        self.msg_cb = msg_cb
        self.subscriber = rospy.Subscriber(topic, msg_type, self._callback, queue_size=1)

    def __enter__(self):
        return self

    def __exit__(self, *err):
        self.subscriber.unregister()

    def _callback(self, msg):
        self.mutex.acquire()
        self.msg = msg
        self.mutex.release()

    def waitForMsg(self):
        """Await and return the message or None on timeout."""
        rospy.loginfo('Waiting for message...')
        if self.timeout is not None:
            timeout_time = rospy.Time.now() + rospy.Duration.from_sec(self.timeout)
        while self.timeout is None or rospy.Time.now() < timeout_time:
            self.mutex.acquire()
            if self.msg is not None:
                rospy.loginfo('Got message.')
                message = self.msg

                if not self.latch:
                    self.msg = None

                self.mutex.release()
                return message
            self.mutex.release()

            if self.preempt_requested():
                self.service_preempt()
                rospy.loginfo('waitForMsg is preempted!')
                return 'preempted'

            rospy.sleep(.1) # TODO: maybe convert ROSInterruptException into valid outcome

        rospy.loginfo('Timeout on waiting for message!')
        return None

    def execute(self, ud):
        """Default simplest execute(), see class description."""
        msg = self.waitForMsg()
        if msg is not None:
            if msg == 'preempted':
                return 'preempted'
            # call callback if there is one
            if self.msg_cb is not None:
                cb_result = self.msg_cb(msg, ud)
                # check if callback wants to dictate output
                if cb_result is not None:
                    if cb_result:
                        return 'succeeded'
                    else:
                        return 'aborted'
            return 'succeeded'
        else:
            return 'aborted'
{% do defined_headers.append('class_WaitForMsgState') %}{% endif %}
{% endblock class_defs %}

{% block body %}
smach.{{ parent_type }}.add('{{ name }}',
        {{ '' | indent(23, true) }}WaitForMsgState('{{ topic }}', {{ msg_type }}{% if msg_cb is defined %}, msg_cb = {% if msg_cb is expression %}{{ msg_cb|exptostr }}{% else %}'{{ msg_cb }}'{% endif %}{% endif %}{% if output_keys is defined %}, {{ render_output_keys(output_keys, indent=0) }}{% endif %}{% if latch is defined %}, latch = {{ latch }}{% endif %}{% if timeout is defined %}, timeout = {{ timeout }}{% endif %}){% if transitions is defined %},
{{ render_transitions(transitions) }}{% endif %}{% if remapping is defined %},
{{ render_remapping(remapping) }}{% endif %})
{% endblock body %}