Science upgrades

src/bringup/launch/science.launch.py

@@ -0,0 +1,199 @@
+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. Science runs in /science to avoid the collision.
+_SCIENCE_NS = "science"
+_SCIENCE_CM = f"/{_SCIENCE_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('deactivate_talon', default_value='false', choices=['true', 'false']),
+        DeclareLaunchArgument('can_interface', default_value='can1'),
+        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)
+    deactivate_talon = LaunchConfiguration('deactivate_talon').perform(context)
+    can_interface = LaunchConfiguration('can_interface').perform(context)
+
+    drive_share = get_package_share_directory('drive_bringup')
+    science_bringup_share = get_package_share_directory('science_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')
+    led_indicator_share = get_package_share_directory('led_indicator')
+
+    # ------------------------------------------------------------------
+    # 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,
+            'can_interface': can_interface,
+        }.items(),
+    )
+
+    # ------------------------------------------------------------------
+    # GPS + heading + LED indicator: 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')),
+    )
+    led = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(led_indicator_share, 'launch', 'led_indicator.launch.py')),
+    )
+
+    # ------------------------------------------------------------------
+    # Science: inlined, namespaced under /science so its CM/topics don't clash with drive.
+    # The science subsystem files (URDF, controllers YAML, joystick YAML) are reused as-is.
+    # ------------------------------------------------------------------
+    science_urdf_xacro = os.path.join(description_share, 'urdf', 'athena_science.urdf.xacro')
+    science_controllers_yaml = os.path.join(science_bringup_share, 'config', 'athena_science_controllers.yaml')
+    science_joystick_yaml = os.path.join(science_bringup_share, 'config', 'joystick.yaml')
+
+    science_urdf = Command([
+        FindExecutable(name='xacro'), ' ',
+        science_urdf_xacro,
+        ' prefix:=""',
+        ' use_mock_hardware:=', use_mock_hardware,
+        ' mock_sensor_commands:=', mock_sensor_commands,
+        ' deactivate_talon:=', deactivate_talon, ' ',
+        ' can_interface:=', can_interface, ' ',
+    ])
+
+    active_controllers = [
+        'science_controller',
+        'laser_gpio_controller',
+        'fluoro_led_gpio_controller',
+    ]
+    inactive_controllers = [
+        'joint_group_velocity_controller',
+        'joint_group_position_controller',
+    ]
+
+    science_cm = Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        namespace=_SCIENCE_NS,
+        output='both',
+        parameters=[science_controllers_yaml],
+        remappings=[('~/robot_description', 'robot_description')],
+    )
+
+    science_rsp = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        namespace=_SCIENCE_NS,
+        output='both',
+        parameters=[{'robot_description': science_urdf}],
+    )
+
+    def spawner(name, inactive=False):
+        args = [name, '-c', _SCIENCE_CM, '--param-file', science_controllers_yaml]
+        if inactive:
+            args.append('--inactive')
+        return Node(
+            package='controller_manager',
+            executable='spawner',
+            namespace=_SCIENCE_NS,
+            arguments=args,
+        )
+
+    jsb = spawner('joint_state_broadcaster')
+    motor_status = spawner('motor_status_controller')
+    active_spawners = [spawner(c) for c in active_controllers]
+    inactive_spawners = [spawner(c, inactive=True) for c in inactive_controllers]
+
+    # Sequence: CM up → JSB → motor_status → active spawners (chained) → inactive spawners (chained).
+    delay_jsb = RegisterEventHandler(OnProcessStart(
+        target_action=science_cm,
+        on_start=[TimerAction(period=3.0, actions=[jsb])],
+    ))
+
+    delay_motor_status = RegisterEventHandler(OnProcessExit(
+        target_action=jsb,
+        on_exit=[motor_status],
+    ))
+
+    chained = []
+    prev = motor_status
+    for sp in active_spawners + inactive_spawners:
+        chained.append(RegisterEventHandler(OnProcessExit(target_action=prev, on_exit=[sp])))
+        prev = sp
+
+    science_hardware = GroupAction([
+        science_cm,
+        science_rsp,
+        delay_jsb,
+        delay_motor_status,
+        *chained,
+    ])
+
+    science_joystick = Node(
+        package='teleop',
+        executable='joystick',
+        namespace=_SCIENCE_NS,
+        name='joystick',
+        output='screen',
+        parameters=[science_joystick_yaml],
+        remappings=[
+            ('controller_input', 'science_manual'),
+            ('/controller_input', '/science_manual'),
+        ],
+    )
+
+    if mode == 'jetson':
+        return [drive, science_hardware, gps, heading, led]
+    elif mode == 'base_station':
+        return [drive, science_joystick]
+    else:  # standalone
+        return [drive, science_hardware, science_joystick, gps, heading, led]

