From 6a5ae35c4f39aed0e6f3169c571c9b2caf0fcb9a Mon Sep 17 00:00:00 2001 From: Lucas Mauser <163037430+Lmauser747@users.noreply.github.com> Date: Wed, 2 Oct 2024 20:46:44 -0400 Subject: [PATCH 01/16] Add files via upload test code to map controller, delete later --- controller_mapping.py | 59 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 59 insertions(+) create mode 100644 controller_mapping.py diff --git a/controller_mapping.py b/controller_mapping.py new file mode 100644 index 0000000..a776b50 --- /dev/null +++ b/controller_mapping.py @@ -0,0 +1,59 @@ +import pygame +import sys +import time +#axis 0 side to side +#axis 1 forward-backward +#axis 2 rotation + + +def init_pygame(): + '''Initializes pygame and the joystick''' + global controller + pygame.init() + pygame.joystick.init() + controller = pygame.joystick.Joystick(0) + +def check_axis(): + print(controller.get_axis(0)) + print(controller.get_axis(1)) + print(controller.get_axis(2)) + +def check_buttons(): + num_of_buttons = controller.get_numbuttons() + for i in range(0, num_of_buttons): + print(controller.get_button(i)) +def check_button(): + print(controller.get_numbuttons()) + +def check_joystick(): + pygame.init() + pygame.joystick.init() + + joystick_count = pygame.joystick.get_count() + + if joystick_count == 0: + print("no joystick") + + pygame.quit() + sys.exit() + else: + joystick = pygame.joystick.Joystick(0) + joystick.init() + print(f"Joystick detected: {joystick.get_name()}") + + +if __name__ == '__main__': + init_pygame() + i = 0 + while i < 30: + + check_axis() + pygame.time.wait(1000) + i = i + 1 + + pygame.quit() + sys.exit() + + + + From 93a742e8455d68aaf3801fd8333da2c1e99a98d8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?JoLucas=C2=8D=20Mauser?= Date: Thu, 3 Oct 2024 14:57:33 -0400 Subject: [PATCH 02/16] rewrote so that controller state updates. also changed mapping from lists to dicts --- controller_mapping.py | 110 +++++++++++++++++++++++++++++------------- 1 file changed, 76 insertions(+), 34 deletions(-) diff --git a/controller_mapping.py b/controller_mapping.py index a776b50..c8327b9 100644 --- a/controller_mapping.py +++ b/controller_mapping.py @@ -6,6 +6,45 @@ #axis 2 rotation +JOY_AXIS = { + 'X':1, 'Y':0, 'Z':2 +} + +JOY_BUTTON = { + 'trigger':0, '2':1, '3':2, '4':3, '5':4, '6':5, '7':6, '8':7, '9':8, '10':9, '11':10, '12':11 +} + +JOY_BUTTON_KEY = { + 0:'trigger', 1:'2', 2:'3', 3:'4', 4:'5', 5:'6', 6:'7', 7:'8', 8:'9', 9:'10', 10:'11', 11:'12' +} + + +axis_state = { + 'X':0.0, + 'Y':0.0, + 'Z':0.0 +} + + +button_state = { + 'trigger':0, + '2':0, + '3':0, + '4':0, + '5':0, + '6':0, + '7':0, + '8':0, + '9':0, + '10':0, + '11':0, + '12':0 +} +no_controller = False +wtf = False + + + def init_pygame(): '''Initializes pygame and the joystick''' global controller @@ -13,47 +52,50 @@ def init_pygame(): pygame.joystick.init() controller = pygame.joystick.Joystick(0) -def check_axis(): - print(controller.get_axis(0)) - print(controller.get_axis(1)) - print(controller.get_axis(2)) -def check_buttons(): - num_of_buttons = controller.get_numbuttons() - for i in range(0, num_of_buttons): - print(controller.get_button(i)) -def check_button(): - print(controller.get_numbuttons()) +def check_event(): + for event in pygame.event.get(): + wtf = False + if event.type == pygame.JOYAXISMOTION: + axis_state[event.axis] = event.value + elif event.type == pygame.JOYBUTTONDOWN: + button_state[event.button] = 1 + elif event.type == pygame.JOYBUTTONUP: + button_state[event.button] = 0 + elif event.type == pygame.JOYDEVICEREMOVED: + no_controller = True + pygame.quit() + sys.exit(0) + state_logger() + else: + wtf = True + + +def state_logger(): + i = 0 + for axis in axis_state: + print(f"Axis{i} value: {axis}", end = '') + i = i + 1 + i = 0 + print() + for button in button_state: + print(f"Button{i} value: {button}", end = '') + i = i + 1 + print() + if no_controller == True : + print("No controller connected") + if wtf == True : + print("Unkown event") -def check_joystick(): - pygame.init() - pygame.joystick.init() - joystick_count = pygame.joystick.get_count() - if joystick_count == 0: - print("no joystick") - pygame.quit() - sys.exit() - else: - joystick = pygame.joystick.Joystick(0) - joystick.init() - print(f"Joystick detected: {joystick.get_name()}") if __name__ == '__main__': init_pygame() - i = 0 - while i < 30: - - check_axis() + while(True): + check_event() + state_logger() pygame.time.wait(1000) - i = i + 1 - - pygame.quit() - sys.exit() - - - - + \ No newline at end of file From 392cd28e224e69277c5f0734d81e319a663fb8b8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?JoLucas=C2=8D=20Mauser?= Date: Mon, 7 Oct 2024 18:04:51 -0400 Subject: [PATCH 03/16] added comments and did some chatgpt debugging --- controller_mapping.py | 43 +++++++++++++++++++++++++++---------------- 1 file changed, 27 insertions(+), 16 deletions(-) diff --git a/controller_mapping.py b/controller_mapping.py index c8327b9..b9f5377 100644 --- a/controller_mapping.py +++ b/controller_mapping.py @@ -1,31 +1,40 @@ import pygame import sys import time -#axis 0 side to side -#axis 1 forward-backward -#axis 2 rotation +''' +TODO: +-test and debug code with actual joystick +-map all of the buttons to their pygame id +-find out how to access the thing on top of the joystick +-move mappings to config file and code to sender file +-delete this file after everything is done +''' + +'''x-axis is foward and backward, y-axis is side to side, z-axis is twisting''' JOY_AXIS = { 'X':1, 'Y':0, 'Z':2 } +'''mappping from button identifier to pygame id''' JOY_BUTTON = { 'trigger':0, '2':1, '3':2, '4':3, '5':4, '6':5, '7':6, '8':7, '9':8, '10':9, '11':10, '12':11 } +'''mapping from pygame id button identifier i don't know why we need this''' JOY_BUTTON_KEY = { 0:'trigger', 1:'2', 2:'3', 3:'4', 4:'5', 5:'6', 6:'7', 7:'8', 8:'9', 9:'10', 10:'11', 11:'12' } - +'''dict to store the state of the axis as floats''' axis_state = { 'X':0.0, 'Y':0.0, 'Z':0.0 } - +'''dict to store the state of the buttons as booleans''' button_state = { 'trigger':0, '2':0, @@ -40,20 +49,26 @@ '11':0, '12':0 } + no_controller = False wtf = False - def init_pygame(): '''Initializes pygame and the joystick''' global controller pygame.init() pygame.joystick.init() + '''check if controller connected''' + if(pygame.joystick.get_count() == 0): + print("No controller found") + pygame.quit() + sys.exit(0) controller = pygame.joystick.Joystick(0) - +'''get the controller events and update the various states accodingly''' def check_event(): + global no_controller, wtf for event in pygame.event.get(): wtf = False if event.type == pygame.JOYAXISMOTION: @@ -70,17 +85,13 @@ def check_event(): else: wtf = True - +'''logger to log the axis states to the command line''' def state_logger(): - i = 0 - for axis in axis_state: - print(f"Axis{i} value: {axis}", end = '') - i = i + 1 - i = 0 + for i, axis in enumerate(axis_state): + print(f"Axis{i} value: {axis_state[axis]}", end = '') print() - for button in button_state: - print(f"Button{i} value: {button}", end = '') - i = i + 1 + for i, button in enumerate(button_state): + print(f"Button{i} value: {button_state[button]}", end = '') print() if no_controller == True : print("No controller connected") From fc2cdce79beef25099cd0939f096b3e43aa63ffe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?JoLucas=C2=8D=20Mauser?= Date: Wed, 9 Oct 2024 20:24:21 -0400 Subject: [PATCH 04/16] finished controller mapping. copied mapping to config.py. started changing sender.py to work with new controller --- controller_mapping.py | 40 ++++++++++++--------- ros/gamepad/src/config.py | 74 +++++++++++++++++++++++---------------- ros/gamepad/src/sender.py | 67 +++++++++++------------------------ 3 files changed, 88 insertions(+), 93 deletions(-) diff --git a/controller_mapping.py b/controller_mapping.py index b9f5377..a56ff5b 100644 --- a/controller_mapping.py +++ b/controller_mapping.py @@ -5,16 +5,17 @@ ''' TODO: --test and debug code with actual joystick --map all of the buttons to their pygame id --find out how to access the thing on top of the joystick -move mappings to config file and code to sender file -delete this file after everything is done ''' '''x-axis is foward and backward, y-axis is side to side, z-axis is twisting''' JOY_AXIS = { - 'X':1, 'Y':0, 'Z':2 + 'X':1, 'Y':0, 'Z':2, 'WHEEL':3 +} + +JOY_AXIS_KEY = { + 1:'X', 0:'Y', 2:'Z', 3:'WHEEL' } '''mappping from button identifier to pygame id''' @@ -31,7 +32,8 @@ axis_state = { 'X':0.0, 'Y':0.0, - 'Z':0.0 + 'Z':0.0, + 'WHEEL':0.0 } '''dict to store the state of the buttons as booleans''' @@ -50,6 +52,11 @@ '12':0 } +hat_state = [ + 0, 0 +] + + no_controller = False wtf = False @@ -70,29 +77,28 @@ def init_pygame(): def check_event(): global no_controller, wtf for event in pygame.event.get(): - wtf = False if event.type == pygame.JOYAXISMOTION: - axis_state[event.axis] = event.value + axis_state[JOY_AXIS_KEY[event.axis]] = event.value elif event.type == pygame.JOYBUTTONDOWN: - button_state[event.button] = 1 + button_state[JOY_BUTTON_KEY[event.button]] = 1 elif event.type == pygame.JOYBUTTONUP: - button_state[event.button] = 0 + button_state[JOY_BUTTON_KEY[event.button]] = 0 + elif event.type == pygame.JOYHATMOTION: + hat_state[0] = event.value[0] + hat_state[1] = event.value[1] elif event.type == pygame.JOYDEVICEREMOVED: no_controller = True pygame.quit() sys.exit(0) state_logger() - else: - wtf = True '''logger to log the axis states to the command line''' def state_logger(): - for i, axis in enumerate(axis_state): - print(f"Axis{i} value: {axis_state[axis]}", end = '') - print() - for i, button in enumerate(button_state): - print(f"Button{i} value: {button_state[button]}", end = '') - print() + for i,j in enumerate(axis_state): + print(f"Axis {JOY_AXIS_KEY[i]} value: {axis_state[JOY_AXIS_KEY[i]]}") + for i,j in enumerate(button_state): + print(f"Button {JOY_BUTTON_KEY[i]} value: {button_state[JOY_BUTTON_KEY[i]]}") + print(f"Hat Y: {hat_state[0]}, HAT X: {hat_state[1]}") if no_controller == True : print("No controller connected") if wtf == True : diff --git a/ros/gamepad/src/config.py b/ros/gamepad/src/config.py index f307690..5008f2a 100644 --- a/ros/gamepad/src/config.py +++ b/ros/gamepad/src/config.py @@ -1,37 +1,51 @@ MIN_ABS_DIFFERENCE = 0 # New ranges for the triggers and sticks (Probably need to adjust these values) -TRIGGER_DEAD_ZONE = 0.09 -TRIGGER_RANGE = 1.0 STICK_DEAD_ZONE = 0.09 -STICK_RANGE = 1.0 +AXIS_RANGE = 1.0 -# Mapping of JoyAxisMotion events to gamepad_state keys -# The number is the axis given by pygame -JOY_AXIS = {1: 'LSY', 0: 'LSX', 4: 'RSY', 3: 'RSX', 2: 'LT', 5: 'RT'} +'''x-axis is foward and backward, y-axis is side to side, z-axis is twisting''' +JOY_AXIS = { + 'X':1, 'Y':0, 'Z':2, 'WHEEL':3 +} -# Mapping of JoyButton events to gamepad_state keys -# The number is the button given by pygame -JOY_BUTTON = {3: 'Y', 1: 'B', 0: 'A', 2: 'X', 9: 'LSZ', 10: 'RSZ', 4: 'LB', 5: 'RB', 8: 'XBOX', 6: 'START', 7: 'MENU'} -JOY_BUTTON_KEY = {'Y': 3, 'B': 1, 'A': 0, 'X': 2, 'LSZ': 9, 'RSZ': 10, 'LB': 4, 'RB': 5, 'XBOX': 8, 'START': 6, 'MENU': 7} +JOY_AXIS_KEY = { + 1:'X', 0:'Y', 2:'Z', 3:'WHEEL' +} -gamepad_state = { - "LSX": 0.0, - "LSY": 0.0, - "RSX": 0.0, - "RSY": 0.0, - "LT": -1.0, - "RT": -1.0, - "DPADX": 0, - "DPADY": 0, - "Y": 0, - "B": 0, - "A": 0, - "X": 0, - "LSZ": 0, - "RSZ": 0, - "LB": 0, - "RB": 0, - "XBOX": 0, - "START": 0, - "MENU": 0, +'''mappping from button identifier to pygame id''' +JOY_BUTTON = { + 'trigger':0, '2':1, '3':2, '4':3, '5':4, '6':5, '7':6, '8':7, '9':8, '10':9, '11':10, '12':11 } + +'''mapping from pygame id button identifier i don't know why we need this''' +JOY_BUTTON_KEY = { + 0:'trigger', 1:'2', 2:'3', 3:'4', 4:'5', 5:'6', 6:'7', 7:'8', 8:'9', 9:'10', 10:'11', 11:'12' +} + +'''dict to store the state of the axis as floats''' +axis_state = { + 'X':0.0, + 'Y':0.0, + 'Z':0.0, + 'WHEEL':0.0 +} + +'''dict to store the state of the buttons as booleans''' +button_state = { + 'trigger':0, + '2':0, + '3':0, + '4':0, + '5':0, + '6':0, + '7':0, + '8':0, + '9':0, + '10':0, + '11':0, + '12':0 +} + +hat_state = [ + 0, 0 +] diff --git a/ros/gamepad/src/sender.py b/ros/gamepad/src/sender.py index e7f278a..549ad9b 100644 --- a/ros/gamepad/src/sender.py +++ b/ros/gamepad/src/sender.py @@ -1,7 +1,9 @@ #!/usr/bin/env python3 +'''WIP: WILL NOT RUN -Lucas''' + import pygame -import sys +import syss import time #ROS @@ -49,19 +51,19 @@ def getMessage(): t = Twist() - t.linear.x = -(gamepad_state['LSY'] * SCALE_TRANSLATIONAL_X + TRIM_X) * REVERSE - t.linear.y = -(gamepad_state['LSX'] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * REVERSE + t.linear.x = -(gamepad_state['X'] * SCALE_TRANSLATIONAL_X + TRIM_X) * REVERSE + t.linear.y = -(gamepad_state['Y'] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * REVERSE t.linear.z = ((gamepad_state['RT'] - gamepad_state['LT']) / 2.0) * SCALE_TRANSLATIONAL_Z + TRIM_Z - if gamepad_state['LB'] == 1: + if gamepad_state['3'] == 1: x = 1 * SCALE_ROTATIONAL_X - elif gamepad_state['RB'] == 1: + elif gamepad_state['3'] == 1: x = -1 * SCALE_ROTATIONAL_X else: x = 0.0 t.angular.x = -x - t.angular.y = (-gamepad_state['RSY'] * SCALE_ROTATIONAL_Y) * REVERSE + t.angular.y = (-gamepad_state['Z'] * SCALE_ROTATIONAL_Y) * REVERSE t.angular.z = -gamepad_state['RSX'] * SCALE_ROTATIONAL_Z new_msg = RovVelocityCommand() @@ -86,61 +88,34 @@ def getTools(): def correct_raw(raw, abbv): '''Corrects the raw value from the gamepad to be in the range [-1.0, 1.0]''' - # Separate the sign from the value - sign = (raw >= 0) * 2 - 1 - raw = abs(raw) - - # Check if the input is a trigger or a stick - if abbv == 'LT' or abbv == 'RT': - dead_zone = TRIGGER_DEAD_ZONE - value_range = TRIGGER_RANGE + if abs(raw) > AXIS_RANGE: + raw = raw/abs(raw) + + if abbv == 'WHEEL': + return raw else: - dead_zone = STICK_DEAD_ZONE - value_range = STICK_RANGE - - if raw < dead_zone: - return 0.0 - - # Remove dead zone and scale the value - raw -= dead_zone - raw *= value_range / (value_range - dead_zone) - raw = 1.0 if raw > value_range else raw / value_range - corrected = round(raw, 3) - corrected *= sign - return corrected + if raw <= STICK_DEAD_ZONE: + raw = 0 + return raw def process_event(event): '''Processes a pygame event''' global tools global is_fine - global gamepad_state + global button_state + global axis_state global is_pool_centric global depth_lock global pitch_lock # Button pressed down events if event.type == pygame.JOYBUTTONDOWN: - gamepad_state[JOY_BUTTON[event.button]] = 1 - if event.button == JOY_BUTTON_KEY['A']: - tools[0] = not tools[0] - - elif event.button == JOY_BUTTON_KEY['B']: - tools[1] = not tools[1] - - elif event.button == JOY_BUTTON_KEY['X']: - tools[2] = not tools[2] - - elif event.button == JOY_BUTTON_KEY['Y'] and LOCKOUT: - tools[3] = not tools[3] + button_state[JOY_BUTTON_KEY[event.button]] = 1 - elif event.button == JOY_BUTTON_KEY['MENU']: - is_pool_centric = not is_pool_centric - - # Button released events elif event.type == pygame.JOYBUTTONUP: - gamepad_state[JOY_BUTTON[event.button]] = 0 + button_state[JOY_BUTTON_KEY[event.button]] = 0 # DPAD buttons elif event.type == pygame.JOYHATMOTION: @@ -162,7 +137,7 @@ def process_event(event): # Joysticks elif event.type == pygame.JOYAXISMOTION: - gamepad_state[JOY_AXIS[event.axis]] = correct_raw(event.value, JOY_AXIS[event.axis]) + axis_state[JOY_AXIS_KEY[event.axis]] = correct_raw(event.value, JOY_AXIS_KEY[event.axis]) # If the gamepad is disconnected, try to reconnect it From 7b551a616f0ad0537c48b7c0bec04d457a28c01c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?JoLucas=C2=8D=20Mauser?= Date: Sat, 12 Oct 2024 14:02:32 -0400 Subject: [PATCH 05/16] remove controller mapping file, added recieve file for testing --- controller_mapping.py | 118 ------------------------------------- ros/gamepad/src/config.py | 3 + ros/gamepad/src/recieve.py | 27 +++++++++ ros/gamepad/src/sender.py | 52 ++++++++++++---- 4 files changed, 71 insertions(+), 129 deletions(-) delete mode 100644 controller_mapping.py create mode 100644 ros/gamepad/src/recieve.py diff --git a/controller_mapping.py b/controller_mapping.py deleted file mode 100644 index a56ff5b..0000000 --- a/controller_mapping.py +++ /dev/null @@ -1,118 +0,0 @@ -import pygame -import sys -import time - - -''' -TODO: --move mappings to config file and code to sender file --delete this file after everything is done -''' - -'''x-axis is foward and backward, y-axis is side to side, z-axis is twisting''' -JOY_AXIS = { - 'X':1, 'Y':0, 'Z':2, 'WHEEL':3 -} - -JOY_AXIS_KEY = { - 1:'X', 0:'Y', 2:'Z', 3:'WHEEL' -} - -'''mappping from button identifier to pygame id''' -JOY_BUTTON = { - 'trigger':0, '2':1, '3':2, '4':3, '5':4, '6':5, '7':6, '8':7, '9':8, '10':9, '11':10, '12':11 -} - -'''mapping from pygame id button identifier i don't know why we need this''' -JOY_BUTTON_KEY = { - 0:'trigger', 1:'2', 2:'3', 3:'4', 4:'5', 5:'6', 6:'7', 7:'8', 8:'9', 9:'10', 10:'11', 11:'12' -} - -'''dict to store the state of the axis as floats''' -axis_state = { - 'X':0.0, - 'Y':0.0, - 'Z':0.0, - 'WHEEL':0.0 -} - -'''dict to store the state of the buttons as booleans''' -button_state = { - 'trigger':0, - '2':0, - '3':0, - '4':0, - '5':0, - '6':0, - '7':0, - '8':0, - '9':0, - '10':0, - '11':0, - '12':0 -} - -hat_state = [ - 0, 0 -] - - -no_controller = False -wtf = False - - -def init_pygame(): - '''Initializes pygame and the joystick''' - global controller - pygame.init() - pygame.joystick.init() - '''check if controller connected''' - if(pygame.joystick.get_count() == 0): - print("No controller found") - pygame.quit() - sys.exit(0) - controller = pygame.joystick.Joystick(0) - -'''get the controller events and update the various states accodingly''' -def check_event(): - global no_controller, wtf - for event in pygame.event.get(): - if event.type == pygame.JOYAXISMOTION: - axis_state[JOY_AXIS_KEY[event.axis]] = event.value - elif event.type == pygame.JOYBUTTONDOWN: - button_state[JOY_BUTTON_KEY[event.button]] = 1 - elif event.type == pygame.JOYBUTTONUP: - button_state[JOY_BUTTON_KEY[event.button]] = 0 - elif event.type == pygame.JOYHATMOTION: - hat_state[0] = event.value[0] - hat_state[1] = event.value[1] - elif event.type == pygame.JOYDEVICEREMOVED: - no_controller = True - pygame.quit() - sys.exit(0) - state_logger() - -'''logger to log the axis states to the command line''' -def state_logger(): - for i,j in enumerate(axis_state): - print(f"Axis {JOY_AXIS_KEY[i]} value: {axis_state[JOY_AXIS_KEY[i]]}") - for i,j in enumerate(button_state): - print(f"Button {JOY_BUTTON_KEY[i]} value: {button_state[JOY_BUTTON_KEY[i]]}") - print(f"Hat Y: {hat_state[0]}, HAT X: {hat_state[1]}") - if no_controller == True : - print("No controller connected") - if wtf == True : - print("Unkown event") - - - - - - -if __name__ == '__main__': - init_pygame() - while(True): - check_event() - state_logger() - pygame.time.wait(1000) - \ No newline at end of file diff --git a/ros/gamepad/src/config.py b/ros/gamepad/src/config.py index 5008f2a..a655ced 100644 --- a/ros/gamepad/src/config.py +++ b/ros/gamepad/src/config.py @@ -3,6 +3,9 @@ STICK_DEAD_ZONE = 0.09 AXIS_RANGE = 1.0 + + + '''x-axis is foward and backward, y-axis is side to side, z-axis is twisting''' JOY_AXIS = { 'X':1, 'Y':0, 'Z':2, 'WHEEL':3 diff --git a/ros/gamepad/src/recieve.py b/ros/gamepad/src/recieve.py new file mode 100644 index 0000000..610f6a5 --- /dev/null +++ b/ros/gamepad/src/recieve.py @@ -0,0 +1,27 @@ +#! /usr/bin/python3 + +import rclpy +from rclpy.node import Node +from shared_msgs.msg import ToolsCommandMsg + +class ReceiveNode(Node): + def __init__(self): + super().__init__("Receive") + self.sub = self.create_subscription(ToolsCommandMsg, '/tools', self.message, 10) + self.get_logger().info("Initialized") + + def message(self, msg): + self.get_logger().info(msg.message) + +def main(args = None): + rclpy.init(args = args) + + node = ReceiveNode() + + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/ros/gamepad/src/sender.py b/ros/gamepad/src/sender.py index 549ad9b..04f7075 100644 --- a/ros/gamepad/src/sender.py +++ b/ros/gamepad/src/sender.py @@ -1,9 +1,7 @@ #!/usr/bin/env python3 -'''WIP: WILL NOT RUN -Lucas''' - import pygame -import syss +import sys import time #ROS @@ -43,7 +41,9 @@ def getMessage(): '''Returns a RovVelocityCommand message based on the current gamepad state''' - global gamepad_state + global button_state + global axis_state + global hat_state global is_fine global is_pool_centric global pitch_lock @@ -51,20 +51,22 @@ def getMessage(): t = Twist() - t.linear.x = -(gamepad_state['X'] * SCALE_TRANSLATIONAL_X + TRIM_X) * REVERSE - t.linear.y = -(gamepad_state['Y'] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * REVERSE - t.linear.z = ((gamepad_state['RT'] - gamepad_state['LT']) / 2.0) * SCALE_TRANSLATIONAL_Z + TRIM_Z + t.linear.x = -(axis_state['X'] * SCALE_TRANSLATIONAL_X + TRIM_X) * REVERSE + t.linear.y = -(axis_state['Y'] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * REVERSE + t.linear.z = (hat_state[1]) * SCALE_TRANSLATIONAL_Z + TRIM_Z - if gamepad_state['3'] == 1: + if hat_state[0] == -1: x = 1 * SCALE_ROTATIONAL_X - elif gamepad_state['3'] == 1: + elif hat_state[0] == 1: x = -1 * SCALE_ROTATIONAL_X else: x = 0.0 + y = button_state['4'] - button_state['3'] + t.angular.x = -x - t.angular.y = (-gamepad_state['Z'] * SCALE_ROTATIONAL_Y) * REVERSE - t.angular.z = -gamepad_state['RSX'] * SCALE_ROTATIONAL_Z + t.angular.y = (-y * SCALE_ROTATIONAL_Y) * REVERSE + t.angular.z = -axis_state['Z'] * SCALE_ROTATIONAL_Z new_msg = RovVelocityCommand() new_msg.twist = t @@ -112,6 +114,28 @@ def process_event(event): # Button pressed down events if event.type == pygame.JOYBUTTONDOWN: button_state[JOY_BUTTON_KEY[event.button]] = 1 + if event.button == JOY_BUTTON_KEY['7']: + tools[0] = not tools[0] + + elif event.button == JOY_BUTTON_KEY['9']: + tools[1] = not tools[1] + + elif event.button == JOY_BUTTON_KEY['11']: + tools[2] = not tools[2] + + elif event.button == JOY_BUTTON_KEY['12'] and LOCKOUT: + tools[3] = not tools[3] + + elif event.button == JOY_BUTTON_KEY['2']: + is_pool_centric = not is_pool_centric + + elif event.button == JOY_BUTTON_KEY['10']: + if is_fine < 3: + is_fine += 1 + elif event.button == JOY_BUTTON_KEY['12']: + if is_fine > 0: + is_fine -= 1 + # Button released events elif event.type == pygame.JOYBUTTONUP: @@ -119,12 +143,17 @@ def process_event(event): # DPAD buttons elif event.type == pygame.JOYHATMOTION: + hat_state[0] = event[0] + hat_state[1] = event[1] + + ''' if event.value[1] == 1: if is_fine < 3: is_fine +=1 elif event.value[1] == -1: if is_fine > 0: is_fine -=1 + else: pass if event.value[0] == -1: @@ -134,6 +163,7 @@ def process_event(event): is_pool_centric = True else: pass + ''' # Joysticks elif event.type == pygame.JOYAXISMOTION: From b0628216d58237c2a20bd1627bfa10dfc0f5ee5f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?JoLucas=C2=8D=20Mauser?= Date: Sat, 12 Oct 2024 14:59:28 -0400 Subject: [PATCH 06/16] removed testing file --- ros/gamepad/src/recieve.py | 27 --------------------------- 1 file changed, 27 deletions(-) delete mode 100644 ros/gamepad/src/recieve.py diff --git a/ros/gamepad/src/recieve.py b/ros/gamepad/src/recieve.py deleted file mode 100644 index 610f6a5..0000000 --- a/ros/gamepad/src/recieve.py +++ /dev/null @@ -1,27 +0,0 @@ -#! /usr/bin/python3 - -import rclpy -from rclpy.node import Node -from shared_msgs.msg import ToolsCommandMsg - -class ReceiveNode(Node): - def __init__(self): - super().__init__("Receive") - self.sub = self.create_subscription(ToolsCommandMsg, '/tools', self.message, 10) - self.get_logger().info("Initialized") - - def message(self, msg): - self.get_logger().info(msg.message) - -def main(args = None): - rclpy.init(args = args) - - node = ReceiveNode() - - rclpy.spin(node) - - node.destroy_node() - rclpy.shutdown() - -if __name__ == "__main__": - main() \ No newline at end of file From 93cb99fa33a663211919aa7ba97376d02f45aa05 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?JoLucas=C2=8D=20Mauser?= Date: Wed, 16 Oct 2024 21:23:09 -0400 Subject: [PATCH 07/16] added test file for new hotas mapping --- test/controller_mapping.py | 171 +++++++++++++++++++++++++++++++++++++ 1 file changed, 171 insertions(+) create mode 100644 test/controller_mapping.py diff --git a/test/controller_mapping.py b/test/controller_mapping.py new file mode 100644 index 0000000..6c4a277 --- /dev/null +++ b/test/controller_mapping.py @@ -0,0 +1,171 @@ +import pygame +import sys +import time + +'''instance_id attribute for events does not work and i don't know why''' + +'''x-axis is foward and backward, y-axis is side to side, z-axis is twisting''' +CONT_AXIS_KEY = { + 'X':1, 'Y':0, 'Z':2, 'SLIDE':3 +} + +THR_AXIS_KEY = { + 'THR':2, 'YAW':3, 'SCR':4, 'SX':0, 'SY':1 +} + +CONT_AXIS = { + 1:'X', 0:'Y', 2:'Z', 3:'SLIDE' +} + +THR_AXIS = { + 2:'THR', 3:'YAW', 4:'SCR', 0:'SX', 1:'SY' +} + + +CONT_BUTTON_KEY = { + 'trigger':0, '2':1, '3':2, '4':3, '5':4, '6':5, '7':6, '8':7, '9':8, '10':9, '11':10, '12':11, '13':12, '14':13, + '15':14, '16':15 +} + +CONT_BUTTON = { + 0:'trigger', 1:'2', 2:'3', 3:'4', 4:'5', 5:'6', 6:'7', 7:'8', 8:'9', 9:'10', 10:'11', 11:'12', 12:'13', 13:'14', + 14:'15', 15:'16' +} + +THR_BUTTON_KEY = { + 0:'T1', 1:'T2', 2:'T3', 3:'T4', 4:'T5', 5:'T6', 6:'T7', 7:'T8', 8:'T9', 9:'T10', 10:'T11', 11:'T12', 12:'T13', 13:'T14' +} + +THR_BUTTON = { + 'T1':0, 'T2':1, 'T3':2, 'T4':3, 'T5':4, 'T6':5, 'T7':6, 'T8':7, 'T9':8, 'T10':9, 'T11':10, 'T12':11, 'T13':12, 'T14':13 +} + +axis_state = { + 'X':0.0, + 'Y':0.0, + 'Z':0.0, + 'SLIDE':0.0, + 'THR':0.0, + 'SCR':0.0, + 'YAW':0.0, + 'SX':0.0, + 'SY':0.0 +} + +button_state = { + 'trigger':0, + '2':0, + '3':0, + '4':0, + '5':0, + '6':0, + '7':0, + '8':0, + '9':0, + '10':0, + '11':0, + '12':0, + '13':0, + '14':0, + '15':0, + '16':0, + 'T1':0, + 'T2':0, + 'T3':0, + 'T4':0, + 'T5':0, + 'T6':0, + 'T7':0, + 'T8':0, + 'T9':0, + 'T10':0, + 'T11':0, + 'T12':0, + 'T13':0, + 'T14':0 +} + +hat_state = [ + 0, 0 +] + + +no_controller = False +wtf = False + + +def init_pygame(): + '''Initializes pygame and the joystick''' + global controller + global throttle + pygame.init() + pygame.joystick.init() + '''check if controller connected''' + if(pygame.joystick.get_count() != 2): + print("Not two controllers") + pygame.quit() + sys.exit(0) + controller = pygame.joystick.Joystick(0) + throttle = pygame.joystick.Joystick(1) + + +'''get the controller events and update the various states accodingly''' +def check_event(): + for event in pygame.event.get(): + print(event.instance_id) + if event.instance_id == 0: + if event.type == pygame.JOYAXISMOTION: + axis_state[CONT_AXIS[event.axis]] = event.value + elif event.type == pygame.JOYBUTTONDOWN: + button_state[CONT_BUTTON[event.button]] = 1 + elif event.type == pygame.JOYBUTTONUP: + button_state[CONT_BUTTON[event.button]] = 0 + elif event.type == pygame.JOYDEVICEREMOVED: + print('Disconnect') + pygame.quit() + sys.exit(0) + if event.instance_id == 1: + if event.type == pygame.JOYAXISMOTION: + axis_state[THR_AXIS[event.axis]] = event.value + elif event.type == pygame.JOYBUTTONDOWN: + button_state[THR_BUTTON[event.button]] = 1 + elif event.type == pygame.JOYBUTTONUP: + button_state[THR_BUTTON[event.button]] = 0 + elif event.type == pygame.JOYDEVICEREMOVED: + print('Disconnect') + pygame.quit() + sys.exit(0) + + + + + +'''logger to log the axis states to the command line''' +def state_logger(): + for axis, state in axis_state.items(): + print(f"Axis {axis} = {state}", end=' ') + print() + + for button, state in button_state.items(): + print(f"Button {button} = {state}") + + + + + + + +if __name__ == '__main__': + init_pygame() + print(pygame.__version__) + while True: + try: + check_event() + state_logger() + except KeyboardInterrupt: + pygame.quit() + sys.exit(0) + time.sleep(1) + + + \ No newline at end of file From 506534dbca23b2b27881a7eeb497c9ea8988a80f Mon Sep 17 00:00:00 2001 From: Icyadam14 Date: Thu, 17 Oct 2024 16:03:50 -0400 Subject: [PATCH 08/16] Works with new controllers. Mappings need to be adjusted. --- ros/gamepad/src/config.py | 68 +++++++ ros/gamepad/src/sender.py | 370 ++++++++++++++++++------------------- test/controller_mapping.py | 6 +- 3 files changed, 252 insertions(+), 192 deletions(-) diff --git a/ros/gamepad/src/config.py b/ros/gamepad/src/config.py index a655ced..5787303 100644 --- a/ros/gamepad/src/config.py +++ b/ros/gamepad/src/config.py @@ -52,3 +52,71 @@ hat_state = [ 0, 0 ] + + +# Store the states of the throttle axies +throttle_axis_state = { + 0: 0.0, + 1: 0.0, + 2: 0.0, + 3: 0.0, + 4: 0.0, + 5: 0.0, + 6: 0.0, + 7: 0.0 +} + +# Store the states of the throttle buttons +throttle_button_state = { + 0: 0, + 1: 0, + 2: 0, + 3: 0, + 4: 0, + 5: 0, + 6: 0, + 7: 0, + 8: 0, + 9: 0, + 10: 0, + 11: 0, + 12: 0, + 13: 0 +} + +# Store the state of throttle hats +throttle_hat_state = { + 0: 0, + 1: 0 +} + +joystick_axis_state = { + 0: 0.0, + 1: 0.0, + 2: 0.0, + 3: 0.0 +} + +joystick_button_state = { + 0: 0, + 1: 0, + 2: 0, + 3: 0, + 4: 0, + 5: 0, + 6: 0, + 7: 0, + 8: 0, + 9: 0, + 10: 0, + 11: 0, + 12: 0, + 13: 0 +} + +joystick_hat_state = { + 0: 0, + 1: 0 +} + +# Map throttle axis to desired axis diff --git a/ros/gamepad/src/sender.py b/ros/gamepad/src/sender.py index 04f7075..4381145 100644 --- a/ros/gamepad/src/sender.py +++ b/ros/gamepad/src/sender.py @@ -23,187 +23,211 @@ SCALE_ROTATIONAL_Y = 1.0 SCALE_ROTATIONAL_Z = 1.0 +REVERSE = 1 + TRIM_X = 0.0 TRIM_Y = 0.0 TRIM_Z = 0.0 -REVERSE = 1 -LOCKOUT = True -#is fine = 0 = std_mode -#is_fine = 1 = fine_mode -#is_fine = 2 = yeet mode -is_fine = 0 -is_pool_centric = False -depth_lock = False -pitch_lock = False GAMEPAD_TIMEOUT = 20 # seconds +JOYSTICK_NAME = "Thrustmaster T.16000M" +THROTTLE_NAME = "Thrustmaster TWCS Throttle" + + +class Contoller: + def __init__(self): + + self.joystick = None + self.throttle = None + self.joystick_id = 0 + self.throttle_id = 0 + + self.reverse = 1 + self.lockout = True + self.is_fine = 0 + self.is_pool_centric = False + self.depth_lock = False + self.pitch_lock = False + self.joystick_axis_state = joystick_axis_state + self.joystick_button_state = joystick_button_state + self.throttle_axis_state = throttle_axis_state + self.throttle_button_state = throttle_button_state + self.mapping = 0 # 0, 1, 2, 3 + + # Initialize the ros node + rclpy.init() + self.node = rclpy.create_node('gp_pub') + + try: + self.init_pygame() + except: + node.get_logger().info('Controllers not found. Please make sure both the joystick and throttle are connected') + if not reconnect_gamepad(): + node.get_logger().info("\nNo gamepad found, exiting") + pygame.quit() + sys.exit(0) + + # Create the publishers + self.pub = self.node.create_publisher(RovVelocityCommand, '/rov_velocity', 10) + self.pub_tools = self.node.create_publisher(ToolsCommandMsg, 'tools', 10) -def getMessage(): - '''Returns a RovVelocityCommand message based on the current gamepad state''' - global button_state - global axis_state - global hat_state - global is_fine - global is_pool_centric - global pitch_lock - global depth_lock + # Create the timers + self.data_thread = self.node.create_timer(0.1, self.pub_data) + self.gamepad_thread = self.node.create_timer(0.001, self.update_gamepad) - t = Twist() + self.node.get_logger().info('Controllers initialized') - t.linear.x = -(axis_state['X'] * SCALE_TRANSLATIONAL_X + TRIM_X) * REVERSE - t.linear.y = -(axis_state['Y'] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * REVERSE - t.linear.z = (hat_state[1]) * SCALE_TRANSLATIONAL_Z + TRIM_Z + rclpy.spin(self.node) - if hat_state[0] == -1: - x = 1 * SCALE_ROTATIONAL_X - elif hat_state[0] == 1: - x = -1 * SCALE_ROTATIONAL_X - else: - x = 0.0 + self.data_thread.destroy() + self.gamepad_thread.destroy() - y = button_state['4'] - button_state['3'] - - t.angular.x = -x - t.angular.y = (-y * SCALE_ROTATIONAL_Y) * REVERSE - t.angular.z = -axis_state['Z'] * SCALE_ROTATIONAL_Z + # Stop the pygame library + pygame.quit() - new_msg = RovVelocityCommand() - new_msg.twist = t - new_msg.is_fine = is_fine - new_msg.is_pool_centric = is_pool_centric - new_msg.depth_lock = depth_lock - new_msg.pitch_lock = pitch_lock + self.node.destroy_node() + rclpy.shutdown() - return(new_msg) + def init_pygame(self): + '''Initializes pygame and the joystick''' + pygame.init() + pygame.joystick.init() + # Should be two joysticks + assert pygame.joystick.get_count() == 2 -def getTools(): - '''Returns a ToolsCommandMsg message based on the current gamepad state''' - global tools + # Determine which joystick is the joystick and which is the throttle + for i in range(pygame.joystick.get_count()): + if pygame.joystick.Joystick(i).get_name() == JOYSTICK_NAME: + self.joystick = pygame.joystick.Joystick(i) + self.joystick_id = i + if pygame.joystick.Joystick(i).get_name() == THROTTLE_NAME: + self.throttle = pygame.joystick.Joystick(i) + self.throttle_id = i - tm = ToolsCommandMsg() - tm.tools = [i for i in tools] - return tm + def update_gamepad(self): + '''Updates the gamepad state''' + # Get all the events from pygame and process them + for event in pygame.event.get(): + self.process_event(event) -def correct_raw(raw, abbv): - '''Corrects the raw value from the gamepad to be in the range [-1.0, 1.0]''' - if abs(raw) > AXIS_RANGE: - raw = raw/abs(raw) - - if abbv == 'WHEEL': - return raw - else: - if raw <= STICK_DEAD_ZONE: - raw = 0 - return raw + def reconnect_gamepad(self): + exit(0) -def process_event(event): - '''Processes a pygame event''' - global tools - global is_fine - global button_state - global axis_state - global is_pool_centric - global depth_lock - global pitch_lock - - # Button pressed down events - if event.type == pygame.JOYBUTTONDOWN: - button_state[JOY_BUTTON_KEY[event.button]] = 1 - if event.button == JOY_BUTTON_KEY['7']: - tools[0] = not tools[0] - - elif event.button == JOY_BUTTON_KEY['9']: - tools[1] = not tools[1] - - elif event.button == JOY_BUTTON_KEY['11']: - tools[2] = not tools[2] - - elif event.button == JOY_BUTTON_KEY['12'] and LOCKOUT: - tools[3] = not tools[3] + + def correct_raw(self, raw): + '''Corrects the raw value from the gamepad to be in the range [-1.0, 1.0]''' + if raw > 0: + if raw >= STICK_DEAD_ZONE: + return -raw + elif raw > 1: + return -1 + else: + return 0 - elif event.button == JOY_BUTTON_KEY['2']: - is_pool_centric = not is_pool_centric + if raw < 0: + if raw <= -STICK_DEAD_ZONE: + return -raw + elif raw < -1: + return 1 + else: + return 0 + + + def process_event(self, event): + '''Processes a pygame event''' + # Check if the event is a joyaxismotion event + if event.type == pygame.JOYAXISMOTION: + # Check if the event is from the joystick or the throttle + if event.joy == self.joystick_id: + self.joystick_axis_state[event.axis] = self.correct_raw(event.value) + elif event.joy == self.throttle_id: + self.throttle_axis_state[event.axis] = self.correct_raw(event.value) + + # Check if the event is a joybuttondown event + elif event.type == pygame.JOYBUTTONDOWN: + # Check if the event is from the joystick or the throttle + if event.joy == self.joystick_id: + self.joystick_button_state[event.button] = 1 + elif event.joy == self.throttle_id: + self.throttle_button_state[event.button] = 1 + + # Check if the event is a joybuttonup event + elif event.type == pygame.JOYBUTTONUP: + # Check if the event is from the joystick or the throttle + if event.joy == self.joystick_id: + self.joystick_button_state[event.button] = 0 + elif event.joy == self.throttle_id: + self.throttle_button_state[event.button] = 0 + + # Check if the event is a joydeviceremoved event + elif event.type == pygame.JOYDEVICEREMOVED: + # Try to reconnect the gamepad + if not self.reconnect_gamepad(): + self.node.get_logger().info("\nNo gamepad found, exiting") + pygame.quit() + sys.exit(0) + + def pub_data(self): + '''Publishes the data to the rov_velocity topic and the tools topic''' + # Get a message to publish for the rov_velocity topic + self.pub.publish(self.getMessage()) + # Get a message to publish for the tools topic + # pub_tools.publish(self.getTools()) + - elif event.button == JOY_BUTTON_KEY['10']: - if is_fine < 3: - is_fine += 1 - elif event.button == JOY_BUTTON_KEY['12']: - if is_fine > 0: - is_fine -= 1 - # Button released events - elif event.type == pygame.JOYBUTTONUP: - button_state[JOY_BUTTON_KEY[event.button]] = 0 - - # DPAD buttons - elif event.type == pygame.JOYHATMOTION: - hat_state[0] = event[0] - hat_state[1] = event[1] + + def getMessage(self): + '''Returns a RovVelocityCommand message based on the current gamepad state''' + global button_state + global axis_state + global hat_state + global is_fine + global is_pool_centric + global pitch_lock + global depth_lock + + t = Twist() + + t.linear.x = -(self.throttle_axis_state[2] * SCALE_TRANSLATIONAL_X + TRIM_X) * REVERSE + t.linear.y = -(self.throttle_axis_state[5] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * REVERSE + t.linear.z = -(self.throttle_axis_state[1] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * REVERSE - ''' - if event.value[1] == 1: - if is_fine < 3: - is_fine +=1 - elif event.value[1] == -1: - if is_fine > 0: - is_fine -=1 - - else: - pass - if event.value[0] == -1: - pitch_lock = not pitch_lock - elif event.value[0] == 1: - depth_lock = not depth_lock - is_pool_centric = True - else: - pass - ''' - - # Joysticks - elif event.type == pygame.JOYAXISMOTION: - axis_state[JOY_AXIS_KEY[event.axis]] = correct_raw(event.value, JOY_AXIS_KEY[event.axis]) - - - # If the gamepad is disconnected, try to reconnect it - elif event.type == pygame.JOYDEVICEREMOVED: - if not reconnect_gamepad(): - node.get_logger().info("\nNo gamepad found, exiting") - pygame.quit() - rclpy.shutdown() - sys.exit(0) - -def pub_data(): - '''Publishes the data to the rov_velocity topic and the tools topic''' - # Get a message to publish for the rov_velocity topic - pub.publish(getMessage()) - # Get a message to publish for the tools topic - pub_tools.publish(getTools()) - -def update_gamepad(): - '''Updates the gamepad state''' - # Get all the events from pygame and process them - for event in pygame.event.get(): - process_event(event) - - -def init_pygame(): - '''Initializes pygame and the joystick''' - global joystick - pygame.init() - pygame.joystick.init() - assert pygame.joystick.get_count() == 1 - joystick = pygame.joystick.Joystick(0) + t.angular.x = -(self.joystick_axis_state[1] * SCALE_ROTATIONAL_X) * REVERSE + t.angular.y = -(self.joystick_axis_state[0] * SCALE_ROTATIONAL_Y) * REVERSE + t.angular.z = -(self.joystick_axis_state[2] * SCALE_ROTATIONAL_Z) * REVERSE + + new_msg = RovVelocityCommand() + new_msg.twist = t + new_msg.is_fine = self.is_fine + new_msg.is_pool_centric = self.is_pool_centric + new_msg.depth_lock = self.depth_lock + new_msg.pitch_lock = self.pitch_lock + + return(new_msg) + + +def getTools(): + '''Returns a ToolsCommandMsg message based on the current gamepad state''' + global tools + + tm = ToolsCommandMsg() + tm.tools = [i for i in tools] + + return tm + + def reconnect_gamepad(): '''Tries to reconnect the gamepad''' - global joystick + global joystick, throttle reconnected = False i = GAMEPAD_TIMEOUT while i >= 0 and not reconnected: @@ -211,10 +235,11 @@ def reconnect_gamepad(): node.get_logger().info('Gamepad disconnected, reconnect within {:2} seconds'.format(i)) pygame.init() pygame.joystick.init() - # make sure there is only one joystick - if pygame.joystick.get_count() == 1: + # make sure there are two joysticks + if pygame.joystick.get_count() == 2: # get the first joystick joystick = pygame.joystick.Joystick(0) + throttle = pygame.joystick.Joystick(1) reconnected = True else: pygame.quit() @@ -227,45 +252,10 @@ def reconnect_gamepad(): if reconnected: node.get_logger().info('\nGamepad reconnected') joystick = pygame.joystick.Joystick(0) + throttle = pygame.joystick.Joystick(1) return reconnected if __name__ == '__main__': - global pub, pub_tools, data_thread, gamepad_thread, node - - # Initialize the ros node - rclpy.init() - node = rclpy.create_node('gp_pub') - - try: - init_pygame() - except: - node.get_logger().info('No gamepad found, please connect a gamepad') - if not reconnect_gamepad(): - node.get_logger().info("\nNo gamepad found, exiting") - pygame.quit() - sys.exit(0) - - - - # Create the publishers - pub = node.create_publisher(RovVelocityCommand, '/rov_velocity', 10) - pub_tools = node.create_publisher(ToolsCommandMsg, 'tools', 10) - - # Create the timers - data_thread = node.create_timer(0.1, pub_data) - gamepad_thread = node.create_timer(0.001, update_gamepad) - - print('ready') - - rclpy.spin(node) - - data_thread.destroy() - gamepad_thread.destroy() - - # Stop the pygame library - pygame.quit() - - node.destroy_node() - rclpy.shutdown() \ No newline at end of file + controller = Contoller() diff --git a/test/controller_mapping.py b/test/controller_mapping.py index 6c4a277..91508fa 100644 --- a/test/controller_mapping.py +++ b/test/controller_mapping.py @@ -146,8 +146,8 @@ def state_logger(): print(f"Axis {axis} = {state}", end=' ') print() - for button, state in button_state.items(): - print(f"Button {button} = {state}") + # for button, state in button_state.items(): + # print(f"Button {button} = {state}") @@ -165,6 +165,8 @@ def state_logger(): except KeyboardInterrupt: pygame.quit() sys.exit(0) + except: + pass time.sleep(1) From 5769de679aa865e956a598306d740f246b0c5f48 Mon Sep 17 00:00:00 2001 From: Icyadam14 Date: Thu, 17 Oct 2024 16:56:33 -0400 Subject: [PATCH 09/16] Reformatted and removed unused stuff. Still need to adjust mappings. --- ros/gamepad/src/config.py | 72 +++++------------ ros/gamepad/src/sender.py | 157 +++++++++++++------------------------- 2 files changed, 74 insertions(+), 155 deletions(-) diff --git a/ros/gamepad/src/config.py b/ros/gamepad/src/config.py index 5787303..fe8c7a4 100644 --- a/ros/gamepad/src/config.py +++ b/ros/gamepad/src/config.py @@ -2,57 +2,22 @@ # New ranges for the triggers and sticks (Probably need to adjust these values) STICK_DEAD_ZONE = 0.09 AXIS_RANGE = 1.0 - - - - -'''x-axis is foward and backward, y-axis is side to side, z-axis is twisting''' -JOY_AXIS = { - 'X':1, 'Y':0, 'Z':2, 'WHEEL':3 -} - -JOY_AXIS_KEY = { - 1:'X', 0:'Y', 2:'Z', 3:'WHEEL' -} - -'''mappping from button identifier to pygame id''' -JOY_BUTTON = { - 'trigger':0, '2':1, '3':2, '4':3, '5':4, '6':5, '7':6, '8':7, '9':8, '10':9, '11':10, '12':11 -} - -'''mapping from pygame id button identifier i don't know why we need this''' -JOY_BUTTON_KEY = { - 0:'trigger', 1:'2', 2:'3', 3:'4', 4:'5', 5:'6', 6:'7', 7:'8', 8:'9', 9:'10', 10:'11', 11:'12' -} - -'''dict to store the state of the axis as floats''' -axis_state = { - 'X':0.0, - 'Y':0.0, - 'Z':0.0, - 'WHEEL':0.0 -} - -'''dict to store the state of the buttons as booleans''' -button_state = { - 'trigger':0, - '2':0, - '3':0, - '4':0, - '5':0, - '6':0, - '7':0, - '8':0, - '9':0, - '10':0, - '11':0, - '12':0 -} - -hat_state = [ - 0, 0 -] - +# Scale factors for the translational and rotational axes +SCALE_TRANSLATIONAL_X = 1.0 +SCALE_TRANSLATIONAL_Y = 1.0 +SCALE_TRANSLATIONAL_Z = 1.0 +SCALE_ROTATIONAL_X = 1.0 +SCALE_ROTATIONAL_Y = 1.0 +SCALE_ROTATIONAL_Z = 1.0 +# Trim values for the translational axes +TRIM_X = 0.0 +TRIM_Y = 0.0 +TRIM_Z = 0.0 + +GAMEPAD_TIMEOUT = 20 # seconds + +JOYSTICK_NAME = "Thrustmaster T.16000M" +THROTTLE_NAME = "Thrustmaster TWCS Throttle" # Store the states of the throttle axies throttle_axis_state = { @@ -90,6 +55,7 @@ 1: 0 } +# Store the states of the joystick axes joystick_axis_state = { 0: 0.0, 1: 0.0, @@ -97,6 +63,7 @@ 3: 0.0 } +# Store the states of the joystick buttons joystick_button_state = { 0: 0, 1: 0, @@ -114,9 +81,8 @@ 13: 0 } +# Stores the states of the joystick hats joystick_hat_state = { 0: 0, 1: 0 } - -# Map throttle axis to desired axis diff --git a/ros/gamepad/src/sender.py b/ros/gamepad/src/sender.py index 4381145..5fa9e1d 100644 --- a/ros/gamepad/src/sender.py +++ b/ros/gamepad/src/sender.py @@ -4,7 +4,7 @@ import sys import time -#ROS +# ROS import rclpy from std_msgs.msg import String, Bool, Empty from shared_msgs.msg import RovVelocityCommand, ToolsCommandMsg @@ -12,47 +12,30 @@ from config import * - -tools = [0, 0, 0, 0, 0] - -SCALE_TRANSLATIONAL_X = 1.0 -SCALE_TRANSLATIONAL_Y = 1.0 -SCALE_TRANSLATIONAL_Z = 1.0 - -SCALE_ROTATIONAL_X = 1.0 -SCALE_ROTATIONAL_Y = 1.0 -SCALE_ROTATIONAL_Z = 1.0 - -REVERSE = 1 - -TRIM_X = 0.0 -TRIM_Y = 0.0 -TRIM_Z = 0.0 - -GAMEPAD_TIMEOUT = 20 # seconds - -JOYSTICK_NAME = "Thrustmaster T.16000M" -THROTTLE_NAME = "Thrustmaster TWCS Throttle" - - -class Contoller: +class Controller: def __init__(self): - + # Pygame variables self.joystick = None self.throttle = None self.joystick_id = 0 self.throttle_id = 0 + # Joystick and throttle states + self.joystick_axis_state = joystick_axis_state + self.joystick_button_state = joystick_button_state + self.throttle_axis_state = throttle_axis_state + self.throttle_button_state = throttle_button_state + + # Pilot variables self.reverse = 1 self.lockout = True self.is_fine = 0 self.is_pool_centric = False self.depth_lock = False self.pitch_lock = False - self.joystick_axis_state = joystick_axis_state - self.joystick_button_state = joystick_button_state - self.throttle_axis_state = throttle_axis_state - self.throttle_button_state = throttle_button_state + self.tools = [0, 0, 0, 0, 0] + + # Mapping variables self.mapping = 0 # 0, 1, 2, 3 # Initialize the ros node @@ -62,9 +45,9 @@ def __init__(self): try: self.init_pygame() except: - node.get_logger().info('Controllers not found. Please make sure both the joystick and throttle are connected') - if not reconnect_gamepad(): - node.get_logger().info("\nNo gamepad found, exiting") + self.node.get_logger().info('Controllers not found. Please make sure both the joystick and throttle are connected') + if not self.reconnect(): + self.node.get_logger().info("\nNo gamepad found, exiting") pygame.quit() sys.exit(0) @@ -74,7 +57,7 @@ def __init__(self): # Create the timers self.data_thread = self.node.create_timer(0.1, self.pub_data) - self.gamepad_thread = self.node.create_timer(0.001, self.update_gamepad) + self.gamepad_thread = self.node.create_timer(0.001, self.update) self.node.get_logger().info('Controllers initialized') @@ -107,18 +90,31 @@ def init_pygame(self): self.throttle_id = i - def update_gamepad(self): + def reconnect(self): + '''Tries to reconnect the gamepad''' + reconnected = False + i = GAMEPAD_TIMEOUT + while i >= 0 and not reconnected: + self.node.get_logger().info('Gamepad disconnected, reconnect within {:2} seconds'.format(i)) + try: + self.init_pygame() + reconnected = True + except: + pygame.time.wait(1000) # Wait 1 second + i -= 1 + + if reconnected: + self.node.get_logger().info('Controllers reconnected') + + return reconnected + + + def update(self): '''Updates the gamepad state''' # Get all the events from pygame and process them for event in pygame.event.get(): self.process_event(event) - - - def reconnect_gamepad(self): - exit(0) - - def correct_raw(self, raw): '''Corrects the raw value from the gamepad to be in the range [-1.0, 1.0]''' if raw > 0: @@ -167,11 +163,13 @@ def process_event(self, event): # Check if the event is a joydeviceremoved event elif event.type == pygame.JOYDEVICEREMOVED: # Try to reconnect the gamepad - if not self.reconnect_gamepad(): + self.node.get_logger().info('Gamepad disconnected. Attempting to reconnect...') + if not self.reconnect(): self.node.get_logger().info("\nNo gamepad found, exiting") pygame.quit() sys.exit(0) + def pub_data(self): '''Publishes the data to the rov_velocity topic and the tools topic''' # Get a message to publish for the rov_velocity topic @@ -180,28 +178,19 @@ def pub_data(self): # pub_tools.publish(self.getTools()) - - - def getMessage(self): '''Returns a RovVelocityCommand message based on the current gamepad state''' - global button_state - global axis_state - global hat_state - global is_fine - global is_pool_centric - global pitch_lock - global depth_lock t = Twist() - t.linear.x = -(self.throttle_axis_state[2] * SCALE_TRANSLATIONAL_X + TRIM_X) * REVERSE - t.linear.y = -(self.throttle_axis_state[5] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * REVERSE - t.linear.z = -(self.throttle_axis_state[1] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * REVERSE - - t.angular.x = -(self.joystick_axis_state[1] * SCALE_ROTATIONAL_X) * REVERSE - t.angular.y = -(self.joystick_axis_state[0] * SCALE_ROTATIONAL_Y) * REVERSE - t.angular.z = -(self.joystick_axis_state[2] * SCALE_ROTATIONAL_Z) * REVERSE + # Set linear velocities + t.linear.x = -(self.throttle_axis_state[2] * SCALE_TRANSLATIONAL_X + TRIM_X) * self.reverse + t.linear.y = -(self.throttle_axis_state[5] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * self.reverse + t.linear.z = -(self.throttle_axis_state[1] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * self.reverse + # Set angular velocities + t.angular.x = -(self.joystick_axis_state[1] * SCALE_ROTATIONAL_X) * self.reverse + t.angular.y = -(self.joystick_axis_state[0] * SCALE_ROTATIONAL_Y) * self.reverse + t.angular.z = -(self.joystick_axis_state[2] * SCALE_ROTATIONAL_Z) * self.reverse new_msg = RovVelocityCommand() new_msg.twist = t @@ -211,51 +200,15 @@ def getMessage(self): new_msg.pitch_lock = self.pitch_lock return(new_msg) + + def getTools(self): + '''Returns a ToolsCommandMsg message based on the current gamepad state''' + tm = ToolsCommandMsg() + tm.tools = [i for i in self.tools] -def getTools(): - '''Returns a ToolsCommandMsg message based on the current gamepad state''' - global tools - - tm = ToolsCommandMsg() - tm.tools = [i for i in tools] - - return tm - - - - -def reconnect_gamepad(): - '''Tries to reconnect the gamepad''' - global joystick, throttle - reconnected = False - i = GAMEPAD_TIMEOUT - while i >= 0 and not reconnected: - try: - node.get_logger().info('Gamepad disconnected, reconnect within {:2} seconds'.format(i)) - pygame.init() - pygame.joystick.init() - # make sure there are two joysticks - if pygame.joystick.get_count() == 2: - # get the first joystick - joystick = pygame.joystick.Joystick(0) - throttle = pygame.joystick.Joystick(1) - reconnected = True - else: - pygame.quit() - assert False - except: - # wait 1 second - pygame.time.wait(1000) - i -= 1 - - if reconnected: - node.get_logger().info('\nGamepad reconnected') - joystick = pygame.joystick.Joystick(0) - throttle = pygame.joystick.Joystick(1) - - return reconnected + return tm if __name__ == '__main__': - controller = Contoller() + controller = Controller() From 3e630c666879afb15d3243aee395b54e0badd36b Mon Sep 17 00:00:00 2001 From: Icyadam14 Date: Thu, 17 Oct 2024 17:05:31 -0400 Subject: [PATCH 10/16] Fixed correct_raw function --- ros/gamepad/src/sender.py | 40 +++++++++++++++++++++++++-------------- 1 file changed, 26 insertions(+), 14 deletions(-) diff --git a/ros/gamepad/src/sender.py b/ros/gamepad/src/sender.py index 5fa9e1d..1b85f39 100644 --- a/ros/gamepad/src/sender.py +++ b/ros/gamepad/src/sender.py @@ -35,7 +35,7 @@ def __init__(self): self.pitch_lock = False self.tools = [0, 0, 0, 0, 0] - # Mapping variables + # Mapping variables (TO CHANGE MAPPING: GO TO getMessage() AND MAKE YOUR CHANGES THERE) self.mapping = 0 # 0, 1, 2, 3 # Initialize the ros node @@ -115,21 +115,22 @@ def update(self): for event in pygame.event.get(): self.process_event(event) + def correct_raw(self, raw): '''Corrects the raw value from the gamepad to be in the range [-1.0, 1.0]''' if raw > 0: if raw >= STICK_DEAD_ZONE: - return -raw + return raw elif raw > 1: - return -1 + return 1 else: return 0 if raw < 0: if raw <= -STICK_DEAD_ZONE: - return -raw + return raw elif raw < -1: - return 1 + return -1 else: return 0 @@ -163,7 +164,7 @@ def process_event(self, event): # Check if the event is a joydeviceremoved event elif event.type == pygame.JOYDEVICEREMOVED: # Try to reconnect the gamepad - self.node.get_logger().info('Gamepad disconnected. Attempting to reconnect...') + self.node.get_logger().info('Controller disconnected. Attempting to reconnect...') if not self.reconnect(): self.node.get_logger().info("\nNo gamepad found, exiting") pygame.quit() @@ -183,14 +184,25 @@ def getMessage(self): t = Twist() - # Set linear velocities - t.linear.x = -(self.throttle_axis_state[2] * SCALE_TRANSLATIONAL_X + TRIM_X) * self.reverse - t.linear.y = -(self.throttle_axis_state[5] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * self.reverse - t.linear.z = -(self.throttle_axis_state[1] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * self.reverse - # Set angular velocities - t.angular.x = -(self.joystick_axis_state[1] * SCALE_ROTATIONAL_X) * self.reverse - t.angular.y = -(self.joystick_axis_state[0] * SCALE_ROTATIONAL_Y) * self.reverse - t.angular.z = -(self.joystick_axis_state[2] * SCALE_ROTATIONAL_Z) * self.reverse + if self.mapping == 0: + # Set linear velocities + t.linear.x = -(self.throttle_axis_state[2] * SCALE_TRANSLATIONAL_X + TRIM_X) * self.reverse + t.linear.y = -(self.throttle_axis_state[5] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * self.reverse + t.linear.z = -(self.throttle_axis_state[1] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * self.reverse + # Set angular velocities + t.angular.x = -(self.joystick_axis_state[1] * SCALE_ROTATIONAL_X) * self.reverse + t.angular.y = -(self.joystick_axis_state[0] * SCALE_ROTATIONAL_Y) * self.reverse + t.angular.z = -(self.joystick_axis_state[2] * SCALE_ROTATIONAL_Z) * self.reverse + + elif self.mapping == 1: + # Set linear velocities + t.linear.x = -(self.throttle_axis_state[2] * SCALE_TRANSLATIONAL_X + TRIM_X) * self.reverse + t.linear.y = -(self.throttle_axis_state[5] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * self.reverse + t.linear.z = -(self.throttle_axis_state[1] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * self.reverse + # Set angular velocities + t.angular.x = -(self.joystick_axis_state[1] * SCALE_ROTATIONAL_X) * self.reverse + t.angular.y = -(self.joystick_axis_state[0] * SCALE_ROTATIONAL_Y) * self.reverse + t.angular.z = -(self.joystick_axis_state[2] * SCALE_ROTATIONAL_Z) * self.reverse new_msg = RovVelocityCommand() new_msg.twist = t From 49ca06c34ee85fd148fed9cb2a029d2c05afe737 Mon Sep 17 00:00:00 2001 From: ethanburmane Date: Thu, 17 Oct 2024 20:49:04 -0400 Subject: [PATCH 11/16] commented controller functions --- ros/gamepad/src/config.py | 93 ++++++++++++++++++++------------------- 1 file changed, 48 insertions(+), 45 deletions(-) diff --git a/ros/gamepad/src/config.py b/ros/gamepad/src/config.py index fe8c7a4..ebdfcd6 100644 --- a/ros/gamepad/src/config.py +++ b/ros/gamepad/src/config.py @@ -10,6 +10,7 @@ SCALE_ROTATIONAL_Y = 1.0 SCALE_ROTATIONAL_Z = 1.0 # Trim values for the translational axes +# Values are added to the translational axis values after scaling TRIM_X = 0.0 TRIM_Y = 0.0 TRIM_Z = 0.0 @@ -21,68 +22,70 @@ # Store the states of the throttle axies throttle_axis_state = { - 0: 0.0, - 1: 0.0, - 2: 0.0, - 3: 0.0, - 4: 0.0, - 5: 0.0, - 6: 0.0, - 7: 0.0 + 0: 0.0, # throttle back joystick x axis (left-to-right) + 1: 0.0, # throttle back joystick y axis (up-and-down) + 2: 0.0, # throttle slide + 3: 0.0, # unknown + 4: 0.0, # unknown + 5: 0.0, # throttle back paddle + 6: 0.0, # unknown + 7: 0.0 # throttle left slider } # Store the states of the throttle buttons throttle_button_state = { - 0: 0, - 1: 0, - 2: 0, - 3: 0, - 4: 0, - 5: 0, - 6: 0, - 7: 0, - 8: 0, - 9: 0, - 10: 0, - 11: 0, - 12: 0, - 13: 0 + 0: 0, # throttle right bottom (orange) button + 1: 0, # throttle back left (orange) button + 2: 0, # throttle back right (orange) button + 3: 0, # throttle back right rocker button up + 4: 0, # throttle back right rocker button down + 5: 0, # throttle back joystick button + 6: 0, # throttle right middle button up + 7: 0, # throttle right middle button forward + 8: 0, # throttle right middle button down + 9: 0, # throttle right middle button back + 10: 0, # throttle right bottm button up + 11: 0, # throttle right bottom button forward + 12: 0, # throttle right bottom button down + 13: 0 # throttle right bottom button back } # Store the state of throttle hats -throttle_hat_state = { - 0: 0, - 1: 0 +throttle_hat_state = { + 0: 0, # throttle hat (right top button) front-to-back (left-to-right) + 1: 0 # throttle hat (right top button) up-and-down } # Store the states of the joystick axes joystick_axis_state = { - 0: 0.0, - 1: 0.0, - 2: 0.0, - 3: 0.0 + 0: 0.0, # joystick y axis movement (left-to-right) + 1: 0.0, # joystick x axis movement (front-to-back) + 2: 0.0, # joystick yaw movement (twist) + 3: 0.0 # bottom controller slider } # Store the states of the joystick buttons joystick_button_state = { - 0: 0, - 1: 0, - 2: 0, - 3: 0, - 4: 0, - 5: 0, - 6: 0, - 7: 0, - 8: 0, - 9: 0, - 10: 0, - 11: 0, - 12: 0, - 13: 0 + 0: 0, # joystick trigger + 1: 0, # joystick top bottom button + 2: 0, # joystick top left button + 3: 0, # joystick top right button + 4: 0, # joystick left top left button + 5: 0, # joystick left top middle button + 6: 0, # joystick left top right button + 7: 0, # joystick left bottom right button + 8: 0, # joystick left bottom middle button + 9: 0, # joystick left bottom left button + 10: 0, # joystick right top right button + 11: 0, # joystick right top middle button + 12: 0, # joystick right top left button + 13: 0, # joystick right bottom left button + 14: 0, # joystick right bottom middle button + 15: 0 # joystick right bottom right button } # Stores the states of the joystick hats joystick_hat_state = { - 0: 0, - 1: 0 + 0: 0, # joystick hat y axis position (left-to-right) + 1: 0 # joystick hat x axis position (front-to-back) } From ff050d9574b74a8c3e606f5dc87c4f196ac22775 Mon Sep 17 00:00:00 2001 From: Icyadam14 Date: Thu, 17 Oct 2024 21:06:23 -0400 Subject: [PATCH 12/16] Added ethans control layout --- ros/gamepad/src/sender.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/ros/gamepad/src/sender.py b/ros/gamepad/src/sender.py index 1b85f39..1736c67 100644 --- a/ros/gamepad/src/sender.py +++ b/ros/gamepad/src/sender.py @@ -36,7 +36,7 @@ def __init__(self): self.tools = [0, 0, 0, 0, 0] # Mapping variables (TO CHANGE MAPPING: GO TO getMessage() AND MAKE YOUR CHANGES THERE) - self.mapping = 0 # 0, 1, 2, 3 + self.mapping = 1 # 0, 1, 2, 3 # Initialize the ros node rclpy.init() @@ -194,14 +194,15 @@ def getMessage(self): t.angular.y = -(self.joystick_axis_state[0] * SCALE_ROTATIONAL_Y) * self.reverse t.angular.z = -(self.joystick_axis_state[2] * SCALE_ROTATIONAL_Z) * self.reverse - elif self.mapping == 1: + else: # Set linear velocities - t.linear.x = -(self.throttle_axis_state[2] * SCALE_TRANSLATIONAL_X + TRIM_X) * self.reverse - t.linear.y = -(self.throttle_axis_state[5] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * self.reverse - t.linear.z = -(self.throttle_axis_state[1] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * self.reverse + t.linear.x = -(self.joystick_axis_state[1] * SCALE_TRANSLATIONAL_X + TRIM_X) * self.reverse + t.linear.y = (self.joystick_axis_state[0] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * self.reverse + t.linear.z = -(self.throttle_axis_state[2] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * self.reverse + # Set angular velocities - t.angular.x = -(self.joystick_axis_state[1] * SCALE_ROTATIONAL_X) * self.reverse - t.angular.y = -(self.joystick_axis_state[0] * SCALE_ROTATIONAL_Y) * self.reverse + t.angular.x = 0.0 # no pitch + t.angular.y = 0.0 # no roll t.angular.z = -(self.joystick_axis_state[2] * SCALE_ROTATIONAL_Z) * self.reverse new_msg = RovVelocityCommand() From 938485d9b4e9aa17ff6e6c1029a0bf627c45c452 Mon Sep 17 00:00:00 2001 From: Icyadam14 Date: Thu, 17 Oct 2024 21:48:35 -0400 Subject: [PATCH 13/16] Better rosified controller --- ros/gamepad/src/sender.py | 44 +++++++++++++++++---------------------- 1 file changed, 19 insertions(+), 25 deletions(-) diff --git a/ros/gamepad/src/sender.py b/ros/gamepad/src/sender.py index 1736c67..5025b12 100644 --- a/ros/gamepad/src/sender.py +++ b/ros/gamepad/src/sender.py @@ -6,14 +6,16 @@ # ROS import rclpy +from rclpy.node import Node from std_msgs.msg import String, Bool, Empty from shared_msgs.msg import RovVelocityCommand, ToolsCommandMsg from geometry_msgs.msg import Twist from config import * -class Controller: +class Controller(Node): def __init__(self): + super().__init__("gp_pub") # Pygame variables self.joystick = None self.throttle = None @@ -37,17 +39,13 @@ def __init__(self): # Mapping variables (TO CHANGE MAPPING: GO TO getMessage() AND MAKE YOUR CHANGES THERE) self.mapping = 1 # 0, 1, 2, 3 - - # Initialize the ros node - rclpy.init() - self.node = rclpy.create_node('gp_pub') try: self.init_pygame() except: - self.node.get_logger().info('Controllers not found. Please make sure both the joystick and throttle are connected') + self.get_logger().info('Controllers not found. Please make sure both the joystick and throttle are connected') if not self.reconnect(): - self.node.get_logger().info("\nNo gamepad found, exiting") + self.get_logger().info("\nNo gamepad found, exiting") pygame.quit() sys.exit(0) @@ -58,19 +56,7 @@ def __init__(self): # Create the timers self.data_thread = self.node.create_timer(0.1, self.pub_data) self.gamepad_thread = self.node.create_timer(0.001, self.update) - - self.node.get_logger().info('Controllers initialized') - - rclpy.spin(self.node) - - self.data_thread.destroy() - self.gamepad_thread.destroy() - - # Stop the pygame library - pygame.quit() - - self.node.destroy_node() - rclpy.shutdown() + self.get_logger().info('Controllers initialized') def init_pygame(self): @@ -95,7 +81,7 @@ def reconnect(self): reconnected = False i = GAMEPAD_TIMEOUT while i >= 0 and not reconnected: - self.node.get_logger().info('Gamepad disconnected, reconnect within {:2} seconds'.format(i)) + self.get_logger().info('Gamepad disconnected, reconnect within {:2} seconds'.format(i)) try: self.init_pygame() reconnected = True @@ -104,7 +90,7 @@ def reconnect(self): i -= 1 if reconnected: - self.node.get_logger().info('Controllers reconnected') + self.get_logger().info('Controllers reconnected') return reconnected @@ -164,9 +150,9 @@ def process_event(self, event): # Check if the event is a joydeviceremoved event elif event.type == pygame.JOYDEVICEREMOVED: # Try to reconnect the gamepad - self.node.get_logger().info('Controller disconnected. Attempting to reconnect...') + self.get_logger().info('Controller disconnected. Attempting to reconnect...') if not self.reconnect(): - self.node.get_logger().info("\nNo gamepad found, exiting") + self.get_logger().info("\nNo gamepad found, exiting") pygame.quit() sys.exit(0) @@ -223,5 +209,13 @@ def getTools(self): return tm -if __name__ == '__main__': +def main(): + rclpy.init(args=None) controller = Controller() + rclpy.spin(controller.node) + controller.node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() From e6abe0e1e875fbff42dcf40b892c70736e3e9d7e Mon Sep 17 00:00:00 2001 From: ethanburmane Date: Sat, 19 Oct 2024 13:50:13 -0400 Subject: [PATCH 14/16] trying to add PM --- ros/gamepad/src/sender.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/ros/gamepad/src/sender.py b/ros/gamepad/src/sender.py index 5025b12..806c49c 100644 --- a/ros/gamepad/src/sender.py +++ b/ros/gamepad/src/sender.py @@ -136,6 +136,10 @@ def process_event(self, event): # Check if the event is from the joystick or the throttle if event.joy == self.joystick_id: self.joystick_button_state[event.button] = 1 + if event.button == self.joystick_button_state[0]: + self.tools[0] = not self.tools[0] + elif event.button == self.joystick_button_state[1]: + self.tools[2] = not self.tools[2] elif event.joy == self.throttle_id: self.throttle_button_state[event.button] = 1 @@ -175,11 +179,16 @@ def getMessage(self): t.linear.x = -(self.throttle_axis_state[2] * SCALE_TRANSLATIONAL_X + TRIM_X) * self.reverse t.linear.y = -(self.throttle_axis_state[5] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * self.reverse t.linear.z = -(self.throttle_axis_state[1] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * self.reverse + # Set angular velocities t.angular.x = -(self.joystick_axis_state[1] * SCALE_ROTATIONAL_X) * self.reverse t.angular.y = -(self.joystick_axis_state[0] * SCALE_ROTATIONAL_Y) * self.reverse t.angular.z = -(self.joystick_axis_state[2] * SCALE_ROTATIONAL_Z) * self.reverse + # Set PM + # PM_grab = self.joystick_button_state[0] + # PM_pos = self.joystick_button_state[1] + else: # Set linear velocities t.linear.x = -(self.joystick_axis_state[1] * SCALE_TRANSLATIONAL_X + TRIM_X) * self.reverse @@ -191,6 +200,10 @@ def getMessage(self): t.angular.y = 0.0 # no roll t.angular.z = -(self.joystick_axis_state[2] * SCALE_ROTATIONAL_Z) * self.reverse + # Set PM + # PM_grab = self.joystick_button_state[0] + # PM_pos = self.joystick_button_state[1] + new_msg = RovVelocityCommand() new_msg.twist = t new_msg.is_fine = self.is_fine From abf26c2a98e1bf15cd73319626eb74bacd21533d Mon Sep 17 00:00:00 2001 From: ethanburmane Date: Sat, 19 Oct 2024 18:02:08 +0000 Subject: [PATCH 15/16] new PM test --- ros/gamepad/src/sender.py | 19 +++++++++++++++---- scripts/build.sh | 2 +- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/ros/gamepad/src/sender.py b/ros/gamepad/src/sender.py index 806c49c..9a63b6d 100644 --- a/ros/gamepad/src/sender.py +++ b/ros/gamepad/src/sender.py @@ -136,10 +136,7 @@ def process_event(self, event): # Check if the event is from the joystick or the throttle if event.joy == self.joystick_id: self.joystick_button_state[event.button] = 1 - if event.button == self.joystick_button_state[0]: - self.tools[0] = not self.tools[0] - elif event.button == self.joystick_button_state[1]: - self.tools[2] = not self.tools[2] + elif event.joy == self.throttle_id: self.throttle_button_state[event.button] = 1 @@ -151,6 +148,20 @@ def process_event(self, event): elif event.joy == self.throttle_id: self.throttle_button_state[event.button] = 0 + self.changed_trigger = False + if self.joystick_button_state[0] == 1 and self.changed_trigger == False: + self.tools[0] = not self.tools[0] + self.changed_trigger = True + if self.joystick_button_state[0] == 0: + self.changed_trigger = False + self.change_buttom = False + if self.joystick_button_state[1] == 1 and self.change_buttom == False: + self.tools[2] = not self.tools[2] + self.change_buttom = True + if self.joystick_button_state[1] == 0: + self.change_buttom = False + + # Check if the event is a joydeviceremoved event elif event.type == pygame.JOYDEVICEREMOVED: # Try to reconnect the gamepad diff --git a/scripts/build.sh b/scripts/build.sh index fba29a8..8c662ac 100755 --- a/scripts/build.sh +++ b/scripts/build.sh @@ -1,4 +1,4 @@ -#!/usr/bin/bash +#!/bin/bash source /opt/ros/humble/setup.bash colcon build From 21e5a233a226651b4755a3462a82bbda59949f34 Mon Sep 17 00:00:00 2001 From: Torin1914 Date: Sat, 19 Oct 2024 14:21:22 -0400 Subject: [PATCH 16/16] got tools to update topic --- ros/gamepad/src/sender.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/ros/gamepad/src/sender.py b/ros/gamepad/src/sender.py index 9a63b6d..6043a12 100644 --- a/ros/gamepad/src/sender.py +++ b/ros/gamepad/src/sender.py @@ -50,12 +50,12 @@ def __init__(self): sys.exit(0) # Create the publishers - self.pub = self.node.create_publisher(RovVelocityCommand, '/rov_velocity', 10) - self.pub_tools = self.node.create_publisher(ToolsCommandMsg, 'tools', 10) + self.pub = self.create_publisher(RovVelocityCommand, '/rov_velocity', 10) + self.pub_tools = self.create_publisher(ToolsCommandMsg, 'tools', 10) # Create the timers - self.data_thread = self.node.create_timer(0.1, self.pub_data) - self.gamepad_thread = self.node.create_timer(0.001, self.update) + self.data_thread = self.create_timer(0.1, self.pub_data) + self.gamepad_thread = self.create_timer(0.001, self.update) self.get_logger().info('Controllers initialized') @@ -152,12 +152,15 @@ def process_event(self, event): if self.joystick_button_state[0] == 1 and self.changed_trigger == False: self.tools[0] = not self.tools[0] self.changed_trigger = True + if self.joystick_button_state[0] == 0: self.changed_trigger = False + self.change_buttom = False if self.joystick_button_state[1] == 1 and self.change_buttom == False: self.tools[2] = not self.tools[2] self.change_buttom = True + if self.joystick_button_state[1] == 0: self.change_buttom = False @@ -177,7 +180,7 @@ def pub_data(self): # Get a message to publish for the rov_velocity topic self.pub.publish(self.getMessage()) # Get a message to publish for the tools topic - # pub_tools.publish(self.getTools()) + self.pub_tools.publish(self.getTools()) def getMessage(self): @@ -236,8 +239,8 @@ def getTools(self): def main(): rclpy.init(args=None) controller = Controller() - rclpy.spin(controller.node) - controller.node.destroy_node() + rclpy.spin(controller) + controller.destroy_node() rclpy.shutdown()