Ackermann gefixt

ExoMy_Software-master/src/joystick_parser_node.py

@@ -30,6 +30,7 @@ last_webgui_time = None
 WEBGUI_PRIORITY_TIMEOUT = 2.0
 
 VELOCITY_EXPO = 2.0
+CARDINAL_SNAP_THRESHOLD = 0.12
 
 DEFAULT_CONTROLLER = "logitech-F710"
 CONTROLLER_FUNCTION_MAPS = {
@@ -163,6 +164,14 @@ def is_button_pressed(buttons, button_index):
     return button_index < len(buttons) and buttons[button_index] == 1
 
 
+def snap_axes_to_cardinal(x, y, threshold):
+    if abs(x) <= threshold and abs(y) > 0.0:
+        return 0.0, math.copysign(abs(y), y)
+    if abs(y) <= threshold and abs(x) > 0.0:
+        return math.copysign(abs(x), x), 0.0
+    return x, y
+
+
 def callback(data):
 
     global locomotion_mode
@@ -205,6 +214,13 @@ def callback(data):
     if controller_function_map["invert_y_axis"]:
         y *= -1
 
+    # Kleine Queranteile sollen beim Geradeausfahren nicht zu einem
+    # unbeabsichtigten Lenkwinkel fuehren.
+    snap_threshold = max(
+        controller_function_map["sensitivity"],
+        CARDINAL_SNAP_THRESHOLD)
+    x, y = snap_axes_to_cardinal(x, y, snap_threshold)
+
     # Reading out button data to set locomotion mode
     # X Button
     if is_button_pressed(data.buttons, controller_function_map["X_button"]):

ExoMy_Software-master/src/rover.py

@@ -18,6 +18,7 @@ class Rover():
 
     def __init__(self):
         self.locomotion_mode = LocomotionMode.FAKE_ACKERMANN
+        self.ackermann_straight_tolerance_deg = 5
 
         # x = Achsabstand, y = Spurbreite
         self.wheel_rx = 14.0
@@ -34,6 +35,9 @@ class Rover():
             self.wheel_fx) / math.tan(max_steering_angle * math.pi / 180.0) + (self.wheel_fy / 2)
         self.ackermann_r_min = max(self.ackermann_fr_min, self.ackermann_rr_min)
 
+    def is_ackermann_straight(self, steering_command):
+        return abs(abs(steering_command) - 90) <= self.ackermann_straight_tolerance_deg
+
     def setLocomotionMode(self, locomotion_mode_command):
         '''
         Sets the locomotion mode
@@ -121,7 +125,7 @@ class Rover():
             if(driving_command == 0):
                 return steering_angles
 
-            if math.cos(math.radians(steering_command)) == 0:
+            if self.is_ackermann_straight(steering_command):
                 return steering_angles
 
             radius = self.ackermann_r_max - \
@@ -231,6 +235,9 @@ class Rover():
             if (v == 0):
                 return motor_speeds
 
+            if self.is_ackermann_straight(steering_command):
+                return [v] * 6
+
             if (radius == self.ackermann_r_max):
                 return [v] * 6
             else: