diff --git a/angler_behaviors/behavior_tree/blocks/__init__.py b/angler_behaviors/behavior_tree/behaviors/__init__.py
similarity index 100%
rename from angler_behaviors/behavior_tree/blocks/__init__.py
rename to angler_behaviors/behavior_tree/behaviors/__init__.py
diff --git a/angler_behaviors/behavior_tree/blocks/planning.py b/angler_behaviors/behavior_tree/behaviors/cleanup.py
similarity index 100%
rename from angler_behaviors/behavior_tree/blocks/planning.py
rename to angler_behaviors/behavior_tree/behaviors/cleanup.py
diff --git a/angler_behaviors/behavior_tree/behaviors/mission.py b/angler_behaviors/behavior_tree/behaviors/mission.py
new file mode 100644
index 0000000..f0f638d
--- /dev/null
+++ b/angler_behaviors/behavior_tree/behaviors/mission.py
@@ -0,0 +1,87 @@
+# Copyright 2023, Evan Palmer
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+import operator
+
+import py_trees
+import py_trees_ros
+from behavior_tree.primitives.control import make_execute_multidof_trajectory_behavior
+from behavior_tree.primitives.planning import make_high_level_planning_behavior
+from std_msgs.msg import Bool
+
+
+def make_save_start_mission_behavior(
+    start_mission_key: str,
+) -> py_trees.behaviour.Behaviour:
+    """Make a behavior that saves the 'start mission' flag to the blackboard.
+
+    Args:
+        start_mission_key: The key at which the flag should be saved.
+
+    Returns:
+        A behavior that saves the key to trigger arming.
+    """
+    return py_trees_ros.subscribers.ToBlackboard(
+        name="ROS2BB: Start mission",
+        topic_name="/angler/cmd/start_mission",
+        topic_type=Bool,
+        qos_profile=py_trees_ros.utilities.qos_profile_latched(),
+        blackboard_variables={start_mission_key: None},
+        clearing_policy=py_trees.common.ClearingPolicy.ON_SUCCESS,
+    )
+
+
+def make_execute_mission_behavior(
+    start_mission_key: str,
+    robot_state_key: str,
+) -> py_trees.behaviour.Behaviour:
+    """Make a behavior that sets up the system prior to beginning a mission.
+
+    Args:
+        start_mission_key: The key at which the signal indicating that a mission should
+            start is stored.
+        robot_state_key: The key at which the robot state is stored.
+
+    Returns:
+        A system setup behavior.
+    """
+    # Start by checking whether or not to start the mission
+    check_start_mission = py_trees.behaviours.CheckBlackboardVariableValue(
+        name="Start the mission?",
+        check=py_trees.common.ComparisonExpression(
+            variable=start_mission_key, value=True, operator=operator.eq
+        ),
+    )
+
+    get_mission_plan = make_high_level_planning_behavior(
+        robot_state_key=robot_state_key,
+        planner_id_key="preplanned_end_effector_waypoint_planner",
+        planning_result_key="planning_result",
+    )
+
+    execute_mission = make_execute_multidof_trajectory_behavior(
+        "planning_result", "tpik_joint_trajectory_controller"
+    )
+
+    return py_trees.composites.Sequence(
+        name="Execute mission",
+        memory=True,
+        children=[check_start_mission, get_mission_plan, execute_mission],
+    )
diff --git a/angler_behaviors/behavior_tree/behaviors/setup.py b/angler_behaviors/behavior_tree/behaviors/setup.py
new file mode 100644
index 0000000..557292f
--- /dev/null
+++ b/angler_behaviors/behavior_tree/behaviors/setup.py
@@ -0,0 +1,60 @@
+# Copyright 2023, Evan Palmer
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+import operator
+
+import py_trees
+from behavior_tree.primitives.arming import make_system_arming_behavior
+
+
+def make_setup_behavior(setup_finished_flag_key: str) -> py_trees.behaviour.Behaviour:
+    """Make a behavior that sets up the system prior to beginning a mission.
+
+    Args:
+        setup_finished_flag_key: The key at which the setup flag is stored.
+
+    Returns:
+        A system setup behavior.
+    """
+    # Don't repeat setup if we have already done it
+    check_setup_finished = py_trees.behaviours.CheckBlackboardVariableValue(
+        name="Setup complete?",
+        check=py_trees.common.ComparisonExpression(
+            variable=setup_finished_flag_key, value=True, operator=operator.eq
+        ),
+    )
+
+    # Once we have a plan, arm so that we can start trajectory tracking
+    arming = make_system_arming_behavior(arm=True, use_passthrough_mode=True)
+
+    # Finish up by indicating that the setup has finished
+    finished_setup = py_trees.behaviours.SetBlackboardVariable(
+        "Setup finished!", setup_finished_flag_key, True, overwrite=True
+    )
+
+    setup = py_trees.composites.Sequence(
+        name="Setup the system for a mission",
+        memory=True,
+        children=[arming, finished_setup],
+    )
+
+    return py_trees.composites.Selector(
+        name="Run system setup", memory=False, children=[check_setup_finished, setup]
+    )
diff --git a/angler_behaviors/behavior_tree/blocks/arming.py b/angler_behaviors/behavior_tree/blocks/arming.py
deleted file mode 100644
index 4592692..0000000
--- a/angler_behaviors/behavior_tree/blocks/arming.py
+++ /dev/null
@@ -1,231 +0,0 @@
-# Copyright 2023, Evan Palmer
-#
-# Permission is hereby granted, free of charge, to any person obtaining a copy
-# of this software and associated documentation files (the "Software"), to deal
-# in the Software without restriction, including without limitation the rights
-# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-# copies of the Software, and to permit persons to whom the Software is
-# furnished to do so, subject to the following conditions:
-#
-# The above copyright notice and this permission notice shall be included in
-# all copies or substantial portions of the Software.
-#
-# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
-# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-# THE SOFTWARE.
-
-import operator
-
-import py_trees
-import py_trees_ros
-from components.blackboard import FunctionOfBlackboardVariables
-from components.service_clients import FromConstant as ServiceClientFromConstant
-from std_msgs.msg import Bool
-from std_srvs.srv import SetBool
-
-
-def make_save_armed_behavior() -> py_trees.behaviour.Behaviour:
-    """Save a command to arm/disarm the system to the blackboard.
-
-    Returns:
-        A ToBlackboard behavior which saves commands to arm/disarm the system to the
-        blackboard.
-    """
-    return py_trees_ros.subscribers.ToBlackboard(
-        name="ROS2BB: Arm",
-        topic_name="/angler/cmd/arm_autonomy",
-        topic_type=Bool,
-        qos_profile=py_trees_ros.utilities.qos_profile_latched(),
-        blackboard_variables={"armed": None},
-        clearing_policy=py_trees.common.ClearingPolicy.NEVER,
-    )
-
-
-def make_block_on_disarm_behavior(
-    tasks: py_trees.behaviour.Behaviour,
-    on_disarm_behavior: py_trees.behaviour.Behaviour | None = None,
-) -> py_trees.behaviour.Behaviour:
-    """Make a behavior that blocks when the system is disarmed.
-
-    Args:
-        tasks: A behavior with the tasks to run.
-        on_disarm_behavior: An optional behavior to run when a disarm is triggered. This
-            will be executed as a Sequence behavior following the disarm behavior.
-
-    Returns:
-        A Selector behavior with the disarm EternalGuard as the highest priority
-        behavior and the provided tasks as second priority.
-    """
-
-    def check_disarm_on_blackboard(
-        blackboard: py_trees.blackboard.Blackboard,
-    ) -> bool:
-        return blackboard.disarm  # type: ignore
-
-    # Default to disarming passthrough mode just in case
-    disarming = make_arming_behavior(False, use_passthrough_mode=True)
-
-    behaviors = [disarming]
-
-    if on_disarm_behavior is not None:
-        behaviors.append(on_disarm_behavior)  # type: ignore
-
-    on_disarm = py_trees.composites.Sequence(
-        name="Execute disarm sequence",
-        memory=True,
-        children=[behaviors],  # type: ignore
-    )
-
-    disarm = py_trees.decorators.EternalGuard(
-        name="Disarm?",
-        condition=check_disarm_on_blackboard,
-        blackboard_keys={"disarm"},
-        child=on_disarm,
-    )
-
-    return py_trees.composites.Selector(
-        name="Block tasks on disarm", memory=False, children=[disarm, tasks]
-    )
-
-
-def make_arming_behavior(arm: bool, use_passthrough_mode: bool):
-    """Create a behavior that arms/disarms the system.
-
-    The full system arming sequence includes:
-    1. Enabling/disabling PWM passthrough mode (if configured)
-    2. Arming/disarming the Blue controller
-    3. Arming/disarming the Angler controller
-
-    Args:
-        arm: Set to `True` to arm the system; set to `False` to disarm.
-        use_passthrough_mode: Use the Blue PWM passthrough mode. Take care when using
-            this mode. All safety checks onboard the system will be disabled.
-            Furthermore, make sure to leave PWM passthrough mode before shutdown, or
-            the system parameters will not be restored.
-
-    Returns:
-        A behavior that runs the full system arming sequence.
-    """
-    # For each task in the arming sequence we construct a sequence node that performs
-    # the following steps:
-    # 1. Sends a request to arm/disarm
-    # 2. Transforms the service response into a bool
-    # 3. Checks whether the response is true (the service completed successfully)
-
-    def check_arming_success(response: SetBool.Response) -> bool:
-        return response.success
-
-    # Set PWM Passthrough mode (this only gets added to the resulting behavior if the
-    # flag is set to true)
-    set_passthrough_mode_request = ServiceClientFromConstant(
-        name="Send PWM passthrough mode change request",
-        service_type=SetBool,
-        service_name="/blue/cmd/enable_passthrough",
-        service_request=SetBool.Request(data=arm),
-        key_response="passthrough_request_response",
-    )
-    transform_set_passthrough_mode_response = FunctionOfBlackboardVariables(
-        name="Transform passthrough mode response to bool",
-        input_keys=["passthrough_request_response"],
-        output_key="passthrough_request_result",
-        function=check_arming_success,
-    )
-    check_set_passthrough_mode_result = (
-        py_trees.behaviours.CheckBlackboardVariableValue(
-            name="Verify that the passthrough mode change request succeeded",
-            check=py_trees.common.ComparisonExpression(
-                variable="passthrough_request_result",
-                value=True,
-                operator=operator.eq,
-            ),
-        )
-    )
-    set_passthrough_mode = py_trees.composites.Sequence(
-        name="PWM passthrough mode " + ("enabled" if arm else "disabled"),
-        memory=True,
-        children=[
-            set_passthrough_mode_request,
-            transform_set_passthrough_mode_response,
-            check_set_passthrough_mode_result,
-        ],
-    )
-
-    # Arm/disarm the Blue controller
-    arm_blue_controller_request = ServiceClientFromConstant(
-        name="Send Blue controller arming/disarming request",
-        service_type=SetBool,
-        service_name="/blue/cmd/arm",
-        service_request=SetBool.Request(data=arm),
-        key_response="blue_arming_request_response",
-    )
-    transform_blue_controller_arming_response = FunctionOfBlackboardVariables(
-        name="Transform Blue controller arming/disarming response to bool",
-        input_keys=["blue_arming_request_response"],
-        output_key="blue_arming_request_result",
-        function=check_arming_success,
-    )
-    check_blue_arming_result = py_trees.behaviours.CheckBlackboardVariableValue(
-        name="Verify that the Blue controller arming/disarming request succeeded",
-        check=py_trees.common.ComparisonExpression(
-            variable="blue_arming_request_result",
-            value=True,
-            operator=operator.eq,
-        ),
-    )
-    arm_blue_controller = py_trees.composites.Sequence(
-        name=("Arm" if arm else "Disarm") + " Blue controller",
-        memory=True,
-        children=[
-            arm_blue_controller_request,
-            transform_blue_controller_arming_response,
-            check_blue_arming_result,
-        ],
-    )
-
-    # Arm/disarm the Angler controller
-    arm_angler_controller_request = ServiceClientFromConstant(
-        name="Send Angler arming/disarming request",
-        service_type=SetBool,
-        service_name="/angler/cmd/arm",
-        service_request=SetBool.Request(data=arm),
-        key_response="angler_arming_request_response",
-    )
-    transform_angler_controller_arming_response = FunctionOfBlackboardVariables(
-        name="Transform Angler controller arming/disarming response to bool",
-        input_keys=["angler_arming_request_response"],
-        output_key="angler_arming_request_result",
-        function=check_arming_success,
-    )
-    check_angler_arming_result = py_trees.behaviours.CheckBlackboardVariableValue(
-        name="Verify that the Angler controller arming/disarming request succeeded",
-        check=py_trees.common.ComparisonExpression(
-            variable="angler_arming_request_result",
-            value=True,
-            operator=operator.eq,
-        ),
-    )
-    arm_angler_controller = py_trees.composites.Sequence(
-        name=("Arm" if arm else "Disarm") + " Angler controller",
-        memory=True,
-        children=[
-            arm_angler_controller_request,
-            transform_angler_controller_arming_response,
-            check_angler_arming_result,
-        ],
-    )
-
-    # Now put everything together
-    behaviors = [arm_blue_controller, arm_angler_controller]
-
-    if use_passthrough_mode:
-        behaviors.insert(0, set_passthrough_mode)
-
-    return py_trees.composites.Sequence(
-        name="Arm system" if arm else "Disarm system",
-        memory=True,
-        children=behaviors,  # type: ignore
-    )
diff --git a/angler_behaviors/behavior_tree/blocks/trajectory.py b/angler_behaviors/behavior_tree/blocks/trajectory.py
deleted file mode 100644
index 41365e2..0000000
--- a/angler_behaviors/behavior_tree/blocks/trajectory.py
+++ /dev/null
@@ -1,19 +0,0 @@
-# Copyright 2023, Evan Palmer
-#
-# Permission is hereby granted, free of charge, to any person obtaining a copy
-# of this software and associated documentation files (the "Software"), to deal
-# in the Software without restriction, including without limitation the rights
-# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-# copies of the Software, and to permit persons to whom the Software is
-# furnished to do so, subject to the following conditions:
-#
-# The above copyright notice and this permission notice shall be included in
-# all copies or substantial portions of the Software.
-#
-# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
-# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-# THE SOFTWARE.
diff --git a/angler_behaviors/behavior_tree/components/__init__.py b/angler_behaviors/behavior_tree/primitives/__init__.py
similarity index 100%
rename from angler_behaviors/behavior_tree/components/__init__.py
rename to angler_behaviors/behavior_tree/primitives/__init__.py
diff --git a/angler_behaviors/behavior_tree/primitives/arming.py b/angler_behaviors/behavior_tree/primitives/arming.py
new file mode 100644
index 0000000..4d873d6
--- /dev/null
+++ b/angler_behaviors/behavior_tree/primitives/arming.py
@@ -0,0 +1,180 @@
+# Copyright 2023, Evan Palmer
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+import operator
+
+import py_trees
+import py_trees_ros
+from behavior_tree.primitives.blackboard import FunctionOfBlackboardVariables
+from behavior_tree.primitives.service_clients import FromConstant
+from std_msgs.msg import Bool
+from std_srvs.srv import SetBool
+
+
+def make_save_robot_state_behavior(arm_system_key: str) -> py_trees.behaviour.Behaviour:
+    """Save a command to arm/disarm the system to the blackboard.
+
+    Returns:
+        A ToBlackboard behavior which saves commands to arm/disarm the system to the
+        blackboard.
+    """
+    return py_trees_ros.subscribers.ToBlackboard(
+        name="ROS2BB: Arm system",
+        topic_name="/angler/cmd/arm_autonomy",
+        topic_type=Bool,
+        qos_profile=py_trees_ros.utilities.qos_profile_latched(),
+        blackboard_variables={arm_system_key: None},
+        clearing_policy=py_trees.common.ClearingPolicy.NEVER,
+    )
+
+
+def make_block_on_disarm_behavior(
+    arm_system_key: str,
+    tasks: py_trees.behaviour.Behaviour,
+    on_disarm_behavior: py_trees.behaviour.Behaviour | None = None,
+) -> py_trees.behaviour.Behaviour:
+    """Make a behavior that blocks when the system is disarmed.
+
+    Args:
+        arm_system_key: The key at which the arm system flag is stored.
+        tasks: A behavior with the tasks to run.
+        on_disarm_behavior: An optional behavior to run when a disarm is triggered. This
+            will be executed as a Sequence behavior following the disarm behavior.
+
+    Returns:
+        A Selector behavior with the disarm EternalGuard as the highest priority
+        behavior and the provided tasks as second priority.
+    """
+
+    def check_disarm_on_blackboard(
+        blackboard: py_trees.blackboard.Blackboard,
+    ) -> bool:
+        # We want to stop when a user issues a disarm command
+        return not blackboard.get(arm_system_key)
+
+    # Default to disarming passthrough mode just in case
+    disarming = make_system_arming_behavior(False, use_passthrough_mode=True)
+
+    behaviors = [disarming]
+
+    if on_disarm_behavior is not None:
+        behaviors.append(on_disarm_behavior)  # type: ignore
+
+    on_disarm = py_trees.composites.Sequence(
+        name="Execute disarm sequence",
+        memory=True,
+        children=[behaviors],  # type: ignore
+    )
+
+    disarm = py_trees.decorators.EternalGuard(
+        name="Disarm?",
+        condition=check_disarm_on_blackboard,
+        blackboard_keys={"disarm"},
+        child=on_disarm,
+    )
+
+    return py_trees.composites.Selector(
+        name="Block tasks on disarm", memory=False, children=[disarm, tasks]
+    )
+
+
+def make_subsystem_arming_behavior(
+    arm: bool, subsystem_name: str, arming_topic: str
+) -> py_trees.behaviour.Behaviour:
+    """Make a behavior that arms/disarms a subsystem (e.g., a whole-body controller).
+
+    Args:
+        arm: True if arming; False if disarming.
+        subsystem_name: The name of the sub-system to arm.
+        arming_topic: The topic over which to send the arming request.
+
+    Returns:
+        A behavior that arms/disarms a subsystem.
+    """
+
+    def check_arming_success(response: SetBool.Response) -> bool:
+        return response.success
+
+    arm_request = FromConstant(
+        name=f"{subsystem_name}: send arming request",
+        service_type=SetBool,
+        service_name=arming_topic,
+        service_request=SetBool.Request(data=arm),
+        key_response="arm_request_response",
+    )
+    transform_response = FunctionOfBlackboardVariables(
+        name=f"{subsystem_name}: transform arming response to bool",
+        input_keys=["arm_request_response"],
+        output_key="arm_request_result",
+        function=check_arming_success,
+    )
+    check_response = py_trees.behaviours.CheckBlackboardVariableValue(
+        name=f"{subsystem_name}: verify response",
+        check=py_trees.common.ComparisonExpression(
+            variable="arm_request_result",
+            value=True,
+            operator=operator.eq,
+        ),
+    )
+
+    return py_trees.composites.Sequence(
+        name=f"{subsystem_name}: Arming" if arm else ": Disarming",
+        memory=True,
+        children=[arm_request, transform_response, check_response],
+    )
+
+
+def make_system_arming_behavior(arm: bool, use_passthrough_mode: bool):
+    """Create a behavior that arms/disarms the system.
+
+    The full system arming sequence includes:
+    1. Enabling/disabling PWM passthrough mode (if configured)
+    2. Arming/disarming the Blue controller
+    3. Arming/disarming the Angler controller
+
+    Args:
+        arm: Set to `True` to arm the system; set to `False` to disarm.
+        use_passthrough_mode: Use the Blue PWM passthrough mode. Take care when using
+            this mode. All safety checks onboard the system will be disabled.
+            Furthermore, make sure to leave PWM passthrough mode before shutdown, or
+            the system parameters will not be restored.
+
+    Returns:
+        A behavior that runs the full system arming sequence.
+    """
+    # Now put everything together
+    behaviors = [
+        make_subsystem_arming_behavior(arm, "Blue Controller", "/blue/cmd/arm"),
+        make_subsystem_arming_behavior(arm, "Angler Controller", "/angler/cmd/arm"),
+    ]
+
+    if use_passthrough_mode:
+        behaviors.insert(
+            0,
+            make_subsystem_arming_behavior(
+                arm, "Passthrough Mode", "/blue/cmd/enable_passthrough"
+            ),
+        )
+
+    return py_trees.composites.Sequence(
+        name="Enable system autonomy" if arm else "Disable system autonomy",
+        memory=True,
+        children=behaviors,  # type: ignore
+    )
diff --git a/angler_behaviors/behavior_tree/components/blackboard.py b/angler_behaviors/behavior_tree/primitives/blackboard.py
similarity index 100%
rename from angler_behaviors/behavior_tree/components/blackboard.py
rename to angler_behaviors/behavior_tree/primitives/blackboard.py
diff --git a/angler_behaviors/behavior_tree/primitives/control.py b/angler_behaviors/behavior_tree/primitives/control.py
new file mode 100644
index 0000000..a32299b
--- /dev/null
+++ b/angler_behaviors/behavior_tree/primitives/control.py
@@ -0,0 +1,119 @@
+# Copyright 2023, Evan Palmer
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+import py_trees
+from behavior_tree.primitives.blackboard import FunctionOfBlackboardVariables
+from control_msgs.action import FollowJointTrajectory
+from geometry_msgs.msg import Transform
+from py_trees_ros.action_clients import FromBlackboard
+from trajectory_msgs.msg import MultiDOFJointTrajectory, MultiDOFJointTrajectoryPoint
+
+
+def make_move_to_end_effector_pose_behavior(
+    desired_pose_key: str, controller_id_key: str
+) -> py_trees.behaviour.Behaviour:
+    """Make a behavior that moves the system to a given end-effector pose.
+
+    Args:
+        desired_pose_key: The blackboard key which holds the desired pose (saved as a
+            Transform).
+        controller_id_key: The key which holds the name of the controller to use.
+
+    Returns:
+        A behavior that moves the system to a desired end-effector pose.
+    """
+
+    def make_move_to_pose_goal(desired_pose: Transform) -> FollowJointTrajectory.Goal:
+        point = MultiDOFJointTrajectoryPoint()
+        point.transforms.append(desired_pose)  # type: ignore
+        goal = FollowJointTrajectory.Goal()
+        goal.multi_dof_trajectory.points.append(point)  # type: ignore
+        return goal
+
+    get_desired_pose = FunctionOfBlackboardVariables(
+        "Get the desired end-effector pose",
+        [desired_pose_key],
+        "desired_end_effector_pose",
+        make_move_to_pose_goal,
+    )
+
+    # Get the controller ID to reconstruct the planning topic
+    blackboard = py_trees.blackboard.Client()
+    blackboard.register_key(key=controller_id_key, access=py_trees.common.Access.READ)
+    controller_id = blackboard.get(controller_id_key)
+
+    move_to_pose = FromBlackboard(
+        "Move to the desired end-effector pose",
+        FollowJointTrajectory,
+        f"/angler/{controller_id}/execute_trajectory",
+        "desired_end_effector_pose",
+    )
+
+    return py_trees.composites.Sequence(
+        "Move to end-effector pose",
+        memory=True,
+        children=[get_desired_pose, move_to_pose],
+    )
+
+
+def make_execute_multidof_trajectory_behavior(
+    trajectory_key: str, controller_id_key: str
+) -> py_trees.behaviour.Behaviour:
+    """Make a behavior that executes a multi-DOF joint trajectory.
+
+    Args:
+        trajectory_key: The blackboard key at which the trajectory is being stored.
+        controller_id_key: The ID of the controller to run.
+
+    Returns:
+        A behavior that executes a multi-DOF joint trajectory.
+    """
+
+    def make_follow_trajectory_goal(
+        trajectory: MultiDOFJointTrajectory,
+    ) -> FollowJointTrajectory.Goal:
+        goal = FollowJointTrajectory.Goal()
+        goal.multi_dof_trajectory = trajectory
+        return goal
+
+    get_desired_trajectory = FunctionOfBlackboardVariables(
+        "Get the desired trajectory to track",
+        [trajectory_key],
+        "desired_trajectory",
+        make_follow_trajectory_goal,
+    )
+
+    # Get the controller ID to reconstruct the planning topic
+    blackboard = py_trees.blackboard.Client()
+    blackboard.register_key(key=controller_id_key, access=py_trees.common.Access.READ)
+    controller_id = blackboard.get(controller_id_key)
+
+    follow_trajectory = FromBlackboard(
+        "Follow the joint trajectory",
+        FollowJointTrajectory,
+        f"/angler/{controller_id}/execute_trajectory",
+        "desired_trajectory",
+    )
+
+    return py_trees.composites.Sequence(
+        "Load and execute a multi-DOF joint trajectory",
+        memory=True,
+        children=[get_desired_trajectory, follow_trajectory],
+    )
diff --git a/angler_behaviors/behavior_tree/primitives/planning.py b/angler_behaviors/behavior_tree/primitives/planning.py
new file mode 100644
index 0000000..216c4e5
--- /dev/null
+++ b/angler_behaviors/behavior_tree/primitives/planning.py
@@ -0,0 +1,90 @@
+# Copyright 2023, Evan Palmer
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+import py_trees
+import py_trees_ros
+from behavior_tree.primitives.blackboard import FunctionOfBlackboardVariables
+from behavior_tree.primitives.service_clients import FromBlackboard
+from moveit_msgs.msg import RobotState
+from moveit_msgs.srv import GetMotionPlan
+
+
+def make_save_armed_behavior(robot_state_key: str) -> py_trees.behaviour.Behaviour:
+    """Save the current robot state.
+
+    Returns:
+        A ToBlackboard behavior which saves the robot state.
+    """
+    return py_trees_ros.subscribers.ToBlackboard(
+        name="ROS2BB: Robot state",
+        topic_name="/angler/state",
+        topic_type=RobotState,
+        qos_profile=py_trees_ros.utilities.qos_profile_latched(),
+        blackboard_variables={robot_state_key: None},
+        clearing_policy=py_trees.common.ClearingPolicy.NEVER,
+    )
+
+
+def make_high_level_planning_behavior(
+    robot_state_key: str, planner_id_key: str, planning_result_key: str
+) -> py_trees.behaviour.Behaviour:
+    """Create a high-level planning behavior.
+
+    Args:
+        robot_state_key: The key at which the robot state is being stored.
+
+    Returns:
+        Behavior that creates a high-level mission plan and saves the result to the
+        blackboard.
+    """
+
+    def make_planning_request(state: RobotState, planner: str) -> GetMotionPlan.Request:
+        request = GetMotionPlan.Request()
+        request.motion_plan_request.start_state = state
+        request.motion_plan_request.planner_id = planner
+        return request
+
+    plan_request_behavior = FunctionOfBlackboardVariables(
+        name="Make high-level planner request",
+        input_keys=[robot_state_key, planner_id_key],
+        output_key="high_level_planner_request",
+        function=make_planning_request,
+    )
+
+    # Get the planner ID to reconstruct the planning topic
+    # This will go away when a planning interface is created that loads planners
+    # according to the planning goal
+    blackboard = py_trees.blackboard.Client()
+    blackboard.register_key(key=planner_id_key, access=py_trees.common.Access.READ)
+    planner_id = blackboard.get(planner_id_key)
+
+    plan_behavior = FromBlackboard(
+        "Plan a high-level mission",
+        GetMotionPlan.Request,
+        f"/angler/{planner_id}/plan",
+        "high_level_planner_request",
+        planning_result_key,
+    )
+
+    return py_trees.composites.Sequence(
+        "Get high-level plan",
+        memory=True,
+        children=[plan_request_behavior, plan_behavior],
+    )
diff --git a/angler_behaviors/behavior_tree/blocks/control.py b/angler_behaviors/behavior_tree/primitives/recording.py
similarity index 86%
rename from angler_behaviors/behavior_tree/blocks/control.py
rename to angler_behaviors/behavior_tree/primitives/recording.py
index 44d6a7f..64a6f65 100644
--- a/angler_behaviors/behavior_tree/blocks/control.py
+++ b/angler_behaviors/behavior_tree/primitives/recording.py
@@ -18,10 +18,13 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 # THE SOFTWARE.
 
