[JointTrajectoryController] Avoid potential race conditions

[destogl]

Check the following comment by @matthew-reynolds:

I think this needs further work, probably in a follow-up PR.

In both the old and new code, I worry that we might end up in a condition where we're overwriting the goal_handle_timer_ while the previous rt_active_goal_ has pending operations, and thus dropping those operations. I think we should call goal_handle_timer_::execute_callback() or rt_active_goal_.readFromNonRT()->runNonRealtime() before creating the new timer.

Similarly, I think we could probably end up in a race condition where something is resetting the rt_active_goal_ right after this function writes the new rt_goal, and we could end up losing the goal handle. This would involve a little more work to solve, for example only resetting the rt_active_goal_ if new_data_available_ == false. That variable is private, but something along those lines.

Would appreciate your thoughts so we can consider appropriate follow-up PRs. This PR leaves the behaviour unchanged from before, and I haven't yet run into either of those cases, so I think we're ok to leave it to a future task.

Originally posted by @matthew-reynolds in #164 (comment)

[destogl]

@matthew-reynolds your solution sounds plausible. It seems we are resetting the rt_active_goal_ from multiple threads. This is not good.

[matthew-reynolds]

@destogl Thanks for opening an issue to track this. Can you assign me to it? I'll get started on it.