From df75a4fb623c4607a0173536fe2312faccd9f7f6 Mon Sep 17 00:00:00 2001 From: Ryan George Date: Tue, 21 Apr 2026 16:01:49 -0400 Subject: [PATCH 1/5] Removed hardcoding from joystick_publisher -Added joy.yaml config file for joystick_publisher -Updated joystick_publisher to read from joy.yaml instead of hardcoding values -Updated joystick.yaml files for arm, drive, and science to match the new config file structure -Added multiple controllers by adding different topics for each controller --- .gitignore | 7 +- .../arm/arm_bringup/config/joystick.yaml | 3 +- .../arm_bringup/launch/athena_arm.launch.py | 2 +- .../manual_arm_cylindrical_controller.yaml | 9 + .../manual_arm_joint_by_joint_controller.yaml | 9 + .../src/manual_arm_cylindrical_controller.cpp | 6 +- .../manual_arm_joint_by_joint_controller.cpp | 2 +- .../drive/drive_bringup/config/joystick.yaml | 3 +- .../launch/athena_drive.launch.py | 11 +- .../science_bringup/config/joystick.yaml | 3 +- .../launch/athena_science.launch.py | 6 +- .../src/science_controller.cpp | 4 +- .../src/science_controller.yaml | 8 + src/teleop/teleop/config/joy.yaml | 27 +++ .../teleop/teleop/joystick_publisher.py | 219 ++++-------------- 15 files changed, 128 insertions(+), 191 deletions(-) create mode 100644 src/teleop/teleop/config/joy.yaml diff --git a/.gitignore b/.gitignore index 86668ec0..94a1cda8 100644 --- a/.gitignore +++ b/.gitignore @@ -178,6 +178,10 @@ cython_debug/ # Prerequisites *.d + +.kiro + + # Compiled Object files *.slo *.lo @@ -207,4 +211,5 @@ cython_debug/ *.app .DS_Store -*.mcap \ No newline at end of file +*.mcap + diff --git a/src/subsystems/arm/arm_bringup/config/joystick.yaml b/src/subsystems/arm/arm_bringup/config/joystick.yaml index 528cb389..7f070545 100644 --- a/src/subsystems/arm/arm_bringup/config/joystick.yaml +++ b/src/subsystems/arm/arm_bringup/config/joystick.yaml @@ -1,7 +1,6 @@ /**: ros__parameters: - # 0: PS4 1: Thrustmaster - joystick_type: 0 + joystick_type: "xbox" # can ids base_yaw_id: 5 diff --git a/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py b/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py index 821ef2a5..ed21b7b5 100644 --- a/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py +++ b/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( diff --git a/src/subsystems/arm/arm_controllers/config/manual_arm_cylindrical_controller.yaml b/src/subsystems/arm/arm_controllers/config/manual_arm_cylindrical_controller.yaml index e0ceb71f..cf3dd667 100644 --- a/src/subsystems/arm/arm_controllers/config/manual_arm_cylindrical_controller.yaml +++ b/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/xbox", + description: "Topic name for joystick input (sensor_msgs/Joy).", + read_only: true, + validation: { + not_empty<>: null, + } + } joint_lengths: { type: double_array, default_value: [], diff --git a/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml b/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml index 03f73d72..8c8c8d72 100644 --- a/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml +++ b/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/xbox", + description: "Topic name for joystick input (sensor_msgs/Joy).", + read_only: true, + validation: { + not_empty<>: null, + } + } joint_max_velocities: { type: double_array, default_value: [], diff --git a/src/subsystems/arm/arm_controllers/src/manual_arm_cylindrical_controller.cpp b/src/subsystems/arm/arm_controllers/src/manual_arm_cylindrical_controller.cpp index 8afa579b..66a32d20 100644 --- a/src/subsystems/arm/arm_controllers/src/manual_arm_cylindrical_controller.cpp +++ b/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( - "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((*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 diff --git a/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp b/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp index a81e4c97..a3a3b5d6 100644 --- a/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp +++ b/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp @@ -90,7 +90,7 @@ controller_interface::CallbackReturn ManualArmJointByJointController::on_configu // Reference Subscriber ref_subscriber_ = get_node()->create_subscription( - "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 diff --git a/src/subsystems/drive/drive_bringup/config/joystick.yaml b/src/subsystems/drive/drive_bringup/config/joystick.yaml index 75451986..aeaa87c8 100644 --- a/src/subsystems/drive/drive_bringup/config/joystick.yaml +++ b/src/subsystems/drive/drive_bringup/config/joystick.yaml @@ -1,4 +1,3 @@ /**: ros__parameters: - # 0: PS4 1: Thrustmaster - joystick_type: 1 \ No newline at end of file + joystick_type: "thrustmaster t.16000m" \ No newline at end of file diff --git a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py index b5a1eeb3..da755bbf 100644 --- a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py +++ b/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,15 +169,18 @@ 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], + parameters=[joystick_config, {'subsystem': 'drive'}], remappings=[ - ('controller_input', 'joy'), - ('/controller_input', '/joy'), + (f'controller_input/{joystick_type}', 'joy'), ], ) diff --git a/src/subsystems/science/science_bringup/config/joystick.yaml b/src/subsystems/science/science_bringup/config/joystick.yaml index df9a1073..1de9d509 100644 --- a/src/subsystems/science/science_bringup/config/joystick.yaml +++ b/src/subsystems/science/science_bringup/config/joystick.yaml @@ -1,4 +1,3 @@ /**: ros__parameters: - # 0: PS4 1: Thrustmaster - joystick_type: 0 + joystick_type: "xbox" diff --git a/src/subsystems/science/science_bringup/launch/athena_science.launch.py b/src/subsystems/science/science_bringup/launch/athena_science.launch.py index 32ea8109..ee59ff6f 100644 --- a/src/subsystems/science/science_bringup/launch/athena_science.launch.py +++ b/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( diff --git a/src/subsystems/science/science_controllers/src/science_controller.cpp b/src/subsystems/science/science_controllers/src/science_controller.cpp index ebbaea9e..3478fbe7 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.cpp +++ b/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( - "/science_manual", subscribers_qos, + params_.joystick_topic, subscribers_qos, std::bind(&ScienceManual::reference_callback, this, std::placeholders::_1)); std::shared_ptr msg = std::make_shared(); @@ -155,7 +155,7 @@ controller_interface::CallbackReturn ScienceManual::on_configure( // State publisher s_publisher_ = get_node()->create_publisher( - "/science_manual/state", rclcpp::QoS(rclcpp::KeepLast(1))); + params_.joystick_topic + "/state", rclcpp::QoS(rclcpp::KeepLast(1))); state_publisher_ = std::make_unique(s_publisher_); if (state_publisher_ && state_publisher_->trylock()) { diff --git a/src/subsystems/science/science_controllers/src/science_controller.yaml b/src/subsystems/science/science_controllers/src/science_controller.yaml index 7371edf2..6602b7c2 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.yaml +++ b/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: [] diff --git a/src/teleop/teleop/config/joy.yaml b/src/teleop/teleop/config/joy.yaml new file mode 100644 index 00000000..0d96b137 --- /dev/null +++ b/src/teleop/teleop/config/joy.yaml @@ -0,0 +1,27 @@ +/**: + ros__parameters: + controllers: + xbox: + axes: + - { name: "left_stick_lr", axis: 0, invert: false, trigger: false } + - { name: "left_stick_ud", axis: 1, invert: false, 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: false, trigger: false } + - { name: "right_trigger", axis: 5, invert: false, trigger: true } + thrustmaster t.16000m: + 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: true, trigger: false } + - { name: "hat_switch_y", axis: 4, invert: true, trigger: false } + - { name: "hat_switch_x", axis: 5, invert: false, trigger: false } + + subsystems: + arm: + output_slots: [left_stick_lr, left_stick_ud, right_stick_lr, right_stick_ud, left_trigger, right_trigger] + science: + output_slots: [left_stick_lr, left_stick_ud, right_stick_lr, right_stick_ud, left_trigger, right_trigger] + drive: + output_slots: [joystick_twist, joystick_x, joystick_y, throttle_level, hat_switch_y, hat_switch_x] diff --git a/src/teleop/teleop/teleop/joystick_publisher.py b/src/teleop/teleop/teleop/joystick_publisher.py index 9330549d..6dbe8e8c 100755 --- a/src/teleop/teleop/teleop/joystick_publisher.py +++ b/src/teleop/teleop/teleop/joystick_publisher.py @@ -1,10 +1,13 @@ #!/usr/bin/env python3 import os import time -import pprint import pygame import numpy as np +import yaml +from ament_index_python.packages import get_package_share_directory + + import rclpy from rclpy.node import Node @@ -15,24 +18,37 @@ class JoystickPublisher(Node): def __init__(self): super().__init__('joystick') - self.publisher_ = self.create_publisher(Joy, 'controller_input', 10) timer_period = 0.1 # seconds - self.timer = self.create_timer(timer_period, self.controller_inputs) + self.timer = self.create_timer(timer_period, self.controller_inputs) # Parameters self.declare_parameters( namespace='', parameters=[ - ('joystick_type', rclpy.Parameter.Type.INTEGER), + ('joystick_type', rclpy.Parameter.Type.STRING), + ('subsystem' , rclpy.Parameter.Type.STRING), ] ) - - self.possible_joysticks = ["sony computer entertainment wireless controller", "thrustmaster t.16000m"] - - # 0: PS4 1: Thrustmaster self.joystick_type = self.get_parameter('joystick_type').value + self.subsystem = self.get_parameter('subsystem').value + self.publisher_ = self.create_publisher(Joy, f'controller_input/{self.joystick_type}', 10) + self.get_logger().info(f"Joystick type: {self.joystick_type}") + self.get_logger().info(f"Subsystem: {self.subsystem}") + + joy_yaml_path = os.path.join(get_package_share_directory('teleop'), 'config', 'joy.yaml') + with open(joy_yaml_path, 'r') as f: + config = yaml.safe_load(f) + + + axes_list = config['ros__parameters']['controllers'][self.joystick_type]['axes'] + + self._axes_config = {e['name']: e for e in axes_list} + slots = config['ros__parameters']['subsystems'][self.subsystem]['output_slots'] + self._slot_params = [self._axes_config[name] for name in slots] + + # Detect Jetson platform self.is_jetson = False @@ -61,7 +77,7 @@ def __init__(self): for i in range(joysticks): js = pygame.joystick.Joystick(i) name = js.get_name().lower() - if(name == self.possible_joysticks[self.joystick_type]): + if(name == self.joystick_type): self.joystick_index = i break @@ -93,9 +109,21 @@ def __init__(self): for i in range(self.controller.get_numhats()): self.hat_data[i] = (0, 0) - self.previous_axes = np.zeros(6) + self.previous_axes = np.zeros(len(self._slot_params)) self.previous_buttons = np.zeros(13) + def _normalize(self, raw, invert, trigger): + if abs(raw) < self.activation: + val = 0.0 + else: + val = (abs(raw) - self.activation) / (1.0 - self.activation) + val = val if raw > 0 else -val + if trigger: + val = (val + 1.0) / 2.0 + if invert: + val = -val + return val + def controller_inputs(self): joystick_vels = self.previous_axes @@ -110,165 +138,18 @@ def controller_inputs(self): elif event.type == pygame.JOYHATMOTION: self.hat_data[event.hat] = event.value - if(self.joystick_type == 0): - - """ - PS4 Controller: - Joystick Velocities - [0] = l/r left joystick - [1] = u/d left joystick - [2] = l/r right joystick - [3] = u/d right joystick - [4] = left trigger - [5] = right trigger - - Button Activations - [0] = x - [1] = circle - [2] = triangle - [3] = square - [4] = left bumper - [5] = right bumper - [6] = left trigger - [7] = right trigger - [8] = share - [9] = options - [10] = playstation button - [11] = left joystick button - [12] = right joystick button - [13] = up arrow - [14] = down arrow - [15] = left arrow - [16] = right arrow - """ - - # Left stick: left and right - if(self.axis_data.get(0) < -self.activation): - joystick_vels[0] = ((self.axis_data[0] + self.activation) / (1 - self.activation)) # normalizes the 0.25 to 1 range to a 0 - 1 range and then mult by max vel - elif(self.axis_data.get(0) > self.activation): - joystick_vels[0] = ((self.axis_data[0] - self.activation) / (1 - self.activation)) - else: - joystick_vels[0] = 0 - - # Left stick: up and down - axis_1_multiplier = 1 if self.is_jetson else -1 - if(self.axis_data.get(1) < -self.activation): - joystick_vels[1] = axis_1_multiplier * ((self.axis_data[1] + self.activation) / (1 -self.activation)) # normalizes the 0.25 to 1 range to a 0 - 1 range and then mult by max vel - elif(self.axis_data.get(1) >self.activation): - joystick_vels[1] = axis_1_multiplier * ((self.axis_data[1] - self.activation) / (1 -self.activation)) - else: - joystick_vels[1] = 0 - - # Right stick: left and right - if(self.axis_data.get(3) < -self.activation): - joystick_vels[2] = ((self.axis_data[3] + self.activation) / (1 -self.activation)) # normalizes the 0.25 to 1 range to a 0 - 1 range and then mult by max vel - elif(self.axis_data.get(3) >self.activation): - joystick_vels[2] = ((self.axis_data[3] - self.activation) / (1 -self.activation)) - else: - joystick_vels[2] = 0 - - # Right stick: up and down - axis_index = 5 if self.is_jetson else 4 - if(self.axis_data.get(axis_index) < -self.activation): - joystick_vels[3] = -((self.axis_data[axis_index] + self.activation) / (1 -self.activation)) # normalizes the 0.25 to 1 range to a 0 - 1 range and then mult by max vel - elif(self.axis_data.get(axis_index) > self.activation): - joystick_vels[3] = -((self.axis_data[axis_index] - self.activation) / (1 -self.activation)) - else: - joystick_vels[3] = 0 - - # Left Trigger - if(self.axis_data.get(2) > 0): - joystick_vels[4] = self.axis_data[2] - else: - joystick_vels[4] = 0 - - # Right Trigger - if(self.axis_data.get(5) > 0): - joystick_vels[5] = self.axis_data[5] - else: - joystick_vels[5] = 0 - - # Buttons - for i in range(13): - button_activations[i] = self.button_data[i] - - elif(self.joystick_type == 1): - - """ - Thrustmaster: - Joystick Velocities - [0] = rotate - [1] = forward/backwards - [2] = left/right - [3] = slider - [4] = empty - [5] = empty - - Button Activations - [0] = trigger - [1] = bottom on stick - [2] = left on stick - [3] = right on stick - [4] = left side - top left - [5] = left side - top middle - [6] = left side - top right - [7] = left side - bottom right - [8] = left side - bottom middle - [9] = left side - bottom left - [10] = right side - top right - [11] = right side - top middle - [12] = right side - top left - [13] = right side - bottom left - [14] = right side - bottom middle - [15] = right side - bottom right - [16] = empty - """ - # self.get_logger().info(str(self.joystick_type)) - # print(self.axis_data.get(2)) - - # Rotate - - joystick_vels[0] = self.axis_data.get(2) - - # if(self.axis_data.get(2) < -self.activation): - # #joystick_vels[0] = ((self.axis_data[0] + self.activation) / (1 - self.activation)) # normalizes the 0.25 to 1 range to a 0 - 1 range and then mult by max vel - # joystick_vels[0] = -self.axis_data[0] - # elif(self.axis_data.get(2) > self.activation): - # #joystick_vels[0] = ((self.axis_data[0] - self.activation) / (1 - self.activation)) - # joystick_vels[0] = self.axis_data[0] - - # else: - # joystick_vels[0] = 0 - - # Up and Down - if(self.axis_data.get(1) < -self.activation): - joystick_vels[1] = -((self.axis_data[1] + self.activation) / (1 -self.activation)) # normalizes the 0.25 to 1 range to a 0 - 1 range and then mult by max vel - elif(self.axis_data.get(1) >self.activation): - joystick_vels[1] = -((self.axis_data[1] - self.activation) / (1 -self.activation)) - else: - joystick_vels[1] = 0 - - # Left/Right - if(self.axis_data.get(0) < -self.activation): - joystick_vels[2] = ((self.axis_data[3] + self.activation) / (1 -self.activation)) # normalizes the 0.25 to 1 range to a 0 - 1 range and then mult by max vel - elif(self.axis_data.get(0) >self.activation): - joystick_vels[2] = ((self.axis_data[3] - self.activation) / (1 -self.activation)) - else: - joystick_vels[2] = 0 - - # Slider - if(self.axis_data.get(3) < -self.activation): - joystick_vels[3] = -((self.axis_data[4] + self.activation) / (1 -self.activation)) # normalizes the 0.25 to 1 range to a 0 - 1 range and then mult by max vel - elif(self.axis_data.get(3) > self.activation): - joystick_vels[3] = -((self.axis_data[4] - self.activation) / (1 -self.activation)) - else: - joystick_vels[3] = 0 - - # Buttons - for i in range(13): - button_activations[i] = self.button_data[i] - else: - print('calibrate:', pprint.pprint(self.axis_data)) + + #axes normalization + for i, slot in enumerate(self._slot_params): + joystick_vels[i] = self._normalize( + self.axis_data.get(slot['axis'], 0.0), + slot['invert'], + slot['trigger'] + ) + + # Buttons + for i in range(13): + button_activations[i] = self.button_data[i] # Save current numpy array for joystick and buttons self.previous_axes = joystick_vels From 65f82a24c43cba49996f6973f303d7bee2f86a21 Mon Sep 17 00:00:00 2001 From: Ryan George Date: Wed, 22 Apr 2026 16:44:00 -0400 Subject: [PATCH 2/5] Fix joystick publisher startup and runtime bugs - Add joy.yaml to setup.py install rules so it deploys to share/ - Fix yaml parsing to include /** top-level key - Add JOYSTICK_NAMES dict to map config keys to pygame device names - Fix hardcoded 13-button assumption causing KeyError on Xbox controller - Fix joystick_type string in drive config (space broke ROS2 remap parsing) --- .../drive/drive_bringup/config/joystick.yaml | 2 +- src/teleop/teleop/config/joy.yaml | 8 ++++---- src/teleop/teleop/setup.py | 1 + src/teleop/teleop/teleop/joystick_publisher.py | 18 ++++++++++++------ 4 files changed, 18 insertions(+), 11 deletions(-) diff --git a/src/subsystems/drive/drive_bringup/config/joystick.yaml b/src/subsystems/drive/drive_bringup/config/joystick.yaml index aeaa87c8..a51807a1 100644 --- a/src/subsystems/drive/drive_bringup/config/joystick.yaml +++ b/src/subsystems/drive/drive_bringup/config/joystick.yaml @@ -1,3 +1,3 @@ /**: ros__parameters: - joystick_type: "thrustmaster t.16000m" \ No newline at end of file + joystick_type: "thrustmaster" \ No newline at end of file diff --git a/src/teleop/teleop/config/joy.yaml b/src/teleop/teleop/config/joy.yaml index 0d96b137..616fcf33 100644 --- a/src/teleop/teleop/config/joy.yaml +++ b/src/teleop/teleop/config/joy.yaml @@ -4,17 +4,17 @@ xbox: axes: - { name: "left_stick_lr", axis: 0, invert: false, trigger: false } - - { name: "left_stick_ud", axis: 1, 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: false, trigger: false } + - { name: "right_stick_ud", axis: 4, invert: true, trigger: false } - { name: "right_trigger", axis: 5, invert: false, trigger: true } - thrustmaster t.16000m: + 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: true, trigger: false } + - { name: "throttle_level", axis: 3, invert: true, trigger: true } - { name: "hat_switch_y", axis: 4, invert: true, trigger: false } - { name: "hat_switch_x", axis: 5, invert: false, trigger: false } diff --git a/src/teleop/teleop/setup.py b/src/teleop/teleop/setup.py index a8fae374..4991bb4c 100644 --- a/src/teleop/teleop/setup.py +++ b/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, diff --git a/src/teleop/teleop/teleop/joystick_publisher.py b/src/teleop/teleop/teleop/joystick_publisher.py index 6dbe8e8c..e05daa64 100755 --- a/src/teleop/teleop/teleop/joystick_publisher.py +++ b/src/teleop/teleop/teleop/joystick_publisher.py @@ -42,10 +42,10 @@ def __init__(self): config = yaml.safe_load(f) - axes_list = config['ros__parameters']['controllers'][self.joystick_type]['axes'] + axes_list = config['/**']['ros__parameters']['controllers'][self.joystick_type]['axes'] self._axes_config = {e['name']: e for e in axes_list} - slots = config['ros__parameters']['subsystems'][self.subsystem]['output_slots'] + slots = config['/**']['ros__parameters']['subsystems'][self.subsystem]['output_slots'] self._slot_params = [self._axes_config[name] for name in slots] @@ -67,17 +67,23 @@ def __init__(self): self.button_data = None self.hat_data = None self.activation = 0.08 + JOYSTICK_NAMES = { + "thrustmaster": "thrustmaster t.16000m", + "xbox": "xbox series x controller", + } + target_name = JOYSTICK_NAMES[self.joystick_type] + self.joystick_index = None # Pygame Controller pygame.init() - pygame.joystick.init() + pygame.joystick.init() joysticks = pygame.joystick.get_count() for i in range(joysticks): js = pygame.joystick.Joystick(i) name = js.get_name().lower() - if(name == self.joystick_type): + if(name == target_name): self.joystick_index = i break @@ -110,7 +116,7 @@ def __init__(self): self.hat_data[i] = (0, 0) self.previous_axes = np.zeros(len(self._slot_params)) - self.previous_buttons = np.zeros(13) + self.previous_buttons = np.zeros(len(self.button_data)) def _normalize(self, raw, invert, trigger): if abs(raw) < self.activation: @@ -148,7 +154,7 @@ def controller_inputs(self): ) # Buttons - for i in range(13): + for i in self.button_data: button_activations[i] = self.button_data[i] # Save current numpy array for joystick and buttons From 2f91237cc218f97279ac529b06350224417555ba Mon Sep 17 00:00:00 2001 From: Ryan George Date: Sun, 26 Apr 2026 16:13:08 -0400 Subject: [PATCH 3/5] integrated airbus into arm setup --- .gitignore | 7 +++---- src/subsystems/arm/arm_bringup/config/joystick.yaml | 2 +- .../config/manual_arm_cylindrical_controller.yaml | 2 +- .../config/manual_arm_joint_by_joint_controller.yaml | 2 +- .../manual_arm_cylindrical_controller.hpp | 2 +- .../manual_arm_joint_by_joint_controller.hpp | 2 +- src/teleop/teleop/config/joy.yaml | 10 +++++++++- src/teleop/teleop/teleop/joystick_publisher.py | 1 + 8 files changed, 18 insertions(+), 10 deletions(-) diff --git a/.gitignore b/.gitignore index 94a1cda8..99853b58 100644 --- a/.gitignore +++ b/.gitignore @@ -178,9 +178,9 @@ cython_debug/ # Prerequisites *.d - +# AI .kiro - +.claude # Compiled Object files *.slo @@ -211,5 +211,4 @@ cython_debug/ *.app .DS_Store -*.mcap - +*.mcap \ No newline at end of file diff --git a/src/subsystems/arm/arm_bringup/config/joystick.yaml b/src/subsystems/arm/arm_bringup/config/joystick.yaml index 7f070545..12fa9fed 100644 --- a/src/subsystems/arm/arm_bringup/config/joystick.yaml +++ b/src/subsystems/arm/arm_bringup/config/joystick.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - joystick_type: "xbox" + joystick_type: "airbus" # can ids base_yaw_id: 5 diff --git a/src/subsystems/arm/arm_controllers/config/manual_arm_cylindrical_controller.yaml b/src/subsystems/arm/arm_controllers/config/manual_arm_cylindrical_controller.yaml index cf3dd667..6475ceef 100644 --- a/src/subsystems/arm/arm_controllers/config/manual_arm_cylindrical_controller.yaml +++ b/src/subsystems/arm/arm_controllers/config/manual_arm_cylindrical_controller.yaml @@ -50,7 +50,7 @@ manual_arm_cylindrical_controller: } joystick_topic: { type: string, - default_value: "controller_input/xbox", + default_value: "controller_input/airbus", description: "Topic name for joystick input (sensor_msgs/Joy).", read_only: true, validation: { diff --git a/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml b/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml index 8c8c8d72..1a92c63c 100644 --- a/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml +++ b/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml @@ -60,7 +60,7 @@ manual_arm_joint_by_joint_controller: # } joystick_topic: { type: string, - default_value: "controller_input/xbox", + default_value: "controller_input/airbus", description: "Topic name for joystick input (sensor_msgs/Joy).", read_only: true, validation: { diff --git a/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_cylindrical_controller.hpp b/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_cylindrical_controller.hpp index b07965db..35ab0d94 100644 --- a/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_cylindrical_controller.hpp +++ b/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; diff --git a/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp b/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp index bb743e92..6fef2db4 100644 --- a/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp +++ b/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 diff --git a/src/teleop/teleop/config/joy.yaml b/src/teleop/teleop/config/joy.yaml index 616fcf33..60ac894b 100644 --- a/src/teleop/teleop/config/joy.yaml +++ b/src/teleop/teleop/config/joy.yaml @@ -17,10 +17,18 @@ - { name: "throttle_level", axis: 3, invert: true, trigger: 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 } + - { name: "hat_switch_y", axis: 4, invert: false, trigger: false } + - { name: "hat_switch_x", axis: 5, invert: false, trigger: false } subsystems: arm: - output_slots: [left_stick_lr, left_stick_ud, right_stick_lr, right_stick_ud, left_trigger, right_trigger] + output_slots: [joystick_y, joystick_x, joystick_twist, throttle_level, hat_switch_y, hat_switch_x] science: output_slots: [left_stick_lr, left_stick_ud, right_stick_lr, right_stick_ud, left_trigger, right_trigger] drive: diff --git a/src/teleop/teleop/teleop/joystick_publisher.py b/src/teleop/teleop/teleop/joystick_publisher.py index e05daa64..67278d7d 100755 --- a/src/teleop/teleop/teleop/joystick_publisher.py +++ b/src/teleop/teleop/teleop/joystick_publisher.py @@ -70,6 +70,7 @@ def __init__(self): JOYSTICK_NAMES = { "thrustmaster": "thrustmaster t.16000m", "xbox": "xbox series x controller", + "aibus": "t.a320 pilot" } target_name = JOYSTICK_NAMES[self.joystick_type] From 54804aed34523a03f4a4f643b9b293e61075ba96 Mon Sep 17 00:00:00 2001 From: Ryan George Date: Sun, 26 Apr 2026 23:07:57 -0400 Subject: [PATCH 4/5] Added Airbus, xbox name, and resolved issues -Added Airbus TCA Sidestick profile and migrated arm to airbus controller -Added range_inverted param for throttle axes on airbus and thrustmaster -Wrote controller documentation in the joy.yaml file --- src/teleop/teleop/config/joy.yaml | 38 ++++++++++++++++++- .../teleop/teleop/joystick_publisher.py | 13 ++++--- 2 files changed, 43 insertions(+), 8 deletions(-) diff --git a/src/teleop/teleop/config/joy.yaml b/src/teleop/teleop/config/joy.yaml index 60ac894b..0e979881 100644 --- a/src/teleop/teleop/config/joy.yaml +++ b/src/teleop/teleop/config/joy.yaml @@ -14,7 +14,7 @@ - { 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: true, trigger: true } + - { 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: @@ -22,14 +22,48 @@ - { 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 } + - { 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 → gripper open (dead slot — always 0, gripper uses buttons[4]) + # [5] hat_switch_x → gripper close (dead slot — always 0, gripper uses buttons[5]) + + # * modifier = buttons[1]; buttons[3] = actuator trigger 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 → linear actuator left / sampler lift + # [2] left_trigger → unused + # [3] right_stick_lr → linear actuator right / sampler lift + # [4] right_stick_ud → auger spin (reverse via buttons[4]) + # [5] right_trigger → scoop (reverse via buttons[5]) + + # buttons[0] = servo scoop B, buttons[2] = pump, buttons[3] = servo scoop A, buttons[12] = lift 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 → unused by twist joy + # [2] joystick_y → unused by twist joy + # [3] throttle_level → forward/reverse speed (linear velocity) + # [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] diff --git a/src/teleop/teleop/teleop/joystick_publisher.py b/src/teleop/teleop/teleop/joystick_publisher.py index 67278d7d..ac0fc12e 100755 --- a/src/teleop/teleop/teleop/joystick_publisher.py +++ b/src/teleop/teleop/teleop/joystick_publisher.py @@ -67,10 +67,10 @@ def __init__(self): self.button_data = None self.hat_data = None self.activation = 0.08 - JOYSTICK_NAMES = { + JOYSTICK_NAMES = {s "thrustmaster": "thrustmaster t.16000m", - "xbox": "xbox series x controller", - "aibus": "t.a320 pilot" + "xbox": "xbox 360 controller", + "airbus": "thrustmaster t.a320 copilot" } target_name = JOYSTICK_NAMES[self.joystick_type] @@ -119,14 +119,14 @@ def __init__(self): self.previous_axes = np.zeros(len(self._slot_params)) self.previous_buttons = np.zeros(len(self.button_data)) - def _normalize(self, raw, invert, trigger): + def _normalize(self, raw, invert, trigger, range_inverted=False): if abs(raw) < self.activation: val = 0.0 else: val = (abs(raw) - self.activation) / (1.0 - self.activation) val = val if raw > 0 else -val if trigger: - val = (val + 1.0) / 2.0 + val = (1.0 - val) / 2.0 if range_inverted else (val + 1.0) / 2.0 if invert: val = -val return val @@ -151,7 +151,8 @@ def controller_inputs(self): joystick_vels[i] = self._normalize( self.axis_data.get(slot['axis'], 0.0), slot['invert'], - slot['trigger'] + slot['trigger'], + slot.get('range_inverted', False) ) # Buttons From d0bfcaab2a1fd626a64bd72ca1959273e32d7e91 Mon Sep 17 00:00:00 2001 From: Ryan George Date: Mon, 27 Apr 2026 19:48:30 -0400 Subject: [PATCH 5/5] =?UTF-8?q?Fixed=20joy=20topic=20remapping=20and=20doc?= =?UTF-8?q?umentation=20-=20Moved=20joy=20remap=20to=20teleop=5Ftwist=5Fjo?= =?UTF-8?q?y=20node=20(correct=20direction)=20-=20Fixed=20science=20lift?= =?UTF-8?q?=20button=20index:=20buttons[12]=20=E2=86=92=20buttons[1]=20(Xb?= =?UTF-8?q?ox=20has=20no=20button=2012)=20-=20Updated=20joy.yaml=20comment?= =?UTF-8?q?s=20to=20clarify=20actual=20axis/button=20assignments?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/athena_drive.launch.py | 4 +-- .../src/science_controller.cpp | 4 +-- src/teleop/teleop/config/joy.yaml | 33 ++++++++++++------- 3 files changed, 24 insertions(+), 17 deletions(-) diff --git a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py index da755bbf..592d3edb 100644 --- a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py +++ b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py @@ -179,9 +179,6 @@ def launch_setup(context, *args, **kwargs): name='joystick', output='screen', parameters=[joystick_config, {'subsystem': 'drive'}], - remappings=[ - (f'controller_input/{joystick_type}', 'joy'), - ], ) teleop_twist_joy = Node( @@ -191,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'), ], ) diff --git a/src/subsystems/science/science_controllers/src/science_controller.cpp b/src/subsystems/science/science_controllers/src/science_controller.cpp index 3478fbe7..375d05f1 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.cpp +++ b/src/subsystems/science/science_controllers/src/science_controller.cpp @@ -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; } diff --git a/src/teleop/teleop/config/joy.yaml b/src/teleop/teleop/config/joy.yaml index 0e979881..2b5be927 100644 --- a/src/teleop/teleop/config/joy.yaml +++ b/src/teleop/teleop/config/joy.yaml @@ -30,15 +30,18 @@ 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 + # 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 → gripper open (dead slot — always 0, gripper uses buttons[4]) - # [5] hat_switch_x → gripper close (dead slot — always 0, gripper uses buttons[5]) + # [4] hat_switch_y → unused (dead slot) + # [5] hat_switch_x → unusued (dead slot) - # * modifier = buttons[1]; buttons[3] = actuator trigger + # 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: @@ -46,13 +49,19 @@ # Slot → axis name → use # [0] left_stick_lr → auger lift - # [1] left_stick_ud → linear actuator left / sampler lift - # [2] left_trigger → unused - # [3] right_stick_lr → linear actuator right / sampler lift - # [4] right_stick_ud → auger spin (reverse via buttons[4]) + # [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]) - # buttons[0] = servo scoop B, buttons[2] = pump, buttons[3] = servo scoop A, buttons[12] = lift + # 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] @@ -61,9 +70,9 @@ # 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 → unused by twist joy + # [1] joystick_x → forward/reverse speed (linear velocity) # [2] joystick_y → unused by twist joy - # [3] throttle_level → forward/reverse speed (linear velocity) + # [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]