Skip to content
Snippets Groups Projects
Commit 48f6aea3 authored by Albert Bhagwan Bahrunani's avatar Albert Bhagwan Bahrunani
Browse files

Cleaned nav with gui bt

parent 3734ccee
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/xml/nav_with_gui_main_tree.xml
+ 36
− 90
View file @ 48f6aea3
<?xml version="1.0"?>
<root main_tree_to_execute="NAV_WITH_GUI_MAIN">
<!-- Pending: adapt demo to changes applied to the basic navigation demo -->
<BehaviorTree ID="NAV_WITH_GUI_MAIN">
<Sequence>
<Action ID="sync_start_nav_gui"/>
<Fallback>
<ReactiveSequence>
<Action ID="get_status_text" nav_status="{status_text}"/>
<Action ID="set_nav_status_nav_gui" nav_status="{status_text}"/>
<Action ID="get_current_velocity" velocity="{velocity}"/>
<Action ID="set_current_velocity_nav_gui" velocity="{velocity}"/>
<Action ID="get_current_path_distance" path_distance="{path_distance}"/>
<Action ID="set_distance_to_goal_nav_gui" distance_to_goal="{path_distance}"/>
<Fallback>
<Sequence>
<SubTree ID="go_to_goal" f_id="f_id" heading_tol="heading_tol" output_go_to_goal="NAV_BT_status" update_goal="update_goal" x="x" x_y_pos_tol="x_y_pos_tol" y="y" yaw="yaw"/>
<SetBlackboard output_key="output_nav" value="{NAV_BT_status}"/>
</Sequence>
<ForceFailure>
<Sequence>
<Action ID="get_current_status_nav" status_code="{NAV_BT_status}"/>
<SetBlackboard output_key="output_nav" value="{NAV_BT_status}"/>
</Sequence>
</ForceFailure>
</Fallback>
</ReactiveSequence>
<ForceFailure>
<Action ID="sync_stop_nav"/>
</ForceFailure>
</Fallback>
</Sequence>
<Sequence>
<Action ID="sync_start_nav_gui"/>
<Fallback>
<ReactiveSequence>
<Action ID="get_status_text" nav_status="{status_text}"/>
<Action ID="set_nav_status_nav_gui" nav_status="{status_text}"/>
<Action ID="get_current_velocity" velocity="{velocity}"/>
<Action ID="set_current_velocity_nav_gui" velocity="{velocity}"/>
<Action ID="get_current_path_distance" path_distance="{path_distance}"/>
<Action ID="set_distance_to_goal_nav_gui" distance_to_goal="{path_distance}"/>
<SubTree ID="go_to_goal" x_goal="x_in" y_goal="y_in" yaw_goal="yaw_in" heading_tol_goal="heading_tol_in" x_y_pos_tol_goal="x_y_pos_tol_in" update_goal="update_goal_in" status="NAV_BT_status_out" frame_id="frame_id_in"/>
</ReactiveSequence>
<ForceFailure>
<Sequence>
<Action ID="get_current_status_nav" status_code="{NAV_BT_status_out}"/>
<!-- If navigation fails we stop the movement of the robot -->
<Action ID="sync_stop_nav"/>
<!-- We are not waiting until the navigation really stops -->
</Sequence>
</ForceFailure>
</Fallback>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="go_to_goal">
<Sequence>
<Action ID="set_goal_frame" frame_id="{f_id}"/>
<Timeout msec="600000">
<RetryUntilSuccesful num_attempts="3">
<Action ID="async_go_to_pose" heading_tol="{heading_tol}" update_goal="{update_goal}" x="{x}" x_y_pos_tol="{x_y_pos_tol}" y="{y}" yaw="{yaw}"/>
</RetryUntilSuccesful>
</Timeout>
<Condition ID="is_succeded_nav"/>
<Action ID="get_current_status_nav" status_code="{NAV_BT_status}"/>
<SetBlackboard output_key="output_go_to_goal" value="{NAV_BT_status}"/>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="async_go_to_pose">
<inout_port name="heading_tol"/>
<inout_port name="update_goal"/>
<inout_port name="x"/>
<inout_port name="x_y_pos_tol"/>
<inout_port name="y"/>
<inout_port name="yaw"/>
</Action>
<Action ID="get_current_path_distance">
<inout_port name="path_distance"/>
</Action>
<Action ID="get_current_status_nav">
<inout_port name="status_code"/>
</Action>
<Action ID="get_current_velocity">
<inout_port name="velocity"/>
</Action>
<Action ID="get_status_text">
<inout_port name="nav_status"/>
</Action>
<SubTree ID="go_to_goal">
<inout_port name="f_id"/>
<inout_port name="heading_tol"/>
<inout_port name="output_go_to_goal"/>
<inout_port name="update_goal"/>
<inout_port name="x"/>
<inout_port name="x_y_pos_tol"/>
<inout_port name="y"/>
<inout_port name="yaw"/>
</SubTree>
<Sequence>
<Action ID="set_goal_frame" 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" 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"/>
<Action ID="set_current_velocity_nav_gui">
<inout_port name="velocity"/>
</Action>
<Action ID="set_distance_to_goal_nav_gui">
<inout_port name="distance_to_goal"/>
</Action>
<Action ID="set_goal_frame">
<inout_port name="frame_id"/>
</Action>
<Action ID="set_nav_status_nav_gui">
<inout_port name="nav_status"/>
</Action>
<Action ID="sync_start_nav_gui"/>
<Action ID="sync_stop_nav"/>
</TreeNodesModel>
<!-- ////////// -->
<Action ID="get_current_status_nav" status_code="{status}"/>
</Sequence>
</BehaviorTree>
</root>
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment