Joy Config + Xbox & Airbus integration

.gitignore

@@ -178,6 +178,10 @@ cython_debug/
 # Prerequisites
 *.d
 
+# AI
+.kiro
+.claude
+
 # Compiled Object files
 *.slo
 *.lo

src/subsystems/arm/arm_bringup/config/joystick.yaml

@@ -1,8 +1,6 @@
 /**:
   ros__parameters:
-    # 0: PS4    1: Thrustmaster    2: Xbox One    3: 8BitDo Ultimate
-    joystick_type: 2
-    joystick_index: -1
+    joystick_type: "airbus"
 
     # can ids
     base_yaw_id: 5

src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py

@@ -193,7 +193,7 @@ def launch_setup(context, *args, **kwargs):
         executable='joystick',
         name='joystick',
         output='screen',
-        parameters=[joystick_config_file],
+        parameters=[joystick_config_file, {'subsystem': 'arm'}],
     )
 
     control_node = Node(

src/subsystems/arm/arm_controllers/config/manual_arm_cylindrical_controller.yaml

@@ -48,6 +48,15 @@ manual_arm_cylindrical_controller:
       forbidden_interface_name_prefix: null
     }
   }
+  joystick_topic: {
+    type: string,
+    default_value: "controller_input/airbus",
+    description: "Topic name for joystick input (sensor_msgs/Joy).",
+    read_only: true,
+    validation: {
+      not_empty<>: null,
+    }
+  }
   joint_lengths: {
     type: double_array,
     default_value: [],

src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml

@@ -58,6 +58,15 @@ manual_arm_joint_by_joint_controller:
   #     forbidden_interface_name_prefix: null
   #   }
   # }
+  joystick_topic: {
+    type: string,
+    default_value: "controller_input/airbus",
+    description: "Topic name for joystick input (sensor_msgs/Joy).",
+    read_only: true,
+    validation: {
+      not_empty<>: null,
+    }
+  }
   joint_max_velocities: {
     type: double_array,
     default_value: [],

src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_cylindrical_controller.hpp

@@ -54,7 +54,7 @@ static constexpr size_t CMD_MY_ITFS = 0;
 static constexpr int joystick_axes = 6;
 
 // amount of joystick buttons
-static constexpr int joystick_buttons = 13;
+static constexpr int joystick_buttons = 17;
 
 // name constants for amount of velocities being commanded (yaw, y, z, thetadot, wrist roll, open claw, close claw)
 static constexpr size_t CMD_VELOCITIES_SIZE = 7;

src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp

@@ -32,7 +32,7 @@ static constexpr size_t CMD_MY_ITFS = 0;
 static constexpr int joystick_axes = 6;
 
 // amount of joystick buttons
-static constexpr int joystick_buttons = 13;
+static constexpr int joystick_buttons = 17;
 
 // TODO(anyone: example setup for control mode (usually you will use some enums defined in messages)
 enum class control_mode_type : std::uint8_t

src/subsystems/arm/arm_controllers/src/manual_arm_cylindrical_controller.cpp

@@ -110,7 +110,7 @@ controller_interface::CallbackReturn ManualArmCylindricalController::on_configur
 
   // Reference Subscriber
   ref_subscriber_ = get_node()->create_subscription<ControllerReferenceMsg>(
-    "controller_input", subscribers_qos,
+    params_.joystick_topic, subscribers_qos,
     std::bind(&ManualArmCylindricalController::reference_callback, this, std::placeholders::_1));
 
   // Create, populate with NaN, and write message to input_ref_ to be used in reference callback
@@ -244,9 +244,9 @@ controller_interface::return_type ManualArmCylindricalController::update(
   if (!std::isnan((*current_ref)->axes[0]))
   {
     //TODO: Get rid of the hardcoded rigamarole (translates to max vel of 1.5 inches/s)
-    command_velocities_[0] = (*current_ref)->axes[2]*0.174533; // l/r right joystick -> yaw 10 dps
+    command_velocities_[0] = (*current_ref)->axes[2]*0.174533; // joystick_y -> yaw 10 dps
     command_velocities_[1] = (*current_ref)->axes[1]*6*0.00635; // u/d left joystick -> vy
-    command_velocities_[2] = (*current_ref)->axes[3]*6*0.00635; // u/d right joystick -> vz
+    command_velocities_[2] = (*current_ref)->axes[3]*6*0.00635; // joystick_x -> vz
     command_velocities_[3] = (*current_ref)->axes[1] * static_cast<float>((*current_ref)->buttons[1]);  // u/d left joystick & circle -> thetadot
     command_velocities_[4] = 0.0; // (*current_ref)->axes[0]; // l/r left joystick -> wrist roll
     command_velocities_[5] = (*current_ref)->axes[4]; // left trigger -> open claw

src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp

@@ -92,7 +92,7 @@ controller_interface::CallbackReturn ManualArmJointByJointController::on_configu
 
   // Reference Subscriber
   ref_subscriber_ = get_node()->create_subscription<ControllerReferenceMsg>(
-    "controller_input", subscribers_qos,
+    params_.joystick_topic, subscribers_qos,
     std::bind(&ManualArmJointByJointController::reference_callback, this, std::placeholders::_1));
 
   // Create, populate with NaN, and write message to input_ref_ to be used in reference callback

src/subsystems/drive/drive_bringup/config/joystick.yaml

@@ -1,4 +1,3 @@
 /**:
   ros__parameters:
-    # 0: PS4    1: Thrustmaster
-    joystick_type: 1
+    joystick_type: "thrustmaster"

src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py

@@ -1,3 +1,5 @@
+import yaml
+
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler, TimerAction
 from launch.conditions import IfCondition
@@ -167,16 +169,16 @@ def launch_setup(context, *args, **kwargs):
 
     robot_description = {"robot_description": robot_description_content}
 
+    with open(joystick_config.perform(context), 'r') as f:
+        joy_cfg = yaml.safe_load(f)
+    joystick_type = joy_cfg['/**']['ros__parameters']['joystick_type']
+
     joystick_publisher = Node(
         package='teleop',
         executable='joystick',
         name='joystick',
         output='screen',
-        parameters=[joystick_config],
-        remappings=[
-            ('controller_input', 'joy'),
-            ('/controller_input', '/joy'),
-        ],
+        parameters=[joystick_config, {'subsystem': 'drive'}],
     )
 
     teleop_twist_joy = Node(
@@ -186,6 +188,7 @@ def launch_setup(context, *args, **kwargs):
         output='screen',
         parameters=[teleop_twist_config],
         remappings=[
+            ('joy', f'controller_input/{joystick_type}'),
             ('/cmd_vel', '/rear_ackermann_controller/reference'),
         ],
     )

src/subsystems/science/science_bringup/config/joystick.yaml

@@ -1,4 +1,3 @@
 /**:
   ros__parameters:
-    # 0: PS4    1: Thrustmaster
-    joystick_type: 0
+    joystick_type: "xbox"

src/subsystems/science/science_bringup/launch/athena_science.launch.py

@@ -144,11 +144,7 @@ def generate_launch_description():
         executable='joystick',
         name='joystick',
         output='screen',
-        parameters = [joystick_config_file],
-        remappings=[
-        ('controller_input', 'science_manual'),
-        ('/controller_input', '/science_manual'),
-    ],
+        parameters=[joystick_config_file, {'subsystem': 'science'}],
     )
 
     control_node = Node(

src/subsystems/science/science_controllers/src/science_controller.cpp

@@ -146,7 +146,7 @@ controller_interface::CallbackReturn ScienceManual::on_configure(
 
   // Reference subscriber
   ref_subscriber_ = get_node()->create_subscription<ControllerReferenceMsg>(
-    "/science_manual", subscribers_qos,
+    params_.joystick_topic, subscribers_qos,
     std::bind(&ScienceManual::reference_callback, this, std::placeholders::_1));
 
   std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
@@ -155,7 +155,7 @@ controller_interface::CallbackReturn ScienceManual::on_configure(
 
   // State publisher
   s_publisher_ = get_node()->create_publisher<ControllerStateMsg>(
-    "/science_manual/state", rclcpp::QoS(rclcpp::KeepLast(1)));
+    params_.joystick_topic + "/state", rclcpp::QoS(rclcpp::KeepLast(1)));
   state_publisher_ = std::make_unique<ControllerStatePublisher>(s_publisher_);
 
   if (state_publisher_ && state_publisher_->trylock()) {
@@ -293,8 +293,8 @@ controller_interface::return_type ScienceManual::update(
   // rack_left_position  += axis_left  * rack_speed * left_gain * dt;
   // rack_right_position -= axis_right * rack_speed * right_gain * dt;
 
-  // ** TEMPORARY FOR SAR
-  bool lift_button = (msg->buttons.size() > 11 && msg->buttons[12]);
+  // ** TEMPORARY FOR SAR 
+  bool lift_button = (msg->buttons.size() > 1 && msg->buttons[1]);
   if (lift_button && !prev_lift_button) {
     lift_toggle = !lift_toggle;
   }

src/subsystems/science/science_controllers/src/science_controller.yaml

@@ -2,6 +2,14 @@
 # Licensed under the Apache License, Version 2.0 (the "License");
 # ...
 science_manual:
+  joystick_topic:
+    type: string
+    default_value: "controller_input/xbox"
+    description: "Topic name for joystick input (sensor_msgs/Joy)."
+    read_only: true
+    validation:
+      not_empty<>: null
+
   state_joints:
     type: string_array
     default_value: []

src/teleop/teleop/config/joy.yaml

@@ -0,0 +1,78 @@
+/**:
+  ros__parameters:
+    controllers:
+      xbox:
+        axes:
+        - { name: "left_stick_lr",  axis: 0, invert: false, trigger: false }
+        - { name: "left_stick_ud",  axis: 1, invert: true, trigger: false }
+        - { name: "left_trigger",   axis: 2, invert: false, trigger: true  }
+        - { name: "right_stick_lr", axis: 3, invert: false, trigger: false }
+        - { name: "right_stick_ud", axis: 4, invert: true, trigger: false }
+        - { name: "right_trigger",  axis: 5, invert: false, trigger: true  }
+      thrustmaster:
+        axes:
+        - { name: "joystick_twist",   axis: 0, invert: false, trigger: false }
+        - { name: "joystick_x",       axis: 1, invert: true,  trigger: false }
+        - { name: "joystick_y",       axis: 2, invert: false, trigger: false }
+        - { name: "throttle_level",   axis: 3, invert: false,  trigger: true, range_inverted: true }
+        - { name: "hat_switch_y",     axis: 4, invert: true,  trigger: false }
+        - { name: "hat_switch_x",     axis: 5, invert: false, trigger: false }
+      airbus:
+        axes:
+        - { name: "joystick_twist",   axis: 0, invert: false, trigger: false }
+        - { name: "joystick_x",       axis: 1, invert: false, trigger: false }
+        - { name: "joystick_y",       axis: 2, invert: false, trigger: false }
+        - { name: "throttle_level",   axis: 3, invert: false, trigger: true, range_inverted: true }
+        - { name: "hat_switch_y",     axis: 4, invert: false, trigger: false }
+        - { name: "hat_switch_x",     axis: 5, invert: false, trigger: false }
+
+    subsystems:
+      arm:
+        # Controller: Airbus TCA Sidestick (joystick_type: airbus)
+
+        # Slot → axis name → Joint-by-Joint use               | Cylindrical use
+        # [0]  joystick_y     → base yaw / wrist roll*        | unused
+        # [1]  joystick_x     → shoulder pitch / wrist pitch* | vy (reach in/out) / theta-dot*
+        # [2]  joystick_twist → unused                        | yaw (rotate base)
+        # [3]  throttle_level → elbow pitch                   | vz (height)
+        # [4]  hat_switch_y   → unused (dead slot)
+        # [5]  hat_switch_x   → unusued (dead slot)
+
+        # center button(buttons[1]) = *modifier
+        # red button(buttons[3]) = actuator trigger
+        # left 1 (buttons[4]) = gripper open
+        #left 2 (buttons[5]) = gripper close
+        output_slots: [joystick_y, joystick_x, joystick_twist, throttle_level, hat_switch_y, hat_switch_x]
+
+      science:
+        # Controller: Xbox (joystick_type: xbox)
+
+        # Slot → axis name       → use
+        # [0]  left_stick_lr    → auger lift
+        # [1]  left_stick_ud    → unused
+        # [2]  right_stick_lr   → unused
+        # [3]  right_stick_ud   → sampler lift
+        # [4]  left_trigger     → auger spin (reverse via buttons[4])
+        # [5]  right_trigger    → scoop (reverse via buttons[5])
+
+        # A(buttons[0]) = servo scoop B (toggle)
+        # B(buttons[1]) = rack & pinion lift (toggle between two hardcoded positions)
+        # X(buttons[2]) = pump (toggle)
+        # Y(buttons[3]) = servo scoop A (toggle)
+        # LB(buttons[4]) = reverse auger spin
+        # RB(buttons[5]) = reverse scoop
+
+        output_slots: [left_stick_lr, left_stick_ud, right_stick_lr, right_stick_ud, left_trigger, right_trigger]
+
+
+      drive:
+        # Controller: Thrustmaster T.16000M (joystick_type: thrustmaster)
+        # All 6 axes published in order → remapped to /joy → consumed by teleop_twist_joy
+        # Slot → axis name       → use
+        # [0]  joystick_twist   → steering (angular velocity)
+        # [1]  joystick_x       → forward/reverse speed (linear velocity)
+        # [2]  joystick_y       → unused by twist joy
+        # [3]  throttle_level   → unused by twist joy
+        # [4]  hat_switch_y     → unused
+        # [5]  hat_switch_x     → unused
+        output_slots: [joystick_twist, joystick_x, joystick_y, throttle_level, hat_switch_y, hat_switch_x]

src/teleop/teleop/setup.py

@@ -10,6 +10,7 @@
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/config', ['config/joy.yaml']),
     ],
     install_requires=['setuptools'],
     zip_safe=True,