Merge emmn nav

.gitignore

@@ -208,4 +208,7 @@ cython_debug/
 *.app
 
 .DS_Store
-*.mcap
+*.mcap
+
+# Local-only navigation sim test harness (not tracked)
+src/subsystems/navigation/nav_sim_harness/

src/athena_sensors/launch/sensors.launch.py

@@ -61,7 +61,7 @@ def generate_launch_description():
             'publish_tf':      publish_odom, # this is really the odom tf, idk why its just called tf
             'publish_map_tf':  publish_map,
             'publish_urdf':   'false', # we take care of this elsewhere
-            'enable_gnss':     'true',
+            'enable_gnss':     enable_gnss,
         }.items(),
         condition=UnlessCondition(sim),
     )

src/bringup/package.xml

@@ -19,6 +19,9 @@
   <exec_depend>science_bringup</exec_depend>
   <exec_depend>description</exec_depend>
   <exec_depend>athena_planner</exec_depend>
+  <exec_depend>led_indicator</exec_depend>
+  <exec_depend>aruco_bt</exec_depend>
+  <exec_depend>yolo_ros_bt</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>xacro</exec_depend>
 

src/description/models/aruco_post/model.config

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>aruco_post</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+
+  <author>
+    <name>UMD Loop Navigation</name>
+  </author>
+
+  <description>
+    URC-style ArUco post: a 3-sided (triangular-prism) marker carrying three
+    identical DICT_4X4_50 tag faces, one per side, mounted on a support post.
+    Faces are 20x20 cm (15 cm marker + 2.5 cm white quiet zone). The tag texture
+    is pre-generated once with nav_sim_harness/scripts/gen_aruco_texture.py and
+    baked into materials/textures/ — nothing generates it at spawn time.
+    Default tag id baked in = 4. Used by the nav_sim_harness Gazebo test harness.
+  </description>
+
+</model>

src/description/models/aruco_post/model.sdf

@@ -0,0 +1,127 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <!--
+    3-sided ArUco post (URC mission post).
+
+    Geometry:
+      * Support post: cylinder, r=0.03 m, from ground (z=0) up to z=0.9 m.
+      * Three identical tag faces forming a triangular prism, centred at z=1.0 m.
+        Each face is a thin 0.20 x 0.20 m panel (X=thickness, Y=width, Z=height)
+        textured with the same DICT_4X4_50 marker on its outward face.
+        The three panels sit at yaw 0 / 120 / 240 deg, offset radially by the
+        equilateral-triangle apothem (0.20 / (2*sqrt(3)) = 0.057735 m) so their
+        edges meet to form a closed prism.
+
+    Texture: materials/textures/aruco_4x4_50_id4.png — pre-baked, NOT generated
+    at spawn time. Regenerate for a different id with
+    nav_sim_harness/scripts/gen_aruco_texture.py and re-commit the PNG.
+
+    Model origin is the base of the post (z=0), so when spawned you set the
+    include <pose> z to the ground height at the drop point.
+  -->
+  <model name="aruco_post">
+    <static>true</static>
+    <link name="link">
+
+      <!-- Support post -->
+      <visual name="post_visual">
+        <pose>0 0 0.45 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.03</radius>
+            <length>0.9</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <ambient>0.5 0.5 0.5 1</ambient>
+          <diffuse>0.6 0.6 0.6 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="post_collision">
+        <pose>0 0 0.45 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.03</radius>
+            <length>0.9</length>
+          </cylinder>
+        </geometry>
+      </collision>
+
+      <!-- Tag face 0 (yaw 0) -->
+      <visual name="tag_face_0">
+        <pose>0.057735 0 1.0 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.005 0.20 0.20</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1 1 1 1</ambient>
+          <diffuse>1 1 1 1</diffuse>
+          <pbr>
+            <metal>
+              <albedo_map>materials/textures/aruco_4x4_50_id4.png</albedo_map>
+              <metalness>0.0</metalness>
+              <roughness>1.0</roughness>
+            </metal>
+          </pbr>
+        </material>
+      </visual>
+
+      <!-- Tag face 1 (yaw 120 deg) -->
+      <visual name="tag_face_1">
+        <pose>-0.0288675 0.05 1.0 0 0 2.0943951</pose>
+        <geometry>
+          <box>
+            <size>0.005 0.20 0.20</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1 1 1 1</ambient>
+          <diffuse>1 1 1 1</diffuse>
+          <pbr>
+            <metal>
+              <albedo_map>materials/textures/aruco_4x4_50_id4.png</albedo_map>
+              <metalness>0.0</metalness>
+              <roughness>1.0</roughness>
+            </metal>
+          </pbr>
+        </material>
+      </visual>
+
+      <!-- Tag face 2 (yaw 240 deg) -->
+      <visual name="tag_face_2">
+        <pose>-0.0288675 -0.05 1.0 0 0 4.1887902</pose>
+        <geometry>
+          <box>
+            <size>0.005 0.20 0.20</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1 1 1 1</ambient>
+          <diffuse>1 1 1 1</diffuse>
+          <pbr>
+            <metal>
+              <albedo_map>materials/textures/aruco_4x4_50_id4.png</albedo_map>
+              <metalness>0.0</metalness>
+              <roughness>1.0</roughness>
+            </metal>
+          </pbr>
+        </material>
+      </visual>
+
+      <!-- Prism collision (bounding cylinder around the three faces) -->
+      <collision name="prism_collision">
+        <pose>0 0 1.0 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.063</radius>
+            <length>0.20</length>
+          </cylinder>
+        </geometry>
+      </collision>
+
+    </link>
+  </model>
+</sdf>