src/description/ros2_control/science/science.dc.ros2_control.xacro

@@ -13,7 +13,7 @@
         <param name="can_interface">${can_interface}</param>
         <param name="can_id">0x70</param>
       </hardware>
-
+<!-- 
       <joint name="dc_auger">
         <param name="node_id">0</param>
         <param name="joint_type">revolute</param>
@@ -32,9 +32,9 @@
         <state_interface name="status"/>
         <state_interface name="motor_temperature"/>
         <state_interface name="torque_current"/>
-      </joint>
+      </joint> -->
 
-      <joint name="dc_scoop">
+      <!-- <joint name="dc_scoop">
         <param name="node_id">1</param>
         <param name="joint_type">revolute</param>
 
@@ -52,7 +52,7 @@
         <state_interface name="status"/>
         <state_interface name="motor_temperature"/>
         <state_interface name="torque_current"/>
-      </joint>
+      </joint> -->
 
     </ros2_control>
 

src/description/ros2_control/science/science.mock.ros2_control.xacro

@@ -42,7 +42,7 @@
                 <state_interface name="velocity"/>
             </joint>
 
-            <joint name="talon_scoop">
+            <joint name="dc_scoop">
                 <param name="node_id">2</param>
                 <command_interface name="position">
                 <param name="min">0</param>

src/description/ros2_control/science/science.servo.ros2_control.xacro

@@ -11,12 +11,31 @@
                 <param name="can_id">0x80</param>
             </hardware>
 
-            <joint name="servo_rack_and_pinion_l">
+            <joint name="servo_scoops_lift_f">
                 <param name="node_id">0x1</param>
                 <param name="gear_ratio">1</param>
-                <param name="max_pos">235</param> <!-- degrees -->
-                <param name ="meters_per_deg">0.00016666666</param> <!-- (TODO) convert motor position in degrees to displacement in meters -->
-                <param name="servo_type">standard</param> <!-- standard or continuous -->
+                <param name="max_vel">300</param> <!-- deg/s -->
+                <param name ="meters_per_deg">0.00016666666</param> <!-- (TODO) When prismatic, convert motor position in degrees to displacement in meters -->
+                <param name="servo_type">continuous</param> <!-- standard or continuous -->
+                <param name="joint_type">revolute</param> <!-- revolute or prismatic -->
+                <command_interface name="position"/>
+                <command_interface name="velocity"/>
+                <command_interface name="status_request"/>
+                <command_interface name="maintenance_request"/>
+                <command_interface name="maintenance_frame_high"/>
+                <command_interface name="maintenance_frame_low"/>
+                <command_interface name="maintenance_data_count"/>
+                <state_interface name="position"/>
+                <state_interface name="velocity"/>
+                <state_interface name="status"/>
+            </joint>
+
+            <joint name="servo_scoops_lift_b">
+                <param name="node_id">0x0</param>
+                <param name="gear_ratio">1</param>
+                <param name="max_vel">300</param> <!-- deg/s -->
+                <param name ="meters_per_deg">0.00016666666</param> <!-- (TODO) When prismatic, convert motor position in degrees to displacement in meters -->
+                <param name="servo_type">continuous</param> <!-- standard or continuous -->
                 <param name="joint_type">revolute</param> <!-- revolute or prismatic -->
                 <command_interface name="position"/>
                 <command_interface name="velocity"/>
@@ -30,12 +49,12 @@
                 <state_interface name="status"/>
             </joint>
 
-            <joint name="servo_rack_and_pinion_r">
+            <joint name="servo_sampler_lift_f">
                 <param name="node_id">0x2</param>
                 <param name="gear_ratio">1</param>
-                <param name="max_pos">235</param> <!-- degrees -->
-                <param name ="meters_per_deg">0.00016666666</param> <!-- (TODO) convert motor position in degrees to displacement in meters -->
-                <param name="servo_type">standard</param> <!-- standard or continuous -->
+                <param name="max_vel">300</param> <!-- deg/s -->
+                <param name ="meters_per_deg">0.00016666666</param> <!-- (TODO) When prismatic, convert motor position in degrees to displacement in meters -->
+                <param name="servo_type">continuous</param> <!-- standard or continuous -->
                 <param name="joint_type">revolute</param> <!-- revolute or prismatic -->
                 <command_interface name="position"/>
                 <command_interface name="velocity"/>
