From 30c23aa3784be1718726b16255511bc3606e16c7 Mon Sep 17 00:00:00 2001 From: Kadin Le Date: Thu, 1 May 2025 23:53:47 -0600 Subject: [PATCH 1/4] Left and right controllers support, idk if any of this works --- .../joy_config/joy_left_params.yaml | 8 ++ .../joy_config/joy_right_params.yaml | 8 ++ .../rov_control/rov_control/rov_control.py | 125 +++++++++++++++--- 3 files changed, 121 insertions(+), 20 deletions(-) create mode 100644 src/driverstation/joy_config/joy_left_params.yaml create mode 100644 src/driverstation/joy_config/joy_right_params.yaml diff --git a/src/driverstation/joy_config/joy_left_params.yaml b/src/driverstation/joy_config/joy_left_params.yaml new file mode 100644 index 0000000..2b4ff11 --- /dev/null +++ b/src/driverstation/joy_config/joy_left_params.yaml @@ -0,0 +1,8 @@ +joy_node: + ros__parameters: + device_id: 0 + device_name: "" + deadzone: 0.1 + autorepeat_rate: 20.0 + sticky_buttons: false + coalesce_interval_ms: 1 \ No newline at end of file diff --git a/src/driverstation/joy_config/joy_right_params.yaml b/src/driverstation/joy_config/joy_right_params.yaml new file mode 100644 index 0000000..33a7c3d --- /dev/null +++ b/src/driverstation/joy_config/joy_right_params.yaml @@ -0,0 +1,8 @@ +joy_node: + ros__parameters: + device_id: 1 + device_name: "" + deadzone: 0.1 + autorepeat_rate: 20.0 + sticky_buttons: false + coalesce_interval_ms: 1 \ No newline at end of file diff --git a/src/rov/rov_control/rov_control/rov_control.py b/src/rov/rov_control/rov_control/rov_control.py index 0b6d03d..6b8c21f 100644 --- a/src/rov/rov_control/rov_control/rov_control.py +++ b/src/rov/rov_control/rov_control/rov_control.py @@ -139,17 +139,23 @@ def __init__( elif isinstance(button_names, dict): self._button_aliases = {name: index for name, index in button_names.items() if index >= 0} + # values of joystick self._axis_values: List[float] = [] self._button_states: List[bool] = [] self._button_transitions: List[int] = [] + # number of axes and buttons for each joystick, used for recognizing different joystick messages + self._num_axes: List[int] = [] + self._num_buttons: List[int] = [] + + self._joystick_update_subscription = node.create_subscription( Joy, joystick_topic_name, self.on_joystick_message, 10 ) - + def axis(self, name_or_index: Union[str, int]) -> Axis: if type(name_or_index) == str and self._axis_aliases.get(name_or_index) != None: try: @@ -173,19 +179,31 @@ def button(self, name_or_index: Union[str, int]) -> Button: return Button(None, False) def on_joystick_message(self, message: Joy): - if len(self._axis_values) < len(message.axes): - self._axis_values.extend([0.0] * (len(message.axes) - len(self._axis_values))) - - for index, axis in enumerate(message.axes): + # Check length of message to determine which joystick + if len(message.axes) not in self._num_axes: + self._num_axes.append(len(message.axes)) + self._axis_values.extend([0.0] * len(message.axes)) + if len(message.buttons) not in self._num_buttons: + self._num_buttons.append(len(message.buttons)) + self._button_states.extend([0.0] * len(message.buttons)) + self._button_transitions.extend([0.0] * len(message.buttons)) + + # axes will be listed from controller with least number of axes to most number of axes + starting_index = 0 + for num in self._num_axes: + if num < len(message.axes): + starting_index += num + + for index, axis in enumerate(message.axes, start = starting_index): self._axis_values[index] = axis - if len(self._button_states) < len(message.buttons): - self._button_states.extend([0.0] * (len(message.buttons) - len(self._button_states))) - - if len(self._button_transitions) < len(message.buttons): - self._button_transitions.extend([0.0] * (len(message.buttons) - len(self._button_transitions))) + # buttons will be listed from controller with least number of buttons to most number of buttons + starting_index = 0 + for num in self._num_buttons: + if num < len(message.buttons): + starting_index += num - for index, button in enumerate(message.buttons): + for index, button in enumerate(message.buttons, start = starting_index): button_state: bool = bool(button) if button_state != self._button_states[index]: @@ -193,6 +211,19 @@ def on_joystick_message(self, message: Joy): self._button_states[index] = button_state + # debug + debug_message = "Axes: \n" + for index, axis in enumerate(self._axis_values): + debug_message += " " + debug_message += str(index) + debug_message += (". %f\n" % axis) + debug_message += "Buttons:\n" + for index, button in enumerate(self._button_states): + debug_message += " " + debug_message += str(index) + debug_message += (". {}".format(button)) + print(debug_message) + class RovControlState(Enum): OFF = 0 SUSPENDED = 1 @@ -207,14 +238,24 @@ def __init__(self, node_name: str = "rov_control", *args, **kwargs): "update_frequency_hz": 60, "mapping": { "axis": { + # Right Joystick "roll": 0, "pitch": 1, "yaw": 2, - "throttle": 3, "hat_x": 4, - "hat_y": 5 + "hat_y": 5, + # Left Joystick + "throttle_y": 6, + "throttle_x": 7, + "throttle_z": 11, + "A1_hat_x": 14, + "A1_hat_y": 15, + "A1_joy_x": 9, + "A1_joy_y": 10, + "middle_flippy_thing": 8 }, "button": { + # Right Joystick "button_1": 0, "button_2": 1, "button_3": 2, @@ -239,7 +280,39 @@ def __init__(self, node_name: str = "rov_control", *args, **kwargs): "base_middle_left": 8, "base_middle_right": 9, "base_back_left": 10, - "base_back_right": 11 + "base_back_right": 11, + # Left Joystick + "" + "fire_top": 33, + "fire_bottom": 12, + "A2": 14, + "B1": 15, + "A3_up": 17, + "A3_right": 18, + "A3_down": 19, + "A3_left": 20, + "A3_center": 21, + "A4_up": 22, + "A4_right": 23, + "A4_down": 24, + "A4_left": 25, + "A4_center": 26, + "C1_up": 27, + "C1_right": 28, + "C1_down": 29, + "C1_left": 30, + "D1": 16, + "scroll_up": 34, + "scroll_down": 35, + "En1_up": 34, + "En1_down": 35, + "left_flippy_up": 36, + "left_flippy_down": 37, + "Sw1_up": 36, + "Sw1_down": 37, + "F1": 38, + "F2": 39, + "F3": 40 } }, "control": { @@ -253,9 +326,9 @@ def __init__(self, node_name: str = "rov_control", *args, **kwargs): }, "drive": { "axis": { - "vx": "hat_y", - "vy": "hat_x", - "vz": "throttle", + "vx": "throttle_x", + "vy": "throttle_y", + "vz": "throttle_z", "omegax": "roll", "omegay": "pitch", "omegaz": "yaw", @@ -271,8 +344,8 @@ def __init__(self, node_name: str = "rov_control", *args, **kwargs): }, "manipulator": { "axis": { - "wrist": "roll", - "clamp": "hat_y" + "wrist": "A1_joy_x", + "clamp": "A1_joy_y" }, "scale": { "wrist": 1.0, @@ -286,7 +359,7 @@ def __init__(self, node_name: str = "rov_control", *args, **kwargs): "timeout_seconds": 1.0 }, "mode_switching": { - "button": "stick_bottom_left" + "button": "A2" } } } @@ -415,6 +488,18 @@ def on_update(self): self.get_parameters_by_prefix("control.drive.axis").items() }) ) + self._manipulator_setpoint_publisher.publish( + ManipulatorSetpoints(**{ + manipulator_axis: max(-1.0, min( + self.get_parameter(f"control.manipulator.scale.{manipulator_axis}").value + * self + ._joystick.axis(joystick_axis.value) + .deadzoned(self.get_parameter(f"control.deadzone.{joystick_axis.value}").value).value, + 1.0)) + for manipulator_axis, joystick_axis in + self.get_parameters_by_prefix("control.manipulator.axis").items() + }) + ) elif self.state == RovControlState.MANIPULATOR_CONTROL: self._manipulator_setpoint_publisher.publish( ManipulatorSetpoints(**{ From 66f1bc01f26d9582f6543545b700636c5c7404cd Mon Sep 17 00:00:00 2001 From: Zac Stanton Date: Fri, 2 May 2025 20:07:32 -0600 Subject: [PATCH 2/4] add missing deadzones --- .../rov_control/rov_control/rov_control.py | 39 ++++++++++++------- 1 file changed, 25 insertions(+), 14 deletions(-) diff --git a/src/rov/rov_control/rov_control/rov_control.py b/src/rov/rov_control/rov_control/rov_control.py index 6b8c21f..7605af0 100644 --- a/src/rov/rov_control/rov_control/rov_control.py +++ b/src/rov/rov_control/rov_control/rov_control.py @@ -212,17 +212,17 @@ def on_joystick_message(self, message: Joy): self._button_states[index] = button_state # debug - debug_message = "Axes: \n" - for index, axis in enumerate(self._axis_values): - debug_message += " " - debug_message += str(index) - debug_message += (". %f\n" % axis) - debug_message += "Buttons:\n" - for index, button in enumerate(self._button_states): - debug_message += " " - debug_message += str(index) - debug_message += (". {}".format(button)) - print(debug_message) + # debug_message = "Axes: \n" + # for index, axis in enumerate(self._axis_values): + # debug_message += " " + # debug_message += str(index) + # debug_message += (". %f\n" % axis) + # debug_message += "Buttons:\n" + # for index, button in enumerate(self._button_states): + # debug_message += " " + # debug_message += str(index) + # debug_message += (". {}".format(button)) + # print(debug_message) class RovControlState(Enum): OFF = 0 @@ -321,6 +321,17 @@ def __init__(self, node_name: str = "rov_control", *args, **kwargs): "pitch": 0.1, "yaw": 0.1, "throttle": 0.1, + "throttle_x": 0.1, + "throttle_y": 0.1, + "throttle_z": 0.1, + "throttle_y": 0.1, + "throttle_x": 0.1, + "throttle_z": 0.1, + "A1_hat_x": 0.1, + "A1_hat_y": 0.1, + "A1_joy_x": 0.1, + "A1_joy_y": 0.1, + "middle_flippy_thing": 0.1, "hat_x": 0.0, "hat_y": 0.0 }, @@ -335,7 +346,7 @@ def __init__(self, node_name: str = "rov_control", *args, **kwargs): }, "scale": { "vx": 1.0, - "vy": 1.0, + "vy": -1.0, "vz": 1.0, "omegax": 1.0, "omegay": -1.0, @@ -348,8 +359,8 @@ def __init__(self, node_name: str = "rov_control", *args, **kwargs): "clamp": "A1_joy_y" }, "scale": { - "wrist": 1.0, - "clamp": 1.0 + "wrist": -1.0, + "clamp": -1.0 } }, "other": { From f8d8dff503e1d2e1336e90550d9d422456926c3b Mon Sep 17 00:00:00 2001 From: Kadin Le Date: Sat, 3 May 2025 07:30:30 -0600 Subject: [PATCH 3/4] Increase hardcoding :)) for joysticks --- src/driverstation/joy_config/joy_startup.sh | 3 + .../rov_control/rov_control/rov_control.py | 92 ++++++++++--------- 2 files changed, 51 insertions(+), 44 deletions(-) create mode 100644 src/driverstation/joy_config/joy_startup.sh diff --git a/src/driverstation/joy_config/joy_startup.sh b/src/driverstation/joy_config/joy_startup.sh new file mode 100644 index 0000000..6f04b04 --- /dev/null +++ b/src/driverstation/joy_config/joy_startup.sh @@ -0,0 +1,3 @@ +#!/bin/bash +source install/setup.bash +ros2 run joy joy_node --ros-args --params-file src/driverstation/joy_config/joy_left_params.yaml --remap joy:=joy_left & ros2 run joy joy_node --ros-args --params-file src/driverstation/joy_config/joy_right_params.yaml --remap joy:=joy_right \ No newline at end of file diff --git a/src/rov/rov_control/rov_control/rov_control.py b/src/rov/rov_control/rov_control/rov_control.py index 7605af0..a8b32d5 100644 --- a/src/rov/rov_control/rov_control/rov_control.py +++ b/src/rov/rov_control/rov_control/rov_control.py @@ -123,10 +123,13 @@ class Joystick: def __init__( self, node: Node, - joystick_topic_name: str, + joystick_topic_name_l: str, + joystick_topic_name_r: str, axis_names: Union[List[str], Dict[str, int]], button_names: Union[List[str], Dict[str, int]] ): + # TODO: Un-emergency fix this to actually use two joystick instances, each with their own topic name, rather than + # all this functionality shoved into the same node bc I didn't feel like editting the button axis-offset mapping stuff. self._axis_aliases: Dict[str, int] = {} if isinstance(axis_names, list): self._axis_aliases = {name: index for index, name in enumerate(axis_names)} @@ -139,20 +142,28 @@ def __init__( elif isinstance(button_names, dict): self._button_aliases = {name: index for name, index in button_names.items() if index >= 0} - # values of joystick - self._axis_values: List[float] = [] - self._button_states: List[bool] = [] - self._button_transitions: List[int] = [] - # number of axes and buttons for each joystick, used for recognizing different joystick messages - self._num_axes: List[int] = [] - self._num_buttons: List[int] = [] + # # number of axes and buttons for each joystick, used for recognizing different joystick messages + self._num_axes: List[int] = [6, 10] + self._num_buttons: List[int] = [12, 79] + # # values of joystick + self._axis_values: List[float] = [0] * sum(self._num_axes) + self._button_states: List[bool] = [0] * sum(self._num_buttons) + self._button_transitions: List[int] = [0] * sum(self._num_buttons) self._joystick_update_subscription = node.create_subscription( Joy, - joystick_topic_name, - self.on_joystick_message, + joystick_topic_name_r, + lambda msg: self.on_joystick_message(msg, 0, 0), + 10 + ) + # TODO Seriously un-hardcode these offsets + # Offset left controller by number of buttons+axes on right control bc that's how the mapping is rn + self._joystick_update_subscription = node.create_subscription( + Joy, + joystick_topic_name_l, + lambda msg: self.on_joystick_message(msg, self._num_buttons[0], self._num_axes[0]), 10 ) @@ -178,32 +189,23 @@ def button(self, name_or_index: Union[str, int]) -> Button: else: return Button(None, False) - def on_joystick_message(self, message: Joy): + def on_joystick_message(self, message: Joy, button_offset:int, axes_offset:int): # Check length of message to determine which joystick - if len(message.axes) not in self._num_axes: - self._num_axes.append(len(message.axes)) - self._axis_values.extend([0.0] * len(message.axes)) - if len(message.buttons) not in self._num_buttons: - self._num_buttons.append(len(message.buttons)) - self._button_states.extend([0.0] * len(message.buttons)) - self._button_transitions.extend([0.0] * len(message.buttons)) + # if len(message.axes) not in self._num_axes: + # self._num_axes.append(len(message.axes)) + # self._axis_values.extend([0.0] * len(message.axes)) + # if len(message.buttons) not in self._num_buttons: + # self._num_buttons.append(len(message.buttons)) + # self._button_states.extend([0.0] * len(message.buttons)) + # self._button_transitions.extend([0.0] * len(message.buttons)) + print("Num Butt (hehe)", len(message.buttons)) + print("Num Axe", len(message.axes)) # axes will be listed from controller with least number of axes to most number of axes - starting_index = 0 - for num in self._num_axes: - if num < len(message.axes): - starting_index += num - for index, axis in enumerate(message.axes, start = starting_index): + for index, axis in enumerate(message.axes, start = axes_offset): self._axis_values[index] = axis - - # buttons will be listed from controller with least number of buttons to most number of buttons - starting_index = 0 - for num in self._num_buttons: - if num < len(message.buttons): - starting_index += num - - for index, button in enumerate(message.buttons, start = starting_index): + for index, button in enumerate(message.buttons, start = button_offset): button_state: bool = bool(button) if button_state != self._button_states[index]: @@ -436,7 +438,8 @@ def __init__(self, node_name: str = "rov_control", *args, **kwargs): self._joystick = Joystick( node = self, - joystick_topic_name = "joy", + joystick_topic_name_l = "joy_left", + joystick_topic_name_r = "joy_right", axis_names = {parameter_name: parameter.value for parameter_name, parameter in self.get_parameters_by_prefix("mapping.axis").items()}, button_names = {parameter_name: parameter.value for parameter_name, parameter in self.get_parameters_by_prefix("mapping.button").items()} ) @@ -512,18 +515,19 @@ def on_update(self): }) ) elif self.state == RovControlState.MANIPULATOR_CONTROL: - self._manipulator_setpoint_publisher.publish( - ManipulatorSetpoints(**{ - manipulator_axis: max(-1.0, min( - self.get_parameter(f"control.manipulator.scale.{manipulator_axis}").value - * self - ._joystick.axis(joystick_axis.value) - .deadzoned(self.get_parameter(f"control.deadzone.{joystick_axis.value}").value).value, - 1.0)) - for manipulator_axis, joystick_axis in - self.get_parameters_by_prefix("control.manipulator.axis").items() - }) - ) + print("HOLY FUDGE AGHHHH YOU'RE IN MANIPULATOR CONTROL MODE AND SHOULDN'T BE, PRESS A2 OR WHATEVER IT IS") + # self._manipulator_setpoint_publisher.publish( + # ManipulatorSetpoints(**{ + # manipulator_axis: max(-1.0, min( + # self.get_parameter(f"control.manipulator.scale.{manipulator_axis}").value + # * self + # ._joystick.axis(joystick_axis.value) + # .deadzoned(self.get_parameter(f"control.deadzone.{joystick_axis.value}").value).value, + # 1.0)) + # for manipulator_axis, joystick_axis in + # self.get_parameters_by_prefix("control.manipulator.axis").items() + # }) + # ) elif self.state == RovControlState.OFF : #send all manipulators zero command self._manipulator_setpoint_publisher.publish( From 600f9021863014c45aeb2245cd66d923bc2b773b Mon Sep 17 00:00:00 2001 From: Lucas Date: Sat, 3 May 2025 07:55:07 -0600 Subject: [PATCH 4/4] It's executable --- src/driverstation/joy_config/joy_startup.sh | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 src/driverstation/joy_config/joy_startup.sh diff --git a/src/driverstation/joy_config/joy_startup.sh b/src/driverstation/joy_config/joy_startup.sh old mode 100644 new mode 100755