src/description/urdf/athena_drive.urdf.xacro

@@ -15,6 +15,7 @@
   <xacro:include filename="$(find description)/urdf/athena_drive/athena_drive_macro.ros2_control.xacro"/>
   <xacro:include filename="imu.xacro"/>
   <xacro:include filename="gps.xacro"/>
+  <xacro:include filename="magnetometer.xacro"/>
   <xacro:include filename="ground_truth_odom.xacro"/>
   <xacro:include filename="zed_camera.xacro"/>
 
@@ -55,6 +56,10 @@
     <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
   </xacro:gps_sensor>
 
+  <xacro:magnetometer_sensor parent="base_footprint">
+    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+  </xacro:magnetometer_sensor>
+
   <xacro:ground_truth_odometry robot_base_frame="base_footprint"/>
 
   <xacro:zed_camera_sensor parent="base_footprint" camera_name="zed" sim_gazebo="$(arg sim_gazebo)">

src/description/urdf/magnetometer.xacro

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="magnetometer_sensor" params="parent *origin">
+        <joint name="mag_joint" type="fixed">
+            <xacro:insert_block name="origin" />
+            <parent link="${parent}" />
+            <child link="mag_link" />
+        </joint>
+
+        <link name="mag_link">
+            <visual>
+                <geometry>
+                    <box size="0.05 0.05 0.05" />
+                </geometry>
+                <material name="red">
+                    <color rgba="1 0 0 1" />
+                </material>
+            </visual>
+        </link>
+
+        <gazebo reference="mag_link">
+            <sensor name="magnetometer" type="magnetometer">
+                <always_on>true</always_on>
+                <update_rate>50</update_rate>
+                <topic>/mag</topic>
+                <ign_frame_id>mag_link</ign_frame_id>
+                <plugin filename="ignition-gazebo-magnetometer-system" name="ignition::gazebo::systems::Magnetometer"/>
+            </sensor>
+        </gazebo>
+    </xacro:macro>
+</robot>

src/description/urdf/zed_camera.xacro

@@ -33,8 +33,8 @@
           <camera name="${camera_name}_depth">
             <horizontal_fov>1.8151</horizontal_fov>
             <image>
-              <width>672</width>
-              <height>376</height>
+              <width>320</width>
+              <height>180</height>
               <format>R_FLOAT32</format>
             </image>
             <clip>
@@ -52,15 +52,15 @@
 
         <sensor name="${camera_name}_rgb_sensor" type="camera">
           <always_on>1</always_on>
-          <update_rate>15</update_rate>
+          <update_rate>30</update_rate>
           <visualize>true</visualize>
           <topic>${camera_name}/zed_node/left/image_rect_color</topic>
           <ign_frame_id>${camera_name}_left_camera_frame_optical</ign_frame_id>
           <camera name="${camera_name}_rgb">
             <horizontal_fov>1.8151</horizontal_fov>
             <image>
-              <width>672</width>
-              <height>376</height>
+              <width>1280</width>
+              <height>720</height>
               <format>RGB_INT8</format>
             </image>
             <clip>

src/msgs/CMakeLists.txt

@@ -19,17 +19,24 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Currentdraw.msg"
   "msg/JointStatus.msg"
   "msg/SystemInfo.msg"
+  "msg/PlannerEvent.msg"
+  "msg/ActiveTarget.msg"
+  "msg/NavStatus.msg"
   "msg/Heading.msg"
   "msg/WaypointEntry.msg"
   "msg/WaypointManagerState.msg"
