Add unified cleanup to Joint Trajectory Server

[hello-amal]

Currently, every branch of the Joint Trajectory Server has to perform its own cleanup before returning. This setup makes it easy to have bugs, e.g., where a developer adds a new return clause but forgets to do all the cleanup. Instead, we should add a unified cleanup function and all returns first call that. See this comment for more info.