simple_offboard: /navigate_global service

clever/CMakeLists.txt

@@ -69,6 +69,7 @@ add_service_files(
   FILES
   GetTelemetry.srv
   Navigate.srv
+  NavigateGlobal.srv
   SetPosition.srv
   SetPositionYawRate.srv
   SetPositionGlobal.srv

clever/src/global_local.py

@@ -9,8 +9,15 @@ from sensor_msgs.msg import NavSatFix
 def global_to_local(lat, lon):
     # TODO: refactor
 
-    position_global = rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=5)
-    pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=5)
+    try:
+        position_global = rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=0.5)
+    except rospy.exceptions.ROSException:
+        raise Exception('No global position')
+
+    try:
+        pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=0.5)
+    except rospy.exceptions.ROSException:
+        raise Exception('No local position')
 
     d = math.hypot(pose.pose.position.x, pose.pose.position.y)
 

clever/src/simple_offboard.py

@@ -154,15 +154,20 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
     ps.header.stamp = stamp
     vs.header.stamp = stamp
 
-    if isinstance(req, srv.NavigateRequest):
+    if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)):
         global current_nav_start, current_nav_start_stamp, current_nav_finish
 
         if update_frame:
             ps.header.frame_id = req.frame_id or LOCAL_FRAME
-            ps.pose.position = Point(req.x, req.y, req.z)
+            ps.pose.position = Point(getattr(req, 'x', 0), getattr(req, 'y', 0), req.z)
             ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
             current_nav_finish = tf_buffer.transform(ps, LOCAL_FRAME, TRANSFORM_TIMEOUT)
 
+            if isinstance(req, srv.NavigateGlobalRequest):
+                # Recalculate x and y from lat and lon
+                current_nav_finish.pose.position.x, current_nav_finish.pose.position.y = \
+                    global_to_local(req.lat, req.lon)
+
         if not continued:
             current_nav_start = pose.pose.position
             current_nav_start_stamp = stamp
@@ -319,7 +324,7 @@ def handle(req):
         rospy.logwarn('No connection to the FCU')
         return {'message': 'No connection to the FCU'}
 
-    if isinstance(req, srv.NavigateRequest) and req.speed <= 0:
+    if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)) and req.speed <= 0:
         rospy.logwarn('Navigate speed must be greater than zero, %s passed')
         return {'message': 'Navigate speed must be greater than zero, %s passed' % req.speed}
 
@@ -371,6 +376,7 @@ def release(req):
 
 
 rospy.Service('navigate', srv.Navigate, handle)
+rospy.Service('navigate_global', srv.NavigateGlobal, handle)
 rospy.Service('set_position', srv.SetPosition, handle)
 rospy.Service('set_position/yaw_rate', srv.SetPositionYawRate, handle)
 rospy.Service('set_position_global', srv.SetPositionGlobal, handle)
@@ -465,7 +471,8 @@ def start_loop():
                 try:
                     stamp = rospy.get_rostime()
 
-                    if getattr(current_req, 'update_frame', False) or isinstance(current_req, srv.NavigateRequest):
+                    if getattr(current_req, 'update_frame', False) or \
+                            isinstance(current_req, (srv.NavigateRequest, srv.NavigateGlobalRequest)):
                         current_pub, current_msg = get_publisher_and_message(current_req, stamp, True,
                                                                              getattr(current_req, 'update_frame', False))
 

clever/srv/NavigateGlobal.srv

@@ -0,0 +1,11 @@
+float32 lat
+float32 lon
+float32 z
+float32 yaw
+float32 speed
+string frame_id
+bool update_frame
+bool auto_arm
+---
+bool success
+string message

docs/simple_offboard.md

@@ -14,6 +14,7 @@ Simple offboard
 * `update_frame` — считать ли систему координат изменяющейся (например, `false` для `local_origin`, `fcu`, `fcu_horiz`, `true` для `marker_map`);
 * `x`, `y` – горизонтальные координаты в системе координат `frame_id`;
 * `z` — высота в системе координат `frame_id`;
+* `lat`, `lon` – широта и долгота (в градусах);
 * `yaw` — рысканье в радианах в системе координат `frame_id` (0 – коптер смотрит по оси X);
 * `yaw_rate` — угловая скорость по рысканью в радианах в секунду (против часовой);
 * `thrust` — уровень газа (от 0 [нет газа] до 1 [полный газ]).
@@ -137,6 +138,26 @@ navigate(x=3, y=2, z=2, speed=1, frame_id='aruco_map', update_frame=True)
 rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, speed: 0.5, frame_id: 'fcu_horiz', update_frame: false, auto_arm: true}"
 ```
 
+### navigate_global
+
+> **Info** Образ версии >0.5.
+
+Полет по прямой в позицию в глобальной системе координат (широта/долгота).
+
+Параметры: lat (широта), lon (долгота), z (высота в системе координат frame_id), yaw (рысканье в системе координат frame_id), update_frame.
+
+Объявление прокси к сервису:
+
+```python
+navigate_global = rospy.ServiceProxy('/navigate_global', srv.NavigateGlobal)
+```
+
+Полет в глобальную точку по прямой (оставаясь на текущей высоте):
+
+```python
+navigate_global(lat=55.707033, lon=37.725010, z=0, frame_id='fcu_horiz')
+```
+
 ### set_position
 
 Установить цель по позиции и рысканью. 
@@ -174,7 +195,9 @@ set_position(x=2, y=2, z=3, frame_id='aruco_map', update_frame=True)  #  пол
 
 ### set_position_global
 
-Полет в позицию в глобальной системе координат (широта/долгота).
+> **Hint** Для полета в точку в глобальной системе координат по прямой используйте более высокоуровневый сервис `navigate_global`.
+
+Установить цель по позиции в глобальной системе координат (широта/долгота).
 
 Параметры: lat (широта), lon (долгота), z (высота в системе координат frame_id), yaw (рысканье в системе координат frame_id), update_frame.