Client: Add threading lock for get_telemetry service

Drone/FlightLib/FlightLib.py

@@ -43,7 +43,11 @@ INTERRUPTER = threading.Event()
 FLIP_MIN_Z = 2.0
 
 checklist = []
+get_telemetry_lock = threading.Lock()
 
+def get_telemetry_locked(*args, **kwargs):
+    with get_telemetry_lock:
+        return get_telemetry(*args, **kwargs)
 
 def arming_wrapper(state=False, *args, **kwargs):
     arming(state)
@@ -109,14 +113,14 @@ def check_ros_services():
 
 @check("FCU connection")
 def check_connection():
-    telemetry = get_telemetry()
+    telemetry = get_telemetry_locked()
     if not telemetry.connected:
         yield ("Flight controller is not connected!")
 
 
 @check("Linear velocity estimation")
 def check_linear_speeds(speed_limit=0.15):
-    telemetry = get_telemetry(frame_id='body')
+    telemetry = get_telemetry_locked(frame_id='body')
 
     if _check_nans(telemetry.vx, telemetry.vy, telemetry.vz):
         yield ("Velocity estimation is not available")
@@ -131,7 +135,7 @@ def check_linear_speeds(speed_limit=0.15):
 
 @check("Angular velocity estimation")
 def check_angular_speeds(rate_limit=0.05):
-    telemetry = get_telemetry(frame_id='body')
+    telemetry = get_telemetry_locked(frame_id='body')
 
     if _check_nans(telemetry.pitch_rate, telemetry.roll_rate, telemetry.yaw_rate):
         yield ("Angular velocities estimation is not available")
@@ -146,7 +150,7 @@ def check_angular_speeds(rate_limit=0.05):
 
 @check("Angles estimation")
 def check_angles(angle_limit=math.radians(5)):
-    telemetry = get_telemetry(frame_id='body')
+    telemetry = get_telemetry_locked(frame_id='body')
 
     if _check_nans(telemetry.pitch, telemetry.roll, telemetry.yaw):
         yield ("Angular velocities estimation is not available")
@@ -173,7 +177,7 @@ def selfcheck():
 
 def navto(x, y, z, yaw=float('nan'), frame_id=FRAME_ID, auto_arm=False, **kwargs):
     set_position(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, auto_arm=auto_arm)
-    #telemetry = get_telemetry(frame_id=frame_id)
+    #telemetry = get_telemetry_locked(frame_id=frame_id)
 
     logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
     #print('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
@@ -190,7 +194,7 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO
     navigate(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, speed=speed, auto_arm=auto_arm)
 
     # waiting for completion
-    telemetry = get_telemetry(frame_id=frame_id)
+    telemetry = get_telemetry_locked(frame_id=frame_id)
     rate = rospy.Rate(freq)
     time_start = time.time()
 
@@ -201,7 +205,7 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO
             interrupter.clear()
             return False
 
-        telemetry = get_telemetry(frame_id=frame_id)
+        telemetry = get_telemetry_locked(frame_id=frame_id)
         rospy.logdebug('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
             telemetry.x, telemetry.y, telemetry.z, telemetry.yaw))
         #print('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
@@ -229,10 +233,10 @@ def reach_altitude(z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, fr
                    freq=FREQUENCY, timeout=TIMEOUT, interrupter=INTERRUPTER, wait=False):
     logger.info('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw))
     #print('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw))
-    current_telem = get_telemetry(frame_id=frame_id)
+    current_telem = get_telemetry_locked(frame_id=frame_id)
     navigate(frame_id=frame_id, x=current_telem.x, y=current_telem.y, z=z, yaw=yaw, speed=speed)
 
-    telemetry = get_telemetry(frame_id=frame_id)
+    telemetry = get_telemetry_locked(frame_id=frame_id)
     rate = rospy.Rate(freq)
     time_start = time.time()
 
@@ -243,7 +247,7 @@ def reach_altitude(z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, fr
             interrupter.clear()
             return
 
-        telemetry = get_telemetry(frame_id=frame_id)
+        telemetry = get_telemetry_locked(frame_id=frame_id)
         logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
             telemetry.x, telemetry.y, telemetry.z, telemetry.yaw))
         #print('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
@@ -276,7 +280,7 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA
     landing()
     #print("Land is started")
 
-    telemetry = get_telemetry(frame_id=frame_id_land)
+    telemetry = get_telemetry_locked(frame_id=frame_id_land)
     rate = rospy.Rate(freq)
     time_start = time.time()
 
@@ -287,7 +291,7 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA
             interrupter.clear()
             return False
 
-        telemetry = get_telemetry(frame_id=frame_id_land)
+        telemetry = get_telemetry_locked(frame_id=frame_id_land)
         logger.info("Landing... | z: {}".format(telemetry.z))
         #print("Landing... | z: {}".format(telemetry.z))
         time_passed = time.time() - time_start
@@ -308,7 +312,7 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA
 def takeoff(height=TAKEOFF_HEIGHT, speed=TAKEOFF_SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID, timeout_takeoff=TIMEOUT, interrupter=INTERRUPTER, emergency_land=False):
     rospy.loginfo("Takeoff started...")
     rate = rospy.Rate(FREQUENCY)
-    start = get_telemetry(frame_id=frame_id)
+    start = get_telemetry_locked(frame_id=frame_id)
     climb = 0.
     time_start = time.time()
     result = navigate(z=height, speed=speed, yaw=float('nan'), frame_id="body", auto_arm=True)
