simple_offboard: /land service added (set copter’s mode to AUTO.LAND)

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)
 
 

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