[nav2] How do I make a custom behavior tree?

edit

I am having __great__ trouble making my own behavior tree. It seems like a good system, but I can't figure out how to debug and construct in the way I intend.

I want the robot to navigate in stages. I ask the user to give me a path, and load it into the blackboard via a custom global planner. Then, I use a custom condition to check if I am close to the start of that path. If not, I navigate to the start of the path, then I navigate the original path. If I am close to the start to begin with, I navigate the original path.

<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
    <Sequence>
        <!-- Allow PathManager to get load its path into the path variable -->
        <ComputePathToPose goal="{goal}" path="{path}" planner_id="PathManager"/>
        <Fallback>
            <RobotReadyForPathCondition path="{path}" max_distance="0.5" max_angle="3.0"/>
            <Sequence>
                <!-- Get the first pose of the path -->
                <ExtractFirstPose path="{path}" extracted_pose="{first_goal}"/>
                <ComputePathToPose goal="{first_goal}" path="{path_to_first}" planner_id="navfn"/>
                <RecoveryNode number_of_retries="10">
                    <Sequence>
                        <Wait wait_duration="1.0"/>
                        <ComputePathToPose goal="{first_goal}" path="{path_to_first}" planner_id="navfn"/>
                    </Sequence>
                    <FollowPath path="{path_to_first}" controller_id="RPPController"/>
                </RecoveryNode>
            </Sequence>
        </Fallback>
        <Wait wait_duration="5.0"/>
        <ComputePathToPose goal="{first_goal}" path="{path}" planner_id="PathManager"/>
        <FollowPath path="{path_to_first}" controller_id="RPPController"/>
    </Sequence>
</BehaviorTree>
</root>

The result of this behavior tree is that it follows the original path to the goal. It's like all my custom logic does literally nothing, and I have no debug tools (that I know of, happy to be proven wrong). How do I go about finishing this tree? Have I made a fatal error in my tree? What tools can I use to continue to debug and read out values that I am interested in?