Sleep inside node is blocking indefinitely

asked 2022-10-09 07:44:01 -0500

nathansf gravatar image

updated 2022-10-11 01:43:49 -0500

ravijoshi gravatar image

Context

I have a robot that publishes joint state messages and subscribes to joint commands with ROS2, and I would like to create a simple python interface for beginners to use like so:

#(inside main.py)
robot = Robot()
while(True):
     measurement = robot.measurement()
     robot.command(blah)
     robot.sleep(0.01)

I implemented the robot.measurement function by starting a subscriber in a background thread with target code rclpy.spin(sub_node). As for the robot.command function, this just publishes a message from inside a publisher node.

Problem

The main problem is implementing the robot.sleep function. I would like it to work with rostime and wall clock time. I have the following code (summarized):

class Robot:

    def __init__(self):
        rclpy.init()
        self.executor = rclpy.executors.SingleThreadedExecutor()
        self.sleep_node = SleepNode()
        self.pub = JointCommandPub()
        self.sub = JointStateSub()
        self.sub_thread = threading.Thread(target=self.sub_thread_fn)
        self.sub_thread.start()

    def sleep(self, sleep_sec):
        self.sleep_node.sleep(sleep_sec)

    def sub_thread_fn(self):
        self.exe = rclpy.executors.SingleThreadedExecutor()
        self.exe.add_node(self.sub)
        self.exe.add_node(self.sleep_node)
        self.exe.spin()
        # also doesn't work: rclpy.spin(self.sub)

    def command(self, joint_angles: np.array):
        self.pub.set_joint_angles(joint_angles)

    def measurement(self):
        self.robot_state.joint_angles = self.sub.latest_joint_angles()
        return self.robot_state

But the issue is that whenever robot.sleep is called, the python program (see above) blocks indefinitely, as if the sleep_node isn't getting new clock messages. Any ideas appreciated!

I thought a lot of people would be implementing simple script interfaces to ROS robots, but haven't found any yet. If someone could post a resource or example on this topic that would be fantastic. Thanks!

edit retag flag offensive close merge delete

Comments

It is a deadlock that is occurring due to your executor. I suggest going through executors and callback in ROS 2. I also faced a similar issue in the past #q404442

ravijoshi gravatar image ravijoshi  ( 2022-10-11 01:41:05 -0500 )edit

@ravijoshi, thanks for the reply. I read those documents but didn't find an answer. Based on the question you linked, are you suggesting I spin before the sleep call? However it seems that in rclpy, spin_some does not exist. Thanks

nathansf gravatar image nathansf  ( 2022-10-11 09:06:19 -0500 )edit