Add SetGoal action and make the SetGoal service blocking - #20
Add SetGoal action and make the SetGoal service blocking#20VitezGabriela wants to merge 4 commits into
Conversation
…ion at same time and add reentrant service callback
VitezGabriela
changed the title
| from foreman_msgs.msg import ForemanErrorState | ||
|
|
||
|
|
||
| def _to_error_msg(snapshot: ErrorSnapshot) -> ForemanErrorState: |
There was a problem hiding this comment.
This is duplicating with the status library. So we have to see what we merge first to reuse as much as possible.
There was a problem hiding this comment.
Yes, what would you propose to do since there we have this function inlined but here we reuse it multiple times. Do you think best would be to make a conversion py file and import it in both?
| while True: | ||
| if not goal_handle.is_active: | ||
| result.success = False | ||
| result.message = f"Goal '{goal_name}' was preempted." |
There was a problem hiding this comment.
On shutdown. Do you prefer it removed?
| self._node.get_logger().info(f"{engine_response.message}") | ||
|
|
||
| feedback = SetGoal.Feedback() | ||
| while True: |
There was a problem hiding this comment.
Are there no mechanisms in python where we wait for a variable to be set, in this case would be at_goal and ˙is_error`?
Maybe better to check those variable in the while loop, instead of while true. A bit more clarity, possibly.
There was a problem hiding this comment.
threading.Condition would work, but cancel and shutdown come from the goal handle, nothing notifies you about those, so you'd still need the 50 ms loop. And it changes also the engine, so that would maybe be another PR
We can check the variables in the while loop, but the loop if it exits you would not know why- So you have to ask again. That would be 8 checks instead of current 4 checks.
| @@ -7,17 +10,20 @@ | |||
| class RosSetGoalServer: | |||
There was a problem hiding this comment.
When I see this. Maybe would actually make sense to make only one file with ROS interfaces. In that way, we might reduce some of the duplication. What do you think? If so, we should then make this in a follow-up PR.
There was a problem hiding this comment.
I agree, we can have one file with the SetGoal service and action. We can do it in another PR
2 participants
Summary
SetGoalROS 2 action interface andRosSetGoalActionServer.ForemanErrorStatemessage for action feedback/result error reporting.SetGoalservice blocking by waiting on Foreman's engine snapshot until the requested goal is reached, fails, or is preempted.set_goalexecution lock so action and service requests cannot overwrite each other while one goal is active.Behavior Change
Previously,
SetGoalservice returned success when the goal request was accepted.With this change, both interfaces now report completion only once Foreman reaches the requested goal:
at_goal = true.at_goal = true.If another
set_goalrequest is already active through either the service or the action, the new request is rejected with:Motivation
The old
SetGoalservice 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
SetGoalreturned 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 --verboseResult:
59 tests, 0 errors, 0 failures, 0 skipped