add delay to actuators publish

coordinator/nodes/match.py

@@ -97,17 +97,17 @@ class CoordinatorNode():
         t0 = +2.0 * (w * x + y * z)
         t1 = +1.0 - 2.0 * (x * x + y * y)
         roll_x = math.atan2(t0, t1)
-     
+
         t2 = +2.0 * (w * y - z * x)
         t2 = +1.0 if t2 > +1.0 else t2
         t2 = -1.0 if t2 < -1.0 else t2
         pitch_y = math.asin(t2)
-     
+
         t3 = +2.0 * (w * z + x * y)
         t4 = +1.0 - 2.0 * (y * y + z * z)
         yaw_z = math.atan2(t3, t4)
-     
-        return roll_x, pitch_y, yaw_z # in radians
+
+        return roll_x, pitch_y, yaw_z  # in radians
 
     def aruco_cb(self, data):
         #if self.enable_aruco_check:
@@ -120,14 +120,13 @@ class CoordinatorNode():
         for aruco in data.transforms:
             if (aruco.fiducial_id == 17):
                 rot = aruco.transform.rotation
-                euler = self.euler_from_quaternion(rot.x , rot.y, rot.z, rot.w )
+                euler = self.euler_from_quaternion(rot.x, rot.y, rot.z, rot.w)
 
                 print(euler)
 
                 if (euler.x > 50):
                     pass
 
-
         self.weathercock_info = None
 
     def colors_cb(self, data):
@@ -280,24 +279,28 @@ class CoordinatorNode():
         msg = Bool()
         msg.data = False
         self.flag_state_pub.publish(msg)
+        time.sleep(0.01)
         self.flag_state_pub.publish(msg)
 
     def raise_flag(self):
         msg = Bool()
         msg.data = True
         self.flag_state_pub.publish(msg)
+        time.sleep(0.01)
         self.flag_state_pub.publish(msg)
 
     def lower_arm(self):
         msg = Bool()
         msg.data = True
         self.arm_state_pub.publish(msg)
+        time.sleep(0.01)
         self.arm_state_pub.publish(msg)
 
     def raise_arm(self):
         msg = Bool()
         msg.data = False
         self.arm_state_pub.publish(msg)
+        time.sleep(0.01)
         self.arm_state_pub.publish(msg)
 
     def hit_wall(self, forward=True):