Skip to content

Commit b321ae0

Browse files
ShannonGriswoldShannonGriswold
committed
Used a dataclass to make controller profiles
1 parent 5c63ff5 commit b321ae0

File tree

2 files changed

+115
-20
lines changed

2 files changed

+115
-20
lines changed

‎src/surface/flight_control/flight_control/manual_control_node.py‎

Lines changed: 111 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
from enum import IntEnum
22
from typing import TYPE_CHECKING
3+
from dataclasses import dataclass
34

45
import rclpy
56
from mavros_msgs.srv import CommandBool
@@ -54,12 +55,37 @@ class ControllerMode(IntEnum):
5455
ARM = 0
5556
TOGGLE_CAMERAS = 1
5657

58+
@dataclass
59+
class ControllerProfile:
60+
manip_left: int = X_BUTTON
61+
manip_right: int = O_BUTTON
62+
valve_clockwise: int = TRI_BUTTON
63+
valve_counterclockwise: int = SQUARE_BUTTON
64+
roll_left: int = L1
65+
roll_right: int = R1
66+
cam_toggle_left: int = PAIRING_BUTTON
67+
cam_toggle_right: int = MENU
68+
arm_button: int = MENU
69+
disarm_button: int = PAIRING_BUTTON
70+
lateral: int = LJOYX
71+
forward: int = LJOYY
72+
vertical_up: int = L2PRESS_PERCENT
73+
vertical_down: int = R2PRESS_PERCENT
74+
yaw: int = RJOYX
75+
pitch: int = RJOYY
76+
77+
CONTROLLER_PROFILES = [
78+
ControllerProfile(),
79+
ControllerProfile(manip_left=L1, manip_right=R1, roll_left=TRI_BUTTON, roll_right=SQUARE_BUTTON),
80+
]
5781

5882
class ManualControlNode(Node):
5983
def __init__(self) -> None:
6084
super().__init__('manual_control_node')
6185

6286
mode_param = self.declare_parameter(CONTROLLER_MODE_PARAM, value=ControllerMode.ARM)
87+
profile_param = self.declare_parameter('controller_profile', value=0)
88+
self.profile = CONTROLLER_PROFILES[profile_param.value]
6389

