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/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml index aa4f8a92..6426cee0 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 @@ -42,7 +42,7 @@ controller_manager: type: forward_command_controller/ForwardCommandController -twodof_joint_trajectory_controller: +/**/twodof_joint_trajectory_controller: ros__parameters: joints: - base_yaw @@ -94,7 +94,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 +129,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,14 +141,14 @@ arm_velocity_controller: - actuator interface_name: velocity -cam_position_controller: +/**/cam_position_controller: ros__parameters: joints: - cam_0 - cam_1 interface_name: position -rotary_encoder_state_request_controller: +/**/rotary_encoder_state_request_controller: ros__parameters: joints: - base_yaw_encoder @@ -156,7 +156,7 @@ rotary_encoder_state_request_controller: - shoulder_pitch_encoder interface_name: state_request -manual_arm_cylindrical_controller: +/**/manual_arm_cylindrical_controller: ros__parameters: joints: - base_yaw @@ -172,7 +172,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 +185,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 +195,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 +207,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 @@ -231,7 +231,7 @@ manual_end_effector_gripper_claw_controller: # - motor_temperature # - torque_current # - status -two_dof_motor_status_controller: +/**/two_dof_motor_status_controller: ros__parameters: joints: - base_yaw @@ -295,7 +295,7 @@ two_dof_motor_status_controller: - status publish_rate: 10.0 -three_dof_motor_status_controller: +/**/three_dof_motor_status_controller: ros__parameters: joints: - base_yaw 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..aec7761f 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 @@ -24,7 +24,7 @@ controller_manager: motor_status_controller: type: general_controllers/MotorStatusController -joint_group_position_controller: +/**/joint_group_position_controller: ros__parameters: joints: - stepper_motor_a @@ -40,7 +40,7 @@ joint_group_position_controller: - servo_auger_lift interface_name: position -joint_group_velocity_controller: +/**/joint_group_velocity_controller: ros__parameters: joints: - stepper_motor_a @@ -56,7 +56,7 @@ joint_group_velocity_controller: - servo_auger_lift interface_name: velocity -science_controller: +/**/science_controller: ros__parameters: # Pumps pump_a: "stepper_motor_a" @@ -80,7 +80,7 @@ science_controller: state_interfaces: [position, velocity] # Laser GPIO Controller Configuration -laser_gpio_controller: +/**/laser_gpio_controller: ros__parameters: gpios: - spectrometry_laser @@ -95,7 +95,7 @@ laser_gpio_controller: - temperature - is_connected -fluoro_led_gpio_controller: +/**/fluoro_led_gpio_controller: ros__parameters: gpios: - fluoro_led @@ -115,7 +115,7 @@ fluoro_led_gpio_controller: # - motor_temperature # - torque_current # - status -motor_status_controller: +/**/motor_status_controller: ros__parameters: joints: - stepper_motor_a