+import py_trees
+import py_trees_ros
 
-def make_move_to_end_effector_pose_behavior():
+
+def make_start_bag_file_behavior(topics: list[str], filename: str | None = None):
     ...
 
 
-def make_execute_trajectory_behavior():
+def make_stop_bag_file_behavior(filename_key: str):
     ...
diff --git a/angler_behaviors/behavior_tree/components/service_clients.py b/angler_behaviors/behavior_tree/primitives/service_clients.py
similarity index 100%
rename from angler_behaviors/behavior_tree/components/service_clients.py
rename to angler_behaviors/behavior_tree/primitives/service_clients.py
diff --git a/angler_behaviors/behavior_tree/tree.py b/angler_behaviors/behavior_tree/tree.py
index 6106f7d..7fa530f 100644
--- a/angler_behaviors/behavior_tree/tree.py
+++ b/angler_behaviors/behavior_tree/tree.py
@@ -21,14 +21,71 @@
 import py_trees
 import py_trees_ros
 import rclpy
+from behavior_tree.behaviors.mission import (
+    make_execute_mission_behavior,
+    make_save_start_mission_behavior,
+)
+from behavior_tree.behaviors.setup import make_setup_behavior
+from behavior_tree.primitives.arming import (
+    make_block_on_disarm_behavior,
+    make_save_robot_state_behavior,
+)
+from behavior_tree.primitives.planning import make_save_armed_behavior
 
 
 def make_angler_tree() -> py_trees.behaviour.Behaviour:
