How to safely use actionlib's goal handles in a real-time concurrent system?
We have been developing a new control system for our robots based on ros_control, and we would like to ask for advice on the proper way to use actionlib's goal handles in a real-time and thread-safe way. I am aware of realtime_tools::RealtimeServerGoalHandle<Action> (had a look at JointTrajectoryController too), but since we also need to cancel and reject goals and send feedback, it might not be sufficient.
Our control system must interact with the existing planning system through an actionlib interface. Goals must be processed in a specific way expected by the planning system. The solution we have in mind involves 2 non-RT threads and 1 RT-thread. One of the non-RT threads serves the goal and cancel callbacks. These callbacks fill in a data structure with a shared pointer to the goal handle, a representation of the goal which can be loaded into the underlying control system, and the request type (new goal, repair, cancel). These structures are passed to the RT thread where they get processed and the control system is driven accordingly. If it is required to accept/reject/cancel/abort/feedback a goal, the pointer to the goal handle is stored in a different data structure that is passed to the second non-RT thread. This thread calls the corresponding functions in the goal handle.
One of the problems here is that a goal handle could be accessed concurrently in the RT thread and second non-RT thread. For example, the non-RT thread could call publishFeedback() on the goal handle while the RT thread accesses the goal inside the handle to verify if a repair is possible. Is this thread-safe if the RT thread only reads from the actual goal? Could something in the action client code make it not safe?
The other problem is that the RT thread needs to check if the cancel request message matches the current goal, and this might not be real-time safe. The == operator of actionlib::ServerGoalHandle<ActionSpec> involves getting the goal ID, which involves mutexes. Is using the == operator real-time safe?
These are questions specific to the approach we have taken, but we are also interested in knowing what other people have done to deal with actionlib goals in real-time control systems.
