BlazeRunner738
BlazeRunner738

Reputation: 81

ROS Smach State Machine: Using single subscriber for transition in multiple states

I have a ROS subscriber that is looking at a message which is mostly False and periodically True, this is essentially suppose to simulate a button press.

I want to transition from IDLE -> ACTIVE if I see a True and again from ACTIVE -> COMPLETE when a True is seen again.

I tried implementing the first two states but I'm not sure how to initialize a subscriber once and use the data from the subscriber within two states for transitioning in both states. Is there a best practice for this?

I've only added the first two states so far for testing purposes but would ultimately need the third.

#!/usr/bin/env python
import rospy
import smach
from std_msgs.msg import Bool
import numpy as np


class Idle(smach.State):  # Define a state and outcomes
    def __init__(self):
        smach.State.__init__(self, outcomes=['start', 'wait'])

    def execute(self, userdata):
        rospy.loginfo('Idle')
        # if toggled
        #     return 'start'
        # return 'wait'


class Active(smach.State):   # Define a state and outcomes
    def __init__(self):
        smach.State.__init__(self, outcomes=['complete'])


    def execute(self, userdata):
        rospy.loginfo('Active')
        return 'complete'


def init_sm(events):
    # create SMACH state machine
    sm = smach.StateMachine(outcomes=['finished'])
    sm.set_initial_state(['IDLE'])

    with sm:  # This opens sm container for adding states:
        smach.StateMachine.add('IDLE', Idle(events),  # Add state and transition
                transitions={'start':'ACTIVE', 'wait' : 'IDLE'})

        smach.StateMachine.add('ACTIVE', Active(),    # Add state and transition
                transitions={'complete':'finished'})

    return sm


# ROS MESSAGE CALLBACKS
def button_cb(msg):
  # NOTE: I want to be able to somehow save a transition variable used in both states here
  print(msg.data)

if __name__ == '__main__':
    # events = Events()
    rospy.init_node('node')
    rospy.sleep(0.5)

    rospy.Subscriber("/button", Bool, button_cb)

    sm = init_sm() # Create state machine
    outcome = sm.execute()   # Execute state machine
    rospy.spin()

Upvotes: 1

Views: 541

Answers (0)

Related Questions