+    """Make a behavior tree that runs the full Angler system.
+
+    Returns:
+        A behavior tree that provides full-system autonomy.
+    """
     root = py_trees.composites.Parallel(
         name="Angler Autonomy",
         policy=py_trees.common.ParallelPolicy.SuccessOnAll(synchronise=False),
     )
 
+    # Add the data gathering behaviors
+    start_mission_key = "start_mission"
+    armed_key = "armed"
+    robot_state_key = "robot_state"
+
+    data_gathering = py_trees.composites.Sequence(
+        name="Data gathering",
+        memory=True,
+        children=[
+            make_save_start_mission_behavior(start_mission_key),
+            make_save_armed_behavior(armed_key),
+            make_save_robot_state_behavior(robot_state_key),
+        ],
+    )
+
+    root.add_child(data_gathering)
+
+    setup_finished_flag_key = "setup_finished"
+    start_mission_key = "start_mission"
+
+    setup_and_execute_mission = py_trees.composites.Sequence(
+        name="Setup and execute mission",
+        memory=True,
+        children=[
+            make_setup_behavior(setup_finished_flag_key),
+            make_execute_mission_behavior(start_mission_key, robot_state_key),
+        ],
+    )
+
+    tasks = make_block_on_disarm_behavior(
+        armed_key,
+        setup_and_execute_mission,
+        on_disarm_behavior=py_trees.behaviours.SetBlackboardVariable(
+            name="Setup needs to be redone",
+            variable_name=setup_finished_flag_key,
+            variable_value=False,
+            overwrite=True,
+        ),
+    )
+
+    root.add_child(tasks)
+
     return root
 
 
diff --git a/angler_planning/planners/base_planner.py b/angler_planning/planners/base_planner.py
index 26dfd5e..0fa207a 100644
--- a/angler_planning/planners/base_planner.py
+++ b/angler_planning/planners/base_planner.py
@@ -38,7 +38,7 @@ def __init__(self, node_name: str) -> None:
         ABC.__init__(self)
 
         self.planning_srv = self.create_service(
-            GetMotionPlan, f"~/angler/{node_name}/plan", self.plan
+            GetMotionPlan, f"/angler/{node_name}/plan", self.plan
         )
 
     @abstractmethod