plugin.xml

@@ -27,15 +27,15 @@
     </qtgui>
   </class>
 
-  <class name="ExamplePlugin" type="qubo_gui.plugin2.Plugin2" base_class_type="rqt_gui_py::Plugin">
+  <class name="TeleopPlugin" type="qubo_gui.teleop.TeleopPlugin" base_class_type="rqt_gui_py::Plugin">
     <description>
-      An example Python GUI plugin to create a great user interface.
+      Operate Qubo manually and autonomously.
     </description>
     <qtgui>
       <group>
-        <label>Visualization</label>
+        <label>Qubo Control</label>
       </group>
-      <label>another plugin of mine</label>
+      <label>Qubo Teleop</label>
       <icon type="theme">system-help</icon>
       <statustip>Great user interface to provide real value.</statustip>
     </qtgui>

qubo_gui/teleop.py

+from inputs import get_gamepad
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Int32, UInt8MultiArray, String
+import math
+import logging
+import threading
+import numpy as np
+
+# ROS
+import rclpy
+
+# rqt and QT
+from qt_gui.plugin import Plugin
+from python_qt_binding import QtCore
+from python_qt_binding.QtCore import Qt, QTimer
+from python_qt_binding.QtWidgets import QWidget, QLabel, QSlider, QPushButton, QHBoxLayout, QVBoxLayout
+
+class TeleopPlugin(Plugin):
+    channel = '/wrench'
+
+    def __init__(self, context):
+        super(TeleopPlugin, self).__init__(context)
+
+        self._context = context
+        self.setObjectName("TeleopPlugin")
+
+        # Access the RQT ROS node.
+        self._node = self._context.node
+
+        self._widget = QWidget()
+        self.label_joy_x = QLabel("X: 000000", parent=self._widget)
+
+        self._joy = XboxController()
+        self._monitor_timer = QTimer(self)
+        self._monitor_timer.timeout.connect(self._monitor_joy)
+        self._monitor_timer.start(10)
+
+        context.add_widget(self._widget)
+
+        self.thrust_publisher_ = self._node.create_publisher(Wrench, self.channel, 10)
+
+    @QtCore.Slot()
+    def _monitor_joy(self):
+        self.label_joy_x.setText(f"X: {self._joy.LeftJoystickX}")
+        
+        x = -self._joy.LeftJoystickY
+        y = -self._joy.LeftJoystickX
+        z = 0 if self._joy.A and self._joy.B else 1 if self._joy.A else -1 if self._joy.B else 0
+        roll = self._joy.RightJoystickX
+        pitch = -self._joy.RightJoystickY
+        yaw = 0 if self._joy.LeftTrigger > 0.5 and self._joy.RightTrigger > 0.5 else 1 if self._joy.LeftTrigger > 0.5 else -1 if self._joy.RightTrigger > 0.5 else 0
+        
+        thrust_arr = [x, y, z, roll, pitch, yaw]
+        thrust_arr = map(XboxController.scale, thrust_arr)
+        thrust_arr = [thrust * max for thrust, max in zip(thrust_arr, self._joy.thrustMax)]
+        thrust_arr = np.clip(thrust_arr, [-max for max in self._joy.thrustMax], self._joy.thrustMax)
+        thrust_arr = list(map(lambda thrust: thrust if abs(thrust) > self._joy.thrustMin else 0, thrust_arr))
+
+        scale = 5
+        thrust_msg = Wrench()
+        thrust_msg.force.x = float(scale * thrust_arr[0])
+        thrust_msg.force.y = float(scale * thrust_arr[1])
+        thrust_msg.force.z = float(scale * thrust_arr[2])
+        thrust_msg.torque.x = float(scale * thrust_arr[3])
+        thrust_msg.torque.y = float(scale * thrust_arr[4])
+        thrust_msg.torque.z = float(scale * thrust_arr[5])
+
+        self.thrust_publisher_.publish(thrust_msg)
+
+class XboxController(object):
+    MAX_TRIG_VAL = math.pow(2, 8)
+    MAX_JOY_VAL = math.pow(2, 15)
+
+    def __init__(self):
+        self.LeftJoystickY = 0
+        self.LeftJoystickX = 0
+        self.RightJoystickY = 0
+        self.RightJoystickX = 0
+        self.LeftTrigger = 0
+        self.RightTrigger = 0
+        self.LeftBumper = 0
+        self.RightBumper = 0
+        self.A = 0
+        self.X = 0
+        self.Y = 0
+        self.B = 0
+        self.LeftThumb = 0
+        self.RightThumb = 0
+        self.Back = 0
+        self.Start = 0
+        self.LeftDPad = False
+        self.RightDPad = False
+        self.UpDPad = False
+        self.DownDPad = False
+        self.pitchMax = 0.8
+        self.rollMax = 0.8
+        self.yawMax = 0.6
+        self.linXMax = 1.4
+        self.linYMax = 1.4
+        self.linZMax = 1.4
+        self.thrustMax = [self.linXMax, self.linYMax, self.linZMax, self.rollMax, self.pitchMax, self.yawMax]
+        self.thrustMin = 0.001
+
+        self.torpedo_pos = 1
+        self.led_mode = 2
+
+        self._monitor_thread = threading.Thread(target=self._monitor_controller, args=())
+        self._monitor_thread.daemon = True
+        self._monitor_thread.start()
+
+    # Apply a cubic scaling to the input value. Makes precise movements on the
+    # joysticks possible, while still allowing full throttle.
+    @staticmethod
+    def scale(x):
+        return x * x * x
+
+    def read(self): # return the buttons/triggers that you care about in this method
+        # Both joysticks have X right positive, Y down positive
+        x = -self.LeftJoystickY
+        y = -self.LeftJoystickX
+        z = 0 if self.A and self.B else 1 if self.A else -1 if self.B else 0
+        roll = self.RightJoystickX
+        pitch = -self.RightJoystickY
+        yaw = 0 if self.LeftTrigger > 0.5 and self.RightTrigger > 0.5 else 1 if self.LeftTrigger > 0.5 else -1 if self.RightTrigger > 0.5 else 0
+        thrust_arr = [x, y, z, roll, pitch, yaw]
+        thrust_arr = map(XboxController.scale, thrust_arr)
+        thrust_arr = [thrust * max for thrust, max in zip(thrust_arr, self.thrustMax)]
+        thrust_arr = np.clip(thrust_arr, [-max for max in self.thrustMax], self.thrustMax)
+        thrust_arr = list(map(lambda thrust: thrust if abs(thrust) > self.thrustMin else 0, thrust_arr))
+
+        if self.X:
+            self.torpedo_pos = 0
+            self.led_mode = 4
+        elif self.Y:
+            self.torpedo_pos = 2
+            self.led_mode = 4
+        else:
+            self.torpedo_pos = 1
+            self.led_mode = 2
+
+        return thrust_arr, self.torpedo_pos, self.led_mode, self.DownDPad, self.Start, self.Y, self.X
+
+    def _monitor_controller(self):
+        while True:
+            events = get_gamepad()
+            for event in events:
+                if event.code == 'ABS_Y':
+                    self.LeftJoystickY = event.state / XboxController.MAX_JOY_VAL # normalize between -1 and 1
+                elif event.code == 'ABS_X':
+                    self.LeftJoystickX = event.state / XboxController.MAX_JOY_VAL # normalize between -1 and 1
+                elif event.code == 'ABS_RY':
+                    self.RightJoystickY = event.state / XboxController.MAX_JOY_VAL # normalize between -1 and 1
+                elif event.code == 'ABS_RX':
+                    self.RightJoystickX = event.state / XboxController.MAX_JOY_VAL # normalize between -1 and 1
+                elif event.code == 'ABS_Z':
+                    self.LeftTrigger = event.state / XboxController.MAX_TRIG_VAL # normalize between 0 and 1
+                elif event.code == 'ABS_RZ':
+                    self.RightTrigger = event.state / XboxController.MAX_TRIG_VAL # normalize between 0 and 1
+                elif event.code == 'BTN_TL':
+                    self.LeftBumper = event.state
+                elif event.code == 'BTN_TR':
+                    self.RightBumper = event.state
+                elif event.code == 'BTN_SOUTH':
+                    self.A = event.state
+                elif event.code == 'BTN_NORTH':
+                    self.X = event.state
+                elif event.code == 'BTN_WEST':
+                    self.Y = event.state
+                elif event.code == 'BTN_EAST':
+                    self.B = event.state
+                elif event.code == 'BTN_THUMBL':
+                    self.LeftThumb = event.state
+                elif event.code == 'BTN_THUMBR':
+                    self.RightThumb = event.state
+                elif event.code == 'BTN_SELECT':
+                    self.Back = event.state
+                elif event.code == 'BTN_START':
+                    self.Start = event.state
+                # elif event.code == 'BTN_TRIGGER_HAPPY1':
+                #     self.LeftDPad = event.state
+                # elif event.code == 'BTN_TRIGGER_HAPPY2':
+                #     self.RightDPad = event.state
+                # elif event.code == 'BTN_TRIGGER_HAPPY3':
+                #     self.UpDPad = event.state
+                # elif event.code == 'BTN_TRIGGER_HAPPY4':
+                #     self.DownDPad = event.state
+                ## BLOCK: PS4 DPad.
+                # Though also these are the events raised for at least Alex's
+                # Xbox controller. Not sure if the NBRF Xbox controller is
+                # different.
+                elif event.code == 'ABS_HAT0Y': # vertical
+                    if event.state == -1:
+                        self.UpDPad = True
+                        self.DownDPad = False
+                    elif event.state == 1:
+                        self.UpDPad = False
+                        self.DownDPad = True
+                    else: # neither pressed
+                        self.UpDPad = False
+                        self.DownDPad = False
+                elif event.code == 'ABS_HAT0X': # horizontal
+                    if event.state == -1:
+                        self.LeftDPad = True
+                        self.RightDPad = False
+                    elif event.state == 1:
+                        self.LeftDPad = False
+                        self.RightDPad = True
+                    else: # neither pressed
+                        self.LeftDPad = False
+                        self.RightDPad = False
+                ## END BLOCK: PS4 DPad
\ No newline at end of file