Skip to content

Commit

Permalink
Auto-format code using Black
Browse files Browse the repository at this point in the history
  • Loading branch information
@actions-user
actions-user committed Oct 19, 2024
1 parent e1da4a4 commit 943af77
Show file tree
Hide file tree
Showing 3 changed files with 239 additions and 180 deletions.
96 changes: 48 additions & 48 deletions ros/gamepad/src/config.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,77 +15,77 @@
TRIM_Y = 0.0
TRIM_Z = 0.0

GAMEPAD_TIMEOUT = 20 # seconds
GAMEPAD_TIMEOUT = 20 # seconds

JOYSTICK_NAME = "Thrustmaster T.16000M"
THROTTLE_NAME = "Thrustmaster TWCS Throttle"

# Store the states of the throttle axies
throttle_axis_state = {
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
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, # 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
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, # throttle hat (right top button) front-to-back (left-to-right)
1: 0 # throttle hat (right top button) up-and-down
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, # 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
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, # 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
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, # joystick hat y axis position (left-to-right)
1: 0 # joystick hat x axis position (front-to-back)
0: 0, # joystick hat y axis position (left-to-right)
1: 0, # joystick hat x axis position (front-to-back)
}
126 changes: 74 additions & 52 deletions ros/gamepad/src/sender.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@

from config import *


class Controller(Node):
def __init__(self):
super().__init__("gp_pub")
Expand All @@ -38,29 +39,30 @@ 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 = 1 # 0, 1, 2, 3
self.mapping = 1 # 0, 1, 2, 3

try:
self.init_pygame()
except:
self.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.get_logger().info("\nNo gamepad found, exiting")
pygame.quit()
sys.exit(0)

# Create the publishers
self.pub = self.create_publisher(RovVelocityCommand, '/rov_velocity', 10)
self.pub_tools = self.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.create_timer(0.1, self.pub_data)
self.gamepad_thread = self.create_timer(0.001, self.update)
self.get_logger().info('Controllers initialized')

self.get_logger().info("Controllers initialized")

def init_pygame(self):
'''Initializes pygame and the joystick'''
"""Initializes pygame and the joystick"""
pygame.init()
pygame.joystick.init()
# Should be two joysticks
Expand All @@ -75,43 +77,42 @@ def init_pygame(self):
self.throttle = pygame.joystick.Joystick(i)
self.throttle_id = i


def reconnect(self):
'''Tries to reconnect the gamepad'''
"""Tries to reconnect the gamepad"""
reconnected = False
i = GAMEPAD_TIMEOUT
while i >= 0 and not reconnected:
self.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
except:
pygame.time.wait(1000) # Wait 1 second
pygame.time.wait(1000) # Wait 1 second
i -= 1

if reconnected:
self.get_logger().info('Controllers reconnected')

return reconnected
self.get_logger().info("Controllers reconnected")

return reconnected

def update(self):
'''Updates the gamepad state'''
"""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(self, raw):
'''Corrects the raw value from the gamepad to be in the range [-1.0, 1.0]'''
"""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

if raw < 0:
if raw <= -STICK_DEAD_ZONE:
return raw
Expand All @@ -120,9 +121,8 @@ def correct_raw(self, raw):
else:
return 0


def process_event(self, event):
'''Processes a pygame 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
Expand All @@ -136,7 +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

elif event.joy == self.throttle_id:
self.throttle_button_state[event.button] = 1

Expand All @@ -152,68 +152,91 @@ 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


# Check if the event is a joydeviceremoved event
elif event.type == pygame.JOYDEVICEREMOVED:
# Try to reconnect the gamepad
self.get_logger().info('Controller disconnected. Attempting to reconnect...')
self.get_logger().info(
"Controller disconnected. Attempting to reconnect..."
)
if not self.reconnect():
self.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'''
"""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
self.pub_tools.publish(self.getTools())


def getMessage(self):
'''Returns a RovVelocityCommand message based on the current gamepad state'''

"""Returns a RovVelocityCommand message based on the current gamepad state"""

t = Twist()

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
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
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
# 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
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
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 = 0.0 # no pitch
t.angular.y = 0.0 # no roll
t.angular.z = -(self.joystick_axis_state[2] * SCALE_ROTATIONAL_Z) * 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
)

# Set PM
# PM_grab = self.joystick_button_state[0]
Expand All @@ -226,25 +249,24 @@ def getMessage(self):
new_msg.depth_lock = self.depth_lock
new_msg.pitch_lock = self.pitch_lock

return(new_msg)
return new_msg

def getTools(self):
'''Returns a ToolsCommandMsg message based on the current gamepad state'''
"""Returns a ToolsCommandMsg message based on the current gamepad state"""

tm = ToolsCommandMsg()
tm.tools = [i for i in self.tools]

return tm



def main():
rclpy.init(args=None)
controller = Controller()
rclpy.spin(controller)
controller.destroy_node()
rclpy.shutdown()

if __name__ == '__main__':
main()

if __name__ == "__main__":
main()
Loading

0 comments on commit 943af77

Please sign in to comment.