Added uint8 test for velocity command and implemented in dbw_read_vel

AUVSL · Oct 17, 2024 · ed063b9 · ed063b9
1 parent bc9f528
commit ed063b9
Show file tree

Hide file tree

Showing 2 changed files with 38 additions and 4 deletions.
diff --git a/dbw/dbw/dbw_read_vel.py b/dbw/dbw/dbw_read_vel.py
@@ -89,15 +89,15 @@ def dbw_callback(self, msg):
 
         if gear == 1:
             print('gear: forward')
-            throttle_cmd = np.uint8(int(hex(int(250.0*(throttle_percentage+100)/200.)), 16))
+            throttle_cmd = np.uint8(250.0*(throttle_percentage+100)/200.)
         elif gear == 2:
             print('gear: backward')
-            throttle_cmd = np.uint8(int(hex(int(250.0 * (100.0 - throttle_percentage) / 200.)), 16))
+            throttle_cmd = np.uint8(250.0*(100.0 - throttle_percentage)/200.)
         else:
             print('gear: neutral')
-            throttle_cmd = np.uint8(int(hex(int(250.0 * (0 + 100) / 200.)), 16))
+            throttle_cmd = np.uint8(250.0*(0+100)/200.)
 
-        steering_cmd = np.uint8(int(hex(int(250.0*(steering_percentage+100)/200.)), 16))
+        steering_cmd = np.uint8(250.0*(steering_percentage+100)/200.)
 
         # throttle
         self.set_2_bytes_number(self.propulsion.data, throttle_cmd, 1)

diff --git a/dbw/test/test_uint8_throttle.py b/dbw/test/test_uint8_throttle.py
@@ -0,0 +1,34 @@
+import numpy as np
+
+throttle_cmd_old = []
+throttle_cmd_new = []
+steering_cmd_old = []
+steering_cmd_new = []
+
+for i in range(12):
+    throttle_percentage = 100/11 * i
+    steering_percentage = 200/11 * i - 100
+
+    gear = i%3
+
+    if gear == 1:
+        print('gear: forward')
+        throttle_cmd_old.append(np.uint8(int(hex(int(250.0*(throttle_percentage+100)/200.)), 16)))
+        throttle_cmd_new.append(np.uint8(250.0*(throttle_percentage+100)/200.))
+    elif gear == 2:
+        print('gear: backward')
+        throttle_cmd_old.append(np.uint8(int(hex(int(250.0*(100.0 - throttle_percentage)/200.)), 16)))
+        throttle_cmd_new.append(np.uint8(250.0*(100.0 - throttle_percentage)/200.))
+
+    else:
+        print('gear: neutral')
+        throttle_cmd_old.append(np.uint8(int(hex(int(250.0*(0+100)/200.)), 16)))
+        throttle_cmd_new.append(np.uint8(250.0*(0+100)/200.))
+
+    steering_cmd_old.append(np.uint8(int(hex(int(250.0*(steering_percentage+100)/200.)), 16)))
+    steering_cmd_new.append(np.uint8(250.0*(steering_percentage+100)/200.))
+
+print(throttle_cmd_old)
+print(throttle_cmd_new)
+print(steering_cmd_old)
+print(steering_cmd_new)