Added old controller code

2023-12-02 19:46:12 +01:00
parent be853eea93
commit d0b9c22eee
4 changed files with 390 additions and 16 deletions
--- a/src/ft24_control/CMakeLists.txt
+++ b/src/ft24_control/CMakeLists.txt
@ -42,6 +42,8 @@ ament_python_install_package(${PROJECT_NAME})
 # Install Python executables
 install(PROGRAMS
  src/imu_covariance_adder.py
+  src/joy.py
+  src/twist.py
  DESTINATION lib/${PROJECT_NAME}
 )

--- a/src/ft24_control/src/joy.py
+++ b/src/ft24_control/src/joy.py
@ -0,0 +1,276 @@
+from math import atan2, degrees, sqrt
+from struct import Struct
+from time import sleep
+from typing import List
+
+import rclpy
+from rclpy.context import Context
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from sensor_msgs.msg import Joy
+
+
+class JSEvent(object):
+    """A joystick even class (struct)
+
+        struct js_event_t
+        {
+            uint32_t time;
+            int16_t value;
+            uint8_t type;
+            uint8_t id;
+        };
+
+    Takes in raw bytes from a file.read(JS_EVENT_SIZE) call.
+
+    Also defines AXIS and BUTTON ids
+    """
+
+    JS_EVENT_SIZE = 8  # 64 bits is 8 bytes
+    EVENT_BUTTON = 0x01
+    EVENT_AXIS = 0x02
+    EVENT_INIT = 0x80
+    MAX_AXIS_COUNT = 27
+    MAX_BUTTON_COUNT = 18
+    AXIS_COUNT = 26
+    BUTTON_COUNT = 17
+
+    AXIS_LEFT_STICK_HORIZONTAL = 0
+    AXIS_LEFT_STICK_VERTICAL = 1
+    AXIS_RIGHT_STICK_HORIZONTAL = 2
+    AXIS_RIGHT_STICK_VERTICAL = 3
+    AXIS_DPAD_UP = 8
+    AXIS_DPAD_RIGHT = 9
+    AXIS_DPAD_DOWN = 10
+    AXIS_DPAD_LEFT = 11  # who knows what the left value should actually be...
+    AXIS_LEFT_TRIGGER = 12
+    AXIS_RIGHT_TRIGGER = 13
+    AXIS_LEFT_BUMPER = 14
+    AXIS_RIGHT_BUMPER = 15
+    AXIS_TRIANGLE = 16
+    AXIS_CIRCLE = 17
+    AXIS_X = 18
+    AXIS_SQUARE = 19
+    AXIS_ACCEL_X = 23  # note: left is positive, right is negative
+    AXIS_ACCEL_Y = 24  # note: back is positive, forward is negative
+    AXIS_ACCEL_Z = 25  # note: can't tell what sign is what
+
+    BUTTON_SELECT = 0
+    BUTTON_LEFT_JOYSTICK = 1
+    BUTTON_RIGHT_JOYSTICK = 2
+    BUTTON_START = 3
+    BUTTON_DPAD_UP = 4
+    BUTTON_DPAD_RIGHT = 5
+    BUTTON_DPAD_DOWN = 6
+    BUTTON_DPAD_LEFT = 7
+    BUTTON_LEFT_TRIGGER = 8
+    BUTTON_RIGHT_TRIGGER = 9
+    BUTTON_LEFT_BUMPER = 10
+    BUTTON_RIGHT_BUMPER = 11
+    BUTTON_TRIANGLE = 12
+    BUTTON_CIRCLE = 13
+    BUTTON_X = 14
+    BUTTON_SQUARE = 15
+    BUTTON_PS3 = 16
+
+    def __init__(self, event_struct):
+        # c.f. https://docs.python.org/3/library/struct.html#format-characters
+        s = Struct("< I h B B")
+        self.time, self.value, self.type, self.id = s.unpack(event_struct)
+        # also c.f. Struct.pack()
+
+        # ignore non-input events
+        self.type = self.type & ~self.EVENT_INIT
+
+    def __repr__(self):
+        struct = "struct js_event_t\n{"
+        # struct += '\n\tuint32_t time : {}'.format(self.time)
+        struct += "\n\tint16_t value : {}".format(self.value)
+        struct += "\n\tuint8_t type  : {}".format(hex(self.type))
+        struct += "\n\tuint8_t id    : {}".format(self.id)
+        struct += "\n};\n"
+        return struct
+
+
+class Joystick(object):
+    def __init__(self, js_file="/dev/input/js1"):
+        # note names beginning with `__` are enforced as private
+        self.file = open(js_file, "rb")
+        # since we're using ID's as indices, account for 0 based indices
+        self.__axis_values = [0] * (JSEvent.MAX_AXIS_COUNT + 1)
+        # since we're using ID's as indices, account for 0 based indices
+        self.__button_values = [0] * (JSEvent.MAX_BUTTON_COUNT + 1)
+
+        self.update_id = -1
+        self.AxisUpdate = False
+        self.ButtonUpdate = False
+        self.event = None
+
+    def __del__(self):
+        self.file.close()
+
+    def __enter__(self):
+        return self
+
+    # __enter__ and __exit__ allow the context manager syntax
+    # with Joystick() as js:
+    # ...
+    def __exit__(self, exc_type, exc_value, traceback):
+        self.file.close()
+
+    def Update(self):
+        self.event = JSEvent(self.file.read(JSEvent.JS_EVENT_SIZE))
+
+        if self.event.type == JSEvent.EVENT_AXIS:
+            self.update_id = self.event.id
+            self.AxisUpdate = True
+            self.ButtonUpdate = False
+            self.__axis_values[self.event.id] = self.event.value
+
+        if self.event.type == JSEvent.EVENT_BUTTON:
+            self.update_id = self.event.id
+            self.AxisUpdate = False
+            self.ButtonUpdate = True
+            self.__button_values[self.event.id] = self.event.value
+
+    def getButtonState(self, button_id):
+        return self.__button_values[button_id]
+
+    def getAxisState(self, axis_id):
+        return self.__axis_values[axis_id]
+
+
+class JoystickController(Joystick):
+    """An implementation specific controller that inherits a basic Joystick. Adds the capability to
+    check for specific updates that you care about and then do something based on that.
+    """
+
+    def __init__(self, js_file="/dev/input/js0"):
+        Joystick.__init__(self, js_file)
+        # direction gives the direction of travel. Defined in terms of r and theta
+        self.direction_vector = (0, 0)
+        # rotation tells the robot to rotate x degrees
+        self.rotation_vector = (0, 0)
+
+    def hasUpdate(self):
+        return self.ButtonUpdate or self.AxisUpdate
+
+    def performUpdates(self):
+        care_about_buttons = [
+            JSEvent.BUTTON_SELECT,
+            JSEvent.BUTTON_LEFT_JOYSTICK,
+            JSEvent.BUTTON_RIGHT_JOYSTICK,
+            JSEvent.BUTTON_START,
+            JSEvent.BUTTON_DPAD_UP,
+            JSEvent.BUTTON_DPAD_RIGHT,
+            JSEvent.BUTTON_DPAD_DOWN,
+            JSEvent.BUTTON_DPAD_LEFT,
+            JSEvent.BUTTON_LEFT_TRIGGER,
+            JSEvent.BUTTON_RIGHT_TRIGGER,
+            JSEvent.BUTTON_LEFT_BUMPER,
+            JSEvent.BUTTON_RIGHT_BUMPER,
+            JSEvent.BUTTON_TRIANGLE,
+            JSEvent.BUTTON_CIRCLE,
+            JSEvent.BUTTON_X,
+            JSEvent.BUTTON_SQUARE,
+            JSEvent.BUTTON_PS3,
+        ]
+        care_about_axes = [
+            JSEvent.AXIS_LEFT_STICK_HORIZONTAL,
+            JSEvent.AXIS_LEFT_STICK_VERTICAL,
+            JSEvent.AXIS_RIGHT_STICK_HORIZONTAL,
+            JSEvent.AXIS_RIGHT_STICK_VERTICAL,
+            JSEvent.AXIS_DPAD_UP,
+            JSEvent.AXIS_DPAD_LEFT,
+            JSEvent.AXIS_DPAD_RIGHT,
+            JSEvent.AXIS_DPAD_DOWN,
+            JSEvent.AXIS_LEFT_TRIGGER,
+            JSEvent.AXIS_RIGHT_TRIGGER,
+            JSEvent.AXIS_LEFT_BUMPER,
+            JSEvent.AXIS_RIGHT_BUMPER,
+            JSEvent.AXIS_TRIANGLE,
+            JSEvent.AXIS_CIRCLE,
+            JSEvent.AXIS_X,
+            JSEvent.AXIS_SQUARE,
+            JSEvent.AXIS_ACCEL_X,
+            JSEvent.AXIS_ACCEL_Y,
+            JSEvent.AXIS_ACCEL_Z,
+        ]
+
+        if self.ButtonUpdate and self.update_id in care_about_buttons:
+            self.__button_update()
+
+        if self.AxisUpdate and self.update_id in care_about_axes:
+            self.__axis_update()
+
+    def __button_update(self):
+        pass
+
+    def __axis_update(self):
+        pass
+   
+class JoyNode(Node):
+    def __init__(
+        self,
+        node_name: str,
+        *,
+        context: Context = None,
+        cli_args: List[str] = None,
+        namespace: str = None,
+        use_global_arguments: bool = True,
+        enable_rosout: bool = True,
+        start_parameter_services: bool = True,
+        parameter_overrides: List[Parameter] = None,
+        allow_undeclared_parameters: bool = False,
+        automatically_declare_parameters_from_overrides: bool = False,
+    ) -> None:
+        super().__init__(
+            node_name,
+            context=context,
+            cli_args=cli_args,
+            namespace=namespace,
+            use_global_arguments=use_global_arguments,
+            enable_rosout=enable_rosout,
+            start_parameter_services=start_parameter_services,
+            parameter_overrides=parameter_overrides,
+            allow_undeclared_parameters=allow_undeclared_parameters,
+            automatically_declare_parameters_from_overrides=automatically_declare_parameters_from_overrides,
+        )
+
+        self.pub = self.create_publisher(Joy, "/joy", 1)
+
+    def run(self):
+        msg = Joy()
+        msg.axes = [0.0, 0.0]
+        msg.buttons = [0]
+        with JoystickController("/dev/input/js0") as controller:
+            while rclpy.ok():
+                controller.Update()
+                sleep(0.0001)
+
+                if controller.hasUpdate():
+                    hor = controller.getAxisState(JSEvent.AXIS_LEFT_STICK_HORIZONTAL)
+                    ver = controller.getAxisState(JSEvent.AXIS_LEFT_STICK_VERTICAL)
+
+                    button_change_mode = controller.getButtonState(JSEvent.BUTTON_CIRCLE) # it says circle but it actually is dpad up
+                    self.get_logger().info(str(button_change_mode))
+
+                    hor /= 2**15
+                    ver /= 2**15
+
+                    msg.axes[JSEvent.AXIS_LEFT_STICK_HORIZONTAL] = hor
+                    msg.axes[JSEvent.AXIS_LEFT_STICK_VERTICAL] = ver
+                    msg.buttons[0] = button_change_mode
+
+                    self.pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = JoyNode("test")
+    node.run()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
--- a/src/ft24_control/src/twist.py
+++ b/src/ft24_control/src/twist.py
@ -0,0 +1,106 @@
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+from std_msgs.msg import String
+from sensor_msgs.msg import Joy
+
+import numpy as np
+
+SCALE_LINEAR_VELOCITY = 0.2
+SCALE_ANGULAR_VELOCITY = 0.7
+PUBLISH_FREQUENCY_HZ = 30
+
+SMOOTH_WEIGHTS = np.array([1, 2, 4, 10, 20])
+
+
+class TwistPublisher(Node):
+    """
+    This node serves as a bridge between the Joy messages published from the PS3 controller
+    interface in joy.py and the Twist messages expected by ros_control.
+    """
+    def __init__(
+        self,
+        smooth_weights=np.ones(5),
+        linear_scale=1.0,
+        angular_scale=1.0,
+        publish_frequency_hz=30,
+    ):
+        super().__init__("twist_publisher")
+
+        self.mode = "manual"
+
+        self.linear_window = np.zeros(smooth_weights.shape[0])
+        self.angular_window = np.zeros(smooth_weights.shape[0])
+        self.linear_recent = 0.0
+        self.angular_recent = 0.0
+
+        self.linear_out = 0.0
+        self.angular_out = 0.0
+
+        self.weights = smooth_weights / smooth_weights.sum()
+        self.window_size = smooth_weights.shape[0]
+
+        self.linear_scale = linear_scale
+        self.angular_scale = angular_scale
+
+        self.publisher_2 = self.create_publisher(Twist, "/cmd_vel_joystick", 1)
+
+        self.create_subscription(Joy, "/joy", self.joy_callback, 1)
+        self.timer = self.create_timer(1 / publish_frequency_hz, self.timer_callback)
+
+    def joy_callback(self, joy_message):
+        x, y = joy_message.axes[:2]
+        self.linear_recent = -y * SCALE_LINEAR_VELOCITY
+        self.angular_recent = -x * SCALE_ANGULAR_VELOCITY
+
+        if joy_message.buttons[0]:
+            self.mode = "manual" if self.mode == "auto" else "auto"
+            self.get_logger().info(f"Switching to {self.mode!r} mode")
+
+  
+    def update_smooth_window(self):
+        self.linear_window = np.append(self.linear_window, self.linear_recent)[
+            -self.window_size :
+        ]
+        self.angular_window = np.append(self.angular_window, self.angular_recent)[
+            -self.window_size :
+        ]
+
+    def calc_smooth_speed(self):
+        self.linear_out = np.average(self.linear_window, weights=self.weights)
+        self.angular_out = np.average(self.angular_window, weights=self.weights)
+
+    def timer_callback(self):
+        
+        if self.mode != "manual":
+            return
+
+        msg = Twist()
+
+        self.update_smooth_window()
+        self.calc_smooth_speed()
+
+        msg.linear.x = self.linear_out
+        msg.angular.z = self.angular_out
+        self.publisher_2.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    minimal_publisher = TwistPublisher(
+        SMOOTH_WEIGHTS,
+        SCALE_LINEAR_VELOCITY,
+        SCALE_ANGULAR_VELOCITY,
+        PUBLISH_FREQUENCY_HZ,
+    )
+
+    rclpy.spin(minimal_publisher)
+
+    minimal_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()