6490
self.rc_pub = self.create_publisher(
6591
PixhawkInstruction, 'uninverted_pixhawk_control', qos_profile_system_default
@@ -93,9 +119,20 @@ def __init__(self) -> None:
93119
self.arm_client = self.create_client(CommandBool, 'mavros/cmd/arming')
94120

95121
self.manip_buttons: dict[int, ManipButton] = {
96-
X_BUTTON: ManipButton('left'),
97-
O_BUTTON: ManipButton('right'),
122+
self.profile.manip_left: ManipButton('left'),
123+
self.profile.manip_right: ManipButton('right'),
98124
}
125+
# if profile == ControllerProfile.PROFILE_0:
126+
# self.manip_buttons: dict[int, ManipButton] = {
127+
# X_BUTTON: ManipButton('left'),
128+
# O_BUTTON: ManipButton('right'),
129+
# }
130+
# elif profile == ControllerProfile.PROFILE_1:
131+
# self.manip_buttons: dict[int, ManipButton] = {
132+
# L1: ManipButton('left'),
133+
# R1: ManipButton('right'),
134+
# }
135+
99136

100137
self.seen_left_cam = False
101138
self.seen_right_cam = False
@@ -112,15 +149,36 @@ def joystick_to_pixhawk(self, msg: Joy) -> None:
112149
buttons: MutableSequence[int] = msg.buttons
113150

114151
instruction = PixhawkInstruction(
115-
forward=float(axes[LJOYY]), # Left Joystick Y
116-
lateral=-float(axes[LJOYX]), # Left Joystick X
117-
vertical=float(axes[L2PRESS_PERCENT] - axes[R2PRESS_PERCENT]) / 2, # L2/R2 triggers
118-
roll=float(buttons[L1] - buttons[R1]), # L1/R1 buttons
119-
pitch=float(axes[RJOYY]), # Right Joystick Y
120-
yaw=-float(axes[RJOYX]), # Right Joystick X
152+
forward=float(axes[self.profile.forward]),
153+
lateral=-float(axes[self.profile.lateral]),
154+
vertical=float(axes[self.profile.vertical_up] - axes[self.profile.vertical_down]) / 2,
155+
roll=float(buttons[self.profile.roll_left] - buttons[self.profile.roll_right]),
156+
pitch=float(axes[self.profile.pitch]),
157+
yaw=-float(axes[self.profile.yaw]),
121158
author=PixhawkInstruction.MANUAL_CONTROL,
122159
)
123160

161+
# if self.profile == ControllerProfile.PROFILE_0:
162+
# instruction = PixhawkInstruction(
163+
# forward=float(axes[LJOYY]), # Left Joystick Y
164+
# lateral=-float(axes[LJOYX]), # Left Joystick X
165+
# vertical=float(axes[L2PRESS_PERCENT] - axes[R2PRESS_PERCENT]) / 2, # L2/R2 triggers
166+
# roll=float(buttons[L1] - buttons[R1]), # L1/R1 buttons
167+
# pitch=float(axes[RJOYY]), # Right Joystick Y
168+
# yaw=-float(axes[RJOYX]), # Right Joystick X
169+
# author=PixhawkInstruction.MANUAL_CONTROL,
170+
# )
171+
# elif self.profile == ControllerProfile.PROFILE_1:
172+
# instruction = PixhawkInstruction(
173+
# forward=float(axes[LJOYY]), # Left Joystick Y
174+
# lateral=-float(axes[LJOYX]), # Left Joystick X
175+
# vertical=float(axes[L2PRESS_PERCENT] - axes[R2PRESS_PERCENT]) / 2, # L2/R2 triggers
176+
# roll=float(buttons[X_BUTTON] - buttons[O_BUTTON]), # L1/R1 buttons
177+
# pitch=float(axes[RJOYY]), # Right Joystick Y
178+
# yaw=-float(axes[RJOYX]), # Right Joystick X
179+
# author=PixhawkInstruction.MANUAL_CONTROL,
180+
# )
181+
124182
self.rc_pub.publish(instruction)
125183

126184
def manip_callback(self, msg: Joy) -> None:
@@ -138,42 +196,76 @@ def manip_callback(self, msg: Joy) -> None:
138196
manip_button.last_button_state = just_pressed
139197

140198
def valve_manip_callback(self, msg: Joy) -> None:
141-
tri_pressed = msg.buttons[TRI_BUTTON] == PRESSED
142-
square_pressed = msg.buttons[SQUARE_BUTTON] == PRESSED
143-
if tri_pressed and not self.valve_manip_state:
199+
# if self.profile == ControllerProfile.PROFILE_0:
200+
# clockwise_pressed = msg.buttons[TRI_BUTTON] == PRESSED
201+
# counter_clockwise_pressed = msg.buttons[SQUARE_BUTTON] == PRESSED
202+
# elif self.profile == ControllerProfile.PROFILE_1:
203+
# clockwise_pressed = msg.buttons[SQUARE_BUTTON] == PRESSED
204+
# counter_clockwise_pressed = msg.buttons[TRI_BUTTON] == PRESSED
205+
clockwise_pressed = msg.buttons[self.profile.valve_clockwise] == PRESSED
206+
counter_clockwise_pressed = msg.buttons[self.profile.valve_counterclockwise] == PRESSED
207+
208+
if clockwise_pressed and not self.valve_manip_state:
144209
self.valve_manip.publish(ValveManip(active=True, pwm=ValveManip.MAX_PWM))
145210
self.valve_manip_state = True
146-
elif square_pressed and not self.valve_manip_state:
211+
elif counter_clockwise_pressed_pressed and not self.valve_manip_state:
147212
self.valve_manip.publish(ValveManip(active=True, pwm=ValveManip.MIN_PWM))
148213
self.valve_manip_state = True
149-
elif self.valve_manip_state and not tri_pressed and not square_pressed:
214+
elif self.valve_manip_state and not clockwise_pressed and not counter_clockwise_pressed:
150215
self.valve_manip.publish(ValveManip(active=False))
151216
self.valve_manip_state = False
217+
# tri_pressed = msg.buttons[TRI_BUTTON] == PRESSED
218+
# square_pressed = msg.buttons[SQUARE_BUTTON] == PRESSED
219+
# if tri_pressed and not self.valve_manip_state:
220+
# self.valve_manip.publish(ValveManip(active=True, pwm=ValveManip.MAX_PWM))
221+
# self.valve_manip_state = True
222+
# elif square_pressed and not self.valve_manip_state:
223+
# self.valve_manip.publish(ValveManip(active=True, pwm=ValveManip.MIN_PWM))
224+
# self.valve_manip_state = True
225+
# elif self.valve_manip_state and not tri_pressed and not square_pressed:
226+
# self.valve_manip.publish(ValveManip(active=False))
227+
# self.valve_manip_state = False
152228

153229
def toggle_cameras(self, msg: Joy) -> None:
154230
"""Cycles through connected cameras on pilot GUI using menu and pairing buttons."""
155231
buttons: MutableSequence[int] = msg.buttons
156232

157-
if buttons[MENU] == PRESSED:
233+
if buttons[self.profile.cam_toggle_right] == PRESSED:
158234
self.seen_right_cam = True
159-
elif buttons[PAIRING_BUTTON] == PRESSED:
235+
elif buttons[self.profile.cam_toggle_left] == PRESSED:
160236
self.seen_left_cam = True
161-
elif buttons[MENU] == UNPRESSED and self.seen_right_cam:
237+
elif buttons[self.profile.cam_toggle_right] == UNPRESSED and self.seen_right_cam:
162238
self.seen_right_cam = False
163239
self.camera_toggle_publisher.publish(CameraControllerSwitch(toggle_right=True))
164-
elif buttons[PAIRING_BUTTON] == UNPRESSED and self.seen_left_cam:
240+
elif buttons[self.profile.cam_toggle_left] == UNPRESSED and self.seen_left_cam:
165241
self.seen_left_cam = False
166242
self.camera_toggle_publisher.publish(CameraControllerSwitch(toggle_right=False))
167243

244+
# if buttons[MENU] == PRESSED:
245+
# self.seen_right_cam = True
246+
# elif buttons[PAIRING_BUTTON] == PRESSED:
247+
# self.seen_left_cam = True
248+
# elif buttons[MENU] == UNPRESSED and self.seen_right_cam:
249+
# self.seen_right_cam = False
250+
# self.camera_toggle_publisher.publish(CameraControllerSwitch(toggle_right=True))
251+
# elif buttons[PAIRING_BUTTON] == UNPRESSED and self.seen_left_cam:
252+
# self.seen_left_cam = False
253+
# self.camera_toggle_publisher.publish(CameraControllerSwitch(toggle_right=False))
254+
168255
def set_arming(self, msg: Joy) -> None:
169256
"""Set the arming state using the menu and pairing buttons."""
170257
buttons: MutableSequence[int] = msg.buttons
171258

172-
if buttons[MENU] == PRESSED:
259+
if buttons[self.profile.arm_button] == PRESSED:
173260
self.arm_client.call_async(ARM_MESSAGE)
174-
elif buttons[PAIRING_BUTTON] == PRESSED:
261+
elif buttons[self.profile.disarm_button] == PRESSED:
175262
self.arm_client.call_async(DISARM_MESSAGE)
176263

264+
# if buttons[MENU] == PRESSED:
265+
# self.arm_client.call_async(ARM_MESSAGE)
266+
# elif buttons[PAIRING_BUTTON] == PRESSED:
267+
# self.arm_client.call_async(DISARM_MESSAGE)
268+
177269

178270
class ManipButton:
179271
def __init__(self, claw: str) -> None:

‎src/surface/flight_control/launch/flight_control_launch.py‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,10 @@ def generate_launch_description() -> LaunchDescription:
77
manual_control_node = Node(
88
package='flight_control',
99
executable='manual_control_node',
10-
parameters=[{'controller_mode': LaunchConfiguration('controller_mode', default=0)}],
10+
parameters=[
11+
{'controller_mode': LaunchConfiguration('controller_mode', default=0)},
12+
{'controller_profile': LaunchConfiguration('controller_profile', default=0)}
13+
],
1114
remappings=[
1215
('/surface/manipulator_control', '/tether/manipulator_control'),
1316
('/surface/valve_manipulator', '/tether/valve_manipulator'),

0 commit comments

Comments
 (0)