Fix cancel goal with empty goal handle

[vicmassy]

There is a corner case where it is possible that the action gets cancelled before the goal handle exists, hence this PR attempts to protect the cancel goal when the goal handle is nullptr.

[robin-mueller]

Hi, I wanted to note that it can be desirable to throw rclcpp_action::exceptions::UnknownGoalHandleError or at least any kind of exception on cancelGoal() if the goal response has not arrived yet. In fact, I am relying on such a case in my application, because I want to make sure that the goal is canceled either way, so I wait until it has arrived and proceed with the cancellation. After merging this pull request, there is no way for me to detect if haltTree() ran into a problem or not. Before I did like below:

try {
  tree.haltTree();
} catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e) {
    // If tree is halted directly after an action started, the goal response might not have been
    // received yet. In this case we assume the goal is about to arrive and asynchronously retry to HALT
}

I have a seperate ROS2 node that tries to halt the tree and doesn't know about when a RosActionNode published goal requests, so it is mandatory for this case to be detectable during halting the tree.

I am falling back to a previous commit, until this issue has been resolved.

[facontidavide]

I am not sure what the CORRECT behavior should be in this case (excluding workarounds, of course).

In general, we want to make sure that if a goal was sent, there is always a cleanup, even if this cause the onHalt method to block.

Also, throwing is not desirable, in my opinion.

Should we block and wait for goalhandle to become valid?

[robin-mueller]

There are three problem solving behaviors that I can imagine:

A: Wait for the goal response during the initial tick (previous state of the node was IDLE and was just ticked for the first time) B: Wait for the goal response during halt() C: Throw an error on halt() if no goal response has arrived yet

I'm unsure whether we should wait according to A or B, but I prefer A as it is more intuitive. We would return RUNNING, when the action performed by the server is actually running. Also, with A there is a possibility to return FAILURE on timeout, with B there isn't and we would need to do C additionally. Furthermore, I believe that there is not too many cases where you would really want to send multiple action goal requests asynchronously before any of them have been answered. If ever, you usually want to execute them asynchronously (would only work in a Parallel control node anyways), so waiting as in A would enforce a sequential initialization of each action and enable individual fallback behaviors if the goal has been rejected. So for example we could move this section to the case when the status of the node is IDLE and alter it so that we return RUNNING only as soon as goal_received_ is true. Of course we would need to keep the node spinning in the meantime.

Technically, the developer has already been urged to consider the fact that a goal request requires some time to be answered by the server and the node might return ActionNodeErrorCode::SEND_GOAL_TIMEOUT, thanks to the configurable parameter server_timeout in RosNodeParams. However, it is surely still breaking for some behavior trees out there to change the implementation in favor of A.