From 2506c8420ff4782333b81ba366694313c18668b2 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 6 Mar 2018 01:01:59 +0300 Subject: [PATCH] simple_offboard: /navigate_global service --- clever/CMakeLists.txt | 1 + clever/src/global_local.py | 11 +++++++++-- clever/src/simple_offboard.py | 15 +++++++++++---- clever/srv/NavigateGlobal.srv | 11 +++++++++++ docs/simple_offboard.md | 25 ++++++++++++++++++++++++- 5 files changed, 56 insertions(+), 7 deletions(-) create mode 100644 clever/srv/NavigateGlobal.srv diff --git a/clever/CMakeLists.txt b/clever/CMakeLists.txt index 96c44158..dfb0307d 100644 --- a/clever/CMakeLists.txt +++ b/clever/CMakeLists.txt @@ -69,6 +69,7 @@ add_service_files( FILES GetTelemetry.srv Navigate.srv + NavigateGlobal.srv SetPosition.srv SetPositionYawRate.srv SetPositionGlobal.srv diff --git a/clever/src/global_local.py b/clever/src/global_local.py index a4f8f047..dd5f7d85 100644 --- a/clever/src/global_local.py +++ b/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) diff --git a/clever/src/simple_offboard.py b/clever/src/simple_offboard.py index 1f83ab49..6a09fd26 100755 --- a/clever/src/simple_offboard.py +++ b/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)) diff --git a/clever/srv/NavigateGlobal.srv b/clever/srv/NavigateGlobal.srv new file mode 100644 index 00000000..68ca45ba --- /dev/null +++ b/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 diff --git a/docs/simple_offboard.md b/docs/simple_offboard.md index 59bb32de..5badf7b9 100644 --- a/docs/simple_offboard.md +++ b/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.