From 02ecbb5bf31e28ecfa83f8956fe37fc2b4519d89 Mon Sep 17 00:00:00 2001
From: Breelyn Styler <breelyn.styler@picknik.ai>
Date: Thu, 10 Sep 2026 19:24:03 -0400
Subject: [PATCH 1/2] feat(hangar_sim): add Build Map objective

Epic #16487 promised operators could build maps through the MoveIt Pro
Desktop App. What shipped instead was a single checked-in map a developer
generated once through a CLI, and localization_launch.py pointed readers at
slam:=True with no Objective to drive it from.

Teleoperate to explore while slam_toolbox builds the grid, then save it on
the operator's confirmation. Teleop mode 1 (JOINT_JOG) is what drives the
base here, and the response timeout accounts for slam_toolbox shelling out
to map_saver_cli.

Refs #18066

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
---
 src/hangar_sim/objectives/build_map.xml | 39 +++++++++++++++++++++++++
 1 file changed, 39 insertions(+)
 create mode 100644 src/hangar_sim/objectives/build_map.xml

diff --git a/src/hangar_sim/objectives/build_map.xml b/src/hangar_sim/objectives/build_map.xml
new file mode 100644
index 000000000..a16818b3a
--- /dev/null
+++ b/src/hangar_sim/objectives/build_map.xml
@@ -0,0 +1,39 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Build Map">
+  <!--Requires slam_toolbox to already be running in mapping mode, which the persist launch starts with slam:=True. Its parameters live in the slam_toolbox block of config/../params/nav2_params.yaml.-->
+  <BehaviorTree
+    ID="Build Map"
+    _description="Teleoperate the robot to explore the hangar while slam_toolbox builds an occupancy grid, then save that map on the operator's confirmation."
+    _favorite="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <!--No SwitchController prologue is needed: the jog branches inside the subtree switch controllers themselves, and SwitchController defaults automatic_activation and automatic_deactivation to true, so it activates the downstream platform_velocity_controller and drops whatever held the wheel interfaces before.-->
+      <!--Teleop mode 1 is JOINT_JOG, which is how the base is driven here: the manipulator group carries linear_x_joint, linear_y_joint and rotational_yaw_joint, and joint_velocity_controller commands them through the platform controller. DoTeleoperateAction holds the confirmation prompt open for as long as teleoperation is active, so the operator explores and then ends this step by answering it.-->
+      <SubTree
+        ID="Request Teleoperation"
+        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+        joint_trajectory_controller_name="joint_trajectory_controller"
+        execution_pipeline="jtc"
+        enable_user_interaction="true"
+        initial_teleop_mode="1"
+        user_interaction_prompt="Drive the base to explore the hangar, turning in place so each surface passes the FRONT lidar - mapping uses the front scan only. Stop moving, then press Continue to save the map, or Abort to discard it."
+      />
+      <!--slam_toolbox resolves a relative name against its own working directory, so this writes a new map rather than overwriting maps/hangar_map.pgm in the source tree. Copy the result into the config package to start using it.-->
+      <Action
+        ID="CallSaveMapService"
+        map_name="hangar_map"
+        service_name="/slam_toolbox/save_map"
+        response_timeout="30.000000"
+        wait_for_server_available_timeout="3.000000"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Build Map">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Navigation" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>

From bc2404d8c754f9d8732262105cfe6923a802307d Mon Sep 17 00:00:00 2001
From: Breelyn Styler <breelyn.styler@picknik.ai>
Date: Thu, 10 Sep 2026 19:59:57 -0400
Subject: [PATCH 2/2] feat(hangar_sim): load the SLAM Behaviors package for
 Build Map

CallSaveMapService ships in moveit_pro_slam_behavior rather than in the
shared Behavior library, so a configuration has to opt in. Only
configurations that build maps then depend on slam_toolbox.

Refs #18066

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
---
 src/hangar_sim/config/config.yaml | 3 +++
 src/hangar_sim/package.xml        | 1 +
 2 files changed, 4 insertions(+)

diff --git a/src/hangar_sim/config/config.yaml b/src/hangar_sim/config/config.yaml
index ec7de13d0..30d71cddb 100644
--- a/src/hangar_sim/config/config.yaml
+++ b/src/hangar_sim/config/config.yaml
@@ -70,6 +70,9 @@ objectives:
       - "moveit_pro::behaviors::VisionBehaviorsLoader"
       - "moveit_pro::behaviors::ConverterBehaviorsLoader"
       - "moveit_pro::behaviors::MujocoBehaviorsLoader"
+      # SLAM Behaviors live in their own package so that only configurations
+      # which build maps depend on slam_toolbox.
+      - "moveit_pro::behaviors::SlamBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:
diff --git a/src/hangar_sim/package.xml b/src/hangar_sim/package.xml
index 938fb2590..7d63c5ccc 100644
--- a/src/hangar_sim/package.xml
+++ b/src/hangar_sim/package.xml
@@ -23,6 +23,7 @@
   <exec_depend>beluga_amcl</exec_depend>
   <exec_depend>dual_laser_merger</exec_depend>
   <exec_depend>laser_filters</exec_depend>
+  <exec_depend>moveit_pro_slam_behavior</exec_depend>
   <exec_depend>nav2_bringup</exec_depend>
   <exec_depend>picknik_accessories</exec_depend>
   <exec_depend>picknik_mujoco_ros</exec_depend>