From f1c9331a21e4e3bcca4697cb42b5930094a62cd7 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Tue, 12 May 2026 14:21:51 -0400 Subject: [PATCH 1/3] add mission launch files --- src/bringup/README.md | 156 ++++++++++++++ src/bringup/launch/autonav.launch.py | 162 +++++++++++++++ src/bringup/launch/delivery.launch.py | 185 +++++++++++++++++ .../launch/equipment_servicing.launch.py | 185 +++++++++++++++++ src/bringup/package.xml | 3 + .../config/athena_arm_controllers.yaml | 192 ++---------------- .../config/athena_science_controllers.yaml | 136 ++----------- 7 files changed, 727 insertions(+), 292 deletions(-) create mode 100644 src/bringup/README.md create mode 100644 src/bringup/launch/autonav.launch.py create mode 100644 src/bringup/launch/delivery.launch.py create mode 100644 src/bringup/launch/equipment_servicing.launch.py diff --git a/src/bringup/README.md b/src/bringup/README.md new file mode 100644 index 00000000..b3b50a9d --- /dev/null +++ b/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 | ✓ | | ✓ | diff --git a/src/bringup/launch/autonav.launch.py b/src/bringup/launch/autonav.launch.py new file mode 100644 index 00000000..c98a4ea3 --- /dev/null +++ b/src/bringup/launch/autonav.launch.py @@ -0,0 +1,162 @@ +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.', + ), + 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) + + 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, + }.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, + }.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] diff --git a/src/bringup/launch/delivery.launch.py b/src/bringup/launch/delivery.launch.py new file mode 100644 index 00000000..46865bb7 --- /dev/null +++ b/src/bringup/launch/delivery.launch.py @@ -0,0 +1,185 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, + TimerAction, +) +from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration +from launch_ros.actions import Node + + +# Drive owns the default /controller_manager. Arm runs in /arm to avoid the collision. +_ARM_NS = "arm" +_ARM_CM = f"/{_ARM_NS}/controller_manager" + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'mode', + default_value='standalone', + choices=['standalone', 'jetson', 'base_station'], + description=( + "'jetson': hardware nodes on the rover. " + "'base_station': operator teleop nodes. " + "'standalone': all nodes on one machine." + ), + ), + DeclareLaunchArgument('use_sim', default_value='false', choices=['true', 'false']), + DeclareLaunchArgument('use_mock_hardware', default_value='false', choices=['true', 'false']), + DeclareLaunchArgument('mock_sensor_commands', default_value='false', choices=['true', 'false']), + DeclareLaunchArgument( + 'robot_controller', + default_value='rear_ackermann_controller', + choices=['front_ackermann_controller', 'ackermann_steering_controller', 'rear_ackermann_controller'], + ), + DeclareLaunchArgument('use_3dof', default_value='false', choices=['true', 'false']), + DeclareLaunchArgument('deactivate_talon', default_value='false', choices=['true', 'false']), + 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_3dof = LaunchConfiguration('use_3dof').perform(context) + deactivate_talon = LaunchConfiguration('deactivate_talon').perform(context) + + drive_share = get_package_share_directory('drive_bringup') + arm_bringup_share = get_package_share_directory('arm_bringup') + description_share = get_package_share_directory('description') + athena_gps_share = get_package_share_directory('athena_gps') + mag_heading_share = get_package_share_directory('mag_heading') + + # ------------------------------------------------------------------ + # Drive: unmodified subsystem launch, root namespace. + # ------------------------------------------------------------------ + drive = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(drive_share, '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, + }.items(), + ) + + # ------------------------------------------------------------------ + # GPS + heading: unmodified, root namespace. + # ------------------------------------------------------------------ + gps = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(athena_gps_share, 'launch', 'gps_launch.py')), + launch_arguments={'sim': use_sim}.items(), + ) + heading = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(mag_heading_share, 'launch', 'mag_heading.launch.py')), + ) + + # ------------------------------------------------------------------ + # Arm: inlined, namespaced under /arm so its CM/topics don't clash with drive. + # The arm subsystem files (URDF, controllers YAML, joystick YAML) are reused as-is. + # ------------------------------------------------------------------ + arm_urdf_xacro = os.path.join(description_share, 'urdf', 'athena_arm.urdf.xacro') + arm_controllers_yaml = os.path.join(arm_bringup_share, 'config', 'athena_arm_controllers.yaml') + arm_joystick_yaml = os.path.join(arm_bringup_share, 'config', 'joystick.yaml') + + arm_urdf = Command([ + FindExecutable(name='xacro'), ' ', + arm_urdf_xacro, + ' prefix:=""', + ' use_mock_hardware:=', use_mock_hardware, + ' mock_sensor_commands:=', mock_sensor_commands, + ' use_3dof:=', use_3dof, + ' deactivate_talon:=', deactivate_talon, ' ', + ]) + + use_3dof_bool = use_3dof.lower() == 'true' + wrist_active = 'manual_3dof_wrist_joint_by_joint_controller' if use_3dof_bool else 'manual_2dof_wrist_joint_by_joint_controller' + wrist_inactive = 'manual_2dof_wrist_joint_by_joint_controller' if use_3dof_bool else 'manual_3dof_wrist_joint_by_joint_controller' + jt_controller = 'threedof_joint_trajectory_controller' if use_3dof_bool else 'twodof_joint_trajectory_controller' + + active_controllers = [ + 'manual_arm_joint_by_joint_controller', + wrist_active, + 'manual_end_effector_gripper_claw_controller', + ] + inactive_controllers = [ + wrist_inactive, + 'manual_arm_cylindrical_controller', + jt_controller, + 'arm_velocity_controller', + ] + + arm_cm = Node( + package='controller_manager', + executable='ros2_control_node', + namespace=_ARM_NS, + output='both', + parameters=[arm_controllers_yaml], + remappings=[('~/robot_description', 'robot_description')], + ) + + arm_rsp = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + namespace=_ARM_NS, + output='both', + parameters=[{'robot_description': arm_urdf}], + ) + + def spawner(name, inactive=False): + args = [name, '-c', _ARM_CM, '--param-file', arm_controllers_yaml] + if inactive: + args.append('--inactive') + return Node( + package='controller_manager', + executable='spawner', + namespace=_ARM_NS, + arguments=args, + ) + + jsb = spawner('joint_state_broadcaster') + active_spawners = [spawner(c) for c in active_controllers] + inactive_spawners = [spawner(c, inactive=True) for c in inactive_controllers] + + # Sequence: CM up → JSB → active spawners (chained) → inactive spawners (chained). + delay_jsb = RegisterEventHandler(OnProcessStart( + target_action=arm_cm, + on_start=[TimerAction(period=3.0, actions=[jsb])], + )) + + chained = [] + prev = jsb + for sp in active_spawners + inactive_spawners: + chained.append(RegisterEventHandler(OnProcessExit(target_action=prev, on_exit=[sp]))) + prev = sp + + arm_hardware = GroupAction([arm_cm, arm_rsp, delay_jsb, *chained]) + + arm_joystick = Node( + package='teleop', + executable='joystick', + namespace=_ARM_NS, + name='joystick', + output='screen', + parameters=[arm_joystick_yaml], + ) + + if mode == 'jetson': + return [drive, arm_hardware, gps, heading] + elif mode == 'base_station': + return [drive, arm_joystick] + else: # standalone + return [drive, arm_hardware, arm_joystick, gps, heading] diff --git a/src/bringup/launch/equipment_servicing.launch.py b/src/bringup/launch/equipment_servicing.launch.py new file mode 100644 index 00000000..46865bb7 --- /dev/null +++ b/src/bringup/launch/equipment_servicing.launch.py @@ -0,0 +1,185 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, + TimerAction, +) +from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration +from launch_ros.actions import Node + + +# Drive owns the default /controller_manager. Arm runs in /arm to avoid the collision. +_ARM_NS = "arm" +_ARM_CM = f"/{_ARM_NS}/controller_manager" + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'mode', + default_value='standalone', + choices=['standalone', 'jetson', 'base_station'], + description=( + "'jetson': hardware nodes on the rover. " + "'base_station': operator teleop nodes. " + "'standalone': all nodes on one machine." + ), + ), + DeclareLaunchArgument('use_sim', default_value='false', choices=['true', 'false']), + DeclareLaunchArgument('use_mock_hardware', default_value='false', choices=['true', 'false']), + DeclareLaunchArgument('mock_sensor_commands', default_value='false', choices=['true', 'false']), + DeclareLaunchArgument( + 'robot_controller', + default_value='rear_ackermann_controller', + choices=['front_ackermann_controller', 'ackermann_steering_controller', 'rear_ackermann_controller'], + ), + DeclareLaunchArgument('use_3dof', default_value='false', choices=['true', 'false']), + DeclareLaunchArgument('deactivate_talon', default_value='false', choices=['true', 'false']), + 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_3dof = LaunchConfiguration('use_3dof').perform(context) + deactivate_talon = LaunchConfiguration('deactivate_talon').perform(context) + + drive_share = get_package_share_directory('drive_bringup') + arm_bringup_share = get_package_share_directory('arm_bringup') + description_share = get_package_share_directory('description') + athena_gps_share = get_package_share_directory('athena_gps') + mag_heading_share = get_package_share_directory('mag_heading') + + # ------------------------------------------------------------------ + # Drive: unmodified subsystem launch, root namespace. + # ------------------------------------------------------------------ + drive = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(drive_share, '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, + }.items(), + ) + + # ------------------------------------------------------------------ + # GPS + heading: unmodified, root namespace. + # ------------------------------------------------------------------ + gps = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(athena_gps_share, 'launch', 'gps_launch.py')), + launch_arguments={'sim': use_sim}.items(), + ) + heading = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(mag_heading_share, 'launch', 'mag_heading.launch.py')), + ) + + # ------------------------------------------------------------------ + # Arm: inlined, namespaced under /arm so its CM/topics don't clash with drive. + # The arm subsystem files (URDF, controllers YAML, joystick YAML) are reused as-is. + # ------------------------------------------------------------------ + arm_urdf_xacro = os.path.join(description_share, 'urdf', 'athena_arm.urdf.xacro') + arm_controllers_yaml = os.path.join(arm_bringup_share, 'config', 'athena_arm_controllers.yaml') + arm_joystick_yaml = os.path.join(arm_bringup_share, 'config', 'joystick.yaml') + + arm_urdf = Command([ + FindExecutable(name='xacro'), ' ', + arm_urdf_xacro, + ' prefix:=""', + ' use_mock_hardware:=', use_mock_hardware, + ' mock_sensor_commands:=', mock_sensor_commands, + ' use_3dof:=', use_3dof, + ' deactivate_talon:=', deactivate_talon, ' ', + ]) + + use_3dof_bool = use_3dof.lower() == 'true' + wrist_active = 'manual_3dof_wrist_joint_by_joint_controller' if use_3dof_bool else 'manual_2dof_wrist_joint_by_joint_controller' + wrist_inactive = 'manual_2dof_wrist_joint_by_joint_controller' if use_3dof_bool else 'manual_3dof_wrist_joint_by_joint_controller' + jt_controller = 'threedof_joint_trajectory_controller' if use_3dof_bool else 'twodof_joint_trajectory_controller' + + active_controllers = [ + 'manual_arm_joint_by_joint_controller', + wrist_active, + 'manual_end_effector_gripper_claw_controller', + ] + inactive_controllers = [ + wrist_inactive, + 'manual_arm_cylindrical_controller', + jt_controller, + 'arm_velocity_controller', + ] + + arm_cm = Node( + package='controller_manager', + executable='ros2_control_node', + namespace=_ARM_NS, + output='both', + parameters=[arm_controllers_yaml], + remappings=[('~/robot_description', 'robot_description')], + ) + + arm_rsp = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + namespace=_ARM_NS, + output='both', + parameters=[{'robot_description': arm_urdf}], + ) + + def spawner(name, inactive=False): + args = [name, '-c', _ARM_CM, '--param-file', arm_controllers_yaml] + if inactive: + args.append('--inactive') + return Node( + package='controller_manager', + executable='spawner', + namespace=_ARM_NS, + arguments=args, + ) + + jsb = spawner('joint_state_broadcaster') + active_spawners = [spawner(c) for c in active_controllers] + inactive_spawners = [spawner(c, inactive=True) for c in inactive_controllers] + + # Sequence: CM up → JSB → active spawners (chained) → inactive spawners (chained). + delay_jsb = RegisterEventHandler(OnProcessStart( + target_action=arm_cm, + on_start=[TimerAction(period=3.0, actions=[jsb])], + )) + + chained = [] + prev = jsb + for sp in active_spawners + inactive_spawners: + chained.append(RegisterEventHandler(OnProcessExit(target_action=prev, on_exit=[sp]))) + prev = sp + + arm_hardware = GroupAction([arm_cm, arm_rsp, delay_jsb, *chained]) + + arm_joystick = Node( + package='teleop', + executable='joystick', + namespace=_ARM_NS, + name='joystick', + output='screen', + parameters=[arm_joystick_yaml], + ) + + if mode == 'jetson': + return [drive, arm_hardware, gps, heading] + elif mode == 'base_station': + return [drive, arm_joystick] + else: # standalone + return [drive, arm_hardware, arm_joystick, gps, heading] diff --git a/src/bringup/package.xml b/src/bringup/package.xml index 90138541..a10e626c 100644 --- a/src/bringup/package.xml +++ b/src/bringup/package.xml @@ -17,6 +17,9 @@ arm_bringup drive_bringup science_bringup + nav_bringup + athena_gps + mag_heading description athena_planner robot_state_publisher diff --git a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml index aa4f8a92..52439e58 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -1,4 +1,4 @@ -controller_manager: +/**/controller_manager: ros__parameters: update_rate: 100 # Hz @@ -29,20 +29,11 @@ controller_manager: manual_end_effector_gripper_claw_controller: type: arm_controllers/ManualEndEffectorGripperClawController - two_dof_motor_status_controller: - type: general_controllers/MotorStatusController - - three_dof_motor_status_controller: - type: general_controllers/MotorStatusController - - cam_position_controller: - type: position_controllers/JointGroupPositionController - - rotary_encoder_state_request_controller: - type: forward_command_controller/ForwardCommandController + motor_status_broadcaster: + type: general_controllers/MotorStatusBroadcaster -twodof_joint_trajectory_controller: +/**/twodof_joint_trajectory_controller: ros__parameters: joints: - base_yaw @@ -94,7 +85,7 @@ twodof_joint_trajectory_controller: # trajectory: 0.05 # goal: 0.03 -threedof_joint_trajectory_controller: +/**/threedof_joint_trajectory_controller: ros__parameters: joints: - base_yaw @@ -129,7 +120,7 @@ threedof_joint_trajectory_controller: # If set to zero, the controller will wait a potentially infinite amount of time. goal_time: 10.0 -arm_velocity_controller: +/**/arm_velocity_controller: ros__parameters: joints: - base_yaw @@ -141,22 +132,7 @@ arm_velocity_controller: - actuator interface_name: velocity -cam_position_controller: - ros__parameters: - joints: - - cam_0 - - cam_1 - interface_name: position - -rotary_encoder_state_request_controller: - ros__parameters: - joints: - - base_yaw_encoder - - elbow_pitch_encoder - - shoulder_pitch_encoder - interface_name: state_request - -manual_arm_cylindrical_controller: +/**/manual_arm_cylindrical_controller: ros__parameters: joints: - base_yaw @@ -172,7 +148,7 @@ manual_arm_cylindrical_controller: - 0.53 - 0.22 -manual_arm_joint_by_joint_controller: +/**/manual_arm_joint_by_joint_controller: ros__parameters: joints: - base_yaw @@ -185,7 +161,7 @@ manual_arm_joint_by_joint_controller: - 0.05148721 virtual_four_bar_coupling_ratio: 1.0 -manual_2dof_wrist_joint_by_joint_controller: +/**/manual_2dof_wrist_joint_by_joint_controller: ros__parameters: joints: - wrist_pitch @@ -195,7 +171,7 @@ manual_2dof_wrist_joint_by_joint_controller: - 0.785398 # 45 dps - 0.785398 -manual_3dof_wrist_joint_by_joint_controller: +/**/manual_3dof_wrist_joint_by_joint_controller: ros__parameters: joints: - wrist_motor_a @@ -207,7 +183,7 @@ manual_3dof_wrist_joint_by_joint_controller: - 0.785398 # TODO - 0.785398 # TODO -manual_end_effector_gripper_claw_controller: +/**/manual_end_effector_gripper_claw_controller: ros__parameters: velocity_joints: - gripper_claw @@ -220,151 +196,13 @@ manual_end_effector_gripper_claw_controller: joint_max_positions: - 0.03 # Actuator max displacement -# Default command interfaces: -# - status_request -# - maintenance_request -# - maintenance_frame_high -# - maintenance_frame_low -# - maintenance_data_count - -# Default state interfaces: -# - motor_temperature -# - torque_current -# - status -two_dof_motor_status_controller: - ros__parameters: - joints: - - base_yaw - - shoulder_pitch - - elbow_pitch - - wrist_pitch - - wrist_roll - - actuator - - gripper_claw - - cam_0 - - cam_1 - gpios: - - limit_switch_0 - - limit_switch_1 - - limit_switch_2 - - limit_switch_3 - - limit_switch_4 - command_interfaces: - gripper_claw: - interfaces: - - status_request - limit_switch_0: - interfaces: - - status_request - limit_switch_1: - interfaces: - - status_request - limit_switch_2: - interfaces: - - status_request - limit_switch_3: - interfaces: - - status_request - limit_switch_4: - interfaces: - - status_request - state_interfaces: - actuator: - interfaces: - - status - cam_0: - interfaces: - - status - cam_1: - interfaces: - - status - limit_switch_0: - interfaces: - - status - limit_switch_1: - interfaces: - - status - limit_switch_2: - interfaces: - - status - limit_switch_3: - interfaces: - - status - limit_switch_4: - interfaces: - - status - publish_rate: 10.0 - -three_dof_motor_status_controller: +/**/motor_status_broadcaster: ros__parameters: joints: - base_yaw - shoulder_pitch - elbow_pitch - - wrist_motor_a - - wrist_motor_b - - wrist_motor_c - - actuator - - gripper_claw - - cam_0 - - cam_1 - gpios: - - limit_switch_0 - - limit_switch_1 - - limit_switch_2 - - limit_switch_3 - - limit_switch_4 - command_interfaces: - gripper_claw: - interfaces: - - status_request - wrist_motor_a: - interfaces: - - status_request - wrist_motor_b: - interfaces: - - status_request - wrist_motor_c: - interfaces: - - status_request - limit_switch_0: - interfaces: - - status_request - limit_switch_1: - interfaces: - - status_request - limit_switch_2: - interfaces: - - status_request - limit_switch_3: - interfaces: - - status_request - limit_switch_4: - interfaces: - - status_request - state_interfaces: - actuator: - interfaces: - - status - cam_0: - interfaces: - - status - cam_1: - interfaces: - - status - limit_switch_0: - interfaces: - - status - limit_switch_1: - interfaces: - - status - limit_switch_2: - interfaces: - - status - limit_switch_3: - interfaces: - - status - limit_switch_4: - interfaces: - - status + interfaces: + - motor_temperature + - torque_current publish_rate: 10.0 diff --git a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml index a75ca5d5..d9c04994 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -1,4 +1,4 @@ -controller_manager: +/**/controller_manager: ros__parameters: update_rate: 100 # Hz @@ -15,16 +15,14 @@ controller_manager: science_controller: type: science_controllers/ScienceManual + # Laser GPIO Controller (CAN-based) laser_gpio_controller: type: gpio_controllers/GpioCommandController - fluoro_led_gpio_controller: - type: gpio_controllers/GpioCommandController - - motor_status_controller: - type: general_controllers/MotorStatusController + motor_status_broadcaster: + type: general_controllers/MotorStatusBroadcaster -joint_group_position_controller: +/**/joint_group_position_controller: ros__parameters: joints: - stepper_motor_a @@ -33,14 +31,14 @@ joint_group_position_controller: - servo_rack_and_pinion_r - servo_scoop_a - servo_scoop_b - - dc_scoop + - talon_scoop - servo_sampler_lift_l - servo_sampler_lift_l - - dc_auger + - talon_auger - servo_auger_lift interface_name: position -joint_group_velocity_controller: +/**/joint_group_velocity_controller: ros__parameters: joints: - stepper_motor_a @@ -49,14 +47,14 @@ joint_group_velocity_controller: - servo_rack_and_pinion_r - servo_scoop_a - servo_scoop_b - - dc_scoop + - talon_scoop - servo_sampler_lift_l - servo_sampler_lift_r - - dc_auger + - talon_auger - servo_auger_lift interface_name: velocity -science_controller: +/**/science_controller: ros__parameters: # Pumps pump_a: "stepper_motor_a" @@ -68,19 +66,19 @@ science_controller: # Scoops scoop_servos: ["servo_scoop_a", "servo_scoop_b"] - scoop_spinner: "dc_scoop" + scoop_spinner: "talon_scoop" # Sampler sampler_lift_l: "servo_sampler_lift_l" sampler_lift_r: "servo_sampler_lift_r" - auger_spinner: "dc_auger" + auger_spinner: "talon_auger" cap: "servo_auger_lift" interface_name: position command_interfaces: [position, velocity] state_interfaces: [position, velocity] # Laser GPIO Controller Configuration -laser_gpio_controller: +/**/laser_gpio_controller: ros__parameters: gpios: - spectrometry_laser @@ -95,106 +93,14 @@ laser_gpio_controller: - temperature - is_connected -fluoro_led_gpio_controller: - ros__parameters: - gpios: - - fluoro_led - command_interfaces: - fluoro_led: - interfaces: - - intensity - -# Default command interfaces: -# - status_request -# - maintenance_request -# - maintenance_frame_high -# - maintenance_frame_low -# - maintenance_data_count - -# Default state interfaces: -# - motor_temperature -# - torque_current -# - status -motor_status_controller: +/**/motor_status_broadcaster: ros__parameters: joints: - stepper_motor_a - stepper_motor_b - - servo_rack_and_pinion_l - - servo_rack_and_pinion_r - - servo_scoop_a - - servo_scoop_b - - dc_scoop - - servo_sampler_lift_l - - servo_sampler_lift_r - - dc_auger - - servo_auger_lift - gpios: - - spectrometry_laser - - fluoro_led - - science_limit_switch - command_interfaces: - spectrometry_laser: - interfaces: - - status_request - fluoro_led: - interfaces: - - status_request - science_limit_switch: - interfaces: - - status_request - dc_scoop: - interfaces: - - status_request - - maintenance_request - - maintenance_frame_high - - maintenance_frame_low - - maintenance_data_count - dc_auger: - interfaces: - - status_request - - maintenance_request - - maintenance_frame_high - - maintenance_frame_low - - maintenance_data_count - state_interfaces: - stepper_motor_a: - interfaces: - - status - stepper_motor_b: - interfaces: - - status - servo_rack_and_pinion_l: - interfaces: - - status - servo_rack_and_pinion_r: - interfaces: - - status - servo_scoop_a: - interfaces: - - status - servo_scoop_b: - interfaces: - - status - dc_scoop: - interfaces: - - status - servo_sampler_lift_l: - interfaces: - - status - servo_sampler_lift_r: - interfaces: - - status - dc_auger: - interfaces: - - status - servo_auger_lift: - interfaces: - - status - fluoro_led: - interfaces: - - status - science_limit_switch: - interfaces: - - status - publish_rate: 10.0 + - talon_lift + - talon_scoop + interfaces: + - motor_temperature + - torque_current + publish_rate: 10.0 \ No newline at end of file From 6cdfee4b2c797a7232f54499c0810bbb216dbe95 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Tue, 12 May 2026 14:25:21 -0400 Subject: [PATCH 2/3] remove unnecessary changes --- .../config/athena_arm_controllers.yaml | 174 +++++++++++++++++- .../config/athena_science_controllers.yaml | 126 +++++++++++-- 2 files changed, 278 insertions(+), 22 deletions(-) diff --git a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml index 52439e58..6426cee0 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -29,8 +29,17 @@ manual_end_effector_gripper_claw_controller: type: arm_controllers/ManualEndEffectorGripperClawController - motor_status_broadcaster: - type: general_controllers/MotorStatusBroadcaster + two_dof_motor_status_controller: + type: general_controllers/MotorStatusController + + three_dof_motor_status_controller: + type: general_controllers/MotorStatusController + + cam_position_controller: + type: position_controllers/JointGroupPositionController + + rotary_encoder_state_request_controller: + type: forward_command_controller/ForwardCommandController /**/twodof_joint_trajectory_controller: @@ -132,6 +141,21 @@ - actuator interface_name: velocity +/**/cam_position_controller: + ros__parameters: + joints: + - cam_0 + - cam_1 + interface_name: position + +/**/rotary_encoder_state_request_controller: + ros__parameters: + joints: + - base_yaw_encoder + - elbow_pitch_encoder + - shoulder_pitch_encoder + interface_name: state_request + /**/manual_arm_cylindrical_controller: ros__parameters: joints: @@ -196,13 +220,151 @@ joint_max_positions: - 0.03 # Actuator max displacement -/**/motor_status_broadcaster: +# Default command interfaces: +# - status_request +# - maintenance_request +# - maintenance_frame_high +# - maintenance_frame_low +# - maintenance_data_count + +# Default state interfaces: +# - motor_temperature +# - torque_current +# - status +/**/two_dof_motor_status_controller: + ros__parameters: + joints: + - base_yaw + - shoulder_pitch + - elbow_pitch + - wrist_pitch + - wrist_roll + - actuator + - gripper_claw + - cam_0 + - cam_1 + gpios: + - limit_switch_0 + - limit_switch_1 + - limit_switch_2 + - limit_switch_3 + - limit_switch_4 + command_interfaces: + gripper_claw: + interfaces: + - status_request + limit_switch_0: + interfaces: + - status_request + limit_switch_1: + interfaces: + - status_request + limit_switch_2: + interfaces: + - status_request + limit_switch_3: + interfaces: + - status_request + limit_switch_4: + interfaces: + - status_request + state_interfaces: + actuator: + interfaces: + - status + cam_0: + interfaces: + - status + cam_1: + interfaces: + - status + limit_switch_0: + interfaces: + - status + limit_switch_1: + interfaces: + - status + limit_switch_2: + interfaces: + - status + limit_switch_3: + interfaces: + - status + limit_switch_4: + interfaces: + - status + publish_rate: 10.0 + +/**/three_dof_motor_status_controller: ros__parameters: joints: - base_yaw - shoulder_pitch - elbow_pitch - interfaces: - - motor_temperature - - torque_current + - wrist_motor_a + - wrist_motor_b + - wrist_motor_c + - actuator + - gripper_claw + - cam_0 + - cam_1 + gpios: + - limit_switch_0 + - limit_switch_1 + - limit_switch_2 + - limit_switch_3 + - limit_switch_4 + command_interfaces: + gripper_claw: + interfaces: + - status_request + wrist_motor_a: + interfaces: + - status_request + wrist_motor_b: + interfaces: + - status_request + wrist_motor_c: + interfaces: + - status_request + limit_switch_0: + interfaces: + - status_request + limit_switch_1: + interfaces: + - status_request + limit_switch_2: + interfaces: + - status_request + limit_switch_3: + interfaces: + - status_request + limit_switch_4: + interfaces: + - status_request + state_interfaces: + actuator: + interfaces: + - status + cam_0: + interfaces: + - status + cam_1: + interfaces: + - status + limit_switch_0: + interfaces: + - status + limit_switch_1: + interfaces: + - status + limit_switch_2: + interfaces: + - status + limit_switch_3: + interfaces: + - status + limit_switch_4: + interfaces: + - status publish_rate: 10.0 diff --git a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml index d9c04994..aec7761f 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -15,12 +15,14 @@ science_controller: type: science_controllers/ScienceManual - # Laser GPIO Controller (CAN-based) laser_gpio_controller: type: gpio_controllers/GpioCommandController - motor_status_broadcaster: - type: general_controllers/MotorStatusBroadcaster + fluoro_led_gpio_controller: + type: gpio_controllers/GpioCommandController + + motor_status_controller: + type: general_controllers/MotorStatusController /**/joint_group_position_controller: ros__parameters: @@ -31,10 +33,10 @@ - servo_rack_and_pinion_r - servo_scoop_a - servo_scoop_b - - talon_scoop + - dc_scoop - servo_sampler_lift_l - servo_sampler_lift_l - - talon_auger + - dc_auger - servo_auger_lift interface_name: position @@ -47,10 +49,10 @@ - servo_rack_and_pinion_r - servo_scoop_a - servo_scoop_b - - talon_scoop + - dc_scoop - servo_sampler_lift_l - servo_sampler_lift_r - - talon_auger + - dc_auger - servo_auger_lift interface_name: velocity @@ -66,12 +68,12 @@ # Scoops scoop_servos: ["servo_scoop_a", "servo_scoop_b"] - scoop_spinner: "talon_scoop" + scoop_spinner: "dc_scoop" # Sampler sampler_lift_l: "servo_sampler_lift_l" sampler_lift_r: "servo_sampler_lift_r" - auger_spinner: "talon_auger" + auger_spinner: "dc_auger" cap: "servo_auger_lift" interface_name: position command_interfaces: [position, velocity] @@ -93,14 +95,106 @@ - temperature - is_connected -/**/motor_status_broadcaster: +/**/fluoro_led_gpio_controller: + ros__parameters: + gpios: + - fluoro_led + command_interfaces: + fluoro_led: + interfaces: + - intensity + +# Default command interfaces: +# - status_request +# - maintenance_request +# - maintenance_frame_high +# - maintenance_frame_low +# - maintenance_data_count + +# Default state interfaces: +# - motor_temperature +# - torque_current +# - status +/**/motor_status_controller: ros__parameters: joints: - stepper_motor_a - stepper_motor_b - - talon_lift - - talon_scoop - interfaces: - - motor_temperature - - torque_current - publish_rate: 10.0 \ No newline at end of file + - servo_rack_and_pinion_l + - servo_rack_and_pinion_r + - servo_scoop_a + - servo_scoop_b + - dc_scoop + - servo_sampler_lift_l + - servo_sampler_lift_r + - dc_auger + - servo_auger_lift + gpios: + - spectrometry_laser + - fluoro_led + - science_limit_switch + command_interfaces: + spectrometry_laser: + interfaces: + - status_request + fluoro_led: + interfaces: + - status_request + science_limit_switch: + interfaces: + - status_request + dc_scoop: + interfaces: + - status_request + - maintenance_request + - maintenance_frame_high + - maintenance_frame_low + - maintenance_data_count + dc_auger: + interfaces: + - status_request + - maintenance_request + - maintenance_frame_high + - maintenance_frame_low + - maintenance_data_count + state_interfaces: + stepper_motor_a: + interfaces: + - status + stepper_motor_b: + interfaces: + - status + servo_rack_and_pinion_l: + interfaces: + - status + servo_rack_and_pinion_r: + interfaces: + - status + servo_scoop_a: + interfaces: + - status + servo_scoop_b: + interfaces: + - status + dc_scoop: + interfaces: + - status + servo_sampler_lift_l: + interfaces: + - status + servo_sampler_lift_r: + interfaces: + - status + dc_auger: + interfaces: + - status + servo_auger_lift: + interfaces: + - status + fluoro_led: + interfaces: + - status + science_limit_switch: + interfaces: + - status + publish_rate: 10.0 From fcfa4b02acd2c44a92fcc5eb4e663fddd527813a Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Tue, 12 May 2026 14:34:41 -0400 Subject: [PATCH 3/3] remove from package.xml --- src/bringup/package.xml | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/bringup/package.xml b/src/bringup/package.xml index a10e626c..90138541 100644 --- a/src/bringup/package.xml +++ b/src/bringup/package.xml @@ -17,9 +17,6 @@ arm_bringup drive_bringup science_bringup - nav_bringup - athena_gps - mag_heading description athena_planner robot_state_publisher