Fix Airsim BUG(GCS delay)

USVAirsim/main.py

@@ -1,46 +1,74 @@
 # PythonClient/car/hello_car.py
+import math
+import threading
+
 import airsim
 import time
 
 from udpCommunication import Udp
 
-udp = Udp()
 
-# 连接到Airsim仿真器
+class Main:
-client = airsim.CarClient()
+    def __init__(self):
+        self.udp = Udp()
 
-client.confirmConnection()  # 每1秒检查一次连接状态，并在控制台中报告，以便用户可以查看连接的进度（实际操作只显示一次，奇怪）。
+        # 连接到Airsim仿真器
-# Connected!
+        self.client = airsim.CarClient()
-# Client Ver:1 (Min Req: 1), Server Ver:1 (Min Req: 1)
+
+        self.client.confirmConnection()  # 每1秒检查一次连接状态，并在控制台中报告，以便用户可以查看连接的进度（实际操作只显示一次，奇怪）。
+        # Connected!
+        # Client Ver:1 (Min Req: 1), Server Ver:1 (Min Req: 1)
+
+        self.client.enableApiControl(True)  # 默认是false，有的车辆不允许用API控制，所以用isApiControlEnabled可以查看是否可以用API控制
+        # 仿真器左上角会显示：ControlMode: API
+
+    def run(self):
+        print(self.udp.command)
+        states = self.client.getCarState()
+        car_state = states.__dict__['kinematics_estimated'].__dict__
+
+        yaw = car_state['orientation'].__dict__['z_val']
+        if yaw< 0:
+            yaw += 2
+        yaw *= math.pi
+
+        states = [car_state['position'].__dict__['x_val'], car_state['position'].__dict__['y_val'],
+                  car_state['position'].__dict__['z_val'],
+                  car_state['orientation'].__dict__['x_val'], car_state['orientation'].__dict__['y_val'],
+                  yaw,
+                  car_state['linear_velocity'].__dict__['x_val'], car_state['linear_velocity'].__dict__['y_val'],
+                  car_state['linear_velocity'].__dict__['z_val'], car_state['angular_velocity'].__dict__['x_val'],
+                  car_state['angular_velocity'].__dict__['y_val'], car_state['angular_velocity'].__dict__['z_val'],
+                  car_state['linear_acceleration'].__dict__['x_val'],
+                  car_state['linear_acceleration'].__dict__['y_val'],
+                  car_state['linear_acceleration'].__dict__['z_val']]
+        self.udp.states = states
+
+        car_controls = airsim.CarControls()  # 获得车辆控制数据
+        car_controls.is_manual_gear = False
+        car_controls.steering = self.udp.command[0] / 1000
+        if self.udp.command[1] < 0:
+            car_controls.is_manual_gear = True
+            car_controls.manual_gear = -1
+        car_controls.throttle = self.udp.command[1] / 1000
+        self.client.setCarControls(car_controls)
+        Udp.send(self.udp)
+        timer_10 = threading.Timer(0.01, self.run)
+        timer_10.start()
+
+    def control(self):
+        while True:
+            Udp.receive(self.udp)
+
+
+
+if __name__ == "__main__":
+    main = Main()
+    main.run()
+    main.control()
 
-client.enableApiControl(True)  # 默认是false，有的车辆不允许用API控制，所以用isApiControlEnabled可以查看是否可以用API控制
-# 仿真器左上角会显示：ControlMode: API
 
-while True:
-    states = client.getCarState()
-    car_state = states.__dict__['kinematics_estimated'].__dict__
 
-    states = [car_state['position'].__dict__['x_val'], car_state['position'].__dict__['y_val'],
-              car_state['position'].__dict__['z_val'],
-              car_state['orientation'].__dict__['x_val'], car_state['orientation'].__dict__['y_val'],
-              car_state['orientation'].__dict__['z_val'],
-              car_state['linear_velocity'].__dict__['x_val'], car_state['linear_velocity'].__dict__['y_val'],
-              car_state['linear_velocity'].__dict__['z_val'], car_state['angular_velocity'].__dict__['x_val'],
-              car_state['angular_velocity'].__dict__['y_val'], car_state['angular_velocity'].__dict__['z_val'],
-              car_state['linear_acceleration'].__dict__['x_val'], car_state['linear_acceleration'].__dict__['y_val'],
-              car_state['linear_acceleration'].__dict__['z_val']]
-    udp.states = states
-    print(states)
-    Udp.send(udp)
-    Udp.receive(udp)
-    car_controls = airsim.CarControls()  # 获得车辆控制数据
-    car_controls.is_manual_gear = False
-    car_controls.steering = udp.command[0] / 1000
-    if udp.command[1] < 0:
-        car_controls.is_manual_gear = True
-        car_controls.manual_gear = -1
-    car_controls.throttle = udp.command[1] / 1000
-    client.setCarControls(car_controls)
 
 # 将车辆初始化处理，后续如果需要重新控制则需要再次调用`enableApiControl`或`armDisarm
 

USVAirsim/udpCommunication.py

@@ -24,7 +24,6 @@ class Udp:
             if self.buffer[0] != calculate_header_lrc(self.buffer):
                 del self.buffer[0]
                 continue
-
             packet_data_length = self.buffer[2]
             packet_length = packet_data_length + 5