Skip to content
Snippets Groups Projects
Commit 05be610d authored by José Enrique Domínguez Vidal's avatar José Enrique Domínguez Vidal
Browse files

Timeout added in case of preempted to allow new planning after new goal

parent d33e4c35
No related branches found
No related tags found
No related merge requests found
......@@ -17,13 +17,23 @@
<BehaviorTree ID="go_to_goal_nav_3d">
<Sequence>
<Action ID="set_goal_frame_nav_3d" frame_id="{frame_id}"/>
<Timeout msec="600000">
<RetryUntilSuccesful num_attempts="3">
<!-- Pending: choose type of navigation required (only position/orientation) -->
<Action ID="async_go_to_pose_nav_3d" x="{x_goal}" y="{y_goal}" yaw="{yaw_goal}" heading_tol="{heading_tol_goal}" x_y_pos_tol="{x_y_pos_tol_goal}" update_goal="{update_goal}"/>
</RetryUntilSuccesful>
</Timeout>
<Condition ID="is_succeded_nav_3d"/>
<RetryUntilSuccesful num_attempts="3">
<Fallback>
<Sequence>
<Timeout msec="600000">
<!-- Pending: choose type of navigation required (only position/orientation) -->
<Action ID="async_go_to_pose_nav_3d" x="{x_goal}" y="{y_goal}" yaw="{yaw_goal}" heading_tol="{heading_tol_goal}" x_y_pos_tol="{x_y_pos_tol_goal}" update_goal="{update_goal}"/>
</Timeout>
<Condition ID="is_succeded_nav_3d"/>
</Sequence>
<Sequence>
<Condition ID="is_preempted_nav_3d"/>
<Timeout msec="5000">
<Action ID="NOP"/>
</Timeout>
</Sequence>
</Fallback>
</RetryUntilSuccesful>
<Action ID="get_current_status_nav_3d" status_code="{status}"/>
</Sequence>
</BehaviorTree>
......
......@@ -29,13 +29,23 @@
<BehaviorTree ID="go_to_goal_nav_3d">
<Sequence>
<Action ID="set_goal_frame_nav_3d" frame_id="{frame_id}"/>
<Timeout msec="600000">
<RetryUntilSuccesful num_attempts="3">
<!-- Pending: choose type of navigation required (only position/orientation) -->
<Action ID="async_go_to_pose_nav_3d" x="{x_goal}" y="{y_goal}" yaw="{yaw_goal}" heading_tol="{heading_tol_goal}" x_y_pos_tol="{x_y_pos_tol_goal}" update_goal="{update_goal}"/>
</RetryUntilSuccesful>
</Timeout>
<Condition ID="is_succeded_nav_3d"/>
<RetryUntilSuccesful num_attempts="3">
<Fallback>
<Sequence>
<Timeout msec="600000">
<!-- Pending: choose type of navigation required (only position/orientation) -->
<Action ID="async_go_to_pose_nav_3d" x="{x_goal}" y="{y_goal}" yaw="{yaw_goal}" heading_tol="{heading_tol_goal}" x_y_pos_tol="{x_y_pos_tol_goal}" update_goal="{update_goal}"/>
</Timeout>
<Condition ID="is_succeded_nav_3d"/>
</Sequence>
<Sequence>
<Condition ID="is_preempted_nav_3d"/>
<Timeout msec="5000">
<Action ID="NOP"/>
</Timeout>
</Sequence>
</Fallback>
</RetryUntilSuccesful>
<Action ID="get_current_status_nav_3d" status_code="{status}"/>
</Sequence>
</BehaviorTree>
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment