Skip to content

Rov control needs to be in main now #72

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Hermanoid merged 4 commits into from
May 3, 2025
Merged

Rov control needs to be in main now #72

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
30c23aa
Left and right controllers support, idk if any of this works
Sodapop465 May 2, 2025
66f1bc0
add missing deadzones
yameatmeyourdead May 3, 2025
f8d8dff
Increase hardcoding :)) for joysticks
Sodapop465 May 3, 2025
600f902
It's executable
Hermanoid May 3, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions src/driverstation/joy_config/joy_left_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
8 changes: 8 additions & 0 deletions src/driverstation/joy_config/joy_right_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
3 changes: 3 additions & 0 deletions src/driverstation/joy_config/joy_startup.sh
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
168 changes: 134 additions & 34 deletions src/rov/rov_control/rov_control/rov_control.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)}
Expand All @@ -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:
Expand All @@ -172,27 +189,43 @@ 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]:
self._button_transitions[index] += 1

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
Expand All @@ -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,
Expand All @@ -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": {
Expand All @@ -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,
Expand All @@ -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": {
Expand All @@ -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"
}
}
}
Expand Down Expand Up @@ -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()}
)
Expand Down Expand Up @@ -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(
Expand All @@ -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(
Expand Down
Loading