From 5328d7c20b575562fe84f12f33e8482eabc8b1df Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Thu, 7 Nov 2019 05:12:17 +0000 Subject: [PATCH] Client: Add threading lock for get_telemetry service --- Drone/FlightLib/FlightLib.py | 36 ++++++++++++++++++++---------------- Drone/copter_client.py | 16 ++++++++-------- 2 files changed, 28 insertions(+), 24 deletions(-) diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index 1ce4fe3..abf8e59 100644 --- a/Drone/FlightLib/FlightLib.py +++ b/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 diff --git a/Drone/copter_client.py b/Drone/copter_client.py index f2ac110..bdf6371 100644 --- a/Drone/copter_client.py +++ b/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')