diff --git a/src/xml/nav_with_gui_main_tree.xml b/src/xml/nav_with_gui_main_tree.xml index fb71571ab4108db531e30d77de22647613315fac..e05408ba4c679b0b78d79e12fd1bed7a3ddb875f 100644 --- a/src/xml/nav_with_gui_main_tree.xml +++ b/src/xml/nav_with_gui_main_tree.xml @@ -1,97 +1,43 @@ <?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>