Add Zed Servo Node

.gitignore

@@ -2,6 +2,7 @@ build/
 install/
 log/
 .vscode/
+.codex
 
 # Byte-compiled / optimized / DLL files
 __pycache__/

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "nav2_config"]
+	path = nav2_config
+	url = https://github.com/sutharsan-311/nav2_config.git

requirements.txt

@@ -2,4 +2,7 @@ pygame
 catkin_pkg
 empy==3.3.4
 lark
-numpy
+numpy<2.0
+opencv-contrib-python==4.10.0.84
+ahrs
+pyserial

src/bringup/README.md

@@ -0,0 +1,156 @@
+# bringup
+
+Mission-level launch files for Athena. Each file composes subsystem launches for a specific competition mission.
+
+## Modes
+
+| Mode | What runs |
+|---|---|
+| `jetson` | Hardware and compute nodes — run on the rover |
+| `base_station` | Operator teleop nodes — run on the laptop |
+| `standalone` | Everything on one machine (default, for development) |
+
+---
+
+## Delivery
+
+Drive + Arm + GPS + Heading
+
+```bash
+# Rover
+ros2 launch bringup delivery.launch.py mode:=jetson
+
+# Operator laptop
+ros2 launch bringup delivery.launch.py mode:=base_station
+
+# Single machine
+ros2 launch bringup delivery.launch.py
+
+# Mock hardware (no physical rover)
+ros2 launch bringup delivery.launch.py use_mock_hardware:=true use_sim:=true
+
+# 3 DOF wrist
+ros2 launch bringup delivery.launch.py use_3dof:=true
+```
+
+### Parameters
+
+| Parameter | Default | Choices | Description |
+|---|---|---|---|
+| `mode` | `standalone` | `standalone` `jetson` `base_station` | Deployment target |
+| `use_sim` | `false` | `true` `false` | Launch RViz2 |
+| `use_mock_hardware` | `false` | `true` `false` | Mock hardware for drive and arm |
+| `mock_sensor_commands` | `false` | `true` `false` | Mock sensor command interfaces (requires `use_mock_hardware`) |
+| `robot_controller` | `rear_ackermann_controller` | `rear_ackermann_controller` `front_ackermann_controller` `ackermann_steering_controller` | Drive controller to start |
+| `use_3dof` | `false` | `true` `false` | Enable 3 DOF wrist joints on arm |
+| `deactivate_talon` | `false` | `true` `false` | Deactivate talon joints in URDF (use with `use_mock_hardware` to prevent CAN traffic) |
+
+---
+
+## Autonomous Navigation
+
+Drive + Nav2 + ZED + GPS + Localizer + Heading
+
+```bash
+# Rover
+ros2 launch bringup autonav.launch.py mode:=jetson
+
+# Operator laptop
+ros2 launch bringup autonav.launch.py mode:=base_station
+
+# Single machine
+ros2 launch bringup autonav.launch.py
+
+# Use EKF localizer instead of ZED spatial localization
+ros2 launch bringup autonav.launch.py use_zed_localizer:=false
+
+# Disable GNSS fusion in ZED
+ros2 launch bringup autonav.launch.py enable_gnss:=false
+
+# Custom Nav2 params
+ros2 launch bringup autonav.launch.py params_file:=/path/to/params.yaml
+
+# Debug nav2 verbosity
+ros2 launch bringup autonav.launch.py log_level:=debug
+```
+
+### Parameters
+
+| Parameter | Default | Choices | Description |
+|---|---|---|---|
+| `mode` | `standalone` | `standalone` `jetson` `base_station` | Deployment target |
+| `use_sim` | `false` | `true` `false` | Enable sim time and RViz2 |
+| `use_mock_hardware` | `false` | `true` `false` | Mock hardware for drive |
+| `mock_sensor_commands` | `false` | `true` `false` | Mock sensor command interfaces (requires `use_mock_hardware`) |
+| `robot_controller` | `rear_ackermann_controller` | `rear_ackermann_controller` `front_ackermann_controller` `ackermann_steering_controller` | Drive controller to start |
+| `use_zed_localizer` | `true` | `true` `false` | Use ZED spatial localization instead of EKF localizer |
+| `enable_gnss` | `true` | `true` `false` | Enable GNSS fusion inside the ZED camera |
+| `use_minimal` | `false` | `true` `false` | Use minimal Nav2 config (empty costmaps, basic BT) for ackermann testing |
+| `params_file` | `nav2_params.yaml` (or `nav2_params_minimal.yaml` when `use_minimal:=true`) | — | Full path to Nav2 params YAML; overrides `use_minimal` if set explicitly |
+| `use_dem` | `false` | `true` `false` | Enable DEM costmap layer |
+| `use_respawn` | `false` | `true` `false` | Respawn nav2 nodes on crash |
+| `log_level` | `info` | — | Log level for nav2 nodes |
+| `use_config` | `false` | `true` `false` | Launch the Nav2 config GUI |
+
+---
+
+## Equipment Servicing
+
+Drive + Arm + GPS + Heading
+
+```bash
+# Rover
+ros2 launch bringup equipment_servicing.launch.py mode:=jetson
+
+# Operator laptop
+ros2 launch bringup equipment_servicing.launch.py mode:=base_station
+
+# Single machine
+ros2 launch bringup equipment_servicing.launch.py
+
+# Mock hardware (no physical rover)
+ros2 launch bringup equipment_servicing.launch.py use_mock_hardware:=true use_sim:=true
+
+# 3 DOF wrist
+ros2 launch bringup equipment_servicing.launch.py use_3dof:=true
+```
+
+### Parameters
+
+| Parameter | Default | Choices | Description |
+|---|---|---|---|
+| `mode` | `standalone` | `standalone` `jetson` `base_station` | Deployment target |
+| `use_sim` | `false` | `true` `false` | Launch RViz2 |
+| `use_mock_hardware` | `false` | `true` `false` | Mock hardware for drive and arm |
+| `mock_sensor_commands` | `false` | `true` `false` | Mock sensor command interfaces (requires `use_mock_hardware`) |
+| `robot_controller` | `rear_ackermann_controller` | `rear_ackermann_controller` `front_ackermann_controller` `ackermann_steering_controller` | Drive controller to start |
+| `use_3dof` | `false` | `true` `false` | Enable 3 DOF wrist joints on arm |
+| `deactivate_talon` | `false` | `true` `false` | Deactivate talon joints in URDF (use with `use_mock_hardware` to prevent CAN traffic) |
+
+---
+
+## Node breakdown by mode
+
+### Delivery / Equipment Servicing
+
+| Node group | jetson | base_station | standalone |
+|---|:---:|:---:|:---:|
+| Drive hardware + controllers | ✓ | | ✓ |
+| Drive teleop (joystick) | | ✓ | ✓ |
+| Arm hardware + controllers | ✓ | | ✓ |
+| Arm teleop (joystick) | | ✓ | ✓ |
+| GPS (athena_gps / pixhawk) | ✓ | | ✓ |
+| Magnetometer heading | ✓ | | ✓ |
+
+### Autonomous Navigation
+
+| Node group | jetson | base_station | standalone |
+|---|:---:|:---:|:---:|
+| Drive hardware + controllers | ✓ | | ✓ |
+| Drive teleop (joystick) | | ✓ | ✓ |
+| ZED camera | ✓ | | ✓ |
+| GPS (athena_gps / pixhawk) | ✓ | | ✓ |
+| ZED spatial localizer (default) | ✓ | | ✓ |
+| EKF localizer (`use_zed_localizer:=false`) | ✓ | | ✓ |
+| Nav2 stack | ✓ | | ✓ |
+| Magnetometer heading | ✓ | | ✓ |