@@ -49,12 +68,13 @@
                 <state_interface name="status"/>
             </joint>
 
-            <joint name="servo_scoop_a">
-                <param name="node_id">0x3</param> <!-- 0x5 -->
+            <joint name="servo_sampler_lift_b">
+                <param name="node_id">0x3</param>
                 <param name="gear_ratio">1</param>
-                <param name="max_pos">178</param> <!-- degrees -->
-                <param name="servo_type">standard</param> <!-- standard / continuous -->
-                <param name="joint_type">revolute</param> <!-- revolute / prismatic -->
+                <param name="max_vel">300</param> <!-- deg/s -->
+                <param name ="meters_per_deg">0.00016666666</param> <!-- (TODO) When prismatic, convert motor position in degrees to displacement in meters -->
+                <param name="servo_type">continuous</param> <!-- standard or continuous -->
+                <param name="joint_type">revolute</param> <!-- revolute or prismatic -->
                 <command_interface name="position"/>
                 <command_interface name="velocity"/>
                 <command_interface name="status_request"/>
@@ -67,12 +87,13 @@
                 <state_interface name="status"/>
             </joint>
 
-            <joint name="servo_scoop_b">
-                <param name="node_id">0x5</param> <!-- 0x3 -->
+            <joint name="servo_conveyor_belt">
+                <param name="node_id">0x4</param>
                 <param name="gear_ratio">1</param>
-                <param name="max_pos">178</param> <!-- degrees -->
-                <param name="servo_type">standard</param> <!-- standard / continuous -->
-                <param name="joint_type">revolute</param> <!-- revolute / prismatic -->
+                <param name="max_vel">300</param> <!-- deg/s -->
+                <param name ="meters_per_deg">0.00016666666</param> <!-- (TODO) When prismatic, convert motor position in degrees to displacement in meters -->
+                <param name="servo_type">continuous</param> <!-- standard or continuous -->
+                <param name="joint_type">revolute</param> <!-- revolute or prismatic -->
                 <command_interface name="position"/>
                 <command_interface name="velocity"/>
                 <command_interface name="status_request"/>
@@ -85,11 +106,10 @@
                 <state_interface name="status"/>
             </joint>
 
-            <joint name="servo_sampler_lift_l">
-                <param name="node_id">0x6</param>
+            <joint name="servo_scoop_f">
+                <param name="node_id">0x5</param> <!-- 0x5 -->
                 <param name="gear_ratio">1</param>
-                <param name="max_pos">360</param> <!-- degrees -->
-                <param name ="meters_per_deg">0.00016666666</param> <!-- (TODO) convert motor position in degrees to displacement in meters -->
+                <param name="max_pos">178</param> <!-- degrees -->
                 <param name="servo_type">standard</param> <!-- standard / continuous -->
                 <param name="joint_type">revolute</param> <!-- revolute / prismatic -->
                 <command_interface name="position"/>
@@ -104,11 +124,10 @@
                 <state_interface name="status"/>
             </joint>
 
-            <joint name="servo_sampler_lift_r">
-                <param name="node_id">0x4</param>
+            <joint name="servo_scoop_b">
+                <param name="node_id">0x6</param> <!-- 0x3 -->
                 <param name="gear_ratio">1</param>
-                <param name="max_pos">360</param> <!-- degrees -->
-                <param name ="meters_per_deg">0.00016666666</param> <!-- (TODO) convert motor position in degrees to displacement in meters -->
+                <param name="max_pos">178</param> <!-- degrees -->
                 <param name="servo_type">standard</param> <!-- standard / continuous -->
                 <param name="joint_type">revolute</param> <!-- revolute / prismatic -->
                 <command_interface name="position"/>
@@ -124,7 +143,7 @@
             </joint>
 
             <joint name="servo_auger_lift">
-                <param name="node_id">0x0</param>
+                <param name="node_id">0x7</param>
                 <param name="gear_ratio">1</param>
                 <param name="max_pos">270</param> <!-- degrees -->
                 <param name ="meters_per_deg">0.00016666666</param> <!-- (TODO) convert motor position in degrees to displacement in meters -->
@@ -141,6 +160,8 @@
                 <state_interface name="velocity"/>
                 <state_interface name="status"/>
             </joint>
+
+
         </ros2_control>
     </xacro:macro>
 </robot>