Smach get value from subscriber topic

Question

I'm pretty new to ROS, and I'm working with SMACH. I have an Arduino UNO with 4 LEDs (green ,yellow, red and blue) and one button. What I'm trying to achieve is a sort of traffic light simulator. My sequence is the following:

turn off red/turn on green -> turn off green/turn on yellow -> turn off yellow/turn on red -> wait for input -> repeat

with sm:
        smach.StateMachine.add('greent', CBState(greentoggle_cb),
                                {'finished': 'yellowt', 'failed': 'outcome'})
        smach.StateMachine.add('yellowt', CBState(yellowtoggle_cb),
                                {'finished': 'redt', 'failed': 'outcome'})
        smach.StateMachine.add('redt', CBState(redtoggle_cb),
                                {'finished': '_wait_', 'failed': 'outcome'})
        smach.StateMachine.add('_wait_', sem_wait(),
                                transitions={'finished': 'bluet', 'failed': 'greent'})
        smach.StateMachine.add('bluet', CBState(bluetoggle_cb),
                                {'finished': 'greent', 'failed': 'outcome'})

In the wait for input section, I wait for 4 seconds for the button to be pressed. The button has its own publisher topic where it posts 1 when it's pressed, and 0 when it's not. The idea is for SMACH to read the button_press topic so that, if it's pressed, the blue LED is turned on, and then the cycle is repeated.

This is the code I wrote for this:

class sem_wait(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['finished', 'failed'])
        self.got = False
            
    def listener():
        rospy.init_node('listen')
        self.subscriber = rospy.Subscriber('button_press', Int16, self.callback)

    def callback(self, data):
        if data.data == 1:
            self.got = True

    def execute(self, data):
        for i in range (0, 40): # wait 4 seconds
            if self.got:
                toggle_topic = rospy.Publisher('/red_led', Empty, queue_size=1)
                rospy.sleep(1)
                msg = Empty()
                result = toggle_topic.publish(msg)
                
                return 'finished'
            time.sleep(.1)
        return 'failed'

However, this doesn't work. It either doesn't read the button in the first place, or it can't translate the button press into what I want it to do.

I'd appreciate any help as I'm stuck here.

---

Originally posted by rubbun on ROS Answers with karma: 16 on 2020-04-25

Post score: 0

Answer 1

I fixed it. I wasn't getting the values from the subscriber because it was never being called in the first place. Putting it on its own "listener()" definition meant it never being executed I guess.

class sem_wait(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['finished', 'failed'])
        self.got = False
        self.subscriber = rospy.Subscriber('/button_press', Int16, self.callback)

    def callback(self, data):
        if data.data == 1:
            self.got = True
        else:
            self.got = False

    def execute(self, data):
        for i in range (0, 30):
            if self.got:
                return 'finished'
            time.sleep(.1)
        return 'failed'

This works just the way I wanted it to.

---

Originally posted by rubbun with karma: 16 on 2020-04-25

This answer was ACCEPTED on the original site

Post score: 0