CPU usage at 100% when running SMACH state machine in ROS [closed]

asked 2022-10-13 16:55:13 -0600

Blazerunner738 gravatar image

I currently have a python node that runs a Smach state machine (library linked here: http://wiki.ros.org/smach)

The state machine starts in an IDLE state which looks like this:

class Idle(smach.State):
def __init__(self):
    smach.State.__init__(
        self,
        outcomes=["state_reset_complete"],
        input_keys=[
            "img_diff",
            "img1",
            "img3",
        ],
        output_keys=[
            "img_diff",
            "img1",
            "img2",
        ],
    )

def execute(self, userdata):
    rospy.loginfo("Idle")
    userdata.img_diff = None
    userdata.img1 = None
    userdata.img2 = None
    rospy.loginfo("Transitioning to WAIT_FOR_CALIBRATION_START")
    return "state_reset_complete"

The WAIT_FOR_CALIBRATION_STATE looks like this:

class WaitForCalibrationStart(smach.State):  # Define a state and outcomes
def __init__(self):
    smach.State.__init__(
        self,
        outcomes=["start", "wait"],
    )
    self.toggle_start = False
    rospy.Subscriber(
        "/toggle_start",
        Bool,
        self.callback,
        queue_size=1,
    )

def callback(self, msg):
    self.toggle_start = msg.data

def execute(self, userdata):
    if self.toggle_start:
        rospy.loginfo("Transitioning to STATE1")
        return "start"
    return "wait"

When I run htop, I see that even in the "WAIT_FOR_CALIBRATION" state, when no processes are running or image algorithms are running, there's still a 100% CPU usage. Does anyone know why this could be happening? Is there a good way to fix this?

Also, not sure if this is related. But ctrl+C doesn't cleanly exit the node sometimes. Especially when not in the IDLE state. Any ideas to fixing this?

This is what my main() looks like:

if __name__ == "__main__":
rospy.init_node("node")

sm = init_sm()  # Create state machine
# Create a thread to execute the smach container
smach_thread = threading.Thread(target=sm.execute)
smach_thread.start()

# Wait for ctrl-c
rospy.spin()

# Request the container to preempt
sm.request_preempt()

# Block until everything is preempted
smach_thread.join()

The CPU is still overloaded if I change main to: if __name__ == "__main__": rospy.init_node("node")

sm = init_sm()  # Create state machine
sm.execute()

# Wait for ctrl-c
rospy.spin()
edit retag flag offensive reopen merge delete

Closed for the following reason duplicate question by gvdhoorn
close date 2022-10-14 01:31:42.338630

Comments

Not sure why this was closed, please link the duplicate since I've been trying to find the answer to this question for a while.

Blazerunner738 gravatar image Blazerunner738  ( 2022-10-14 15:11:16 -0600 )edit
ravijoshi gravatar image ravijoshi  ( 2022-10-15 11:00:33 -0600 )edit