Speed Max einstellbar

ExoMy_Software-master/src/joystick_parser_node.py

@@ -6,6 +6,7 @@ import math
 import os
 import struct
 import threading
+import time
 
 import rospy
 from sensor_msgs.msg import Joy
@@ -21,6 +22,8 @@ global last_locomotion_mode
 locomotion_mode = LocomotionMode.ACKERMANN.value
 last_locomotion_mode = locomotion_mode
 motors_enabled = True
+_speed_limit = 100
+_speed_limit_last_read = 0.0
 AXIS_DEADZONE = 0.1
 last_start_button_pressed = False
 last_webgui_time = None
@@ -72,6 +75,18 @@ CONTROLLER_FUNCTION_MAPS = {
 }
 
 
+def get_speed_limit():
+    global _speed_limit, _speed_limit_last_read
+    now = time.time()
+    if now - _speed_limit_last_read > 2.0:
+        try:
+            _speed_limit = int(rospy.get_param('/speed_limit_percent', 100))
+        except Exception:
+            pass
+        _speed_limit_last_read = now
+    return _speed_limit
+
+
 def _rumble_thread(duration_ms):
     EV_FF = 0x15
     FF_RUMBLE = 0x50
@@ -229,9 +244,9 @@ def callback(data):
 
     rover_cmd.motors_enabled = motors_enabled
 
-    # The velocity is decoded as value between 0...100
+    # The velocity is decoded as value between 0...100, capped by speed limit
     stick_length = min(math.sqrt(x*x + y*y), 1.0)
-    rover_cmd.vel = int(100 * math.pow(stick_length, VELOCITY_EXPO))
+    rover_cmd.vel = int(get_speed_limit() * math.pow(stick_length, VELOCITY_EXPO))
 
     # The steering is described as an angle between -180...180
     # Which describe the joystick position as follows: