diff --git a/clever/src/simple_offboard.py b/clever/src/simple_offboard.py index f422f2e7..1f83ab49 100755 --- a/clever/src/simple_offboard.py +++ b/clever/src/simple_offboard.py @@ -83,6 +83,8 @@ NAVIGATE_AFTER_ARMED = rospy.Duration(rospy.get_param('~navigate_after_armed', F TRANSFORM_TIMEOUT = rospy.Duration(rospy.get_param('~transform_timeout', 3)) SETPOINT_RATE = rospy.get_param('~setpoint_rate', 30) LOCAL_FRAME = rospy.get_param('~local_frame', 'local_origin') +LAND_MODE = rospy.get_param('~land_mode', 'AUTO.LAND') +LAND_TIMEOUT = rospy.Duration(rospy.get_param('~land_timeout', 2)) def offboard_and_arm(): @@ -346,6 +348,21 @@ def handle(req): return {'success': False, 'message': str(e)} +def land(req): + rospy.loginfo('request %s mode' % LAND_MODE) + res = set_mode(custom_mode=LAND_MODE) + if not res.mode_sent: + return {'message': 'Cannot send %s mode request' % LAND_MODE} + + start = rospy.get_rostime() + while True: + if state.mode == LAND_MODE: + return {'success': True} + if rospy.get_rostime() - start > LAND_TIMEOUT: + return {'message': '%s mode request timed out' % LAND_MODE} + rospy.sleep(0.1) + + def release(req): global current_pub current_pub = None @@ -364,6 +381,7 @@ rospy.Service('set_attitude', srv.SetAttitude, handle) rospy.Service('set_attitude/yaw_rate', srv.SetAttitudeYawRate, handle) rospy.Service('set_rates', srv.SetRates, handle) rospy.Service('set_rates/yaw', srv.SetRatesYaw, handle) +rospy.Service('land', Trigger, land) rospy.Service('release', Trigger, release) diff --git a/docs/simple_offboard.md b/docs/simple_offboard.md index dba23bf5..2b2d8e41 100644 --- a/docs/simple_offboard.md +++ b/docs/simple_offboard.md @@ -5,7 +5,7 @@ Simple offboard `simple_offboard` является высокоуровневым способом взаимодействия с полетным контроллером. Для более низкоуровневой работы см. [mavros](/docs/mavros.md). -Основные сервисы – `get_telemetry` (получение всей телеметрии разом), `navigate` (полет в заданную точку по прямой). +Основные сервисы – `get_telemetry` (получение всей телеметрии разом), `navigate` (полет в заданную точку по прямой), `land` (переход в режим посадки). Общие для сервисов параметры: @@ -239,7 +239,35 @@ set_velocity_yaw_rate(vx=0.2, vy=0.0, vz=0, yaw_rate=0.5, frame_id: 'fcu_horiz', Посадка ------- -Для посадки можно использовать режим ``AUTO.LAND``. Land detector должен быть включен и указан в ``LPE_FUSION``. Параметр `COM_DISARM_LAND` должен быть установлен в значение > 0. +> **Info** Образ версии >0.5. + +Для посадки можно использовать сервис `/land`. При вызове сервиса коптер автоматически переведется в режим AUTO.LAND (или аналогичный). + +Объявление прокси к сервису: + +```python +land = rospy.ServiceProxy('/land', Trigger) +``` + +Посадка коптера: + +```python +res = land() + +if res.success: + # коптер успешно переведен в режим AUTO.LAND + # ... +``` + +Пример использования сервиса из командной строки: + +```bash +rosservice call /land "{}" +``` + +> **Note** Для автоматического отключения винтов после посадки PX4-параметр `COM_DISARM_LAND` должен быть установлен в значение > 0. + +В предущих версиях для посадки необходимо переводит коптер в режим ``AUTO.LAND``, используя mavros. ```python from mavros_msgs.srv import SetMode