Thrust publisher

qubo_gui/teleop.py

@@ -6,6 +6,9 @@ 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
@@ -13,12 +16,17 @@ 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)
 
@@ -29,16 +37,41 @@ class TeleopPlugin(Plugin):
 
         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