@@ -322,7 +326,7 @@ def takeoff(height=TAKEOFF_HEIGHT, speed=TAKEOFF_SPEED, tolerance=TOLERANCE, fra
             interrupter.clear()
             return 'interrupted'
 
-        climb = abs(get_telemetry(frame_id=frame_id).z - start.z)
+        climb = abs(get_telemetry_locked(frame_id=frame_id).z - start.z)
         rospy.logdebug("Takeoff to {:.2f} of {:.2f} meters".format(climb, height))
 
         time_passed = time.time() - time_start
@@ -341,7 +345,7 @@ def takeoff(height=TAKEOFF_HEIGHT, speed=TAKEOFF_SPEED, tolerance=TOLERANCE, fra
 def flip(min_z = FLIP_MIN_Z, frame_id = FRAME_ID): #TODO Flip in different directions
     logger.info("Flip started!")
 
-    start_telemetry = get_telemetry(frame_id=frame_id)  # memorize starting position
+    start_telemetry = get_telemetry_locked(frame_id=frame_id)  # memorize starting position
     
     if start_telemetry.z < min_z - TOLERANCE:
         logger.warning("Can't do flip! Flip failed!")
@@ -355,7 +359,7 @@ def flip(min_z = FLIP_MIN_Z, frame_id = FRAME_ID): #TODO Flip in different direc
         set_rates(roll_rate=30, thrust=0.2)  # maximum roll rate
 
         while True:
-            telem = get_telemetry()
+            telem = get_telemetry_locked()
 
             if abs(telem.roll) > math.pi/2:
                 break

Drone/copter_client.py

@@ -30,7 +30,7 @@ static_bloadcaster = tf2_ros.StaticTransformBroadcaster()
 Telemetry = namedtuple("Telemetry", "git_version animation_id battery_v battery_p system_status calibration_status mode selfcheck current_position start_position")
 telemetry = Telemetry('nan', 'No animation', 'nan', 'nan', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_POS', 'NO_POS')
 
-get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
+# get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
 
 logging.basicConfig(  # TODO all prints as logs
    level=logging.DEBUG, # INFO
@@ -327,7 +327,7 @@ def _response_animation_id(*args, **kwargs):
 @messaging.request_callback("batt_voltage")
 def _response_batt(*args, **kwargs):
     if check_state_topic(wait_new_status=True):
-        return get_telemetry('body').voltage
+        return FlightLib.get_telemetry_locked('body').voltage
     else:
         stop_subscriber()
         return float('nan')
@@ -336,7 +336,7 @@ def _response_batt(*args, **kwargs):
 @messaging.request_callback("cell_voltage")
 def _response_cell(*args, **kwargs):
     if check_state_topic(wait_new_status=True):
-        return get_telemetry('body').cell_voltage
+        return FlightLib.get_telemetry_locked('body').cell_voltage
     else:
         stop_subscriber()
         return float('nan')
@@ -355,7 +355,7 @@ def _response_cal_status(*args, **kwargs):
 
 @messaging.request_callback("position")
 def _response_position(*args, **kwargs):
-    telem = get_telemetry(client.active_client.FRAME_ID)
+    telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
     return "{:.2f} {:.2f} {:.2f} {:.1f} {}".format(
         telem.x, telem.y, telem.z, math.degrees(telem.yaw), client.active_client.FRAME_ID)
 
@@ -384,7 +384,7 @@ def _command_move_start_to_current_position(*args, **kwargs):
                                         )
     logger.debug("x_start = {}, y_start = {}".format(x_start, y_start))
     if not math.isnan(x_start):
-        telem = get_telemetry(client.active_client.FRAME_ID)
+        telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
         logger.debug("x_telem = {}, y_telem = {}".format(telem.x, telem.y))
         if not math.isnan(telem.x):
             client.active_client.config.set('PRIVATE', 'x0', telem.x - x_start)
@@ -407,7 +407,7 @@ def _command_reset_start(*args, **kwargs):
 
 @messaging.message_callback("set_z_to_ground")
 def _command_set_z(*args, **kwargs):
-    telem = get_telemetry(client.active_client.FRAME_ID)
+    telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
     client.active_client.config.set('PRIVATE', 'z0', telem.z)
     client.active_client.rewrite_config()
     client.active_client.load_config()
@@ -489,7 +489,7 @@ def _command_takeoff(*args, **kwargs):
 def _command_takeoff_z(*args, **kwargs):
     z_str = kwargs.get("z", None)
     if z_str is not None:
-        telem = get_telemetry(client.active_client.FRAME_ID)
+        telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
         logger.info("Takeoff to z = {} at {}".format(z_str, datetime.datetime.now()))
         task_manager.add_task(0, 0, FlightLib.reach_point,
                           task_kwargs={
@@ -662,7 +662,7 @@ def telemetry_loop():
         services_unavailable = FlightLib.check_ros_services_unavailable()
         if not services_unavailable:
             try:
-                ros_telemetry = get_telemetry(client.active_client.FRAME_ID)
+                ros_telemetry = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
                 if ros_telemetry.connected:
                     telemetry = telemetry._replace(battery_v = '{:.2f}'.format(ros_telemetry.voltage))
                     batt_empty_param = get_param('BAT_V_EMPTY')