[Bug in ROS | Solved] Extending SimpleActionServer/-Client to allow REJECTING goals

edit

Update: This is a known bug, refer to my comment in this bug tracker on Github for a possible solution and more details. More information can also be found in my accepted answer below.

Hi folks,

I am trying to extend the functionality of the SimpleActionServer / -Client classes to allow rejecting new goals if (and only if) there are currently none being processed. That is, I would like to replace/disable the PENDING state logically in a sense that when there is currently an ACTIVE goal, it will not be preempted and the very recently received goal will automatically be rejected by the server. In contrast, once a new goal is received and there is no goal being currently processed, the server should accept the new goal. However, I would like to retain the ability to cancel/abort a goal (both by the client and the server) just in case this matters.

To achieve this, I've modified the actionlib tutorials (namely 1, 2, 3, and 4). You can find my modified learning_actionlib package for download here. Modifications are basically in the goalCallback() function, simply search for CHANGED in order_action_server_imp.h. You can compile it in any Indigo/Jade/Kinetic workspace and run it (from your catkin_ws dir) using

roslaunch src/learning_actionlib/launch/test.launch

or

roslaunch src/learning_actionlib/launch/test_avg.launch

The problem with that code is as follows: When one of the above launchfiles is started, a single server and multiple clients are being launched at the 'same' time (-> workload creation). On my slower machine (i7 4600U, VMware player), it regularily happens that 1 or more clients incorrectly timeout while one client always (correctly) returns SUCCEEDED and all the others correctly report REJECTED. However, I would assume that exactly one client succeeds and all others get rejected. It seems that the server correctly publishes the REJECTED states as the following (DEBUG) log shows but the clients don't get these information: http://pastebin.com/m9k5xniD . In particular and according to that log, the server correctly receives all goals, only processes the first (lines 7-12) and rejects new ones (lines 17, 23, 29) -> 4 clients. Note that it doesn't matter if test.launch or test_avg.launch is used, both have the same problem. I guess the problem is related to spinning/multithreading (and parallel code execution in general as this only rarely happens on my fast 12-core machine). Also, if I decrease the #cores in my VM, the probability of occurrence increases. Thus, if you don't have the problem, you may need to restart the launchfiles a few times or increase the number of clients.

Please also note that I am occasionally get a

double free or corruption

(related to a std::vector::push_back used inside a boost::mutex) and

average_server: /usr/include/boost/thread/pthread/recursive_mutex.hpp:110: void boost::recursive_mutex::lock(): Assertion `!pthread_mutex_lock(&m)' failed.

error when terminating roslaunch by Ctrl+C. Not sure if this is related. In general, I am not sure if ... (more)