Smach Iterator re-subscribes the topic every time it iterates

edit

I use SMACH Iterator for doing a forward step (state: FORWARD_STEP), stop (state: STOP) and iterate. The problem is that every time it enters the FORWARD_STEP state, it seems like re-subscribing to the topic, making it behave strange. In particular, I set my state to move 30cm every time. But whenever it iterates the state, the distance increases (60cm, 90cm, etc), and the odom value stucks to its previous based on the iteration (it doesn't stuck the first time, it stucks one time in the second iteration, it stucks 3 times in the 3rd iteration and so on).

Here is my state:

#!/usr/bin/env python

import roslib
import rospy
import smach
import smach_ros
from smach import Iterator, StateMachine, CBState
from smach_ros import ConditionState, IntrospectionServer, SimpleActionState, MonitorState, ServiceState

from geometry_msgs.msg import Twist, PoseStamped
from math import *
# import starting_point_sm
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry


motor_command_publisher = rospy.Publisher('/spraybase/cmd_vel', Twist, queue_size=100) 


# define state FORWARD_STEP
class forwardStepState(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded','aborted'])

        self.distance_travelled = 0.0
        self.check_odom = False
        self.stop_flag = False
        self.prev_odom_pos_y = 0.0


    def odom_cb(self, msg, userdata):

        if not self.stop_flag:

            if not self.check_odom:
                self.prev_odom_pos_y = msg.pose.pose.position.y
                self.check_odom = True

            # Positional error
            self.pos_error = abs(msg.pose.pose.position.y - self.prev_odom_pos_y)
            self.distance_travelled += self.pos_error

            rospy.loginfo("msg.pose.pose.position.y %f", msg.pose.pose.position.y)

            # Update
            self.prev_odom_pos_y = msg.pose.pose.position.y

            # Step condition
            if self.distance_travelled > 0.3:
                rospy.loginfo("Travelled %f distance", 0.3)
                self.distance_travelled = 0.0
                self.check_odom = False
                self.stop_flag = True
                # userdata.step_done = True
            else: 
                rospy.loginfo("Forward distance: %f", self.distance_travelled)


    def execute(self, userdata):

        rospy.loginfo('Executing state FORWARD_STEP')

        self.odom_sub = rospy.Subscriber("/spraybase/odom", Odometry, self.odom_cb , callback_args=userdata, queue_size=1) #robot_pose_ekf/odom_combined 

        timeout_time = rospy.Time.now()+rospy.Duration(60)
        self.stop_flag = False 

        # If we don't get there in time, abort the goal
        while timeout_time > rospy.Time.now() :
            motor_command = Twist()
            if self.stop_flag : 
                rospy.loginfo("Travelled step distance!")
                return 'succeeded'

            motor_command.linear.x = 0.5
            motor_command_publisher.publish(motor_command)

        rospy.loginfo("1 minute passed. Do you want to continue?")
        return 'aborted'


class forwardStepIter():

    def __init__(self):
        rospy.init_node('forward_step_iter')

        # Create the top level SMACH state machine
        sm_top = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])

        # Open the container
        with sm_top:

            smach.StateMachine.add('FORWARD', forwardStepState(), transitions={'succeeded':'FORWARD'})

        # Execute SMACH plan
        outcome = sm_top.execute()

        rospy.spin()


if __name__ == '__main__':
    try:
        forwardStepIter()
    except rospy.ROSInterruptException:
        rospy.loginfo("Task completed.")

And the output:

1st iteration:

2nd iteration:

3rd iteration:

Any help will be appreciated