Add SetGoal action and make the SetGoal service blocking

[VitezGabriela]

Summary

• Add the SetGoal ROS 2 action interface and RosSetGoalActionServer.
• Add ForemanErrorState message for action feedback/result error reporting.
• Make the SetGoal service blocking by waiting on Foreman's engine snapshot until the requested goal is reached, fails, or is preempted.
• Add a shared set_goal execution lock so action and service requests cannot overwrite each other while one goal is active.
• Use a reentrant callback group for the blocking service so concurrent service callers can reach the lock and get rejected instead of silently waiting in the callback queue.
• Add unit tests

Behavior Change

Previously, SetGoal service returned success when the goal request was accepted.

With this change, both interfaces now report completion only once Foreman reaches the requested goal:

• The action result succeeds once Foreman reports at_goal = true.
• The service response succeeds once Foreman reports at_goal = true.

If another set_goal request is already active through either the service or the action, the new request is rejected with:

Another set_goal request is already active.

Motivation

The old SetGoal service could only report whether Foreman accepted a requested goal. It returned before Foreman's transition loop had finished switching controllers and components, so clients had no Foreman-level ROS signal for when the requested state was actually reached.

This could create a race for clients that need the target controller to be active immediately after requesting a goal. For example, MoveIt could send a trajectory after SetGoal returned success while the joint trajectory controller was still inactive, causing the trajectory goal to be rejected.

The action solves this for action clients, while the blocking service keeps compatibility for existing service clients.

Testing

colcon build --packages-up-to foreman
colcon test --packages-up-to foreman
colcon test-result --verbose

Result: 59 tests, 0 errors, 0 failures, 0 skipped

[destogl]

can this actually happen?

[VitezGabriela]

On shutdown. Do you prefer it removed?

[destogl]

But then I am not sure if this is the correct comment. Can we on shutdown call "cancel" on the current goal if such is running/exits? Would be cool to have only one execution interruption path we can reuse.

[VitezGabriela]

I merged into one block, so shutdown and cancel take the same path. We couldn't literally call cancel on shutdown, because by then the goal handle is already gone and canceled() does nothing.

I also added a fix because on shutdown the wait loop of action never stopped, so the node was never properly destroyed, it just hung and had to be killed manually. The blocking service did the same thing. Fixed it by adding a shutting down flag and adding it to destroy node.

[destogl]

@copilot resolve the merge conflicts in this pull request

[VitezGabriela]

@destogl Should i rename the service and action to have a private namespace?

[destogl]

@destogl Should i rename the service and action to have a private namespace?

yes please :)

Also run the pre-commit again, I have updated linters just today.