From 53f2fcd678656c30eb39381b5050106b2c2ab68a Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 26 Nov 2017 03:26:58 +0300 Subject: [PATCH] Take offboard_and_arm out of the handle_lock + error if no FCU connection --- clever/src/simple_offboard.py | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/clever/src/simple_offboard.py b/clever/src/simple_offboard.py index c091c068..6ddd0dd7 100755 --- a/clever/src/simple_offboard.py +++ b/clever/src/simple_offboard.py @@ -236,21 +236,25 @@ handle_lock = Lock() def handle(req): global current_pub, current_msg, current_req - with handle_lock: - try: - stamp = rospy.get_rostime() - current_pub, current_msg = get_publisher_and_message(req, stamp) - rospy.loginfo('Topic: %s, message: %s', current_pub.name, current_msg) - current_msg.header.stamp = stamp - current_pub.publish(current_msg) + if not state.connected: + return {'message': 'No connection to the FCU'} - offboard_and_arm() - return {'success': True} + try: + with handle_lock: + stamp = rospy.get_rostime() + current_pub, current_msg = get_publisher_and_message(req, stamp) + rospy.loginfo('Topic: %s, message: %s', current_pub.name, current_msg) - except Exception as e: - rospy.logerr(str(e)) - return {'success': False, 'message': str(e)} + current_msg.header.stamp = stamp + current_pub.publish(current_msg) + + offboard_and_arm() + return {'success': True} + + except Exception as e: + rospy.logerr(str(e)) + return {'success': False, 'message': str(e)} def release(req):