I currently have a python node that runs a Smach state machine (library linked here: http://wiki.ros.org/smach). I'm using ROS noetic.
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()
