Ask Your Question
1

ROS 2 ActionServer callback causes subscriber to stop receiving

asked 2020-07-04 13:44:28 -0500

exuberantshark gravatar image

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()
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2020-07-05 14:12:11 -0500

johnconn gravatar image

updated 2020-07-06 16:18:06 -0500

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

edit flag offensive delete link more

Comments

1

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?

exuberantshark gravatar image exuberantshark  ( 2020-07-06 12:43:24 -0500 )edit
2

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.

johnconn gravatar image johnconn  ( 2020-07-06 13:05:36 -0500 )edit

@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...

jmaker gravatar image jmaker  ( 2020-07-07 12:32:34 -0500 )edit

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
jmaker gravatar image jmaker  ( 2020-07-07 12:34:11 -0500 )edit

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)
jmaker gravatar image jmaker  ( 2020-07-07 12:34:34 -0500 )edit

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()
jmaker gravatar image jmaker  ( 2020-07-07 12:34:53 -0500 )edit

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"
jmaker gravatar image jmaker  ( 2020-07-07 13:47:34 -0500 )edit

I think what posted works! I had a little bug that was unrelated to this. Thanks.

jmaker gravatar image jmaker  ( 2020-07-07 14:13:43 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2020-07-04 13:44:28 -0500

Seen: 159 times

Last updated: Jul 06