ROS 2 ActionServer callback causes subscriber to stop receiving

Question

I am building a single node which subscribes to receive Pose information. The subscriber callback updates class state fields (like self.x). This node also contains an ActionServer, which upon client request, will use the state information (which should be continually updated from the pose subscription) to execute a "move_to." This action repeatedly publishes Twist information on a different topic.

On startup, I can tell that the subscriber is working effectively, and the Node continually updates its pose successfully. However, once the ActionServer execution callback starts, I notice that the subscriber callback is never called, and as a result the pose state is never updated.

I would like to create a Node which can continually receive new state information updates while it executes an action. Is this possible / am I going about this in the correct manner?

Here is an abbreviated (extensive math removed) version of my Node definition:

class ActionSrv(Node):

    def __init__(self):
        super().__init__('action_srv')

        self.action_server = ActionServer(
            self,
            CustomAction,
            'custom_action',
            self.move_to_callback)

        self.subscription = self.create_subscription(
            Pose,
            'pose',
            self.pose_callback,
            10)

        self.publisher = self.create_publisher(Twist, 'output', 10)
        
        self.x_dest = 0.0
        self.y_dest = 0.0
        self.x = 0.0
        self.y = 0.0
        self.r = 999.99

        self.twist = Twist()

    def pose_callback(self, pose):
        self.get_logger().info('Pose: %s' % (pose))
        self.x = pose.x
        self.y = pose.y


    def move_to_callback(self, goal_handle):
        self.get_logger().info('Executing Custom Action...')  # once this code begins, fields such as self.x are never updated
        self.x_dest = goal_handle.request.x_dest
        self.y_dest = goal_handle.request.y_dest
        self.r = self.get_distance()

        feedback_msg = MoveTo.Feedback()

        while self.r > 0.2:
            
            # perform calculations

            # publish updates
            self.publisher.publish(self.twist)
            
            # give feedback
            feedback_msg.x_curr = self.x
            feedback_msg.y_curr = self.y
            goal_handle.publish_feedback(feedback_msg)

        goal_handle.succeed()

        result = MoveTo.Result()
        result.x_final = self.x
        result.y_final = self.y
        return result


def main(args=None):
    rclpy.init(args=args)

    server = ActionSrv()

    rclpy.spin(server)


if __name__ == '__main__':
    main()

---

Originally posted by exuberantshark on ROS Answers with karma: 23 on 2020-07-04

Post score: 2

Answer 1

rclpy is spinning your node on a single thread. While you are processing the action server request, the rclpy logic that would service node callbacks is blocked.

You can fix this by using a MultiThreadedExecutor, which uses a pool of threads to service callbacks/goal requests.

Example usage of MultiThreadedExecutor can be found in the ros2 examples repo

---

EDIT: This step was not needed, preserving it so answer comments make sense

While in your move_to_callback polling loop, drop a time.sleep(0.0) to yield the thread, and then the MultiThreadedExecutor should come in and service your subscription callback

---

Originally posted by johnconn with karma: 553 on 2020-07-05

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by exuberantshark on 2020-07-06:
Thank you. This was a very helpful answer and my callbacks are working now.

Note: I did not find that the time.sleep(0.0) addition was necessary for functionality. Does the sleep call create/force a definitive point for yielding the thread? Perhaps without it, my thread execution/scheduling is non-deterministic, but that did not happen to matter a lot for my application?

Comment by johnconn on 2020-07-06:
it's not clear to me how python handles yielding threads, to be honest, I just parroted the first stack overflow answer I found on how to yield python threads. I'll edit that sleep out of my answer.

Comment by jmaker on 2020-07-07:
@johnconn Thanks for pointing at those executor examples! Those have been really helpful.

@exuberantshark Thank you for posing the question. I am facing a similar problem in wanting to use both a subscriber and an action server together in a single node but seeing that my subscriber calls being blocked after 3 calls. Using the MultiThreadedExecutor and adding nodes to it so the callbacks could be done in parallel (like the example johnconn linked to), I am still seeing that my subscriber runs 3 iteration before it stops updating even though a publisher is continually publishing. Did you see something similar as you were iterating before getting it your callbacks to work?

EDIT: I couldn't post my entire script in one block. Apologies...

Comment by jmaker on 2020-07-07:
class SimpleActionServer(Node):

    def __init__(self):
        super().__init__('py_action_server')
        self._action_server = ActionServer(
            self,
            Frame,
            'frame',
            self.execute_callback)
        self.current_frame = 0

    def execute_callback(self, goal_handle):
        self.get_logger().info('Executing goal...')
        feedback_msg = Frame.Feedback()
        feedback_msg.progress = self.current_frame
           
        while self.current_frame < goal_handle.request.order:

            time.sleep(.5)
            feedback_msg.progress = self.current_frame
            self.get_logger().info(f'Feedback: {feedback_msg.progress}')
            goal_handle.publish_feedback(feedback_msg)
            
        goal_handle.succeed()
        result = Frame.Result()
        result.response = self.current_frame
        return result

Comment by jmaker on 2020-07-07:
class MinimalSubscriber(Node):

    def __init__(self,actionSever):
        super().__init__('minimal_subscriber')
        self.subscription = self.create_subscription(
            Num,                                            
            'topic',
            self.listener_callback,
            1)
        self.subscription
        self.__window_name = "sub"
        self.action_server = actionSever

    def listener_callback(self, msg):
        # subscriber updates current_frame from the publisher 
        self.action_server.current_frame = msg.num
        self.get_logger().info('I heard: "%d"' % msg.num)

Comment by jmaker on 2020-07-07:
def main(args=None): rclpy.init(args=args)

    try:
        py_action_server = SimpleActionServer()
        py_sub = MinimalSubscriber(actionSever=py_action_server)
        executor = MultiThreadedExecutor(num_threads=4)
        executor.add_node(py_action_server)
        executor.add_node(py_sub)

        try:
            executor.spin()
        finally:
            executor.shutdown()
            py_action_server.destroy_node()

    finally:
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Comment by jmaker on 2020-07-07:
This is the output of the sub/action server node above when I have a publisher publishing increasing values without having an action client sending a request. After 3 iteration it freezes.

Current value: 0 updated value 0
[INFO] [1594147488.305113087] [minimal_subscriber]: I heard: "0"
Current value: 0 updated value 1
[INFO] [1594147489.277334707] [minimal_subscriber]: I heard: "1"
Current value: 1 updated value 2
[INFO] [1594147490.262435445] [minimal_subscriber]: I heard: "2"

Comment by jmaker on 2020-07-07:
I think what posted works! I had a little bug that was unrelated to this. Thanks.