Ros node publishes old value

edit

Hello,

I have made this simple service nodes that changes status like a finite machine. The problem is that the variable 'currentstatus' should be published at a constant rate with the current value. But in this code every time the loop runs "currentstatus" goes back to its old value "START" and publishes old value.

import rospy
from std_msgs.msg import String
from ros_training_4.srv import *


def handle_trigger_transition(req):

    rospy.loginfo("Processing request...")

    if currentstatus == "START":
        START(req)
    elif currentstatus == "at-loc-1":
        at_loc_1(req)
    elif currentstatus == "at-loc-2":
        at_loc_2(req)
    elif currentstatus == "grasped-obj":
        grasped_obj(req)
    elif currentstatus == "placed-obj":
        placed_obj(req)

    else:
        rospy.loginfo("Status not found")
    rospy.spin()


def START(req):

        if req.transition == 1:
            rospy.loginfo("move-1")
            currentstatus = "at-loc-1"

        elif req.transition == 2:
            rospy.loginfo("move-2")
            currentstatus = "at-loc-2"

        elif req.transition == 0:
            rospy.loginfo("Reset")
            currentstatus = "START"

        else:
            rospy.loginfo("Invalid command")
        pub.publish(currentstatus)
        rospy.spin()

def at_loc_1(req):
    if req.transition == 2:
        rospy.loginfo("move-2")
        currentstatus = "at-loc-2"
    elif req.transition == 0:
        rospy.loginfo("Reset")
        currentstatus = "START"
    else:
        rospy.loginfo("Invalid command")
    exit
def at_loc_2(req):
    if req.transition == 3:
        rospy.loginfo("grasp")
        currentstatus = "grasped-obj"
    elif req.transition == 0:
        rospy.loginfo("Reset")
        currentstatus = "START"
    else:
        rospy.loginfo("Invalid command")
    exit
def grasped_obj(req):

    if req.transition == 4:
        rospy.loginfo("place")
        currentstatus = "placed-obj"
    elif req.transition == 0:
        rospy.loginfo("Reset")
        currentstatus = "START"
    else:
        rospy.loginfo("Invalid command")

def placed_obj(req):
    if req.transition == 1:
        currentstatus = "at-loc-1"
    elif req.transition == 0:
        currentstatus = "START"
    else:
        rospy.loginfo("Invalid command")
    exit


def TriggerTransitionServer():

    rate = rospy.Rate(2)
    s = rospy.Service('transition', MyService, handle_trigger_transition)


    while not rospy.is_shutdown():
        pub.publish(currentstatus)
        rate.sleep()
    rospy.spin()

    #f = rospy.get_pram("frequency")


if __name__ == '__main__':
    if not('update' in locals()):
        currentstatus = "START"

    rospy.init_node('fsm')
    pub = rospy.Publisher('state', String, queue_size=10)

    TriggerTransitionServer()