Skip loop delay if new goal is already available

[ggorjup]

Summary

There is a bug in SimpleActionClient that causes unnecessary delays of 0.1s if goals are sent in quick succession and the action server status is not set at the very end of the execute_callback.

Steps to Reproduce

1. Create and run a simple action server, where a short delay is added after setting action status in the callback:

#! /usr/bin/env python3
import actionlib
import rospy
from actionlib_tutorials.msg import AveragingAction, AveragingResult


class AvgAction(object):

    def __init__(self):
        self._as = actionlib.SimpleActionServer('averaging_action', AveragingAction,
                                                execute_cb=self.execute_cb, auto_start=False)
        self._as.start()

    def execute_cb(self, goal):
        self._as.set_succeeded(AveragingResult())
        rospy.sleep(0.01)   # Add a short delay after setting action status


if __name__ == '__main__':
    rospy.init_node('action_server')
    server = AvgAction()
    rospy.spin()

2. Send 10 goals to the server in succession and record time taken to complete them:

#! /usr/bin/env python3

import time
import actionlib
import rospy
from actionlib_tutorials.msg import AveragingAction, AveragingGoal


def send_goals():
    # Create client and goal
    client = actionlib.SimpleActionClient('averaging_action', AveragingAction)
    client.wait_for_server()
    goal = AveragingGoal()

    # Call server several times in succession and record time
    time_init = time.time()
    for _ in range(10):
        client.send_goal(goal)
        client.wait_for_result()
    rospy.loginfo(f"Time taken: {time.time() - time_init} seconds.")


if __name__ == '__main__':
    rospy.init_node('action_client')
    send_goals()

Expected behavior

The time required to process 10 goals is ~0.1 seconds.

Actual behavior

The time required to process 10 goals is ~1.0 seconds.

Implications

This behavior can cause unnecessary delays when processing a queue of actions. Even though the delay per goal is only 0.1 seconds, this can stack up quickly when the queue is longer.

Cause

The self.execute_condition.wait(loop_duration.to_sec()) statement in SimpleActionServer.executeLoop() starts a threading.Condition that waits until it is interrupted (notified) or until it reaches the specified timeout of 0.1 seconds. The notification is triggered in the SimpleActionServer.internal_goal_callback() whenever a new goal is received.

In the example case, the action is set_succeeded() slightly before the callback is exited. The client is therefore able to send the next goal while the server is still processing the execute_callback of the previous one. The new goal is received before entering the execute_condition.wait(), and the interrupt/notification is triggered with no effect. When exiting the execute_callback and entering the execute_condition.wait(), there is nothing to interrupt the lock because the new goal is already here. The lock therefore waits until its full timeout before looping around and explicitly checking for goal presence.

Solution

Start the execute_condition.wait() only if a new goal is not already present. Check for goal presence after acquiring the execute_condition lock to ensure the new_goal flag can not be changed by the other thread during/after checking.