Add XML parsing support for custom Control Nodes

[Sarath18]

Signed-off-by: Sarathkrishnan Ramesh sarathkrishnan99@gmail.com

Basic Info

Info
Ticket address	Related to Groot #84
Primay OS tested on	Ubuntu 18.04
Tested using	Navigation2 + TurtleBot3 simulation

Description

Added support for parsing custom control nodes when exported using Groot. For example:

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <Control ID="PipelineSequence" name="NavigateWithReplanning">
            <Decorator ID="DistanceController" distance="1.0">
                <Action ID="ComputePathToPose" goal="{goal}" path="{path}" planner_id="GridBased"/>
            </Decorator>
            <Action ID="FollowPath" controller_id="FollowPath" path="{path}"/>
        </Control>
    </BehaviorTree>
    <TreeNodesModel>
        <!--- Other Nodes -->
        <Control ID="PipelineSequence"/>
    </TreeNodesModel>
</root>

Future Work

I am not a 100% sure but Groot doesn't support monitoring when using custom Control Nodes.

[facontidavide]

thanks a lot

[najiib49]

Hi all!

I was learning behavior tree and was trying to add some dummy sequence node on navigate_to_pose_w_replanning_and_recovery.xml through Groot. I specifically added the sequence node on the navigation subtree of Navigate To Pose With Replanning and Recovery¶. In this sequence I tried to do two spins before pipelinesequence gets ticked.

Here's how my tree looks like

However, the problem I've faced was that when I saved this BT from groot, the underlying custom nodes in the xml changed into this parsing xml format mentioned in this PR. for example <PipelineSequence name="NavigateWithReplanning"> changed to this <Control ID="PipelineSequence" name="NavigateWithReplanning"> as well as all the other custom nodes.

When I run Nav2 and tried Nav2 goal in rviz, nav2 computes the path to pose but never follows that path. It just goes into the Recovery Subtree¶. So I tried adding the sequence with the two spins by directly editing the navigate_to_pose_w_replanning_and_recovery.xml file instead of editing and saving it in Groot. I was able to achieve the behaviour I was looking for where the robot spins and then follows the computed path when Nav2 goal is selected in rviz.

I was wondering if anyone faced this issue when editing nav2 custom nodes in groot?

Note: this issue was persistent even when I loaded navigate_to_pose_w_replanning_and_recovery tree in groot and saved it without adding any additional nodes.