Skip to content

Commit

Permalink
Client: Add threading lock for get_telemetry service
Browse files Browse the repository at this point in the history
  • Loading branch information
@goldarte
goldarte committed Nov 7, 2019
1 parent 4db1839 commit 5328d7c
Show file tree
Hide file tree
Showing 2 changed files with 28 additions and 24 deletions.
36 changes: 20 additions & 16 deletions Drone/FlightLib/FlightLib.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,11 @@
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)
Expand Down Expand Up @@ -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")
Expand All @@ -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")
Expand All @@ -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")
Expand All @@ -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))
Expand All @@ -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()

Expand All @@ -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(
Expand Down Expand Up @@ -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()

Expand All @@ -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(
Expand Down Expand Up @@ -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()

Expand All @@ -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
Expand All @@ -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)
Expand All @@ -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
Expand All @@ -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!")
Expand All @@ -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
Expand Down
16 changes: 8 additions & 8 deletions Drone/copter_client.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@
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
Expand Down Expand Up @@ -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')
Expand All @@ -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')
Expand All @@ -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)

Expand Down Expand Up @@ -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)
Expand All @@ -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()
Expand Down Expand Up @@ -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={
Expand Down Expand Up @@ -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')
Expand Down

0 comments on commit 5328d7c

Please sign in to comment.