+  "msg/LocalPlannerStuck.msg"
   "srv/SetController.srv"
   "srv/AddWaypoint.srv"
   "srv/ClearWaypoints.srv"
   "msg/LedStatus.msg"
   "srv/StatusReq.srv"
   "srv/MaintenanceReq.srv"
+  "srv/LatLonToENU.srv"
+  "srv/SetTarget.srv"
   "action/NavigateToGPS.action"
   "action/NavigateToWaypoint.action"
+  "action/NavigateToTarget.action"
   DEPENDENCIES std_msgs geometry_msgs sensor_msgs builtin_interfaces action_msgs
 )
 

src/msgs/action/NavigateToTarget.action

@@ -0,0 +1,24 @@
+uint8 GPS=0
+uint8 METER=1
+uint8 GNSS_ONLY=0
+uint8 ARUCO_POST_1=1
+uint8 ARUCO_POST_2=2
+uint8 OBJECT=3
+uint8 LOCAL=4
+
+string target_id
+float64 lat
+float64 lon
+float64 x_m
+float64 y_m
+uint8 goal_type
+uint8 target_type
+float64 tolerance_m
+bool is_return
+---
+bool success
+string message
+---
+float64 distance_to_goal_m
+float64 cross_track_error_m
+string state

src/msgs/msg/ActiveTarget.msg

@@ -0,0 +1,12 @@
+uint8 GNSS_ONLY=0
+uint8 ARUCO_POST=1
+uint8 OBJECT=2
+uint8 LOCAL=3
+uint8 GPS=0
+uint8 METER=1
+string target_id
+uint8 target_type
+float64 tolerance_m
+geometry_msgs/PoseStamped goal_enu
+uint8 goal_source
+string status

src/msgs/msg/LocalPlannerStuck.msg

@@ -0,0 +1,25 @@
+# Published by the local planner (vector_field_planner) when its tentacle FSM
+# enters the REPLAN state — i.e. forward arcs are blocked and reverse arcs
+# cannot open a new vantage on their own.
+#
+# Future consumers (mission_executive, waypoint manager, web GUI) use this to
+# insert a detour waypoint, mark the original waypoint as failed after N
+# attempts, or surface "rover is stuck" to the operator.
+#
+# Lifecycle:
+#   - On REPLAN entry:  publish once with stuck=true.
+#   - While stuck:      heartbeat at ~1 Hz with stuck=true (so a late
+#                       subscriber catches up after a restart).
+#   - On REPLAN exit:   publish once with stuck=false.
+#
+# The "suggested_detour" is the local planner's best guess at a pose that
+# would escape the current obstacle pocket — derived from the highest-
+# clearance tentacle (forward OR reverse, whichever was clearer). Consumers
+# may use it directly or override with their own policy.
+
+std_msgs/Header header              # stamp at publish time; frame_id = map frame
+bool stuck                          # true while in REPLAN, false on exit
+geometry_msgs/PoseStamped stuck_pose         # rover pose when REPLAN entered
+geometry_msgs/PoseStamped suggested_detour   # local planner's escape suggestion
+float64 best_forward_clearance_m    # clearance along best forward tentacle (m); 0 if none clear
+float64 best_reverse_clearance_m    # clearance along best reverse tentacle (m); 0 if none clear

src/msgs/msg/NavStatus.msg

@@ -0,0 +1,10 @@
+string state
+string active_target_id
+uint8 active_target_type
+uint8 goal_source
+float64 distance_to_goal_m
+float64 cross_track_error_m
+float64 heading_error_rad
+float64 robot_speed_mps
+bool is_return
+uint8 last_planner_event

src/msgs/msg/PlannerEvent.msg

@@ -0,0 +1,5 @@
+uint8 NEW_GOAL=0
+uint8 PLANNING=1
+uint8 PLAN_SUCCEEDED=2
+uint8 PLAN_FAILED=3
+uint8 event

src/msgs/srv/LatLonToENU.srv

@@ -0,0 +1,6 @@
+float64 lat
+float64 lon
+---
+float64 x
+float64 y
+float64 z

src/msgs/srv/SetTarget.srv

@@ -0,0 +1,18 @@
+uint8 GPS=0
+uint8 METER=1
+uint8 GNSS_ONLY=0
+uint8 ARUCO_POST_1=1
+uint8 ARUCO_POST_2=2
+uint8 OBJECT=3
+uint8 LOCAL=4
+string target_id
+float64 lat
+float64 lon
+float64 x_m
+float64 y_m
+uint8 goal_type
+uint8 target_type
+float64 tolerance_m
+---
+bool success
+string message