src/bringup/launch/autonav.launch.py

@@ -0,0 +1,170 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    default_params = PathJoinSubstitution([
+        FindPackageShare('athena_planner'), 'config', 'nav2_params.yaml'
+    ])
+    minimal_params = PathJoinSubstitution([
+        FindPackageShare('athena_planner'), 'config', 'nav2_params_minimal.yaml'
+    ])
+    selected_params = PythonExpression([
+        "'", minimal_params, "' if '", LaunchConfiguration('use_minimal'), "' == 'true' else '", default_params, "'"
+    ])
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'mode',
+            default_value='standalone',
+            choices=['standalone', 'jetson', 'base_station'],
+            description=(
+                "'jetson': hardware + nav nodes on the rover. "
+                "'base_station': operator teleop nodes. "
+                "'standalone': all nodes on one machine."
+            ),
+        ),
+        DeclareLaunchArgument(
+            'use_sim',
+            default_value='false',
+            choices=['true', 'false'],
+            description='Enable sim time and RViz2.',
+        ),
+        DeclareLaunchArgument(
+            'use_mock_hardware',
+            default_value='false',
+            choices=['true', 'false'],
+            description='Start drive with mock hardware mirroring commands to states.',
+        ),
+        DeclareLaunchArgument(
+            'mock_sensor_commands',
+            default_value='false',
+            choices=['true', 'false'],
+            description='Enable mock command interfaces for sensors. Only used when use_mock_hardware is true.',
+        ),
+        DeclareLaunchArgument(
+            'robot_controller',
+            default_value='rear_ackermann_controller',
+            choices=['front_ackermann_controller', 'ackermann_steering_controller', 'rear_ackermann_controller'],
+            description='Drive controller to start.',
+        ),
+        DeclareLaunchArgument(
+            'use_zed_localizer',
+            default_value='true',
+            choices=['true', 'false'],
+            description='Use ZED spatial localization instead of the EKF localizer.',
+        ),
+        DeclareLaunchArgument(
+            'enable_gnss',
+            default_value='true',
+            choices=['true', 'false'],
+            description='Enable GNSS fusion inside the ZED camera.',
+        ),
+        DeclareLaunchArgument(
+            'use_minimal',
+            default_value='false',
+            choices=['true', 'false'],
+            description='Use minimal Nav2 config (empty costmaps, basic BT) for ackermann testing.',
+        ),
+        DeclareLaunchArgument(
+            'params_file',
+            default_value=selected_params,
+            description='Full path to the Nav2 params YAML. Defaults to minimal or full config based on use_minimal.',
+        ),
+        DeclareLaunchArgument(
+            'use_dem',
+            default_value='false',
+            choices=['true', 'false'],
+            description='Enable DEM costmap layer.',
+        ),
+        DeclareLaunchArgument(
+            'use_respawn',
+            default_value='false',
+            choices=['true', 'false'],
+            description='Respawn nav2 nodes if they crash.',
+        ),
+        DeclareLaunchArgument(
+            'log_level',
+            default_value='info',
+            description='Log level for nav2 nodes.',
+        ),
+        DeclareLaunchArgument(
+            'use_config',
+            default_value='false',
+            choices=['true', 'false'],
+            description='Launch the Nav2 config GUI.',
+        ),
+        DeclareLaunchArgument(
+            'can_interface',
+            default_value='can1',
+            description='CAN interface to use for hardware interfaces.',
+        ),
+        OpaqueFunction(function=launch_setup),
+    ])
+
+
+def launch_setup(context, *args, **kwargs):
+    mode = LaunchConfiguration('mode').perform(context)
+    use_sim = LaunchConfiguration('use_sim').perform(context)
+    use_mock_hardware = LaunchConfiguration('use_mock_hardware').perform(context)
+    mock_sensor_commands = LaunchConfiguration('mock_sensor_commands').perform(context)
+    robot_controller = LaunchConfiguration('robot_controller').perform(context)
+    use_zed_localizer = LaunchConfiguration('use_zed_localizer').perform(context)
+    enable_gnss = LaunchConfiguration('enable_gnss').perform(context)
+    params_file = LaunchConfiguration('params_file').perform(context)
+    use_dem = LaunchConfiguration('use_dem').perform(context)
+    use_respawn = LaunchConfiguration('use_respawn').perform(context)
+    log_level = LaunchConfiguration('log_level').perform(context)
+    use_config = LaunchConfiguration('use_config').perform(context)
+    can_interface = LaunchConfiguration('can_interface').perform(context)
+
+    drive_bringup = get_package_share_directory('drive_bringup')
+    nav_bringup = get_package_share_directory('nav_bringup')
+    mag_heading = get_package_share_directory('mag_heading')
+
+    drive = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(drive_bringup, 'launch', 'athena_drive.launch.py')
+        ),
+        launch_arguments={
+            'mode': mode,
+            'use_sim': use_sim,
+            'use_mock_hardware': use_mock_hardware,
+            'mock_sensor_commands': mock_sensor_commands,
+            'robot_controller': robot_controller,
+            'can_interface': can_interface,
+        }.items(),
+    )
+
+    nav = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav_bringup, 'launch', 'nav_bringup.launch.py')
+        ),
+        launch_arguments={
+            'sim': use_sim,
+            'use_zed_localizer': use_zed_localizer,
+            'enable_gnss': enable_gnss,
+            'params_file': params_file,
+            'use_dem': use_dem,
+            'use_respawn': use_respawn,
+            'log_level': log_level,
+            'use_config': use_config,
+            'can_interface': can_interface,
+        }.items(),
+    )
+
+    heading = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(mag_heading, 'launch', 'mag_heading.launch.py')
+        ),
+    )
+
+    if mode == 'base_station':
+        return [drive]
+    else:
+        return [drive, nav, heading]