From b626f1120df93d3d9a335eacf90014f584f3cc85 Mon Sep 17 00:00:00 2001
From: Bilal Gill <bilal.gill@technipfmc.com>
Date: Thu, 2 Apr 2026 12:36:36 -0400
Subject: [PATCH 1/2] Add new navigate with replanning Objective and only have
 front lidar on the ur5e

---
 src/hangar_sim/description/ur5e_ridgeback.xml |  7 ++-
 ...igate_to_clicked_point_with_replanning.xml | 46 +++++++++++++++++++
 2 files changed, 49 insertions(+), 4 deletions(-)
 create mode 100644 src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml
diff --git a/src/hangar_sim/description/ur5e_ridgeback.xml b/src/hangar_sim/description/ur5e_ridgeback.xml
index 4216f099..e1e7a90e 100644
--- a/src/hangar_sim/description/ur5e_ridgeback.xml
+++ b/src/hangar_sim/description/ur5e_ridgeback.xml
@@ -248,7 +248,7 @@
                91 beams x 3 deg = 270 deg FOV, sweeping CCW from beam-0 at +45 deg in robot frame.
                Rear: phi=+45 deg -> R_base = Rot_Z(45)*Rot_Y(90) -> quat=(0.6533,-0.2706,0.6533,0.2706).
                angle_min=0 in rangefinder user params so the plugin sets _ROS frame X = beam-0 direction. -->
-          <body name="lidar_rear_mount" pos="-0.45 0 0.15">
+<!--           <body name="lidar_rear_mount" pos="-0.45 0 0.15">
             <inertial
               mass="0.25"
               pos="0 0 0"
@@ -262,7 +262,6 @@
                 quat="0.6533 -0.2706 0.6533 0.2706"
               />
             </replicate>
-            <!-- TIM571 body: dark cylinder, visual-only -->
             <geom
               name="lidar_rear_body"
               type="cylinder"
@@ -273,7 +272,7 @@
               conaffinity="0"
               group="2"
             />
-          </body>
+          </body> -->
           <geom
             name="chassis_link"
             mesh="chassis_link"
@@ -658,6 +657,6 @@
          beam-0 at -135 deg and beam-90 at +135 deg in the robot frame. -->
     <rangefinder site="lidar_front" user="0.05 0.0 4.7124 0.05236 0.05 25.0" />
     <!-- Rear TIM571: same convention — angle_min=0, frame X points at beam-0 (+45 deg robot frame). -->
-    <rangefinder site="lidar_rear" user="0.05 0.0 4.7124 0.05236 0.05 25.0" />
+    <!-- <rangefinder site="lidar_rear" user="0.05 0.0 4.7124 0.05236 0.05 25.0" /> -->
   </sensor>
 </mujoco>
diff --git a/src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml b/src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml
new file mode 100644
index 00000000..e9fbe18d
--- /dev/null
+++ b/src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml
@@ -0,0 +1,46 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root
+  BTCPP_format="4"
+  main_tree_to_execute="Navigate to Clicked Point with Replanning"
+>
+  <!--//////////-->
+  <BehaviorTree
+    ID="Navigate to Clicked Point with Replanning"
+    _description="Navigate to a point clicked by the user in the UI. This Objective will repeatedly replan the path as new obstacles appear on the local costmap."
+    _favorite="true"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="CreateStampedPose"
+        reference_frame="base_link"
+        stamped_pose="{start}"
+      />
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+        deactivate_controllers="velocity_force_controller;joint_trajectory_controller;platform_velocity_controller"
+      />
+      <Action
+        ID="GetPoseFromUser"
+        is_normal="true"
+        pose_prompt="Click point on UI"
+        user_pose="{goal}"
+        view_name="Visualization"
+      />
+      <Action
+        ID="NavigateToPoseAction"
+        action_name="/navigate_to_pose"
+        behavior_tree_path="/opt/ros/humble/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"
+        pose_stamped="{goal}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Navigate to Clicked Point with Replanning">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Navigation" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>

From f9221460d94c49b834245da3c8e025273b0f28fb Mon Sep 17 00:00:00 2001
From: Bilal Gill <bilal.gill@technipfmc.com>
Date: Thu, 2 Apr 2026 13:06:15 -0400
Subject: [PATCH 2/2] fix pre-commit

---
 src/hangar_sim/description/ur5e_ridgeback.xml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/hangar_sim/description/ur5e_ridgeback.xml b/src/hangar_sim/description/ur5e_ridgeback.xml
index e1e7a90e..58f62cf5 100644
--- a/src/hangar_sim/description/ur5e_ridgeback.xml
+++ b/src/hangar_sim/description/ur5e_ridgeback.xml
@@ -248,7 +248,7 @@
                91 beams x 3 deg = 270 deg FOV, sweeping CCW from beam-0 at +45 deg in robot frame.
                Rear: phi=+45 deg -> R_base = Rot_Z(45)*Rot_Y(90) -> quat=(0.6533,-0.2706,0.6533,0.2706).
                angle_min=0 in rangefinder user params so the plugin sets _ROS frame X = beam-0 direction. -->
-<!--           <body name="lidar_rear_mount" pos="-0.45 0 0.15">
+          <!--           <body name="lidar_rear_mount" pos="-0.45 0 0.15">
             <inertial
               mass="0.25"
               pos="0 0 0"
