Verbessert

ExoMy_Software-master/src/joystick_parser_node.py

@@ -8,13 +8,55 @@ import math
 # Define locomotion modes
 global locomotion_mode
 global motors_enabled
+global last_start_button_pressed
 
 locomotion_mode = LocomotionMode.ACKERMANN.value
 motors_enabled = True
-
 AXIS_DEADZONE = 0.1
+last_start_button_pressed = False
+
 VELOCITY_EXPO = 2.0
 
+DEFAULT_CONTROLLER = "logitech-F710"
+CONTROLLER_FUNCTION_MAPS = {
+    "webgui": {
+        "x_axis": 0,
+        "y_axis": 1,
+        "invert_x_axis": False,
+        "X_button": 0,
+        "Y_button": 3,
+        "A_button": 1,
+        "B_button": 2,
+        "start_button": 9,
+        "select_button": 8,
+        "sensitivity": 0.15,
+    },
+    "logitech-F710": {
+        "x_axis": 0,
+        "y_axis": 1,
+        "invert_x_axis": False,
+        "X_button": 0,
+        "Y_button": 3,
+        "A_button": 1,
+        "B_button": 2,
+        "start_button": 9,
+        "select_button": 8,
+        "sensitivity": 0.10,
+    },
+    "xbox-one": {
+        "x_axis": 0,
+        "y_axis": 1,
+        "invert_x_axis": False,
+        "X_button": 2,
+        "Y_button": 3,
+        "A_button": 0,
+        "B_button": 1,
+        "start_button": 7,
+        "select_button": 6,
+        "sensitivity": 0.11,
+    },
+}
+
 
 def apply_deadzone(value, deadzone):
     if abs(value) <= deadzone:
@@ -24,10 +66,34 @@ def apply_deadzone(value, deadzone):
     return math.copysign(scaled, value)
 
 
+def get_controller_function_map(data):
+    if data.header.frame_id == "webgui":
+        return CONTROLLER_FUNCTION_MAPS["webgui"]
+
+    controller_name = rospy.get_param("controller", DEFAULT_CONTROLLER)
+    if controller_name in CONTROLLER_FUNCTION_MAPS:
+        return CONTROLLER_FUNCTION_MAPS[controller_name]
+
+    rospy.logwarn("Unbekannter Controller '%s', nutze Fallback '%s'.",
+                  controller_name, DEFAULT_CONTROLLER)
+    return CONTROLLER_FUNCTION_MAPS[DEFAULT_CONTROLLER]
+
+
+def get_axis_value(axes, axis_index):
+    if axis_index < len(axes):
+        return axes[axis_index]
+    return 0.0
+
+
+def is_button_pressed(buttons, button_index):
+    return button_index < len(buttons) and buttons[button_index] == 1
+
+
 def callback(data):
 
     global locomotion_mode
     global motors_enabled
+    global last_start_button_pressed
 
     rover_cmd = RoverCommand()
 
@@ -40,29 +106,40 @@ def callback(data):
     # Left Stick    | Control speed and direction
     # START Button  | Enable and disable motors
 
+    controller_function_map = get_controller_function_map(data)
+
     # Reading out joystick data
-    y = apply_deadzone(data.axes[1], AXIS_DEADZONE)
-    x = apply_deadzone(data.axes[0], AXIS_DEADZONE)
+    y = apply_deadzone(
+        get_axis_value(data.axes, controller_function_map["y_axis"]),
+        controller_function_map["sensitivity"])
+    x = apply_deadzone(
+        get_axis_value(data.axes, controller_function_map["x_axis"]),
+        controller_function_map["sensitivity"])
+
+    if controller_function_map["invert_x_axis"]:
+        x *= -1
 
     # Reading out button data to set locomotion mode
     # X Button
-    if (data.buttons[0] == 1):
+    if is_button_pressed(data.buttons, controller_function_map["X_button"]):
         locomotion_mode = LocomotionMode.POINT_TURN.value
     # A Button
-    if (data.buttons[1] == 1):
+    if is_button_pressed(data.buttons, controller_function_map["A_button"]):
         locomotion_mode = LocomotionMode.ACKERMANN.value
     # B Button
-    if (data.buttons[2] == 1):
+    if is_button_pressed(data.buttons, controller_function_map["B_button"]):
         pass
     # Y Button
-    if (data.buttons[3] == 1):
+    if is_button_pressed(data.buttons, controller_function_map["Y_button"]):
         locomotion_mode = LocomotionMode.CRABBING.value
 
     rover_cmd.locomotion_mode = locomotion_mode
 
     # Enable and disable motors
     # START Button
-    if (data.buttons[9] == 1):
+    start_button_pressed = is_button_pressed(
+        data.buttons, controller_function_map["start_button"])
+    if start_button_pressed and not last_start_button_pressed:
         if motors_enabled is True:
             motors_enabled = False
             rospy.loginfo("Motors disabled!")
@@ -73,12 +150,13 @@ def callback(data):
             rospy.logerr(
                 "Exceptional value for [motors_enabled] = {}".format(motors_enabled))
             motors_enabled = False
+    last_start_button_pressed = start_button_pressed
 
     rover_cmd.motors_enabled = motors_enabled
 
     # The velocity is decoded as value between 0...100
     stick_length = min(math.sqrt(x*x + y*y), 1.0)
-    rover_cmd.vel = 100 * math.pow(stick_length, VELOCITY_EXPO)
+    rover_cmd.vel = int(100 * math.pow(stick_length, VELOCITY_EXPO))
 
     # The steering is described as an angle between -180...180
     # Which describe the joystick position as follows:
@@ -86,7 +164,7 @@ def callback(data):
     # 0      +-180
     #   -90
     #
-    rover_cmd.steering = math.atan2(y, x)*180.0/math.pi
+    rover_cmd.steering = int(math.atan2(y, x)*180.0/math.pi)
 
     rover_cmd.connected = True