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/driverstation/joy_config/joy_startup.sh b/src/driverstation/joy_config/joy_startup.sh new file mode 100755 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 0b6d03d..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,17 +142,31 @@ def __init__( elif isinstance(button_names, dict): self._button_aliases = {name: index for name, index in button_names.items() if index >= 0} - 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] = [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 + ) + 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: @@ -172,20 +189,23 @@ def button(self, name_or_index: Union[str, int]) -> Button: else: 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): + 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)) + 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 + + for index, axis in enumerate(message.axes, start = axes_offset): 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))) - - for index, button in enumerate(message.buttons): + for index, button in enumerate(message.buttons, start = button_offset): button_state: bool = bool(button) if button_state != self._button_states[index]: @@ -193,6 +213,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 +240,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 +282,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": { @@ -248,21 +323,32 @@ 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 }, "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", }, "scale": { "vx": 1.0, - "vy": 1.0, + "vy": -1.0, "vz": 1.0, "omegax": 1.0, "omegay": -1.0, @@ -271,12 +357,12 @@ 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, - "clamp": 1.0 + "wrist": -1.0, + "clamp": -1.0 } }, "other": { @@ -286,7 +372,7 @@ def __init__(self, node_name: str = "rov_control", *args, **kwargs): "timeout_seconds": 1.0 }, "mode_switching": { - "button": "stick_bottom_left" + "button": "A2" } } } @@ -352,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()} ) @@ -415,7 +502,6 @@ def on_update(self): self.get_parameters_by_prefix("control.drive.axis").items() }) ) - elif self.state == RovControlState.MANIPULATOR_CONTROL: self._manipulator_setpoint_publisher.publish( ManipulatorSetpoints(**{ manipulator_axis: max(-1.0, min( @@ -428,6 +514,20 @@ def on_update(self): self.get_parameters_by_prefix("control.manipulator.axis").items() }) ) + elif self.state == RovControlState.MANIPULATOR_CONTROL: + 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(