From 2dda726d3ec63968b233698b965b710b07ba6947 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 31 Oct 2022 02:32:45 +0600 Subject: [PATCH 01/21] clover.launch: turn on disable_on_vpe by default --- clover/launch/clover.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clover/launch/clover.launch b/clover/launch/clover.launch index 8c77a0f4..d438bad5 100644 --- a/clover/launch/clover.launch +++ b/clover/launch/clover.launch @@ -45,7 +45,7 @@ - + From 45042cd6f51e60805847cf8f2222c9db00169ba7 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 4 Nov 2022 03:05:59 +0600 Subject: [PATCH 02/21] docs: updates to camera and computer vision article --- docs/en/camera.md | 60 +++++++++++++++++++++++++---------------------- docs/ru/camera.md | 60 +++++++++++++++++++++++++---------------------- 2 files changed, 64 insertions(+), 56 deletions(-) diff --git a/docs/en/camera.md b/docs/en/camera.md index ba0dbd3a..a4079a45 100644 --- a/docs/en/camera.md +++ b/docs/en/camera.md @@ -14,7 +14,7 @@ The `clover` service must be restarted after the launch-file has been edited: sudo systemctl restart clover ``` -You may use rqt or [web_video_server](web_video_server.md) to view the camera stream. +You may use [rqt](rviz.md) or [web_video_server](web_video_server.md) to view the camera stream. ## Troubleshooting @@ -52,8 +52,6 @@ The [SD card image](image.md) comes with a preinstalled [OpenCV](https://opencv. ### Python -Main article: http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython. - An example of creating a subscriber for a topic with an image from the main camera for processing with OpenCV: ```python @@ -61,12 +59,14 @@ import rospy import cv2 from sensor_msgs.msg import Image from cv_bridge import CvBridge +from clover import long_callback -rospy.init_node('computer_vision_sample') +rospy.init_node('cv') bridge = CvBridge() +@long_callback def image_callback(data): - cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image + img = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image # Do any image processing with cv2... image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback) @@ -74,19 +74,31 @@ image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback) rospy.spin() ``` +> **Note** Image processing may take significant time to finish. This can cause an [issue](https://github.com/ros/ros_comm/issues/1901) in rospy library, which would lead to processing stale camera frames. To solve this problem you need to use `long_callback` decorator from `clover` library, as in the example above. + +#### Limiting CPU usage + +When using the `main_camera/image_raw` topic, the script will process the maximum number of frames from the camera, actively utilizing the CPU (up to 100%). In tasks, where processing each camera frame is not critical, you can use the topic, where the frames are published at rate 5 Hz: `main_camera/image_raw_throttled`: + +```python +image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1) +``` + +#### Publishing images + To debug image processing, you can publish a separate topic with the processed image: ```python image_pub = rospy.Publisher('~debug', Image) ``` -Publishing the processed image (at the end of the image_callback function): +Publishing the processed image: ```python -image_pub.publish(bridge.cv2_to_imgmsg(cv_image, 'bgr8')) +image_pub.publish(bridge.cv2_to_imgmsg(img, 'bgr8')) ``` -The obtained images can be viewed using [web_video_server](web_video_server.md). +The published images can be viewed using [web_video_server](web_video_server.md) or [rqt](rviz.md). #### Retrieving one frame @@ -97,7 +109,7 @@ import rospy from sensor_msgs.msg import Image from cv_bridge import CvBridge -rospy.init_node('computer_vision_sample') +rospy.init_node('cv') bridge = CvBridge() # ... @@ -119,40 +131,32 @@ QR codes recognition in Python: ```python import rospy from pyzbar import pyzbar +import cv2 from cv_bridge import CvBridge from sensor_msgs.msg import Image +from clover import long_callback +rospy.init_node('cv') bridge = CvBridge() -rospy.init_node('barcode_test') - -# Image subscriber callback function -def image_callback(data): - cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image - barcodes = pyzbar.decode(cv_image) +@long_callback +def image_callback(msg): + img = bridge.imgmsg_to_cv2(msg, 'bgr8') + barcodes = pyzbar.decode(img) for barcode in barcodes: - b_data = barcode.data.decode("utf-8") + b_data = barcode.data.decode('utf-8') b_type = barcode.type (x, y, w, h) = barcode.rect xc = x + w/2 yc = y + h/2 - print("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc)) + print('Found {} with data {} with center at x={}, y={}'.format(b_type, b_data, xc, yc)) -image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1) +image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1) rospy.spin() ``` -The script will take up to 100% CPU capacity. To slow down the script artificially, you can use [throttling](http://wiki.ros.org/topic_tools/throttle) of frames from the camera, for example, at 5 Hz (`main_camera.launch`): - -> **Note** Starting from [image](image.md) version **0.24** `image_raw_throttled` topic is available without addition configuration. - -```xml - -``` - -The topic for the subscriber in this case should be changed for `main_camera/image_raw_throttled`. +> **Hint** See other computer vision examples in the `~/examples` directory of the [RPi image](image.md). ## Video recording diff --git a/docs/ru/camera.md b/docs/ru/camera.md index 0f9b6a14..ea4082c6 100644 --- a/docs/ru/camera.md +++ b/docs/ru/camera.md @@ -54,8 +54,6 @@ raspistill -o test.jpg ### Python -Основная статья: http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython. - Пример создания подписчика на топик с изображением с основной камеры для обработки с использованием OpenCV: ```python @@ -63,12 +61,14 @@ import rospy import cv2 from sensor_msgs.msg import Image from cv_bridge import CvBridge +from clover import long_callback -rospy.init_node('computer_vision_sample') +rospy.init_node('cv') bridge = CvBridge() +@long_callback def image_callback(data): - cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image + img = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image # Do any image processing with cv2... image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback) @@ -76,19 +76,31 @@ image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback) rospy.spin() ``` +> **Note** Обработка изображения может занимать значительное время. Это может вызвать [проблему](https://github.com/ros/ros_comm/issues/1901) в библиотеке rospy, которая приведет к обработке устаревших кадров с камеры. Для решения этой проблемы необходимо использовать декоратор `long_callback` из библиотеки `clover`, как в примере выше. + +#### Ограничение использования CPU + +При использовании топика `main_camera/image_raw` скрипт будет обрабатывать максимальное количество кадров с камеры, активно используя CPU (вплоть до 100%). В задачах, где обработка каждого кадра не критична, можно использовать топик, где кадры публикуются с частотой 5 Гц: `main_camera/image_raw_throttled`: + +```python +image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1) +``` + +#### Публикация изображений + Для отладки обработки изображения можно публиковать отдельный топик с обработанным изображением: ```python image_pub = rospy.Publisher('~debug', Image) ``` -Публикация обработанного изображения (в конце функции image_callback): +Публикация обработанного изображения: ```python -image_pub.publish(bridge.cv2_to_imgmsg(cv_image, 'bgr8')) +image_pub.publish(bridge.cv2_to_imgmsg(img, 'bgr8')) ``` -Получаемые изображения можно просматривать используя [web_video_server](web_video_server.md). +Получаемые изображения можно просматривать используя [web_video_server](web_video_server.md) или [rqt](rviz.md). #### Получение одного кадра @@ -99,12 +111,12 @@ import rospy from sensor_msgs.msg import Image from cv_bridge import CvBridge -rospy.init_node('computer_vision_sample') +rospy.init_node('cv') bridge = CvBridge() # ... -# Получение кадра: +# Retrieve a frame: img = bridge.imgmsg_to_cv2(rospy.wait_for_message('main_camera/image_raw', Image), 'bgr8') ``` @@ -121,40 +133,32 @@ img = bridge.imgmsg_to_cv2(rospy.wait_for_message('main_camera/image_raw', Image ```python import rospy from pyzbar import pyzbar +import cv2 from cv_bridge import CvBridge from sensor_msgs.msg import Image +from clover import long_callback +rospy.init_node('cv') bridge = CvBridge() -rospy.init_node('barcode_test') - -# Image subscriber callback function -def image_callback(data): - cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image - barcodes = pyzbar.decode(cv_image) +@long_callback +def image_callback(msg): + img = bridge.imgmsg_to_cv2(msg, 'bgr8') + barcodes = pyzbar.decode(img) for barcode in barcodes: - b_data = barcode.data.decode("utf-8") + b_data = barcode.data.decode('utf-8') b_type = barcode.type (x, y, w, h) = barcode.rect xc = x + w/2 yc = y + h/2 - print("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc)) + print('Found {} with data {} with center at x={}, y={}'.format(b_type, b_data, xc, yc)) -image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1) +image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1) rospy.spin() ``` -Скрипт будет занимать 100% процессора. Для искусственного замедления работы скрипта можно запустить [throttling](http://wiki.ros.org/topic_tools/throttle) кадров с камеры, например, в 5 Гц (`main_camera.launch`): - -> **Note** Начиная с версии [образа](image.md) **0.24** топик `image_raw_throttled` доступен без дополнительной конфигурации. - -```xml - -``` - -Топик для подписчика в этом случае необходимо поменять на `main_camera/image_raw_throttled`. +> **Hint** Смотрите другие примеры по работе с компьютерным зрением в каталоге `~/examples` [образа для RPi](image.md). ## Запись видео From 460c3fdbe15c400dece9a73259db3d4d2d6736d8 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 21 Dec 2022 20:49:46 +0300 Subject: [PATCH 03/21] Whitespaces fixes --- clover/examples/camera.py | 2 +- clover_blocks/www/python.js | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/clover/examples/camera.py b/clover/examples/camera.py index 9f69d6de..33532146 100644 --- a/clover/examples/camera.py +++ b/clover/examples/camera.py @@ -5,7 +5,7 @@ # - cuts out a central square from the camera image; # - publishes this cropped image to the topic `/cv/center`; # - computes the average color of it; -# - prints its name to the console. +# - prints its name to the console. import rospy import cv2 diff --git a/clover_blocks/www/python.js b/clover_blocks/www/python.js index 1589cfdf..9cf67938 100644 --- a/clover_blocks/www/python.js +++ b/clover_blocks/www/python.js @@ -81,7 +81,7 @@ function generateROSDefinitions() { code += `get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)\n`; code += `navigate = rospy.ServiceProxy('navigate', srv.Navigate)\n`; if (rosDefinitions.navigateGlobal) { - code += `navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)\n`; + code += `navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)\n`; } if (rosDefinitions.setVelocity) { code += `set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)\n`; From 0cfdac43ec73333141c467a411e91acaa223672f Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 12 Jan 2023 11:00:05 +0300 Subject: [PATCH 04/21] Significant update to simple_offboard node * Allow using nans for most of services parameters * Add terrain frame * Remove yaw_rate parameter from most services * Add set_yaw and set_yaw_rate services * Correct order for pitch and roll everywhere to match XYZ convention * Add simple_offboard/state topic * Add essential tests * Stop publishing setpoints when land called --- clover/CMakeLists.txt | 13 +- clover/examples/navigate_wait.py | 7 +- clover/msg/State.msg | 38 ++ clover/src/autotest/autotest_aruco.py | 7 +- clover/src/autotest/autotest_flight.py | 27 +- clover/src/simple_offboard.cpp | 600 +++++++++++++++++-------- clover/srv/GetTelemetry.srv | 4 +- clover/srv/Navigate.srv | 1 - clover/srv/NavigateGlobal.srv | 1 - clover/srv/SetAltitude.srv | 5 + clover/srv/SetAttitude.srv | 2 +- clover/srv/SetPosition.srv | 1 - clover/srv/SetRates.srv | 2 +- clover/srv/SetVelocity.srv | 1 - clover/srv/SetYaw.srv | 5 + clover/srv/SetYawRate.srv | 4 + clover/test/offboard.py | 402 +++++++++++++++++ clover/test/offboard.test | 10 + clover_blocks/www/blocks.js | 21 +- clover_blocks/www/index.html | 2 +- clover_blocks/www/python.js | 7 +- docs/en/parameters.md | 4 +- docs/en/simple_offboard.md | 12 +- docs/en/snippets.md | 8 +- docs/ru/parameters.md | 4 +- docs/ru/simple_offboard.md | 8 +- docs/ru/snippets.md | 6 +- 27 files changed, 942 insertions(+), 260 deletions(-) create mode 100644 clover/msg/State.msg create mode 100644 clover/srv/SetAltitude.srv create mode 100644 clover/srv/SetYaw.srv create mode 100644 clover/srv/SetYawRate.srv create mode 100755 clover/test/offboard.py create mode 100644 clover/test/offboard.test diff --git a/clover/CMakeLists.txt b/clover/CMakeLists.txt index a2bfb3dc..7eca3f4c 100644 --- a/clover/CMakeLists.txt +++ b/clover/CMakeLists.txt @@ -80,11 +80,10 @@ catkin_python_setup() ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) +add_message_files( + FILES + State.msg +) ## Generate services in the 'srv' folder add_service_files( @@ -92,6 +91,9 @@ add_service_files( GetTelemetry.srv Navigate.srv NavigateGlobal.srv + SetAltitude.srv + SetYaw.srv + SetYawRate.srv SetPosition.srv SetVelocity.srv SetAttitude.srv @@ -306,4 +308,5 @@ endif() if (CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest(test/basic.test) + add_rostest(test/offboard.test) endif() diff --git a/clover/examples/navigate_wait.py b/clover/examples/navigate_wait.py index 99637caf..acb37f70 100644 --- a/clover/examples/navigate_wait.py +++ b/clover/examples/navigate_wait.py @@ -16,11 +16,8 @@ set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude) set_rates = rospy.ServiceProxy('set_rates', srv.SetRates) land = rospy.ServiceProxy('land', Trigger) -def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \ - frame_id='body', tolerance=0.2, auto_arm=False): - - res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \ - frame_id=frame_id, auto_arm=auto_arm) +def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False): + res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm) if not res.success: return res diff --git a/clover/msg/State.msg b/clover/msg/State.msg new file mode 100644 index 00000000..038df4fc --- /dev/null +++ b/clover/msg/State.msg @@ -0,0 +1,38 @@ +uint8 MODE_NONE = 0 +uint8 MODE_NAVIGATE = 1 +uint8 MODE_NAVIGATE_GLOBAL = 2 +uint8 MODE_POSITION = 3 +uint8 MODE_VELOCITY = 4 +uint8 MODE_ATTITUDE = 5 +uint8 MODE_RATES = 6 + +uint8 YAW_MODE_YAW = 0 +uint8 YAW_MODE_YAW_RATE = 1 +uint8 YAW_MODE_YAW_TOWARDS = 2 + +# type of offboard control +uint8 mode +uint8 yaw_mode + +# targets +float32 x +float32 y +float32 z +float32 speed +float32 lat +float32 lon +float32 vx +float32 vy +float32 vz +float32 roll +float32 pitch +float32 yaw +float32 roll_rate +float32 pitch_rate +float32 yaw_rate +float32 thrust + +# frames of reference +string xy_frame_id +string z_frame_id +string yaw_frame_id diff --git a/clover/src/autotest/autotest_aruco.py b/clover/src/autotest/autotest_aruco.py index d53c98ca..0fbe6fa7 100755 --- a/clover/src/autotest/autotest_aruco.py +++ b/clover/src/autotest/autotest_aruco.py @@ -35,11 +35,8 @@ def print_current_map_position(): dist = rospy.wait_for_message('rangefinder/range', Range).range print('Map position:\tx={:.1f}\ty={:.1f}\tz={:.1f}\tyaw={:.1f}\tdist={:.2f}'.format(telem.x, telem.y, telem.z, telem.yaw, dist)) -def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \ - frame_id='body', tolerance=0.2, auto_arm=False): - - res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \ - frame_id=frame_id, auto_arm=auto_arm) +def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False): + res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm) if not res.success: return res diff --git a/clover/src/autotest/autotest_flight.py b/clover/src/autotest/autotest_flight.py index 19adfdbf..371db7df 100755 --- a/clover/src/autotest/autotest_flight.py +++ b/clover/src/autotest/autotest_flight.py @@ -2,7 +2,7 @@ import rospy import math -from math import nan +from math import nan, inf import signal import sys from clover import srv @@ -15,6 +15,8 @@ rospy.init_node('autotest_flight', disable_signals=True) # disable signals to al get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate)) navigate_global = handle_response(rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)) +set_yaw = handle_response(rospy.ServiceProxy('set_yaw', srv.SetYaw)) +set_yaw_rate = handle_response(rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate)) set_position = handle_response(rospy.ServiceProxy('set_position', srv.SetPosition)) set_velocity = handle_response(rospy.ServiceProxy('set_velocity', srv.SetVelocity)) set_attitude = handle_response(rospy.ServiceProxy('set_attitude', srv.SetAttitude)) @@ -28,11 +30,8 @@ def interrupt(sig, frame): signal.signal(signal.SIGINT, interrupt) -def navigate_wait(x=0, y=0, z=0, yaw=nan, yaw_rate=0, speed=0.5, \ - frame_id='body', tolerance=0.2, auto_arm=False): - - res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \ - frame_id=frame_id, auto_arm=auto_arm) +def navigate_wait(x=0, y=0, z=0, yaw=nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False): + res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm) if not res.success: return res @@ -69,17 +68,17 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body') rospy.sleep(2) set_position(frame_id='body') -input('Rotate right 90° [enter] ') -navigate(yaw=-math.pi / 2, frame_id='navigate_target') +input('Rotate right 90° using set_yaw [enter] ') +set_yaw(yaw=-math.pi / 2, frame_id='navigate_target') rospy.sleep(3) input('Use set_attitude to fly backwards [enter]') -set_attitude(pitch=-0.3, roll=0, yaw=0, thrust=0.5, frame_id='body') +set_attitude(roll=0, pitch=-0.3, yaw=0, thrust=0.5, frame_id='body') rospy.sleep(0.3) set_position(frame_id='body') input('Use set_attitude to fly right [enter]') -set_attitude(pitch=0, roll=0.3, yaw=0, thrust=0.5, frame_id='body') +set_attitude(roll=0.3, pitch=0, yaw=0, thrust=0.5, frame_id='body') rospy.sleep(0.5) set_position(frame_id='body') @@ -88,13 +87,13 @@ set_rates(roll_rate=1.2, thrust=0.5) rospy.sleep(0.4) set_position(frame_id='body') -input('Rotate 360° to the right using yaw_rate [enter]') -set_position(x=nan, y=nan, z=nan, frame_id='body', yaw=nan, yaw_rate=-1) +input('Rotate 360° to the right using set_yaw_rate [enter]') +set_yaw_rate(yaw_rate=-1) rospy.sleep(2 * math.pi) set_position(frame_id='body') -input('Return to start point [enter]') -navigate_wait(x=start.x, y=start.y, z=start.z, yaw=start.yaw, speed=1, frame_id='map') +input('Return to start point heading forward [enter]') +navigate_wait(x=start.x, y=start.y, z=start.z, yaw=inf, speed=1, frame_id='map') input('Land [enter]') land() diff --git a/clover/src/simple_offboard.cpp b/clover/src/simple_offboard.cpp index fc755d77..0781d78d 100644 --- a/clover/src/simple_offboard.cpp +++ b/clover/src/simple_offboard.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -37,14 +38,19 @@ #include #include #include +#include #include #include #include +#include +#include +#include #include #include #include #include +#include using std::string; using std::isnan; @@ -54,6 +60,7 @@ using namespace clover; using mavros_msgs::PositionTarget; using mavros_msgs::AttitudeTarget; using mavros_msgs::Thrust; +using mavros_msgs::Altitude; // tf2 tf2_ros::Buffer tf_buffer; @@ -81,33 +88,40 @@ bool land_only_in_offboard, nav_from_sp, check_kill_switch; std::map reference_frames; // Publishers -ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub; +ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub, state_pub; // Service clients ros::ServiceClient arming, set_mode; // Containers ros::Timer setpoint_timer; -tf::Quaternion tq; PoseStamped position_msg; PositionTarget position_raw_msg; -AttitudeTarget att_raw_msg; -Thrust thrust_msg; -TwistStamped rates_msg; +//TwistStamped rates_msg; TransformStamped target, setpoint; geometry_msgs::TransformStamped body; +geometry_msgs::TransformStamped terrain; // State PoseStamped nav_start; -PoseStamped setpoint_position, setpoint_position_transformed; -Vector3Stamped setpoint_velocity, setpoint_velocity_transformed; -QuaternionStamped setpoint_attitude, setpoint_attitude_transformed; -float setpoint_yaw_rate; +PointStamped setpoint_position; +PointStamped setpoint_altitude; +Vector3Stamped setpoint_velocity; +float setpoint_yaw, setpoint_roll, setpoint_pitch; +Vector3 setpoint_rates; +string yaw_frame_id; +float setpoint_thrust; float nav_speed; +float setpoint_lat = NAN, setpoint_lon = NAN; bool busy = false; bool wait_armed = false; bool nav_from_sp_flag = false; +// Last published +PoseStamped setpoint_pose_local; +Vector3Stamped setpoint_velocity_local; +float yaw_local; + enum setpoint_type_t { NONE, NAVIGATE, @@ -115,7 +129,10 @@ enum setpoint_type_t { POSITION, VELOCITY, ATTITUDE, - RATES + RATES, + _ALTITUDE, + _YAW, + _YAW_RATE, }; enum setpoint_type_t setpoint_type = NONE; @@ -170,7 +187,7 @@ void handleLocalPosition(const PoseStamped& pose) { local_position = pose; publishBodyFrame(); - // TODO: terrain?, home? + // TODO: home? } // wait for transform without interrupting publishing setpoints @@ -188,6 +205,20 @@ inline bool waitTransform(const string& target, const string& source, return false; } +void handleAltitude(const Altitude& alt) +{ + // publish terrain frame + if (!std::isfinite(alt.bottom_clearance)) return; + // terrain.header.stamp = alt.header.stamp; + + if (!waitTransform(local_frame, body.child_frame_id, alt.header.stamp, ros::Duration(0.1))) return; + + auto t = tf_buffer.lookupTransform(local_frame, body.child_frame_id, alt.header.stamp); + t.child_frame_id = terrain.child_frame_id; + t.transform.translation.z -= alt.bottom_clearance; + static_transform_broadcaster->sendTransform(t); +} + #define TIMEOUT(msg, timeout) (msg.header.stamp.isZero() || (ros::Time::now() - msg.header.stamp > timeout)) bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res) @@ -207,11 +238,11 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res) res.vx = NAN; res.vy = NAN; res.vz = NAN; - res.pitch = NAN; res.roll = NAN; + res.pitch = NAN; res.yaw = NAN; - res.pitch_rate = NAN; res.roll_rate = NAN; + res.pitch_rate = NAN; res.yaw_rate = NAN; res.voltage = NAN; res.cell_voltage = NAN; @@ -341,20 +372,20 @@ inline float getDistance(const Point& from, const Point& to) return hypot(from.x - to.x, from.y - to.y, from.z - to.z); } -void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoint) +void getNavigateSetpoint(const ros::Time& stamp, const float speed, Point& nav_setpoint) { if (wait_armed) { // don't start navigating if we're waiting arming nav_start.header.stamp = stamp; } - float distance = getDistance(nav_start.pose.position, setpoint_position_transformed.pose.position); + float distance = getDistance(nav_start.pose.position, setpoint_pose_local.pose.position); float time = distance / speed; float passed = std::min((stamp - nav_start.header.stamp).toSec() / time, 1.0); - nav_setpoint.x = nav_start.pose.position.x + (setpoint_position_transformed.pose.position.x - nav_start.pose.position.x) * passed; - nav_setpoint.y = nav_start.pose.position.y + (setpoint_position_transformed.pose.position.y - nav_start.pose.position.y) * passed; - nav_setpoint.z = nav_start.pose.position.z + (setpoint_position_transformed.pose.position.z - nav_start.pose.position.z) * passed; + nav_setpoint.x = nav_start.pose.position.x + (setpoint_pose_local.pose.position.x - nav_start.pose.position.x) * passed; + nav_setpoint.y = nav_start.pose.position.y + (setpoint_pose_local.pose.position.y - nav_start.pose.position.y) * passed; + nav_setpoint.z = nav_start.pose.position.z + (setpoint_pose_local.pose.position.z - nav_start.pose.position.z) * passed; } PoseStamped globalToLocal(double lat, double lon) @@ -385,44 +416,101 @@ PoseStamped globalToLocal(double lat, double lon) return pose; } +// publish navigate_target frame +void publishTarget(ros::Time stamp, bool _static = false) +{ + bool single_frame = (setpoint_position.header.frame_id == setpoint_altitude.header.frame_id); + + // handle yaw for target frame + if (setpoint_yaw_type == YAW || setpoint_yaw_type == YAW_RATE) { // use last set yaw for yaw_rate + if (setpoint_altitude.header.frame_id == yaw_frame_id) { + target.transform.rotation = tf::createQuaternionMsgFromYaw(setpoint_yaw); + } else { + single_frame = false; + target.transform.rotation = tf::createQuaternionMsgFromYaw(yaw_local); + } + } else if (setpoint_yaw_type == TOWARDS) { + single_frame = false; + target.transform.rotation = tf::createQuaternionMsgFromYaw(yaw_local); + } + + if (_static && single_frame) { + // publish at user's command, if all frames are the same + target.header.frame_id = setpoint_position.header.frame_id; + target.header.stamp = stamp; + target.transform.translation.x = setpoint_position.point.x; + target.transform.translation.y = setpoint_position.point.y; + target.transform.translation.z = setpoint_position.point.z; + + } else if (!_static) { + // publish at each iteration, if frames are different + target.header = setpoint_pose_local.header; + target.transform.translation.x = setpoint_pose_local.pose.position.x; + target.transform.translation.y = setpoint_pose_local.pose.position.y; + target.transform.translation.z = setpoint_pose_local.pose.position.z; + } + + static_transform_broadcaster->sendTransform(target); +} + void publish(const ros::Time stamp) { if (setpoint_type == NONE) return; position_raw_msg.header.stamp = stamp; - thrust_msg.header.stamp = stamp; - rates_msg.header.stamp = stamp; - try { - // transform position and/or yaw - if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION || setpoint_type == VELOCITY || setpoint_type == ATTITUDE) { - setpoint_position.header.stamp = stamp; - tf_buffer.transform(setpoint_position, setpoint_position_transformed, local_frame, ros::Duration(0.05)); + // transform position + if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) { + setpoint_position.header.stamp = stamp; + setpoint_altitude.header.stamp = stamp; + // transform xy + try { + auto xy = tf_buffer.transform(setpoint_position, local_frame, ros::Duration(0.05)); + setpoint_pose_local.header = xy.header; + setpoint_pose_local.pose.position.x = xy.point.x; + setpoint_pose_local.pose.position.y = xy.point.y; + } catch (tf2::TransformException& ex) { + // can't transform xy, use last known + ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what()); } - - // transform velocity - if (setpoint_type == VELOCITY) { - setpoint_velocity.header.stamp = stamp; - tf_buffer.transform(setpoint_velocity, setpoint_velocity_transformed, local_frame, ros::Duration(0.05)); + // transform altitude + try { + setpoint_pose_local.pose.position.z = tf_buffer.transform(setpoint_altitude, local_frame, ros::Duration(0.05)).point.z; + } catch (tf2::TransformException& ex) { + // can't transform altitude, use last known + ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what()); } - - } catch (const tf2::TransformException& e) { - ROS_WARN_THROTTLE(10, "can't transform"); } + // transform yaw + if (setpoint_yaw_type == YAW) { + try { + QuaternionStamped q; + q.header.stamp = stamp; + q.header.frame_id = yaw_frame_id; + q.quaternion = tf::createQuaternionMsgFromYaw(setpoint_yaw); + yaw_local = tf2::getYaw(tf_buffer.transform(q, local_frame, ros::Duration(0.05)).quaternion); + } catch (tf2::TransformException& ex) { + // can't transform yaw, use last known + ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what()); + } + } + + // compute navigate setpoint if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) { - position_msg.pose.orientation = setpoint_position_transformed.pose.orientation; // copy yaw getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position); if (setpoint_yaw_type == TOWARDS) { - double yaw_towards = atan2(position_msg.pose.position.y - nav_start.pose.position.y, - position_msg.pose.position.x - nav_start.pose.position.x); - position_msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, yaw_towards); + yaw_local = atan2(position_msg.pose.position.y - nav_start.pose.position.y, + position_msg.pose.position.x - nav_start.pose.position.x); } + + position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local); } if (setpoint_type == POSITION) { - position_msg = setpoint_position_transformed; + position_msg = setpoint_pose_local; + position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local); } if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) { @@ -439,14 +527,14 @@ void publish(const ros::Time stamp) PositionTarget::IGNORE_AFY + PositionTarget::IGNORE_AFZ + PositionTarget::IGNORE_YAW; - position_raw_msg.yaw_rate = setpoint_yaw_rate; + position_raw_msg.yaw_rate = setpoint_rates.z; position_raw_msg.position = position_msg.pose.position; position_raw_pub.publish(position_raw_msg); } // publish setpoint frame if (!setpoint.child_frame_id.empty()) { - if (setpoint.header.stamp == position_msg.header.stamp) { + if (setpoint.header.stamp >= position_msg.header.stamp) { return; // avoid TF_REPEATED_DATA warnings } @@ -458,9 +546,22 @@ void publish(const ros::Time stamp) setpoint.header.stamp = position_msg.header.stamp; transform_broadcaster->sendTransform(setpoint); } + + // publish dynamic target frame + publishTarget(stamp); } if (setpoint_type == VELOCITY) { + // transform velocity to local frame + setpoint_velocity.header.stamp = stamp; + try { + setpoint_velocity_local = tf_buffer.transform(setpoint_velocity, local_frame, ros::Duration(0.05)); + } catch (tf2::TransformException& ex) { + // can't transform velocity, use last known + ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what()); + } + + // publish velocity position_raw_msg.type_mask = PositionTarget::IGNORE_PX + PositionTarget::IGNORE_PY + PositionTarget::IGNORE_PZ + @@ -468,14 +569,22 @@ void publish(const ros::Time stamp) PositionTarget::IGNORE_AFY + PositionTarget::IGNORE_AFZ; position_raw_msg.type_mask += setpoint_yaw_type == YAW ? PositionTarget::IGNORE_YAW_RATE : PositionTarget::IGNORE_YAW; - position_raw_msg.velocity = setpoint_velocity_transformed.vector; - position_raw_msg.yaw = tf2::getYaw(setpoint_position_transformed.pose.orientation); - position_raw_msg.yaw_rate = setpoint_yaw_rate; + position_raw_msg.velocity = setpoint_velocity_local.vector; + position_raw_msg.yaw = yaw_local; + position_raw_msg.yaw_rate = setpoint_rates.z; position_raw_pub.publish(position_raw_msg); } if (setpoint_type == ATTITUDE) { - attitude_pub.publish(setpoint_position_transformed); + PoseStamped msg; + msg.header.stamp = stamp; + msg.header.frame_id = local_frame; + msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(setpoint_roll, setpoint_pitch, yaw_local); + attitude_pub.publish(msg); + + Thrust thrust_msg; + thrust_msg.header.stamp = stamp; + thrust_msg.thrust = setpoint_thrust; thrust_pub.publish(thrust_msg); } @@ -484,11 +593,12 @@ void publish(const ros::Time stamp) // thrust_pub.publish(thrust_msg); // mavros rates topics waits for rates in local frame // use rates in body frame for simplicity + AttitudeTarget att_raw_msg; att_raw_msg.header.stamp = stamp; att_raw_msg.header.frame_id = fcu_frame; att_raw_msg.type_mask = AttitudeTarget::IGNORE_ATTITUDE; - att_raw_msg.body_rate = rates_msg.twist.angular; - att_raw_msg.thrust = thrust_msg.thrust; + att_raw_msg.body_rate = setpoint_rates; + att_raw_msg.thrust = setpoint_thrust; attitude_raw_pub.publish(att_raw_msg); } } @@ -528,10 +638,59 @@ inline void checkState() throw std::runtime_error("No connection to FCU, https://clover.coex.tech/connection"); } +void publishState() +{ + clover::State msg; + msg.mode = setpoint_type; + msg.yaw_mode = setpoint_yaw_type; + + if (setpoint_position.header.frame_id.empty()) { + msg.x = NAN; + msg.y = NAN; + msg.z = NAN; + } else { + msg.x = setpoint_position.point.x; + msg.y = setpoint_position.point.y; + msg.z = setpoint_altitude.point.z; + } + + msg.speed = nav_speed; + msg.lat = setpoint_lat; + msg.lon = setpoint_lon; + msg.vx = setpoint_velocity.vector.x; + msg.vy = setpoint_velocity.vector.y; + msg.vz = setpoint_velocity.vector.z; + msg.roll = setpoint_roll; + msg.pitch = setpoint_pitch; + msg.yaw = !yaw_frame_id.empty() ? setpoint_yaw : NAN; + + msg.roll_rate = setpoint_rates.x; + msg.pitch_rate = setpoint_rates.y; + msg.yaw_rate = setpoint_rates.z; + msg.thrust = setpoint_thrust; + + if (setpoint_type == VELOCITY) { + msg.xy_frame_id = setpoint_velocity.header.frame_id; + msg.z_frame_id = setpoint_velocity.header.frame_id; + } else { + msg.xy_frame_id = setpoint_position.header.frame_id; + msg.z_frame_id = setpoint_altitude.header.frame_id; + } + msg.yaw_frame_id = yaw_frame_id; + + state_pub.publish(msg); +} + +inline float safe(float value) { + return std::isfinite(value) ? value : 0; +} + #define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); } +#define ENSURE_NON_INF(var) { if (std::isinf(var)) throw std::runtime_error(#var " argument cannot be Inf"); } + bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz, - float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate, // editorconfig-checker-disable-line + float roll, float pitch, float yaw, float roll_rate, float pitch_rate, float yaw_rate, // editorconfig-checker-disable-line float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm, // editorconfig-checker-disable-line uint8_t& success, string& message) // editorconfig-checker-disable-line { @@ -558,69 +717,40 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl auto search = reference_frames.find(frame_id); const string& reference_frame = search == reference_frames.end() ? frame_id : search->second; - // Serve "partial" commands + ENSURE_NON_INF(x); + ENSURE_NON_INF(y); + ENSURE_NON_INF(z); + ENSURE_NON_INF(speed); // TODO: allow inf + ENSURE_NON_INF(vx); + ENSURE_NON_INF(vy); + ENSURE_NON_INF(vz); + ENSURE_NON_INF(roll); + ENSURE_NON_INF(pitch); + ENSURE_NON_INF(roll_rate); + ENSURE_NON_INF(pitch_rate); + ENSURE_NON_INF(yaw_rate); + ENSURE_NON_INF(thrust); - if (!auto_arm && std::isfinite(yaw) && - isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) && - isnan(pitch) && isnan(roll) && isnan(thrust) && - isnan(lat) && isnan(lon)) { - // change only the yaw - if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) { - if (!waitTransform(setpoint_position.header.frame_id, frame_id, stamp, transform_timeout)) - throw std::runtime_error("Can't transform from " + frame_id + " to " + setpoint_position.header.frame_id); - - message = "Changing yaw only"; - - QuaternionStamped q; - q.header.frame_id = frame_id; - q.header.stamp = stamp; - q.quaternion = tf::createQuaternionMsgFromYaw(yaw); // TODO: pitch=0, roll=0 is not totally correct - setpoint_position.pose.orientation = tf_buffer.transform(q, setpoint_position.header.frame_id).quaternion; - setpoint_yaw_type = YAW; - goto publish_setpoint; - } else { - throw std::runtime_error("Setting yaw is possible only when position or velocity setpoints active"); - } - } - - if (!auto_arm && std::isfinite(yaw_rate) && - isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) && - isnan(pitch) && isnan(roll) && isnan(yaw) && isnan(thrust) && - isnan(lat) && isnan(lon)) { - // change only the yaw rate - if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) { - message = "Changing yaw rate only"; - - setpoint_yaw_type = YAW_RATE; - setpoint_yaw_rate = yaw_rate; - goto publish_setpoint; - } else { - throw std::runtime_error("Setting yaw rate is possible only when position or velocity setpoints active"); - } - } - - // Serve normal commands - - if (sp_type == NAVIGATE || sp_type == POSITION) { - ENSURE_FINITE(x); - ENSURE_FINITE(y); - ENSURE_FINITE(z); - } else if (sp_type == NAVIGATE_GLOBAL) { + if (sp_type == NAVIGATE_GLOBAL) { ENSURE_FINITE(lat); ENSURE_FINITE(lon); - ENSURE_FINITE(z); - } else if (sp_type == VELOCITY) { - ENSURE_FINITE(vx); - ENSURE_FINITE(vy); - ENSURE_FINITE(vz); - } else if (sp_type == ATTITUDE) { - ENSURE_FINITE(pitch); - ENSURE_FINITE(roll); - ENSURE_FINITE(thrust); - } else if (sp_type == RATES) { - ENSURE_FINITE(pitch_rate); - ENSURE_FINITE(roll_rate); - ENSURE_FINITE(thrust); + } + + if (isfinite(x) != isfinite(y)) { + throw std::runtime_error("x and y can be set only together"); + } + + if (isfinite(yaw_rate)) { + if (sp_type > RATES && setpoint_type == ATTITUDE) { + throw std::runtime_error("Yaw rate cannot be set in attitude mode."); + } + } + + // set_altitude + if (sp_type == _ALTITUDE) { + if (setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) { + throw std::runtime_error("Altitude cannot be set in velocity, attitude or rates mode."); + } } if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) { @@ -634,20 +764,13 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl speed = default_speed; } - if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) { - if (yaw_rate != 0 && !std::isnan(yaw)) - throw std::runtime_error("Yaw value should be NaN for setting yaw rate"); - - if (std::isnan(yaw_rate) && std::isnan(yaw)) - throw std::runtime_error("Both yaw and yaw_rate cannot be NaN"); - } - if (sp_type == NAVIGATE_GLOBAL) { if (TIMEOUT(global_position, global_position_timeout)) throw std::runtime_error("No global position"); } - if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE) { + // if any value need to be transformed to reference frame + if (isfinite(x) || isfinite(y) || isfinite(z) || isfinite(vx) || isfinite(vy) || isfinite(vz) || isfinite(yaw)) { // make sure transform from frame_id to reference frame available if (!waitTransform(reference_frame, frame_id, stamp, transform_timeout)) throw std::runtime_error("Can't transform from " + frame_id + " to " + reference_frame); @@ -664,15 +787,26 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl auto xy_in_req_frame = tf_buffer.transform(pose_local, frame_id); x = xy_in_req_frame.pose.position.x; y = xy_in_req_frame.pose.position.y; + setpoint_lat = lat; + setpoint_lon = lon; } // Everything fine - switch setpoint type - setpoint_type = sp_type; + if (sp_type <= RATES) { + setpoint_type = sp_type; + } - if (sp_type != NAVIGATE && sp_type != NAVIGATE_GLOBAL) { + if (setpoint_type != NAVIGATE && setpoint_type != NAVIGATE_GLOBAL) { nav_from_sp_flag = false; } + if (auto_arm || setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) { + // invalidate position setpoint + setpoint_position.header.frame_id = ""; + setpoint_altitude.header.frame_id = ""; + yaw_frame_id = ""; + } + if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) { // starting point if (nav_from_sp && nav_from_sp_flag) { @@ -681,89 +815,139 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl } else { nav_start = local_position; } - nav_speed = speed; + + if (!isnan(speed)) { + nav_speed = speed; + } + nav_from_sp_flag = true; } - // if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) { - // if (std::isnan(yaw) && yaw_rate == 0) { - // // keep yaw unchanged - // // TODO: this is incorrect, because we need yaw in desired frame - // yaw = tf2::getYaw(local_position.pose.orientation); - // } - // } + // handle position + if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) { - if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) { - // destination point and/or attitude - PoseStamped ps; - ps.header.frame_id = frame_id; - ps.header.stamp = stamp; - ps.pose.position.x = x; - ps.pose.position.y = y; - ps.pose.position.z = z; - ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid + PointStamped desired; + desired.header.frame_id = frame_id; + desired.header.stamp = stamp; + desired.point.x = safe(x); + desired.point.y = safe(y); + desired.point.z = safe(z); - if (sp_type == ATTITUDE) { - ps.pose.position.x = 0; - ps.pose.position.y = 0; - ps.pose.position.z = 0; - ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); - } else if (std::isnan(yaw)) { - setpoint_yaw_type = YAW_RATE; - setpoint_yaw_rate = yaw_rate; - } else if (std::isinf(yaw) && yaw > 0) { - // yaw towards - setpoint_yaw_type = TOWARDS; - yaw = 0; - setpoint_yaw_rate = 0; - } else { - setpoint_yaw_type = YAW; - setpoint_yaw_rate = 0; - ps.pose.orientation = tf::createQuaternionMsgFromYaw(yaw); + // transform to reference frame + desired = tf_buffer.transform(desired, reference_frame); + + // set horizontal position + if (isfinite(x) && isfinite(y)) { + setpoint_position = desired; + } else if (setpoint_position.header.frame_id.empty()) { + // TODO: use transform for current stamp + setpoint_position.header = local_position.header; + setpoint_position.point = local_position.pose.position; } - tf_buffer.transform(ps, setpoint_position, reference_frame); + // set altitude + if (isfinite(z)) { + setpoint_altitude = desired; + } else if (setpoint_altitude.header.frame_id.empty()) { + setpoint_altitude.header = local_position.header; + setpoint_altitude.point = local_position.pose.position; + } } + // handle velocity if (sp_type == VELOCITY) { - Vector3Stamped vel; - vel.header.frame_id = frame_id; - vel.header.stamp = stamp; - vel.vector.x = vx; - vel.vector.y = vy; - vel.vector.z = vz; - tf_buffer.transform(vel, setpoint_velocity, reference_frame); + // TODO: allow setting different modes by altitude and xy + Vector3Stamped desired; + desired.header.frame_id = frame_id; + desired.header.stamp = stamp; + desired.vector.x = safe(vx); + desired.vector.y = safe(vy); + desired.vector.z = safe(vz); + + // transform to reference frame + desired = tf_buffer.transform(desired, reference_frame); + setpoint_velocity.header = desired.header; + + // set horizontal velocity + if (isfinite(vx) && isfinite(vy)) { + setpoint_velocity.vector.x = desired.vector.x; + setpoint_velocity.vector.y = desired.vector.y; + } + + // set vertical velocity + if (isfinite(vz)) { + setpoint_velocity.vector.z = desired.vector.z; + } } - if (sp_type == ATTITUDE || sp_type == RATES) { - thrust_msg.thrust = thrust; + // handle yaw + if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE || sp_type == _YAW) { + if (isfinite(yaw)) { + setpoint_yaw_type = YAW; + QuaternionStamped desired; + desired.header.frame_id = frame_id; + desired.header.stamp = stamp; + desired.quaternion = tf::createQuaternionMsgFromYaw(yaw); + + // transform to reference frame + desired = tf_buffer.transform(desired, reference_frame); + setpoint_yaw = tf2::getYaw(desired.quaternion); + yaw_frame_id = reference_frame; + + } else if (isinf(yaw) && yaw > 0) { + // yaw towards + setpoint_yaw_type = TOWARDS; + + } else if (yaw_frame_id.empty() || sp_type == _YAW) { + // yaw is nan and not set previously OR set_yaw(yaw=nan) was called + setpoint_yaw_type = YAW; + setpoint_yaw = tf2::getYaw(local_position.pose.orientation); // set yaw to current yaw + yaw_frame_id = local_position.header.frame_id; + } } - if (sp_type == RATES) { - rates_msg.twist.angular.x = roll_rate; - rates_msg.twist.angular.y = pitch_rate; - rates_msg.twist.angular.z = yaw_rate; + // handle roll + if (isfinite(roll)) { + setpoint_roll = roll; + } + + // handle pitch + if (isfinite(pitch)) { + setpoint_pitch = pitch; + } + + // handle yaw rate + if (isfinite(yaw_rate)) { + setpoint_yaw_type = YAW_RATE; + setpoint_rates.z = yaw_rate; + } + + // handle pitch rate + if (isfinite(roll_rate)) { + setpoint_rates.x = roll_rate; + } + + // handle roll rate + if (isfinite(pitch_rate)) { + setpoint_rates.y = pitch_rate; + } + + // handle thrust + if (isfinite(thrust)) { + setpoint_thrust = thrust; } wait_armed = auto_arm; -publish_setpoint: publish(stamp); // calculate initial transformed messages first setpoint_timer.start(); - // publish target frame - if (!target.child_frame_id.empty()) { - if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) { - target.header.frame_id = setpoint_position.header.frame_id; - target.header.stamp = stamp; - target.transform.translation.x = setpoint_position.pose.position.x; - target.transform.translation.y = setpoint_position.pose.position.y; - target.transform.translation.z = setpoint_position.pose.position.z; - target.transform.rotation = setpoint_position.pose.orientation; - static_transform_broadcaster->sendTransform(target); - } + if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) { + publishTarget(stamp, true); } + publishState(); + if (auto_arm) { offboardAndArm(); wait_armed = false; @@ -788,27 +972,39 @@ publish_setpoint: } bool navigate(Navigate::Request& req, Navigate::Response& res) { - return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message); + return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message); } bool navigateGlobal(NavigateGlobal::Request& req, NavigateGlobal::Response& res) { - return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message); + return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message); +} + +bool setAltitude(SetAltitude::Request& req, SetAltitude::Response& res) { + return serve(_ALTITUDE, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message); +} + +bool setYaw(SetYaw::Request& req, SetYaw::Response& res) { + return serve(_YAW, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message); +} + +bool setYawRate(SetYawRate::Request& req, SetYawRate::Response& res) { + return serve(_YAW_RATE, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, "", false, res.success, res.message); } bool setPosition(SetPosition::Request& req, SetPosition::Response& res) { - return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message); + return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message); } bool setVelocity(SetVelocity::Request& req, SetVelocity::Response& res) { - return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message); + return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message); } bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) { - return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch, req.roll, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message); + return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.roll, req.pitch, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message); } bool setRates(SetRates::Request& req, SetRates::Response& res) { - return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch_rate, req.roll_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message); + return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.roll_rate, req.pitch_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message); } bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) @@ -840,9 +1036,7 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) auto start = ros::Time::now(); while (ros::ok()) { if (state.mode == "AUTO.LAND") { - res.success = true; - busy = false; - return true; + break; } if (ros::Time::now() - start > land_timeout) throw std::runtime_error("Land request timed out"); @@ -851,6 +1045,18 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) r.sleep(); } + // stop setpoints and invalidate position setpoint + setpoint_timer.stop(); + setpoint_type = NONE; + setpoint_position.header.frame_id = ""; + setpoint_altitude.header.frame_id = ""; + yaw_frame_id = ""; + publishState(); + + res.success = true; + busy = false; + return true; + } catch (const std::exception& e) { res.message = e.what(); ROS_INFO("%s", e.what()); @@ -863,6 +1069,11 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) bool release(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) { setpoint_timer.stop(); + setpoint_type = NONE; + setpoint_position.header.frame_id = ""; + setpoint_altitude.header.frame_id = ""; + yaw_frame_id = ""; + publishState(); res.success = true; return true; } @@ -888,6 +1099,7 @@ int main(int argc, char **argv) nh_priv.param("check_kill_switch", check_kill_switch, true); nh_priv.param("default_speed", default_speed, 0.5f); nh_priv.param("body_frame", body.child_frame_id, "body"); + nh_priv.param("terrain_frame", terrain.child_frame_id, "terrain"); nh_priv.getParam("reference_frames", reference_frames); // Default reference frames @@ -923,6 +1135,12 @@ int main(int argc, char **argv) auto manual_control_sub = nh.subscribe(mavros + "/manual_control/control", 1, &handleMessage); auto local_position_sub = nh.subscribe(mavros + "/local_position/pose", 1, &handleLocalPosition); + ros::Subscriber altitude_sub; + if (!body.child_frame_id.empty() && !terrain.child_frame_id.empty()) { + terrain.header.frame_id = local_frame; + altitude_sub = nh.subscribe(mavros + "/altitude", 1, &handleAltitude); + } + // Setpoint publishers position_pub = nh.advertise(mavros + "/setpoint_position/local", 1); position_raw_pub = nh.advertise(mavros + "/setpoint_raw/local", 1); @@ -931,10 +1149,16 @@ int main(int argc, char **argv) rates_pub = nh.advertise(mavros + "/setpoint_attitude/cmd_vel", 1); thrust_pub = nh.advertise(mavros + "/setpoint_attitude/thrust", 1); + // State publisher + state_pub = nh_priv.advertise("state", 1, true); + // Service servers auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry); auto na_serv = nh.advertiseService("navigate", &navigate); auto ng_serv = nh.advertiseService("navigate_global", &navigateGlobal); + auto sl_serv = nh.advertiseService("set_altitude", &setAltitude); + auto ya_serv = nh.advertiseService("set_yaw", &setYaw); + auto yr_serv = nh.advertiseService("set_yaw_rate", &setYawRate); auto sp_serv = nh.advertiseService("set_position", &setPosition); auto sv_serv = nh.advertiseService("set_velocity", &setVelocity); auto sa_serv = nh.advertiseService("set_attitude", &setAttitude); @@ -948,7 +1172,7 @@ int main(int argc, char **argv) position_msg.header.frame_id = local_frame; position_raw_msg.header.frame_id = local_frame; position_raw_msg.coordinate_frame = PositionTarget::FRAME_LOCAL_NED; - rates_msg.header.frame_id = fcu_frame; + //rates_msg.header.frame_id = fcu_frame; ROS_INFO("ready"); ros::spin(); diff --git a/clover/srv/GetTelemetry.srv b/clover/srv/GetTelemetry.srv index b9ae484e..a5b6e9e7 100644 --- a/clover/srv/GetTelemetry.srv +++ b/clover/srv/GetTelemetry.srv @@ -13,11 +13,11 @@ float32 alt float32 vx float32 vy float32 vz -float32 pitch float32 roll +float32 pitch float32 yaw -float32 pitch_rate float32 roll_rate +float32 pitch_rate float32 yaw_rate float32 voltage float32 cell_voltage diff --git a/clover/srv/Navigate.srv b/clover/srv/Navigate.srv index f607a437..7a4943ed 100644 --- a/clover/srv/Navigate.srv +++ b/clover/srv/Navigate.srv @@ -2,7 +2,6 @@ float32 x float32 y float32 z float32 yaw -float32 yaw_rate float32 speed string frame_id bool auto_arm diff --git a/clover/srv/NavigateGlobal.srv b/clover/srv/NavigateGlobal.srv index 69fbc444..5822f2b0 100644 --- a/clover/srv/NavigateGlobal.srv +++ b/clover/srv/NavigateGlobal.srv @@ -2,7 +2,6 @@ float64 lat float64 lon float32 z float32 yaw -float32 yaw_rate float32 speed string frame_id bool auto_arm diff --git a/clover/srv/SetAltitude.srv b/clover/srv/SetAltitude.srv new file mode 100644 index 00000000..1b817ec5 --- /dev/null +++ b/clover/srv/SetAltitude.srv @@ -0,0 +1,5 @@ +float32 z +string frame_id +--- +bool success +string message diff --git a/clover/srv/SetAttitude.srv b/clover/srv/SetAttitude.srv index b41c1072..ef8cf8b2 100644 --- a/clover/srv/SetAttitude.srv +++ b/clover/srv/SetAttitude.srv @@ -1,5 +1,5 @@ -float32 pitch float32 roll +float32 pitch float32 yaw float32 thrust string frame_id diff --git a/clover/srv/SetPosition.srv b/clover/srv/SetPosition.srv index 20c73f05..80a01b29 100644 --- a/clover/srv/SetPosition.srv +++ b/clover/srv/SetPosition.srv @@ -2,7 +2,6 @@ float32 x float32 y float32 z float32 yaw -float32 yaw_rate string frame_id bool auto_arm --- diff --git a/clover/srv/SetRates.srv b/clover/srv/SetRates.srv index f6ebddf9..b284a5c8 100644 --- a/clover/srv/SetRates.srv +++ b/clover/srv/SetRates.srv @@ -1,5 +1,5 @@ -float32 pitch_rate float32 roll_rate +float32 pitch_rate float32 yaw_rate float32 thrust bool auto_arm diff --git a/clover/srv/SetVelocity.srv b/clover/srv/SetVelocity.srv index 25e174ac..908be1b8 100644 --- a/clover/srv/SetVelocity.srv +++ b/clover/srv/SetVelocity.srv @@ -2,7 +2,6 @@ float32 vx float32 vy float32 vz float32 yaw -float32 yaw_rate string frame_id bool auto_arm --- diff --git a/clover/srv/SetYaw.srv b/clover/srv/SetYaw.srv new file mode 100644 index 00000000..3e61d323 --- /dev/null +++ b/clover/srv/SetYaw.srv @@ -0,0 +1,5 @@ +float32 yaw +string frame_id +--- +bool success +string message diff --git a/clover/srv/SetYawRate.srv b/clover/srv/SetYawRate.srv new file mode 100644 index 00000000..82a21640 --- /dev/null +++ b/clover/srv/SetYawRate.srv @@ -0,0 +1,4 @@ +float32 yaw_rate +--- +bool success +string message diff --git a/clover/test/offboard.py b/clover/test/offboard.py new file mode 100755 index 00000000..426fb453 --- /dev/null +++ b/clover/test/offboard.py @@ -0,0 +1,402 @@ +import rospy +import pytest +from pytest import approx +import threading +import mavros_msgs.msg +from geometry_msgs.msg import PoseStamped +from clover import srv +from clover.msg import State +from math import nan, inf +import tf2_ros +import tf2_geometry_msgs + +@pytest.fixture() +def node(): + return rospy.init_node('offboard_test', anonymous=True) + +@pytest.fixture +def tf_buffer(): + buf = tf2_ros.Buffer() + tf2_ros.TransformListener(buf) + return buf + +def get_state(): + return rospy.wait_for_message('/simple_offboard/state', State, timeout=1) + +def get_navigate_target(tf_buffer): + target = tf_buffer.lookup_transform('map', 'navigate_target', rospy.get_rostime(), rospy.Duration(1)) + assert target.child_frame_id == 'navigate_target' + return target + +def test_offboard(node, tf_buffer): + navigate = rospy.ServiceProxy('navigate', srv.Navigate) + set_position = rospy.ServiceProxy('set_position', srv.SetPosition) + set_altitude = rospy.ServiceProxy('set_altitude', srv.SetAltitude) + set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw) + set_yaw_rate = rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate) + set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity) + set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude) + set_rates = rospy.ServiceProxy('set_rates', srv.SetRates) + get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) + res = navigate() + assert res.success == False + assert res.message.startswith('State timeout') + + telem = get_telemetry() + assert telem.connected == False + + state_pub = rospy.Publisher('/mavros/state', mavros_msgs.msg.State, latch=True, queue_size=1) + state_msg = mavros_msgs.msg.State(mode='OFFBOARD', armed=True) + + def publish_state(): + r = rospy.Rate(2) + while not rospy.is_shutdown(): + state_msg.header.stamp = rospy.Time.now() + state_pub.publish(state_msg) + r.sleep() + + # start publishing state + threading.Thread(target=publish_state, daemon=True).start() + rospy.sleep(0.5) + + telem = get_telemetry() + assert telem.connected == False + + res = navigate() + assert res.success == False + assert res.message.startswith('No connection to FCU') + + state_msg.connected = True + rospy.sleep(1) + + telem = get_telemetry() + assert telem.connected == True + + res = navigate() + assert res.success == False + assert res.message.startswith('No local position') + + local_position_pub = rospy.Publisher('/mavros/local_position/pose', PoseStamped, latch=True, queue_size=1) + local_position_msg = PoseStamped() + local_position_msg.header.frame_id = 'map' + local_position_msg.pose.position.x = 1 + local_position_msg.pose.position.y = 2 + local_position_msg.pose.position.z = 3 + local_position_msg.pose.orientation.w = 1 + + def publish_local_position(): + r = rospy.Rate(30) + while not rospy.is_shutdown(): + local_position_msg.header.stamp = rospy.Time.now() + local_position_pub.publish(local_position_msg) + r.sleep() + + # start publishing local position + threading.Thread(target=publish_local_position, daemon=True).start() + rospy.sleep(0.5) + + # check body frame + body = tf_buffer.lookup_transform('map', 'body', rospy.get_rostime(), rospy.Duration(1)) + assert body.child_frame_id == 'body' + assert body.transform.translation.x == approx(1) + assert body.transform.translation.y == approx(2) + assert body.transform.translation.z == approx(3) + + res = navigate(x=3, y=2, z=1, frame_id='map') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 3 + assert state.y == 2 + assert state.z == 1 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'map' + assert state.yaw_frame_id == 'map' + target = get_navigate_target(tf_buffer) + assert target.header.frame_id == 'map' + assert target.transform.translation.x == approx(3) + assert target.transform.translation.y == approx(2) + assert target.transform.translation.z == approx(1) + assert target.transform.rotation.x == 0 + assert target.transform.rotation.y == 0 + assert target.transform.rotation.z == 0 + assert target.transform.rotation.w == 1 + + # try to set only the y + res = navigate(x=nan, y=1, z=nan) + assert res.success == False + assert res.message.startswith('x and y can be set only together') + + # set z in body frame + res = navigate(x=nan, y=nan, z=1, frame_id='body') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 3 + assert state.y == 2 + assert state.z == 4 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'map' + assert state.yaw_frame_id == 'map' + + # set xy in test frame + res = navigate(x=1, y=2, z=nan, frame_id='test') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 1 + assert state.y == 2 + assert state.z == 4 + assert state.yaw == 0 + assert state.xy_frame_id == 'test' + assert state.z_frame_id == 'map' + assert state.yaw_frame_id == 'test' + + # auto_arm should invalidate the setpoint + res = navigate(x=nan, y=nan, z=1, frame_id='map', auto_arm=True) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 1 + assert state.y == 2 + assert state.z == 1 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'map' + assert state.yaw_frame_id == 'map' + + # set_attitude should invalidate the setpoint + res = set_attitude() + assert res.success == True + + res = navigate(x=5, y=6, z=nan, yaw=nan, frame_id='map') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 5 + assert state.y == 6 + assert state.z == 3 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'map' + assert state.yaw_frame_id == 'map' + + # test set_altitude + res = set_altitude(z=7, frame_id='test') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 5 + assert state.y == 6 + assert state.z == 7 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + assert state.yaw_frame_id == 'map' + + # test set_yaw + res = set_yaw(yaw=0.5, frame_id='test2') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 5 + assert state.y == 6 + assert state.z == 7 + assert state.yaw == 0.5 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + assert state.yaw_frame_id == 'test2' + + # test set_yaw_rate + res = set_yaw_rate(yaw_rate=2) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW_RATE + assert state.x == 5 + assert state.y == 6 + assert state.z == 7 + assert state.yaw_rate == 2 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + + # navigate(yaw=nan) should keep yaw rate mode + res = navigate(x=nan, y=nan, z=nan, yaw=nan) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW_RATE + assert state.x == 5 + assert state.y == 6 + assert state.z == 7 + assert state.yaw_rate == 2 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + + # set_yaw(nan) should change back to yaw mode + res = set_yaw(yaw=nan) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.yaw == 0 + assert state.yaw_frame_id == 'map' + + # test set_position + res = set_position(x=nan, y=nan, z=13, yaw=nan, frame_id='test2') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_POSITION + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 5 + assert state.y == 6 + assert state.z == 13 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test2' + assert state.yaw_frame_id == 'map' + + # set_altitude should not change the mode + res = set_altitude(z=3, frame_id='test') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_POSITION + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 5 + assert state.y == 6 + assert state.z == 3 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + assert state.yaw_frame_id == 'map' + + # set_yaw should not change the main mode + res = set_yaw(yaw=1, frame_id='test2') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_POSITION + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 5 + assert state.y == 6 + assert state.z == 3 + assert state.yaw == 1 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + assert state.yaw_frame_id == 'test2' + + # test set_velocity + res = set_velocity(vx=1, frame_id='body') + state = get_state() + assert state.mode == State.MODE_VELOCITY + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.vx == 1 + assert state.vy == 0 + assert state.vz == 0 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'map' + assert state.yaw_frame_id == 'map' + + # set_altitude should not work in velocity mode + res = set_altitude(z=3, frame_id='test') + assert res.success == False + assert res.message.startswith('Altitude cannot be set in') + + # test set_attitude + res = set_attitude(roll=0.1, pitch=0.2, yaw=0.3, thrust=0.5) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_ATTITUDE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.roll == approx(0.1) + assert state.pitch == approx(0.2) + assert state.yaw == approx(0.3) + assert state.thrust == approx(0.5) + assert state.yaw_frame_id == 'map' + msg = rospy.wait_for_message('/mavros/setpoint_attitude/attitude', PoseStamped, timeout=3) + # Tait-Bryan ZYX angle (rzyx) converted to quaternion + assert msg.pose.orientation.x == approx(0.0342708) + assert msg.pose.orientation.y == approx(0.10602051) + assert msg.pose.orientation.z == approx(0.14357218) + assert msg.pose.orientation.w == approx(0.98334744) + msg = rospy.wait_for_message('/mavros/setpoint_attitude/thrust', mavros_msgs.msg.Thrust, timeout=3) + assert msg.thrust == approx(0.5) + + # set_yaw should work in attitude mode + res = set_yaw(yaw=0.7, frame_id='test2') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_ATTITUDE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.roll == approx(0.1) + assert state.pitch == approx(0.2) + assert state.yaw == approx(0.7) + assert state.thrust == approx(0.5) + assert state.yaw_frame_id == 'test2' + + # set_yaw_rate should not work in attitude mode + res = set_yaw_rate(yaw_rate=0.3) + assert res.success == False + assert res.message.startswith('Yaw rate cannot be set in') + + # test set_rates + res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=0.3, thrust=0.6) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_RATES + assert state.yaw_mode == State.YAW_MODE_YAW_RATE + assert state.roll_rate == approx(0) + assert state.pitch_rate == approx(0) + assert state.yaw_rate == approx(0.3) + assert state.thrust == approx(0.6) + msg = rospy.wait_for_message('/mavros/setpoint_raw/attitude', mavros_msgs.msg.AttitudeTarget, timeout=3) + assert msg.thrust == approx(0.6) + + res = set_rates(roll_rate=0.3, pitch_rate=0.2, yaw_rate=0.1, thrust=0.4) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_RATES + assert state.yaw_mode == State.YAW_MODE_YAW_RATE + assert state.roll_rate == approx(0.3) + assert state.pitch_rate == approx(0.2) + assert state.yaw_rate == approx(0.1) + assert state.thrust == approx(0.4) + + res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=nan, thrust=0.3) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_RATES + assert state.yaw_mode == State.YAW_MODE_YAW_RATE + assert state.roll_rate == approx(0.3) + assert state.pitch_rate == approx(0.2) + assert state.yaw_rate == approx(0.1) + assert state.thrust == approx(0.3) + msg = rospy.wait_for_message('/mavros/setpoint_raw/attitude', mavros_msgs.msg.AttitudeTarget, timeout=3) + assert msg.type_mask == mavros_msgs.msg.AttitudeTarget.IGNORE_ATTITUDE + assert msg.body_rate.x == approx(0.3) + assert msg.body_rate.y == approx(0.2) + assert msg.body_rate.z == approx(0.1) + + # set_yaw_rate should work in rates mode + res = set_yaw_rate(yaw_rate=0.4) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_RATES + assert state.yaw_mode == State.YAW_MODE_YAW_RATE + assert state.roll_rate == approx(0.3) + assert state.pitch_rate == approx(0.2) + assert state.yaw_rate == approx(0.4) + assert state.thrust == approx(0.3) + + res = set_rates(roll_rate=inf) + assert res.success == False + assert res.message == 'roll_rate argument cannot be Inf' diff --git a/clover/test/offboard.test b/clover/test/offboard.test new file mode 100644 index 00000000..470fe4fe --- /dev/null +++ b/clover/test/offboard.test @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/clover_blocks/www/blocks.js b/clover_blocks/www/blocks.js index b10e315f..f5062b3d 100644 --- a/clover_blocks/www/blocks.js +++ b/clover_blocks/www/blocks.js @@ -15,6 +15,7 @@ const COLOR_GPIO = 200; const DOCS_URL = 'https://clover.coex.tech/en/blocks.html'; var frameIds = [["body", "BODY"], ["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"], ["map", "MAP"]]; +var frameIdsWithTerrain = frameIds.concat([["terrain", "TERRAIN"]]); function considerFrameId(e) { if (!(e instanceof Blockly.Events.Change || e instanceof Blockly.Events.Create)) return; @@ -22,7 +23,7 @@ function considerFrameId(e) { var frameId = this.getFieldValue('FRAME_ID'); // set appropriate coordinates labels if (this.getInput('X')) { // block has x-y-z fields - if (frameId == 'BODY' || frameId == 'NAVIGATE_TARGET' || frameId == 'BASE_LINK') { + if (frameId == 'BODY' || frameId == 'NAVIGATE_TARGET' || frameId == 'BASE_LINK' || frameId == 'TERRAIN') { this.getInput('X').fieldRow[0].setValue('forward'); this.getInput('Y').fieldRow[0].setValue('left'); this.getInput('Z').fieldRow[0].setValue('up'); @@ -59,8 +60,8 @@ function updateSetpointBlock(e) { this.getInput('VY').setVisible(velocity); this.getInput('VZ').setVisible(velocity); this.getInput('YAW').setVisible(attitude); - this.getInput('PITCH').setVisible(attitude); this.getInput('ROLL').setVisible(attitude); + this.getInput('PITCH').setVisible(attitude); this.getInput('THRUST').setVisible(attitude); this.getInput('RELATIVE_TO').setVisible(type != 'RATES'); @@ -73,7 +74,7 @@ function updateSetpointBlock(e) { Blockly.Blocks['navigate'] = { init: function () { - let navFrameId = frameIds.slice(); + let navFrameId = frameIdsWithTerrain.slice(); navFrameId.push(['global', 'GLOBAL_LOCAL']) navFrameId.push(['global, WGS 84 alt.', 'GLOBAL']) this.appendDummyInput() @@ -163,14 +164,14 @@ Blockly.Blocks['setpoint'] = { this.appendValueInput("VZ") .setCheck("Number") .appendField("vz"); - this.appendValueInput("PITCH") - .setCheck("Number") - .appendField("pitch") - .setVisible(false); this.appendValueInput("ROLL") .setCheck("Number") .appendField("roll") .setVisible(false); + this.appendValueInput("PITCH") + .setCheck("Number") + .appendField("pitch") + .setVisible(false); this.appendValueInput("YAW") .setCheck("Number") .appendField("yaw") @@ -213,7 +214,7 @@ Blockly.Blocks['get_position'] = { .appendField("current") .appendField(new Blockly.FieldDropdown([["x", "X"], ["y", "Y"], ["z", "Z"], ["vx", "VX"], ["vy", "VY"], ["vz", "VZ"]]), "FIELD") .appendField("relative to") - .appendField(new Blockly.FieldDropdown(frameIds), "FRAME_ID"); + .appendField(new Blockly.FieldDropdown(frameIdsWithTerrain), "FRAME_ID"); this.appendValueInput("ID") .setCheck("Number") .appendField("with ID") @@ -247,7 +248,7 @@ Blockly.Blocks['get_attitude'] = { init: function () { this.appendDummyInput() .appendField("current") - .appendField(new Blockly.FieldDropdown([["pitch", "PITCH"], ["roll", "ROLL"], ["pitch rate", "PITCH_RATE"], ["roll rate", "ROLL_RATE"], ["yaw rate", "YAW_RATE"]]), "FIELD"); + .appendField(new Blockly.FieldDropdown([["roll", "ROLL"], ["pitch", "PITCH"], ["roll rate", "ROLL_RATE"], ["pitch rate", "PITCH_RATE"], ["yaw rate", "YAW_RATE"]]), "FIELD"); this.setOutput(true, "Number"); this.setColour(COLOR_STATE); this.setTooltip("Returns current orientation or angle rates in degree or degree per second (not radian)."); @@ -509,7 +510,7 @@ Blockly.Blocks['distance'] = { .appendField("z"); this.appendDummyInput() .appendField("relative to") - .appendField(new Blockly.FieldDropdown([["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"]]), "FRAME_ID"); + .appendField(new Blockly.FieldDropdown([["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"], ["terrain", "TERRAIN"]]), "FRAME_ID"); this.appendValueInput("ID") .setCheck("Number") .appendField("with ID") diff --git a/clover_blocks/www/index.html b/clover_blocks/www/index.html index 8f394198..fcd4060f 100644 --- a/clover_blocks/www/index.html +++ b/clover_blocks/www/index.html @@ -69,8 +69,8 @@ 0 0 0 - 0 0 + 0 0 0.5 0 diff --git a/clover_blocks/www/python.js b/clover_blocks/www/python.js index 9cf67938..42de72a2 100644 --- a/clover_blocks/www/python.js +++ b/clover_blocks/www/python.js @@ -276,10 +276,11 @@ Blockly.Python.angle = function(block) { } Blockly.Python.set_yaw = function(block) { + rosDefinitions.setYaw = true; simpleOffboard(); let yaw = Blockly.Python.valueToCode(block, 'YAW', Blockly.Python.ORDER_NONE); let frameId = buildFrameId(block); - let code = `navigate(x=float('nan'), y=float('nan'), z=float('nan'), yaw=${yaw}, frame_id=${frameId})\n`; + let code = `set_yaw(yaw=${yaw}, frame_id=${frameId})\n`; if (block.getFieldValue('WAIT') == 'TRUE') { rosDefinitions.waitYaw = true; simpleOffboard(); @@ -328,11 +329,11 @@ Blockly.Python.setpoint = function(block) { } else if (type == 'ATTITUDE') { rosDefinitions.setAttitude = true; simpleOffboard(); - return `set_attitude(pitch=${pitch}, roll=${roll}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`; + return `set_attitude(roll=${roll}, pitch=${pitch}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`; } else if (type == 'RATES') { rosDefinitions.setRates = true; simpleOffboard(); - return `set_rates(pitch_rate=${pitch}, roll_rate=${roll}, yaw_rate=${yaw}, thrust=${thrust})\n`; + return `set_rates(roll_rate=${roll}, pitch_rate=${pitch}, yaw_rate=${yaw}, thrust=${thrust})\n`; } } diff --git a/docs/en/parameters.md b/docs/en/parameters.md index caef83fd..6bf4b2d3 100644 --- a/docs/en/parameters.md +++ b/docs/en/parameters.md @@ -60,8 +60,8 @@ The `SYS_MC_EST_GROUP` parameter defines the estimator subsystem to use. Estimator subsystem is a group of modules that calculates the current state of the copter using readings from the sensors. The copter state includes: -* Angle rate of the copter – pitch_rate, roll_rate, yaw_rate; -* Copter orientation (in the local coordinate system) – pitch, roll, yaw (one of presentations); +* Angle rate of the copter – roll_rate, pitch_rate, yaw_rate; +* Copter orientation (in the local coordinate system) – roll, pitch, yaw (one of presentations); * Copter position (in the local coordinate system) – x, y, z; * Copter speed (in the local coordinate system) – vx, vy, vz; * Global coordinates of the copter – latitude, longitude, altitude; diff --git a/docs/en/simple_offboard.md b/docs/en/simple_offboard.md index 14985dfa..de0a8186 100644 --- a/docs/en/simple_offboard.md +++ b/docs/en/simple_offboard.md @@ -51,11 +51,11 @@ Response format: * `lat, lon` – drone latitude and longitude *(degrees)*, requires [GPS](gps.md) module; * `alt` – altitude in the global coordinate system (according to [WGS-84](https://ru.wikipedia.org/wiki/WGS_84) standard, not AMSL!), requires [GPS](gps.md) module; * `vx, vy, vz` – drone velocity *(m/s)*; -* `pitch` – pitch angle *(radians)*; * `roll` – roll angle *(radians)*; +* `pitch` – pitch angle *(radians)*; * `yaw` — yaw angle *(radians)*; -* `pitch_rate` — angular pitch velocity *(rad/s)*; * `roll_rate` – angular roll velocity *(rad/s)*; +* `pitch_rate` — angular pitch velocity *(rad/s)*; * `yaw_rate` – angular yaw velocity *(rad/s)*; * `voltage` – total battery voltage *(V)*; * `cell_voltage` – battery cell voltage *(V)*. @@ -261,22 +261,22 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body') ### set_attitude -Set pitch, roll, yaw and throttle level (similar to [the `STABILIZED` mode](modes.md)). This service may be used for lower level control of the drone behavior, or controlling the drone when no reliable data on its position is available. +Set roll, pitch, yaw and throttle level (similar to [the `STABILIZED` mode](modes.md)). This service may be used for lower level control of the drone behavior, or controlling the drone when no reliable data on its position is available. Parameters: -* `pitch`, `roll`, `yaw` – requested pitch, roll, and yaw angle *(radians)*; +* `roll`, `pitch`, `yaw` – requested roll, pitch, and yaw angle *(radians)*; * `thrust` — throttle level, ranges from 0 (no throttle, propellers are stopped) to 1 (full throttle). * `auto_arm` – switch the drone to `OFFBOARD` mode and arm automatically (**the drone will take off**); * `frame_id` – [coordinate system](frames.md) for `yaw` (Default value: `map`). ### set_rates -Set pitch, roll, and yaw rates and the throttle level (similar to [the `ACRO` mode](modes.md)). This is the lowest drone control level (excluding direct control of motor rotation speed). This service may be used to automatically perform aerobatic tricks (e.g., flips). +Set roll, pitch, and yaw rates and the throttle level (similar to [the `ACRO` mode](modes.md)). This is the lowest drone control level (excluding direct control of motor rotation speed). This service may be used to automatically perform aerobatic tricks (e.g., flips). Parameters: -* `pitch_rate`, `roll_rate`, `yaw_rate` – pitch, roll, and yaw rates *(rad/s)*; +* `roll_rate`, `pitch_rate`, `yaw_rate` – pitch, roll, and yaw rates *(rad/s)*; * `thrust` — throttle level, ranges from 0 (no throttle, propellers are stopped) to 1 (full throttle). * `auto_arm` – switch the drone to `OFFBOARD` and arm automatically (**the drone will take off**); diff --git a/docs/en/snippets.md b/docs/en/snippets.md index 315e1f55..052270b5 100644 --- a/docs/en/snippets.md +++ b/docs/en/snippets.md @@ -144,7 +144,7 @@ Determine whether the copter is turned upside-down: PI_2 = math.pi / 2 telem = get_telemetry() -flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2 +flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2 ``` ### # {#angle-hor} @@ -155,8 +155,8 @@ Calculate the copter horizontal angle: PI_2 = math.pi / 2 telem = get_telemetry() -flipped = not -PI_2 <= telem.pitch <= PI_2 or not -PI_2 <= telem.roll <= PI_2 -angle_to_horizon = math.atan(math.hypot(math.tan(telem.pitch), math.tan(telem.roll))) +flipped = not -PI_2 <= telem.roll <= PI_2 or not -PI_2 <= telem.pitch <= PI_2 +angle_to_horizon = math.atan(math.hypot(math.tan(telem.roll), math.tan(telem.pitch))) if flipped: angle_to_horizon = math.pi - angle_to_horizon ``` @@ -324,7 +324,7 @@ def flip(): while True: telem = get_telemetry() - flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2 + flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2 if flipped: break diff --git a/docs/ru/parameters.md b/docs/ru/parameters.md index 312de3e0..089a3742 100644 --- a/docs/ru/parameters.md +++ b/docs/ru/parameters.md @@ -60,8 +60,8 @@ Estimator это подсистема, которая вычисляет текущее состояние (state) коптера, используя показания с датчиков. В состояние коптера входит: -* угловая скорость коптера – pitch_rate, roll_rate, yaw_rate; -* ориентация коптера (в локальной системе координат) – pitch (тангаж), roll (крен), yaw (рысканье) (одно из представлений); +* угловая скорость коптера – roll_rate, pitch_rate, yaw_rate; +* ориентация коптера (в локальной системе координат) – roll (крен), pitch (тангаж), yaw (рысканье) (одно из представлений); * позиция коптера (в локальной системе координат) – x, y, z; * скорость коптера (в локальной системе координат) – vx, vy, vz; * глобальные координаты коптера – latitude, longitude, altitude; diff --git a/docs/ru/simple_offboard.md b/docs/ru/simple_offboard.md index c9d51474..36a948d9 100644 --- a/docs/ru/simple_offboard.md +++ b/docs/ru/simple_offboard.md @@ -51,11 +51,11 @@ land = rospy.ServiceProxy('land', Trigger) * `lat, lon` – широта, долгота *(градусы)*, необходимо наличие [GPS](gps.md); * `alt` – высота в глобальной системе координат (стандарт [WGS-84](https://ru.wikipedia.org/wiki/WGS_84), не AMSL!), необходимо наличие [GPS](gps.md); * `vx, vy, vz` – скорость коптера *(м/с)*; -* `pitch` – угол по тангажу *(радианы)*; * `roll` – угол по крену *(радианы)*; +* `pitch` – угол по тангажу *(радианы)*; * `yaw` – угол по рысканью *(радианы)*; -* `pitch_rate` – угловая скорость по тангажу *(рад/с)*; * `roll_rate` – угловая скорость по крену *(рад/с)*; +* `pitch_rate` – угловая скорость по тангажу *(рад/с)*; * `yaw_rate` – угловая скорость по рысканью *(рад/с)*; * `voltage` – общее напряжение аккумулятора *(В)*; * `cell_voltage` – напряжение аккумулятора на ячейку *(В)*. @@ -265,7 +265,7 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body') Параметры: -* `pitch`, `roll`, `yaw` – необходимый угол по тангажу, крену и рысканью *(радианы)*; +* `roll`, `pitch`, `yaw` – необходимый угол по тангажу, крену и рысканью *(радианы)*; * `thrust` – уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ); * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**); * `frame_id` – [система координат](frames.md), в которой задан `yaw` (по умолчанию: `map`). @@ -276,7 +276,7 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body') Параметры: -* `pitch_rate`, `roll_rate`, `yaw_rate` – угловая скорость по тангажу, крену и рыканью *(рад/с)*; +* `roll_rate`, `pitch_rate`, `yaw_rate` – угловая скорость по тангажу, крену и рыканью *(рад/с)*; * `thrust` – уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ). * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**); diff --git a/docs/ru/snippets.md b/docs/ru/snippets.md index 45b1be6f..89d15d60 100644 --- a/docs/ru/snippets.md +++ b/docs/ru/snippets.md @@ -154,7 +154,7 @@ new_pose = tf_buffer.transform(pose, frame_id, transform_timeout) PI_2 = math.pi / 2 telem = get_telemetry() -flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2 +flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2 ``` ### # {#angle-hor} @@ -165,7 +165,7 @@ flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2 PI_2 = math.pi / 2 telem = get_telemetry() -flipped = not -PI_2 <= telem.pitch <= PI_2 or not -PI_2 <= telem.roll <= PI_2 +flipped = not -PI_2 <= telem.roll <= PI_2 or not -PI_2 <= telem.pitch <= PI_2 angle_to_horizon = math.atan(math.hypot(math.tan(telem.pitch), math.tan(telem.roll))) if flipped: angle_to_horizon = math.pi - angle_to_horizon @@ -335,7 +335,7 @@ def flip(): while True: telem = get_telemetry() - flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2 + flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2 if flipped: break From f78a03ec8943b596d5a99b893188a159d5319888 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 13 Jan 2023 12:06:55 +0300 Subject: [PATCH 05/21] Change default EKF2_HGT_MODE to 3 (vision) --- clover_simulation/airframes/4500_clover | 2 +- docs/en/parameters.md | 2 +- docs/ru/parameters.md | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/clover_simulation/airframes/4500_clover b/clover_simulation/airframes/4500_clover index 0039c50d..57c7ca81 100644 --- a/clover_simulation/airframes/4500_clover +++ b/clover_simulation/airframes/4500_clover @@ -31,7 +31,7 @@ param set-default EKF2_OF_DELAY 0 param set-default EKF2_OF_QMIN 10 param set-default EKF2_OF_N_MIN 0.05 param set-default EKF2_OF_N_MAX 0.2 -param set-default EKF2_HGT_MODE 2 # 0 = baro, 1 = gps, 2 = range, 3 = vision +param set-default EKF2_HGT_MODE 3 # 0 = baro, 1 = gps, 2 = range, 3 = vision param set-default EKF2_EVA_NOISE 0.1 param set-default EKF2_EVP_NOISE 0.1 param set-default EKF2_EV_DELAY 0 diff --git a/docs/en/parameters.md b/docs/en/parameters.md index 6bf4b2d3..fc0c2a47 100644 --- a/docs/en/parameters.md +++ b/docs/en/parameters.md @@ -44,7 +44,7 @@ In case of using EKF2 (official firmware): |`EKF2_OF_QMIN`|10|| |`EKF2_OF_N_MIN`|0.05|| |`EKF2_OF_N_MAX`|0.2|| -|`EKF2_HGT_MODE`|2 (*Range sensor*)|If the [rangefinder](laser.md) is present and flying over horizontal floor| +|`EKF2_HGT_MODE`|3 (*Vision*)|If the [rangefinder](laser.md) is present and flying over horizontal floor – 2 (*Range sensor*)| |`EKF2_EVA_NOISE`|0.1|| |`EKF2_EVP_NOISE`|0.1|| |`EKF2_EV_DELAY`|0|| diff --git a/docs/ru/parameters.md b/docs/ru/parameters.md index 089a3742..4af73524 100644 --- a/docs/ru/parameters.md +++ b/docs/ru/parameters.md @@ -44,7 +44,7 @@ |`EKF2_OF_QMIN`|10|| |`EKF2_OF_N_MIN`|0.05|| |`EKF2_OF_N_MAX`|0.2|| -|`EKF2_HGT_MODE`|2 (*Range sensor*)|При наличии [дальномера](laser.md) и полете над ровным полом| +|`EKF2_HGT_MODE`|3 (*Vision*)|При наличии [дальномера](laser.md) и полете над ровным полом — 2 (*Range sensor*)| |`EKF2_EVA_NOISE`|0.1|| |`EKF2_EVP_NOISE`|0.1|| |`EKF2_EV_DELAY`|0|| From 25ae694d1f3db2ef0855ebc2901b8866e208eac7 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 13 Jan 2023 12:58:51 +0300 Subject: [PATCH 06/21] simulator: add red circle model for recognizing --- .../materials/scripts/red_circle.material | 16 ++++++++++++ .../materials/textures/red_circle.png | Bin 0 -> 8050 bytes .../models/red_circle/model.config | 13 ++++++++++ .../models/red_circle/red_circle.sdf | 24 ++++++++++++++++++ .../models/red_circle/red_circle.svg | 7 +++++ 5 files changed, 60 insertions(+) create mode 100644 clover_simulation/models/red_circle/materials/scripts/red_circle.material create mode 100644 clover_simulation/models/red_circle/materials/textures/red_circle.png create mode 100644 clover_simulation/models/red_circle/model.config create mode 100644 clover_simulation/models/red_circle/red_circle.sdf create mode 100644 clover_simulation/models/red_circle/red_circle.svg diff --git a/clover_simulation/models/red_circle/materials/scripts/red_circle.material b/clover_simulation/models/red_circle/materials/scripts/red_circle.material new file mode 100644 index 00000000..9741c598 --- /dev/null +++ b/clover_simulation/models/red_circle/materials/scripts/red_circle.material @@ -0,0 +1,16 @@ +material red_circle +{ + technique + { + pass + { + scene_blend alpha_blend + texture_unit + { + texture red_circle.png + filtering none + scale 1.0 1.0 + } + } + } +} diff --git a/clover_simulation/models/red_circle/materials/textures/red_circle.png b/clover_simulation/models/red_circle/materials/textures/red_circle.png new file mode 100644 index 0000000000000000000000000000000000000000..66c963f09a2ebf81315b5be24c4e3fda4af9fb26 GIT binary patch literal 8050 zcmXY0c_7pO|KC05Tsd+s&5^T6$vNekD@H|biKtNSqikR0=wL>evobNtQOIqd_(Zv~ zDCJryG8H8u*YCAHzdyFU_k6$K&)4&Qy`GO}nxli2AfGfJ1OgExSep|e5GaNF;Xwfk zsq;`j_~8w+b_<6<)sAvMP<$)V9T0a%Shz+whXzDM`(8c^iH?re3JN(Ne#STKtXAmd zz=H2a(h!LJ4#C{aC8qG(WOV)6v$VM%i{a$^75DjS@w>$CSt#Z5pzibV2ueviIsKDb zH$FW7z3x=T%+YVVyT07|9kJ?jL8(GBpxz1U#G8&FSm@ml6;y48-Mdj4xIMOda#Afs ze|>VJa53;vE9c~7{NQ$TPI1Z5#&=;_+QEa0v+$eGI~xmrZGEb_NFE?-_IT?lX<3~sB%kEn`_Lk!`DkPL;L zs~^vw@-b@;(~Ol`lM8!IuQ7#mD(q|rY$}vVKZ7}KnP1>D5+4&AlNpm6Lyl#xICs^a zhE513zRC|~S{Ik}CR*BRDo*N(5P-XzB=of&-<%(i6PRQkE`d6>;A z+<3sUeu6)!Kk8p66(b+UTziYL(AAq>B-xT|dv@CL`>kGDvMPXVrXu{LKscIcFNKD> ziw_*!l!sKqv>vEs+xE7VU%&~f^P9Je)gi-p!VZQ>jZ#NkhwV<=HhhYNE?r@(_?x&K zq9$KQ-v4g?RY2BcJ95{GDSz;Yv9%tpP^j?OEbCe9h>t|-duRLIXh-qqmt35a^B0sW zb&e>knb7+9=bVyUvH70e9&8>qf?X!C!>BLi?5R{dBD02~sq@V_4Y*OE@8|jgrU_r02&#)0=Fw(+SVgONwDSqYs)!s@2K5q}MOnq8q*6EY;yH?Ylde z1&jxRhr34kg`jhg66NJhmhVA*iC)!ISDrQr{p%dmbz-Lne*$XlemO$Ol;1zk!Uz+i zzU1)6bndA1wa?5>n1af3Bg=P0L85p6jWLS>?v_;b#>XKi%!#^JaF@tRX`M<)W1E#e;A^0Ki@jxLMSkqsOJA-a6TjNf3akBPHrgmpU26d$exn z-y}z8#ahf`^Yb%g9+jrbrLOH-($x^UT@5GqrLL)o9M_Vw%$P+GRYbx`Po2Jn$EZgJ zwcla(kc2kuR{zxEIf@_k*;!17vQlfkD8nT$w!o^@%O`l7o2~*0F#V>8T?{iV`g?^R zG#s)SO|T>R$Gq$3_N{`<49;vkREjz+2LJGNfE;>@h%}tvHmG+C{RhHKIE$N3e;C z7)?3)pax~$9AtvzSPPKG;sSh=l$vFOW~lS~pBW8UXSl@6PyNJ8UkoQ{r-xQ=$d=r0 zLhHCiW)u#=woNevnGMCI0G(5JFhog--fO8BNuiJw2?s?+$5r#l;d_DSe911A1$(Xv z#aPa(QiB)c*it&7VAX^CxR3M9b*7jNkX$MnVnu1DM^LDi@nXh)2bP>Qgb3eaWy7E% z8`&5l>x+bS_f?myo+zMBFujLpOb_-~_`E+KDSByt7<4HQ_)qWs8)mF|+uaC?sAUaa zF&)(*q@gNj!E($KN=Qj9wL$N$A6BEJ&QyFq-(UeGNq%X>9yh-=B6!NG9IxnQ5n0qT z%QcowIH^YXlHWWwNk_W;hh-Z=UuF!%7vWf_V3;B;Fc-rL?efNsIzR77%1WXC<7N?Q zxR$6bM=wck#VcYUSMd!mln6<`uuF6P+f5OatGlOQX)%XJX9A7KZHW8S$)&w*T(Z<4 zEN!fS^B~Y`ZV#h=gWT1-339XjPy$l4PJQ$np;24Tg8F=tSJq~h#WITvYT~1}RDJ)K zi2*XQEU)s*;%6_GCH;E2BhJY36`nLO*FM;ZXS=XJvG(8?f;B21+slqcRJ|+s zGHM(+2)hpy4|aytC@tS&nHh{uz|!=uan!;?ofH|2lTfRy^-@3{?gy#}HeKWRYEei0 zS0O`tmvhUK>Z@XbDv-4!OSgb`7*wVjD+&4f5ftnwoM(QDnCSe;b${|Ob@+)$vHoQ z+55P%4oqOa+RwI^CD~Psi@w?U_a}wUFiOiWdhcEWV>q0OKRYi)bvdIV{DZ^Vqdam3 zFeETVeSAT*x`eO7TeH?%S!Ns^@}^i$3a-IVon6n`a~A^^c)Mv)cE*~RBx~CZ_9E5R zYvOf0P4Z(M9z29#x*=)Orufc@IM}+kt2}iitp}>0x%=(}LiCbUn8~XOD}5W{ZCn_R zHkYd(LxZK|p*_%Fjn0HNr7+4;G8ofGW!#bybD^*hHCn?GO*^NB@ zO?2&y4@Q-+cTTzT{4sg??tf56$eV&drijMrqzfIu~QRWY&om& z=gM6(d#u7UU@|jT{EF*R$HERmv`#y0_vg?8MM9~c=F$b7nlE!^s2}&XGvEg4IED@? zAwd{DUy?dQnp;_sqS6_%cHR6Nv7~KHUkgN-Qf-qdnt~hnXyar0=a<>^5bviLP<7OT zyxC1x+hY@zDX9n8YCbhnoy{OK-a<-2rxVLC3=qhcga;-vUzb)*@o! zLZ@A+BlpUf#qGsM{~RC}UIPY;d@M8i(((986v&WUoE#++4I3)wvt^}>rU|R+@YfZ@ zWl^B^=&&@3J>H{( zO8+oCtaEb1)^%#thMGKV;_vl(kl%oYpL+k#c^qNocXE{Xt0`dcBTH6oF^<&afl21f z3y(<-|NA0*IyW|a>v{CsiX`jZv2x!=Y^l;8#J_D`Ua)l)31{TgNHmrZs3VX*(zh20 z6?eQt0%1o`eaEQD?~by2PbM}O!7i4$G4`WKC|a13aUv*sCN{c-I^ z1G4N*V2e^8Xjp`L=U6bgDM5))T3R-=`HowA;L$Mmh`(#7lfMPHKMy_&i!}ibzD-?c z?s>9BR7YNRS#`q^1a%hzJCTZne4AWuxv#aYVOdk9XM)W8*VawP5g&hnB^IvhZiO|% zg62JZ2rS3^OR0I=2d7dsNv)*V_8}Z081yKLIts;{6kU64iXSkfYJrgK!AYS&HQ`lL z9N~4;KX%BU|1&{k;`I9JJ@>E3TX(;48S1&XnsyzS`Ehcv!l&)qiTgdxj6#!v$d*0I z%x&i_p9io^950*1*uK;Pqy5ZPBJ2%%=@9b^ydA*262n)6V>}ie1HFdw&Wwv#uL%fD z`pPUk{^Dkavv_LXDJuQ8aixrgFoCFkT8%P?!KUIEZt{^)-;)czh>SxMrG2mjVz^OG zzoE+Z;~$8}L!>TiYW|sw7m4qh@bV3PS_D>;pn=~E>|Hp?3`ZDvY-XiTwx*tLFtd6% zZJsf<%N5fvm!M1_!i{(|u;GFi^vPdu0FUHPNiem<5@{Y-EpGxf;7N#r|AorLY+R7E zvqbNAr}s=`W&+*8+aJx%QEe*Fjd~f4HjiG&zlz-h$5IavL~l zZl@+ZR_7r>Aa;F4Eb#l6|L?Cjd%4oQfBFyN@tI~>Dn0p~2v^sQ33w(h(qAR734`-f z+Fug}l+=BQ4L8GGAVkVU6R4;4LIuVeBlrl!nG1#}4Yi`9)Diug+*eg3ny)DlIM-}l zsr2+F?qzSImM?*7@ByI+uJ0dUA1HGM!4WttHTBG#I?|!}oOI+r%JKo9W!63%;kIrM zFSwI7pvmPo`^%fsch{t8xAh{q9%!ayWMb{hkpRvyo&;8IBgTyk&8#l{H4-2JPKkv?j|)8IOe zap}7>{QG};MIQk+GPv{<1UTm!_p5jEJD2i7o-30B7AQI)&bv$&|4*7JDVq9^;0OY7 z;Us09J+|f@qA6`b9lA^+Mq1jWzQ-`(d5dBQ_01;_CHE6cmPc<%3` z79>{Z$)EGxI-nOfqS8@KCH;?lw-7KdevoIA_F~g^T9oNsJc@+ioPGSCT3~eg39c!<0N1EK~$pY zmJ*0Nlw92zj)F`$nVW6Z&2IsG`H>@JNK;l3|3Sg@+cZ@Qq zMYB2Z@mt9pggAMxJtFgRsJN+=hi~BzB)^R6tx>i z>WcR^U-@gWpjID_u`nsMnK{X9+xDm8Mkj21L})_CSGwkb7%3!$RTXZX)XJ+i@|jiA zAT6%ybCnAC`j@3*6zb~h89WHNvFrT>4>msZi&?Kc^IkQ`#4&z{=OD(CkH3k8QMUUfA!}- z|CQeTJoM*HPAx?7Q2FWS?<=3!QR(@u8NutpC)7J9{ve2xjthoz$w1@j!>2Wc&V};w zdpxJoec!O&of13k{S20x2cLjmcWc$EUeqXmZ{$7vo!=0g6X3BnH8{O#x1wxxM8`Fx))@k8|Ce*?d0?7x!^Sbowt=}{~19)fgJJXs~W zWd{m~w7yH)=G6Qz^&v0eVBd!*7DFZw{qx^%;|TD_(qpv#>8NF8kj(hm#dFJ;bSnM+ z;MYc@yOLr&`0iqt)t(#Qr|qaCfg6-}E5?GYC3uKV4P5KXi+FJCn4j>>(UKFqUa6w6*RC}zuMsS7xURy zdwy}sg*pO13~S9?x2~D0eag*AcIWCZV*7E9Tsbj@t%D42Slbx1XU~c|g7AByaX^H>%5fZBpRM<>iEoCZSzyp zxvM$2hM6l{szP7hB4ENI%pvmDpKu6)rBMy}y?BcJD-Z^K4mZ4RO+1n~UGtN4)d9PZ zE-VQ}|6T>DAUWapY) zXCEAOfDAFeAhgNnI<}KUU|GAPjBfWEf&=eA8tP}aMb=*7)4?I*+g6gQkkkl}<^b9i zW)!&}olVbNKJO!>qrg2uKdmcj=38qe%N?g$4z;3A$nWpv^`8`(Nnwg48CT#0+xdHq z5-4a5Cv3cP(VNzJA0Z9GvGaPPS*Tph&$~&Thf3q}T90dG)2G(I-0g$g*kK_e%N4}~ zt$l1EEyA(SrFcKT;mFJ&h0`a7tGx>ocr9r$=uoJ>(UECWxzEl{rJ; z)P=%&Uf&h^93T#oGsi{7uJsPU0*o)im%g@4R{9?raC??dX@=>`tNy`h5jdHxWJ$e8 zMt9SYFiI?ZR7~Lfl^X@P0YCAuqea>hg#=xGU?oL%B);nn8^>E=(mG{5d%Y|vqOuvh zurAIXVI9CR;EBg)X#hVcjV<`%oTe`rW6O2v{h`eD2dq6UdKyU3Y8bOqf%StVOd;t? z%b6^*7Cl+y-|dNKtO{tJ|9r(|=yy|R1jXyzZ~0#T(S2dsFzHMD5YR3jVpsU?6OwuW zoZcp`FVIxVeg4yNCV#=@jH;7@r^t|*IcEjl31`s2Lsy;d^Z*R**ZU=4fZ0gqF!Lu^ z_jdhRW~RTFr-8MpKp+|7?}d^@KGXm(k#!tIKfS-t7CoQ)K~tF2Z%fYosCgVG_(qL= z)_=PmAU4w9xxtpf@pZXvI(M45_aW5l*laUcVSGsvtnieSRcDS{c^k|g^)L1`=*0>Q zUBC7yRbN71CXWji-h0BQpV=RpjnPppJpqzy2;BkaLJjnLKOjp)okUmg%55(Y1hR+( zNmo~JgE%>I!3l(;^nw@roKB*bfi-WbJGjvbor4+zq=#{7 z+d49n-B6a4ts@=iYp)W%Z?yeM2tbmEr4uOGgD*crOT9o$b|Nr{9PaZX)nzS8@q0_o zOxa2R6)rNDN%uOJtca%u;TZYGR!-DQ7>&|g$ca@f2f@S0F%gp0wHsRgl7rN7o89Z* zAs8cB+ZZ8BXrM4RUL@?LRoSL6vW89g&npAOwtVKq0!uYsDh5vTR+FQrYfxzBMv`lX zYCVApsPFQXRedRc(px6Oc+V@-OyL;VHrJo`+qR2fs^O;IMt<$>si*E!QC#a^^c7V*a+66)VI(bw1T6O z-U}VJ_6wuZ2#WF(eB2&CE4Ce*^wh+r4MYW~|{q!`T-8@&U0U1!C4&=CYUsDS+bbVhB~kO?VOnP4lF|)FVF?l8!Wi ze0Ao|No>i^b^6jhu%Jo+-NdjJNS^%VnFg11N6oj6=v)zYhr40zCF}NuDJ~smdI7N> zHA$~YEs5VOeWyY36}^OAl4na{!Cd7~6z}NXvz-gXQ82wpcIx(Wc}po2=+`W?EWIec z6w2JeEw_MqkUN+M-5isTky$#$Ji}K$e9jG(K(5rk>aKtDPd98gQiQ+aL8ddQoxglI ztW|nVAk2WK4n{?ZZrD;~KW{KI;~jetCM}q9mU)V#%Y}ud1k;f^m0)7f0}VxCvC>}zm)77#h;rx9l7Rea0$)hi~)>w9r!5!5#cwa`GptT`f z9S>3dd@^IhDhV?S{?!^_<`zhgw(Y1a`J!k!MO7GhmY{+^Tj}GRZOT)>-Cs=E*rb2(OQ>HkG1=e zD-yXQSVm48Mt0}*P05Y(N~5D}N$1J7B|n`-O&LPhOV}{mujqQuIs$93G}VA{&z?Nt292pI4!?xwlRnt0H+jdv0AZ9(CT-Qdv&F&p61^ z5FS0gQva`KMaX88%89zKu&+E9JhjH~6ILtqjslXB%MfoTswu6%K`DtK^8`PlAu{eL z{=&g|vsr9^jpO5!j}RgE)Y1x6u6S?JV!atY#8wSq!Ca2P9g7+7aoD*1pN&s71Foh6 z{m4eCovZ3T7dGeI(|5-CeDrE1ni4ms + + Red Circle + 1.0 + red_circle.sdf + + Oleg Kalachev + okalachev@gmail.com + + + Red circle of size 40 cm on the floor for recognizing by a drone + + diff --git a/clover_simulation/models/red_circle/red_circle.sdf b/clover_simulation/models/red_circle/red_circle.sdf new file mode 100644 index 00000000..caf0008e --- /dev/null +++ b/clover_simulation/models/red_circle/red_circle.sdf @@ -0,0 +1,24 @@ + + + + true + + 0 0 1e-3 0 0 0 + + false + + + 0.4 0.4 1e-3 + + + + + + + + + diff --git a/clover_simulation/models/red_circle/red_circle.svg b/clover_simulation/models/red_circle/red_circle.svg new file mode 100644 index 00000000..3321ece0 --- /dev/null +++ b/clover_simulation/models/red_circle/red_circle.svg @@ -0,0 +1,7 @@ + + + + red_circle + + + From 59518fddd1c32080d2c185d07865a591653f2a8d Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 13 Jan 2023 12:59:26 +0300 Subject: [PATCH 07/21] examples: add program to recognize and follow the red circle --- clover/examples/red_circle.py | 97 +++++++++++++++++++++++++++++++++++ 1 file changed, 97 insertions(+) create mode 100644 clover/examples/red_circle.py diff --git a/clover/examples/red_circle.py b/clover/examples/red_circle.py new file mode 100644 index 00000000..9fc726df --- /dev/null +++ b/clover/examples/red_circle.py @@ -0,0 +1,97 @@ +# This example makes the drone find and follow the red circle. +# To test in the simulator, place 'Red Circle' model on the floor. +# More information: https://clover.coex.tech/red_circle + +# Input topic: main_camera/image_raw (camera image) +# Output topics: +# cv/mask (red color mask) +# cv/red_circle (position of the center of the red circle in 3D space) + +import rospy +import cv2 +import numpy as np +from math import nan +from sensor_msgs.msg import Image, CameraInfo +from geometry_msgs.msg import PointStamped, Point +from cv_bridge import CvBridge +from clover import long_callback, srv +import tf2_ros +import tf2_geometry_msgs + +rospy.init_node('cv', disable_signals=True) # disable signals to allow interrupting with ctrl+c + +get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) +set_position = rospy.ServiceProxy('set_position', srv.SetPosition) + +bridge = CvBridge() + +tf_buffer = tf2_ros.Buffer() +tf_listener = tf2_ros.TransformListener(tf_buffer) + +mask_pub = rospy.Publisher('~mask', Image, queue_size=1) +point_pub = rospy.Publisher('~red_circle', PointStamped, queue_size=1) + +# read camera info +camera_info = rospy.wait_for_message('main_camera/camera_info', CameraInfo) +camera_matrix = np.float64(camera_info.K).reshape(3, 3) +distortion = np.float64(camera_info.D).flatten() + + +def img_xy_to_point(xy, dist): + xy = cv2.undistortPoints(xy, camera_matrix, distortion, P=camera_matrix)[0][0] + + # Shift points to center + xy -= camera_info.width // 2, camera_info.height // 2 + + fx = camera_matrix[0, 0] + fy = camera_matrix[1, 1] + + return Point(x=xy[0] * dist / fx, y=xy[1] * dist / fy, z=dist) + +def get_center_of_mass(mask): + M = cv2.moments(mask) + if M['m00'] == 0: + return None + return M['m10'] // M['m00'], M['m01'] // M['m00'] + +follow_red_circle = False + +@long_callback +def image_callback(msg): + img = bridge.imgmsg_to_cv2(msg, 'bgr8') + img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + + # we need to use two ranges for red color + mask1 = cv2.inRange(img_hsv, (0, 150, 150), (15, 255, 255)) + mask2 = cv2.inRange(img_hsv, (160, 150, 150), (180, 255, 255)) + + # combine two masks using bitwise OR + mask = cv2.bitwise_or(mask1, mask2) + + # publish the mask + if mask_pub.get_num_connections() > 0: + mask_pub.publish(bridge.cv2_to_imgmsg(mask, 'mono8')) + + # calculate x and y of the circle + xy = get_center_of_mass(mask) + if xy is None: + return + + # calculate and publish the position of the circle in 3D space + altitude = get_telemetry('terrain').z + xy3d = img_xy_to_point(xy, altitude) + target = PointStamped(header=msg.header, point=xy3d) + point_pub.publish(target) + + if follow_red_circle: + # follow the target + setpoint = tf_buffer.transform(target, 'map', timeout=rospy.Duration(0.2)) + set_position(x=setpoint.point.x, y=setpoint.point.y, z=nan, yaw=nan, frame_id=setpoint.header.frame_id) + +# process each camera frame: +image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1) + +rospy.loginfo('Hit enter to follow the red circle') +input() +follow_red_circle = True +rospy.spin() From 56a2be8170106463a7dcff00b745b4d6b6becd21 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 13 Jan 2023 12:59:43 +0300 Subject: [PATCH 08/21] docs: add redirect from /red_circle to camera article --- redirects.json | 1 + 1 file changed, 1 insertion(+) diff --git a/redirects.json b/redirects.json index d6a5ac81..a851c75e 100644 --- a/redirects.json +++ b/redirects.json @@ -65,6 +65,7 @@ { "from": "clover_vm/", "to": "en/simulation_vm.html" }, { "from": "gpio/", "to": "en/gpio.html" }, { "from": "blocks/", "to": "en/blocks.html" }, + { "from": "red_circle/", "to": "en/camera.html" }, { "from": "ru/microsd_images.html", "to": "image.html" }, { "from": "en/microsd_images.html", "to": "image.html" }, From 87a51221bc586c81b2cd469a608e3bc6b43b6af7 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 7 Feb 2023 10:02:02 +0300 Subject: [PATCH 09/21] docs: update documentation for autonomous flights --- docs/en/aruco_marker.md | 6 --- docs/en/simple_offboard.md | 101 +++++++++++++++++++++++++++---------- docs/ru/aruco_marker.md | 6 --- docs/ru/simple_offboard.md | 101 +++++++++++++++++++++++++++---------- 4 files changed, 148 insertions(+), 66 deletions(-) diff --git a/docs/en/aruco_marker.md b/docs/en/aruco_marker.md index f2208f25..4fec4958 100644 --- a/docs/en/aruco_marker.md +++ b/docs/en/aruco_marker.md @@ -72,12 +72,6 @@ Sample code to fly to a point 1 metre to the left and 2 metres above marker with navigate(frame_id='aruco_7', x=-1, y=0, z=2) ``` -Sample code to rotate counterclockwise while hovering 1.5 metres above marker id 10: - -```python -navigate(frame_id='aruco_10', x=0, y=0, z=1.5, yaw_rate=0.5) -``` - Note that if the required marker isn't detected for 0.5 seconds after the `navigate` command, the command will be ignored. These frames may also be used in other services that accept TF frames (like `get_telemetry`). The following code will get the drone's position relative to the marker with id 3: diff --git a/docs/en/simple_offboard.md b/docs/en/simple_offboard.md index de0a8186..4fcf855c 100644 --- a/docs/en/simple_offboard.md +++ b/docs/en/simple_offboard.md @@ -1,5 +1,7 @@ # Autonomous flight +> **Note** The following applies to [image versions](image.md) **0.24** and up. Older documentation is still available for [for version **0.23**](https://github.com/CopterExpress/clover/blob/v0.23/docs/en/simple_offboard.md). + The `simple_offboard` module of the `clover` package is intended for simplified programming of the autonomous drone flight (`OFFBOARD` [flight mode](modes.md)). It allows setting the desired flight tasks, and automatically transforms [coordinates between frames](frames.md). `simple_offboard` is a high level system for interacting with the flight controller. For a more low level system, see [mavros](mavros.md). @@ -20,6 +22,9 @@ rospy.init_node('flight') # 'flight' is name of your ROS node get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) navigate = rospy.ServiceProxy('navigate', srv.Navigate) navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal) +set_altitude = rospy.ServiceProxy('set_altitude', srv.SetAltitude) +set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw) +set_yaw_rate = rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate) set_position = rospy.ServiceProxy('set_position', srv.SetPosition) set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity) set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude) @@ -100,7 +105,6 @@ Parameters: * `x`, `y`, `z` — coordinates *(m)*; * `yaw` — yaw angle *(radians)*; -* `yaw_rate` – angular yaw velocity (will be used if yaw is set to `NaN`) *(rad/s)*; * `speed` – flight speed (setpoint speed) *(m/s)*; * `auto_arm` – switch the drone to `OFFBOARD` mode and arm automatically (**the drone will take off**); * `frame_id` – [coordinate system](frames.md) for values `x`, `y`, `z` and `yaw`. Example: `map`, `body`, `aruco_map`. Default value: `map`. @@ -119,7 +123,7 @@ Flying in a straight line to point 5:0 (altitude 2) in the local system of coord navigate(x=5, y=0, z=3, speed=0.8) ``` -Flying to point 5:0 without changing the yaw angle (`yaw` = `NaN`, `yaw_rate` = 0): +Flying to point 5:0 without changing the yaw angle: ```python navigate(x=5, y=0, z=3, speed=0.8, yaw=float('nan')) @@ -149,22 +153,10 @@ Flying to point 3:2 (with the altitude of 2 m) in the [ArUco map](aruco.md) coor navigate(x=3, y=2, z=2, speed=1, frame_id='aruco_map') ``` -Rotating on the spot at the speed of 0.5 rad/s (counterclockwise): - -```python -navigate(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0.5, frame_id='body') -``` - -Flying 3 meters forwards at the speed of 0.5 m/s, yaw-rotating at the speed of 0.2 rad/s: - -```python -navigate(x=3, y=0, z=0, speed=0.5, yaw=float('nan'), yaw_rate=0.2, frame_id='body') -``` - Ascending to the altitude of 2 m (command line): ```(bash) -rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed: 0.5, frame_id: 'body', auto_arm: true}" +rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, speed: 0.5, frame_id: 'body', auto_arm: true}" ``` > **Note** Consider using the `navigate_target` frame instead of `body` for missions that primarily use relative movements forward/back/left/right. This negates inaccuracies in relative point calculations. @@ -178,7 +170,6 @@ Parameters: * `lat`, `lon` — latitude and longitude *(degrees)*; * `z` — altitude *(m)*; * `yaw` — yaw angle *(radians)*; -* `yaw_rate` – angular yaw velocity (used for setting the yaw to `NaN`) *(rad/s)*; * `speed` – flight speed (setpoint speed) *(m/s)*; * `auto_arm` – switch the drone to `OFFBOARD` and arm automatically (**the drone will take off**); * `frame_id` – [coordinate system](frames.md) for `z` and `yaw` (Default value: `map`). @@ -191,7 +182,7 @@ Flying to a global point at the speed of 5 m/s, while maintaining current altitu navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, frame_id='body') ``` -Flying to a global point without changing the yaw angle (`yaw` = `NaN`, `yaw_rate` = 0): +Flying to a global point without changing the yaw angle: ```python navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, yaw=float('nan'), frame_id='body') @@ -200,7 +191,71 @@ navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, yaw=float('nan'), fr Flying to a global point (command line): ```bash -rosservice call /navigate_global "{lat: 55.707033, lon: 37.725010, z: 0.0, yaw: 0.0, yaw_rate: 0.0, speed: 5.0, frame_id: 'body', auto_arm: false}" +rosservice call /navigate_global "{lat: 55.707033, lon: 37.725010, z: 0.0, yaw: 0.0, speed: 5.0, frame_id: 'body', auto_arm: false}" +``` + +### set_altitude + +Change the desired flight altitude. The service is used to set the altitude and its coordinate system independently, after calling [`navigate`](#navigate) or [`set_position`](#setposition). + +Parameters: + +* `z` – flight altitude *(m)*; +* `frame_id` – [coordinate system](frames.md) for computing the altitude. + +Set the desired altitude to 2 m relative to the floor: + +```python +set_altitude(z=2, frame_id='terrain') +``` + +Set the desired altitude to 1 m relative to [the ArUco map](aruco.md): + +```python +set_altitude(z=1, frame_id='aruco_map') +``` + +### set_yaw + +Change the desired yaw angle (and its coordinate system), keeping the previous command in effect. + +Parameters: + +* `yaw` — yaw angle *(radians)*; +* `frame_id` – [coordinate system](frames.md) for computing the yaw. + +Rotate by 90 degrees clockwise (the previous command continues): + +```python +set_yaw(yaw=math.radians(-90), frame_id='body') +``` + +Set the desired yaw angle to zero relative to [the ArUco map](aruco.md): + +```python +set_yaw(yaw=0, frame_id='aruco_map') +``` + +Stop yaw rotation (caused by [`set_yaw_rate`](#setyawrate) call): + +```python +set_yaw(yaw=float('nan')) +``` + +### set_yaw_rate + +The the desired angular yaw velocity, keeping the previous command in effect. + +Parameters: + +* `yaw_rate` – angular yaw velocity *(rad/s)*; + +The positive direction of `yaw_rate` rotation (when viewed from the top) is counterclockwise. + +Start yaw rotation at 0.5 rad/s (the previous command continues): + +```python +set_yaw_rate(yaw_rate=0.5) ``` ### set_position @@ -213,7 +268,6 @@ Parameters: * `x`, `y`, `z` — point coordinates *(m)*; * `yaw` — yaw angle *(radians)*; -* `yaw_rate` – angular yaw velocity (used for setting the yaw to NaN) *(rad/s)*; * `auto_arm` – switch the drone to `OFFBOARD` and arm automatically (**the drone will take off**); * `frame_id` – [coordinate system](frames.md) for `x`, `y`, `z` and `yaw` parameters (Default value: `map`). @@ -235,19 +289,12 @@ Assigning the target point 1 m ahead of the current position: set_position(x=1, y=0, z=0, frame_id='body') ``` -Rotating on the spot at the speed of 0.5 rad/s: - -```python -set_position(x=0, y=0, z=0, frame_id='body', yaw=float('nan'), yaw_rate=0.5) -``` - ### set_velocity Set speed and yaw setpoints. * `vx`, `vy`, `vz` – flight speed *(m/s)*; * `yaw` — yaw angle *(radians)*; -* `yaw_rate` – angular yaw velocity (used for setting the yaw to NaN) *(rad/s)*; * `auto_arm` – switch the drone to `OFFBOARD` and arm automatically (**the drone will take off**); * `frame_id` – [coordinate system](frames.md) for `vx`, `vy`, `vz` and `yaw` (Default value: `map`). @@ -280,7 +327,7 @@ Parameters: * `thrust` — throttle level, ranges from 0 (no throttle, propellers are stopped) to 1 (full throttle). * `auto_arm` – switch the drone to `OFFBOARD` and arm automatically (**the drone will take off**); -The positive direction of `yaw_rate` rotation (when viewed from the top) is counterclockwise,`pitch_rate` rotation is forward, `roll_rate` rotation is to the left. +The positive direction of `yaw_rate` rotation (when viewed from the top) is counterclockwise, `pitch_rate` rotation is forward, `roll_rate` rotation is to the left. ### land diff --git a/docs/ru/aruco_marker.md b/docs/ru/aruco_marker.md index d7599544..d8f7d5cd 100644 --- a/docs/ru/aruco_marker.md +++ b/docs/ru/aruco_marker.md @@ -84,12 +84,6 @@ navigate(frame_id='aruco_5', x=0, y=0, z=1) navigate(frame_id='aruco_7', x=-1, y=0, z=2) ``` -Вращаться против часовой стрелки на высоте 1.5 метра над маркером 10: - -```python -navigate(frame_id='aruco_10', x=0, y=0, z=1.5, yaw_rate=0.5) -``` - Если необходимый маркер не появится в поле зрения в течение полусекунды, дрон продолжит выполнять предыдущую команду. Подобные значения `frame_id` можно использовать и в других сервисах, например `get_telemetry`. Получение расположения дрона относительно маркера 3: diff --git a/docs/ru/simple_offboard.md b/docs/ru/simple_offboard.md index 36a948d9..e48fc5e9 100644 --- a/docs/ru/simple_offboard.md +++ b/docs/ru/simple_offboard.md @@ -1,5 +1,7 @@ # Автономный полет +> **Note** Документация для версий [образа](image.md), начиная с версии **0.24**. Для более ранних версий см. [документацию для версии **0.23**](https://github.com/CopterExpress/clover/blob/v0.23/docs/ru/simple_offboard.md). + Модуль `simple_offboard` пакета `clover` предназначен для упрощенного программирования автономного полета дрона ([режим](modes.md) `OFFBOARD`). Он позволяет устанавливать желаемые полетные задачи и автоматически трансформирует [систему координат](frames.md). `simple_offboard` является высокоуровневым способом взаимодействия с полетным контроллером. Для более низкоуровневой работы см. [mavros](mavros.md). @@ -20,6 +22,9 @@ rospy.init_node('flight') get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) navigate = rospy.ServiceProxy('navigate', srv.Navigate) navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal) +set_altitude = rospy.ServiceProxy('set_altitude', srv.SetAltitude) +set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw) +set_yaw_rate = rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate) set_position = rospy.ServiceProxy('set_position', srv.SetPosition) set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity) set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude) @@ -100,7 +105,6 @@ rosservice call /get_telemetry "{frame_id: ''}" * `x`, `y`, `z` – координаты *(м)*; * `yaw` – угол по рысканью *(радианы)*; -* `yaw_rate` – угловая скорость по рысканью (применяется при установке yaw в `NaN`) *(рад/с)*; * `speed` – скорость полета (скорость движения setpoint) *(м/с)*; * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**); * `frame_id` – [система координат](frames.md), в которой заданы `x`, `y`, `z` и `yaw` (по умолчанию: `map`). @@ -119,7 +123,7 @@ navigate(x=0, y=0, z=1.5, speed=0.5, frame_id='body', auto_arm=True) navigate(x=5, y=0, z=3, speed=0.8) ``` -Полет в точку 5:0 без изменения угла по рысканью (`yaw` = `NaN`, `yaw_rate` = 0): +Полет в точку 5:0 без изменения угла по рысканью: ```python navigate(x=5, y=0, z=3, speed=0.8, yaw=float('nan')) @@ -149,22 +153,10 @@ navigate(yaw=math.radians(-90), frame_id='body') navigate(x=3, y=2, z=2, speed=1, frame_id='aruco_map') ``` -Вращение на месте со скоростью 0.5 рад/c (против часовой): - -```python -navigate(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0.5, frame_id='body') -``` - -Полет вперед 3 метра со скоростью 0.5 м/с, вращаясь по рысканью со скоростью 0.2 рад/с: - -```python -navigate(x=3, y=0, z=0, speed=0.5, yaw=float('nan'), yaw_rate=0.2, frame_id='body') -``` - Взлет на высоту 2 м (командная строка): ```bash -rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed: 0.5, frame_id: 'body', auto_arm: true}" +rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, speed: 0.5, frame_id: 'body', auto_arm: true}" ``` > **Note** При программировании миссии дрона в терминах "вперед-назад-влево-вправо" рекомендуется использовать систему координат `navigate_target` вместо `body`, чтобы не учитывать неточность прилета дрона в предыдущую целевую точку при вычислении следующей. @@ -178,12 +170,11 @@ rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed * `lat`, `lon` – широта и долгота *(градусы)*; * `z` – высота *(м)*; * `yaw` – угол по рысканью *(радианы)*; -* `yaw_rate` – угловая скорость по рысканью (при установке yaw в `NaN`) *(рад/с)*; * `speed` – скорость полета (скорость движения setpoint) *(м/с)*; * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**); * `frame_id` – [система координат](frames.md), в которой заданы `z` и `yaw` (по умолчанию: `map`). -> **Note** Для полета без изменения угла по рысканью достаточно установить `yaw` в `NaN` (значение угловой скорости по умолчанию – 0). +> **Note** Для полета без изменения угла по рысканью достаточно установить `yaw` в `NaN`. Полет в глобальную точку со скоростью 5 м/с, оставаясь на текущей высоте (`yaw` установится в 0, коптер сориентируется передом на восток): @@ -191,7 +182,7 @@ rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, frame_id='body') ``` -Полет в глобальную точку без изменения угла по рысканью (`yaw` = `NaN`, `yaw_rate` = 0): +Полет в глобальную точку без изменения угла по рысканью: ```python navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, yaw=float('nan'), frame_id='body') @@ -200,7 +191,71 @@ navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, yaw=float('nan'), fr Полет в глобальную точку (командная строка): ```bash -rosservice call /navigate_global "{lat: 55.707033, lon: 37.725010, z: 0.0, yaw: 0.0, yaw_rate: 0.0, speed: 5.0, frame_id: 'body', auto_arm: false}" +rosservice call /navigate_global "{lat: 55.707033, lon: 37.725010, z: 0.0, yaw: 0.0, speed: 5.0, frame_id: 'body', auto_arm: false}" +``` + +### set_altitude + +Изменить целевую высоту полета. Сервис используется для независимой установки высоты (и системы координат для расчета высота) в режимах полета [`navigate`](#navigate) и [`set_position`](#setposition). + +Параметры: + +* `z` – высота полета *(м)*; +* `frame_id` – [система координат](frames.md) для расчета высоты полета. + +Установить высоту полета в 2 м относительно пола: + +```python +set_altitude(z=2, frame_id='terrain') +``` + +Установить высоту полета в 1 м относительно [маркерного поля](aruco.md): + +```python +set_altitude(z=1, frame_id='aruco_map') +``` + +### set_yaw + +Изменить целевой угол по рысканью (и систему координат для его расчета), оставив предыдущую команду в силе. + +Параметры: + +* yaw – угол по рысканью *(радианы)*; +* frame_id – [система координат](frames.md) для расчета угла по рысканью. + +Повернуться на 90 градусов по часовой (продолжая выполнять предыдущую команду): + +```python +set_yaw(yaw=math.radians(-90), frame_id='body') +``` + +Установить угол по рысканью в ноль в системе координат [маркерного поля](aruco.md): + +```python +set_yaw(yaw=0, frame_id='aruco_map') +``` + +Остановить вращение по рысканью (при использовании [`set_yaw_rate`](#setyawrate)): + +```python +set_yaw(yaw=float('nan')) +``` + +### set_yaw_rate + +Изменить целевую угловую скорость по рысканью, оставив предыдущую команду в силе. + +Параметры: + +* yaw_rate – угловая скорость по рысканью *(рад/с)*. + +Положительное направление вращения (при виде сверху) – против часовой. + +Начать вращение на месте со скоростью 0.5 рад/c против часовой (продолжая выполнять предыдущую команду): + +```python +set_yaw_rate(yaw_rate=0.5) ``` ### set_position @@ -213,7 +268,6 @@ rosservice call /navigate_global "{lat: 55.707033, lon: 37.725010, z: 0.0, yaw: * `x`, `y`, `z` – координаты точки *(м)*; * `yaw` – угол по рысканью *(радианы)*; -* `yaw_rate` – угловая скорость по рысканью (при установке yaw в NaN) *(рад/с)*; * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**); * `frame_id` – [система координат](frames.md), в которой заданы `x`, `y`, `z` и `yaw` (по умолчанию: `map`). @@ -235,19 +289,12 @@ set_position(x=0, y=0, z=3, frame_id='body') set_position(x=1, y=0, z=0, frame_id='body') ``` -Вращение на месте со скоростью 0.5 рад/c: - -```python -set_position(x=0, y=0, z=0, frame_id='body', yaw=float('nan'), yaw_rate=0.5) -``` - ### set_velocity Установить скорости и рысканье. * `vx`, `vy`, `vz` – требуемая скорость полета *(м/с)*; * `yaw` – угол по рысканью *(радианы)*; -* `yaw_rate` – угловая скорость по рысканью (при установке yaw в NaN) *(рад/с)*; * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**); * `frame_id` – [система координат](frames.md), в которой заданы `vx`, `vy`, `vz` и `yaw` (по умолчанию: `map`). From df66deb32c348c8f66530688a52bdf01e65512e2 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 7 Feb 2023 10:03:41 +0300 Subject: [PATCH 10/21] docs: add running flight autotests to testing plan --- docs/ru/testing.md | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/docs/ru/testing.md b/docs/ru/testing.md index aff01f86..201874c2 100644 --- a/docs/ru/testing.md +++ b/docs/ru/testing.md @@ -46,6 +46,13 @@ * **Показывает возникающие ошибки и опечатки, допущенные в .launch файлах** * **Проверка на throttling** +### Автоматизированные тесты + +* **Корректная работы автоматизированных тестов**: + * Тест автономного полета: `rosrun clover autotest_flight.py` + * Тест автономного полета по маркерам: `rosrun clover autotest_aruco.py` + * Тест LED-ленты: `rosrun clover autotest_led.py` + ### Тесты simple_offboard * **Корректная работа simple_offboard – взлет, полет в точку в любом фрейме, отсутствие проблем с `yaw` и `yaw_rate`** @@ -53,6 +60,7 @@ * **В фрейме `aruco_map`** * **В фрейме `map`** * **В фрейме `navigate_target`** +* **В фрейме `terrain`**. * Корректное выполнения флипа * **Возможность лететь к отдельным маркерам в карте, которые вне кадра и в кадре** * **Корректное детектирование статуса kill switch при выполнение команды с флагом `auto_arm`** @@ -70,11 +78,6 @@ * Полет по Optical Flow над 1 маркером * `aruco_map` не падает в случае маленьких размеров карты и маркеров -### Тесты [pigpiod](gpio.md) - -* Корректная работа pigpiod, возможность работы с сонаром, сервой и электромагнитом по мануалу -* Одновременная работа pigpiod и rpi_ws281x (правильная работа светодиодной ленты и сервы) - ### Тесты [LED-ленты](leds.md) * **Работает нода LED ленты на RPi 4** @@ -83,6 +86,11 @@ * **Низкоуровневое управление отдельными диодами** * **Высокоуровневое управление эффектами** +### Тесты [pigpiod](gpio.md) + +* Корректная работа pigpiod, возможность работы с сонаром, сервой и электромагнитом по мануалу +* Одновременная работа pigpiod и rpi_ws281x (правильная работа светодиодной ленты и сервы) + ### [Блочное программирование](blocks.md) * Корректная работа функционала блочного программирования From 4c576ba5d496136b07f52614331e6a2deaabba7f Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 22 Feb 2023 00:42:06 +0300 Subject: [PATCH 11/21] builder: print largest installed packages --- builder/image-validate.sh | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/builder/image-validate.sh b/builder/image-validate.sh index c4c8a45b..cf11647c 100755 --- a/builder/image-validate.sh +++ b/builder/image-validate.sh @@ -37,3 +37,7 @@ apt-cache show openvpn echo "Move /etc/ld.so.preload back to its original position" mv /etc/ld.so.preload.disabled-for-build /etc/ld.so.preload + +echo "Largest packages installed" +sudo -E sh -c 'apt-get install -y debian-goodies' +dpigs -H -n 100 From ae05710a3768151f3d16d5cd4089652fac7f77f7 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 27 Mar 2023 19:18:21 +0300 Subject: [PATCH 12/21] blocks: fix set_yaw block implementation (#487) --- clover_blocks/www/python.js | 3 +++ 1 file changed, 3 insertions(+) diff --git a/clover_blocks/www/python.js b/clover_blocks/www/python.js index 42de72a2..a614e5e3 100644 --- a/clover_blocks/www/python.js +++ b/clover_blocks/www/python.js @@ -83,6 +83,9 @@ function generateROSDefinitions() { if (rosDefinitions.navigateGlobal) { code += `navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)\n`; } + if (rosDefinitions.setYaw) { + code += `set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw)\n`; + } if (rosDefinitions.setVelocity) { code += `set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)\n`; } From 68903373b0a9c8a8de251a6689c9ff8daee2281e Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 11 Apr 2023 00:57:43 +0300 Subject: [PATCH 13/21] simple_offboard: reset stored setpoint on auto_arm only if needed to be armed CopterExpress/clover_vm#13 --- clover/src/simple_offboard.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/clover/src/simple_offboard.cpp b/clover/src/simple_offboard.cpp index 0781d78d..dab4a122 100644 --- a/clover/src/simple_offboard.cpp +++ b/clover/src/simple_offboard.cpp @@ -800,7 +800,8 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl nav_from_sp_flag = false; } - if (auto_arm || setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) { + bool to_auto_arm = auto_arm && (state.mode != "OFFBOARD" || !state.armed); + if (to_auto_arm || setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) { // invalidate position setpoint setpoint_position.header.frame_id = ""; setpoint_altitude.header.frame_id = ""; From 5e9f442996153d54864c440e00667834ce30df6a Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 11 Apr 2023 01:28:38 +0300 Subject: [PATCH 14/21] simple_offboard: add `terrain_frame_mode` parameter, CopterExpress/clover_vm#14 `altitude` mode takes the current altitude from the estimator `range` mode takes the current altitude from a simple range topic --- clover/src/simple_offboard.cpp | 38 ++++++++++++++++++++++++++-------- clover/test/offboard.py | 29 +++++++++++++++++++++++++- 2 files changed, 57 insertions(+), 10 deletions(-) diff --git a/clover/src/simple_offboard.cpp b/clover/src/simple_offboard.cpp index dab4a122..6aeecb65 100644 --- a/clover/src/simple_offboard.cpp +++ b/clover/src/simple_offboard.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -86,6 +87,7 @@ float default_speed; bool auto_release; bool land_only_in_offboard, nav_from_sp, check_kill_switch; std::map reference_frames; +string terrain_frame_mode; // Publishers ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub, state_pub; @@ -205,18 +207,27 @@ inline bool waitTransform(const string& target, const string& source, return false; } +void publishTerrain(const double distance, const ros::Time& stamp) +{ + if (!waitTransform(local_frame, body.child_frame_id, stamp, ros::Duration(0.1))) return; + + auto t = tf_buffer.lookupTransform(local_frame, body.child_frame_id, stamp); + t.child_frame_id = terrain.child_frame_id; + t.transform.translation.z -= distance; + static_transform_broadcaster->sendTransform(t); +} + void handleAltitude(const Altitude& alt) { - // publish terrain frame if (!std::isfinite(alt.bottom_clearance)) return; - // terrain.header.stamp = alt.header.stamp; + publishTerrain(alt.bottom_clearance, alt.header.stamp); +} - if (!waitTransform(local_frame, body.child_frame_id, alt.header.stamp, ros::Duration(0.1))) return; - - auto t = tf_buffer.lookupTransform(local_frame, body.child_frame_id, alt.header.stamp); - t.child_frame_id = terrain.child_frame_id; - t.transform.translation.z -= alt.bottom_clearance; - static_transform_broadcaster->sendTransform(t); +void handleRange(const Range& range) +{ + if (!std::isfinite(range.range)) return; + // TODO: check it's facing down + publishTerrain(range.range, range.header.stamp); } #define TIMEOUT(msg, timeout) (msg.header.stamp.isZero() || (ros::Time::now() - msg.header.stamp > timeout)) @@ -1101,6 +1112,7 @@ int main(int argc, char **argv) nh_priv.param("default_speed", default_speed, 0.5f); nh_priv.param("body_frame", body.child_frame_id, "body"); nh_priv.param("terrain_frame", terrain.child_frame_id, "terrain"); + nh_priv.param("terrain_frame_mode", terrain_frame_mode, "altitude"); nh_priv.getParam("reference_frames", reference_frames); // Default reference frames @@ -1139,7 +1151,15 @@ int main(int argc, char **argv) ros::Subscriber altitude_sub; if (!body.child_frame_id.empty() && !terrain.child_frame_id.empty()) { terrain.header.frame_id = local_frame; - altitude_sub = nh.subscribe(mavros + "/altitude", 1, &handleAltitude); + if (terrain_frame_mode == "altitude") { + altitude_sub = nh.subscribe(mavros + "/altitude", 1, &handleAltitude); + } else if (terrain_frame_mode == "range") { + string range_topic = nh_priv.param("range_topic", string("rangefinder/range")); + altitude_sub = nh.subscribe(range_topic, 1, &handleRange); + } else { + ROS_FATAL("Unknown terrain_frame_mode: %s, valid values: altitude, range", terrain_frame_mode.c_str()); + ros::shutdown(); + } } // Setpoint publishers diff --git a/clover/test/offboard.py b/clover/test/offboard.py index 426fb453..bfa51314 100755 --- a/clover/test/offboard.py +++ b/clover/test/offboard.py @@ -3,6 +3,7 @@ import pytest from pytest import approx import threading import mavros_msgs.msg +from mavros_msgs.srv import SetMode from geometry_msgs.msg import PoseStamped from clover import srv from clover.msg import State @@ -45,6 +46,7 @@ def test_offboard(node, tf_buffer): telem = get_telemetry() assert telem.connected == False + # mocked state publisher state_pub = rospy.Publisher('/mavros/state', mavros_msgs.msg.State, latch=True, queue_size=1) state_msg = mavros_msgs.msg.State(mode='OFFBOARD', armed=True) @@ -59,6 +61,13 @@ def test_offboard(node, tf_buffer): threading.Thread(target=publish_state, daemon=True).start() rospy.sleep(0.5) + # set_mode service mock + def set_mode(req): + state_msg.mode = req.custom_mode # set mocked mode to requested + return True, + + rospy.Service('/mavros/set_mode', SetMode, set_mode) + telem = get_telemetry() assert telem.connected == False @@ -157,7 +166,23 @@ def test_offboard(node, tf_buffer): assert state.z_frame_id == 'map' assert state.yaw_frame_id == 'test' - # auto_arm should invalidate the setpoint + # auto_arm should not invalidate the setpoint if not effective + res = navigate(x=nan, y=nan, z=1, frame_id='map', auto_arm=True) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 1 + assert state.y == 2 + assert state.z == 1 + assert state.yaw == 0 + assert state.xy_frame_id == 'test' + assert state.z_frame_id == 'map' + assert state.yaw_frame_id == 'map' + + # auto_arm should invalidate the setpoint if effective + state_msg.mode = 'STABILIZED' # pretend we are not in OFFBOARD mode + rospy.sleep(1) res = navigate(x=nan, y=nan, z=1, frame_id='map', auto_arm=True) assert res.success == True state = get_state() @@ -170,6 +195,8 @@ def test_offboard(node, tf_buffer): assert state.xy_frame_id == 'map' assert state.z_frame_id == 'map' assert state.yaw_frame_id == 'map' + state_msg.mode = 'OFFBOARD' + rospy.sleep(1) # set_attitude should invalidate the setpoint res = set_attitude() From 19fde7095f0ca48f401d0b9eca00a39878bf7001 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 11 Apr 2023 01:40:58 +0300 Subject: [PATCH 15/21] simple_offboard: add test for land service --- clover/test/offboard.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/clover/test/offboard.py b/clover/test/offboard.py index bfa51314..02f69e5a 100755 --- a/clover/test/offboard.py +++ b/clover/test/offboard.py @@ -7,6 +7,7 @@ from mavros_msgs.srv import SetMode from geometry_msgs.msg import PoseStamped from clover import srv from clover.msg import State +from std_srvs.srv import Trigger from math import nan, inf import tf2_ros import tf2_geometry_msgs @@ -39,6 +40,8 @@ def test_offboard(node, tf_buffer): set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude) set_rates = rospy.ServiceProxy('set_rates', srv.SetRates) get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) + land = rospy.ServiceProxy('land', Trigger) + res = navigate() assert res.success == False assert res.message.startswith('State timeout') @@ -427,3 +430,8 @@ def test_offboard(node, tf_buffer): res = set_rates(roll_rate=inf) assert res.success == False assert res.message == 'roll_rate argument cannot be Inf' + + # test land service + res = land() + assert res.success == True + assert state_msg.mode == 'AUTO.LAND' # check that the mode was set correctly From 808726b4b70c52258c42ac139fac25534e769bf3 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 11 Apr 2023 13:33:35 +0300 Subject: [PATCH 16/21] Try to tix the build --- builder/assets/noetic-rosdep-clover.yaml | 720 +++++++++++++++++++++++ builder/image-ros.sh | 2 +- 2 files changed, 721 insertions(+), 1 deletion(-) diff --git a/builder/assets/noetic-rosdep-clover.yaml b/builder/assets/noetic-rosdep-clover.yaml index a595b87e..93aa3508 100644 --- a/builder/assets/noetic-rosdep-clover.yaml +++ b/builder/assets/noetic-rosdep-clover.yaml @@ -16,3 +16,723 @@ web_video_server: ws281x: debian: buster: [ros-noetic-ws281x] +catkin: + debian: + buster: [ros-noetic-catkin] +genmsg: + debian: + buster: [ros-noetic-genmsg] +gencpp: + debian: + buster: [ros-noetic-gencpp] +geneus: + debian: + buster: [ros-noetic-geneus] +genlisp: + debian: + buster: [ros-noetic-genlisp] +gennodejs: + debian: + buster: [ros-noetic-gennodejs] +genpy: + debian: + buster: [ros-noetic-genpy] +bond_core: + debian: + buster: [ros-noetic-bond-core] +cmake_modules: + debian: + buster: [ros-noetic-cmake-modules] +class_loader: + debian: + buster: [ros-noetic-class-loader] +common_msgs: + debian: + buster: [ros-noetic-common-msgs] +common_tutorials: + debian: + buster: [ros-noetic-common-tutorials] +cpp_common: + debian: + buster: [ros-noetic-cpp-common] +desktop: + debian: + buster: [ros-noetic-desktop] +diagnostics: + debian: + buster: [ros-noetic-diagnostics] +executive_smach: + debian: + buster: [ros-noetic-executive-smach] +geometry: + debian: + buster: [ros-noetic-geometry] +geometry_tutorials: + debian: + buster: [ros-noetic-geometry-tutorials] +gl_dependency: + debian: + buster: [ros-noetic-gl-dependency] +image_common: + debian: + buster: [ros-noetic-image-common] +image_pipeline: + debian: + buster: [ros-noetic-image-pipeline] +image_transport_plugins: + debian: + buster: [ros-noetic-image-transport-plugins] +laser_pipeline: + debian: + buster: [ros-noetic-laser-pipeline] +mavlink: + debian: + buster: [ros-noetic-mavlink] +media_export: + debian: + buster: [ros-noetic-media-export] +message_generation: + debian: + buster: [ros-noetic-message-generation] +message_runtime: + debian: + buster: [ros-noetic-message-runtime] +mk: + debian: + buster: [ros-noetic-mk] +nodelet_core: + debian: + buster: [ros-noetic-nodelet-core] +orocos_kdl: + debian: + buster: [ros-noetic-orocos-kdl] +perception: + debian: + buster: [ros-noetic-perception] +perception_pcl: + debian: + buster: [ros-noetic-perception-pcl] +python_orocos_kdl: + debian: + buster: [ros-noetic-python-orocos-kdl] +qt_dotgraph: + debian: + buster: [ros-noetic-qt-dotgraph] +qt_gui: + debian: + buster: [ros-noetic-qt-gui] +qt_gui_py_common: + debian: + buster: [ros-noetic-qt-gui-py-common] +qwt_dependency: + debian: + buster: [ros-noetic-qwt-dependency] +robot: + debian: + buster: [ros-noetic-robot] +ros: + debian: + buster: [ros-noetic-ros] +ros_base: + debian: + buster: [ros-noetic-ros-base] +ros_comm: + debian: + buster: [ros-noetic-ros-comm] +ros_core: + debian: + buster: [ros-noetic-ros-core] +ros_environment: + debian: + buster: [ros-noetic-ros-environment] +ros_tutorials: + debian: + buster: [ros-noetic-ros-tutorials] +rosapi: + debian: + buster: [ros-noetic-rosapi] +rosbag_migration_rule: + debian: + buster: [ros-noetic-rosbag-migration-rule] +rosbash: + debian: + buster: [ros-noetic-rosbash] +rosboost_cfg: + debian: + buster: [ros-noetic-rosboost-cfg] +rosbridge_server: + debian: + buster: [ros-noetic-rosbridge-server] +rosbridge_suite: + debian: + buster: [ros-noetic-rosbridge-suite] +rosbuild: + debian: + buster: [ros-noetic-rosbuild] +rosclean: + debian: + buster: [ros-noetic-rosclean] +roscpp_core: + debian: + buster: [ros-noetic-roscpp-core] +roscpp_traits: + debian: + buster: [ros-noetic-roscpp-traits] +roscreate: + debian: + buster: [ros-noetic-roscreate] +rosgraph: + debian: + buster: [ros-noetic-rosgraph] +roslang: + debian: + buster: [ros-noetic-roslang] +roslint: + debian: + buster: [ros-noetic-roslint] +roslisp: + debian: + buster: [ros-noetic-roslisp] +rosmake: + debian: + buster: [ros-noetic-rosmake] +rosmaster: + debian: + buster: [ros-noetic-rosmaster] +rospack: + debian: + buster: [ros-noetic-rospack] +roslib: + debian: + buster: [ros-noetic-roslib] +rosparam: + debian: + buster: [ros-noetic-rosparam] +rospy: + debian: + buster: [ros-noetic-rospy] +rosserial: + debian: + buster: [ros-noetic-rosserial] +rosserial_msgs: + debian: + buster: [ros-noetic-rosserial-msgs] +rosserial_python: + debian: + buster: [ros-noetic-rosserial-python] +rosservice: + debian: + buster: [ros-noetic-rosservice] +rostime: + debian: + buster: [ros-noetic-rostime] +roscpp_serialization: + debian: + buster: [ros-noetic-roscpp-serialization] +python_qt_binding: + debian: + buster: [ros-noetic-python-qt-binding] +roslaunch: + debian: + buster: [ros-noetic-roslaunch] +rosunit: + debian: + buster: [ros-noetic-rosunit] +angles: + debian: + buster: [ros-noetic-angles] +libmavconn: + debian: + buster: [ros-noetic-libmavconn] +rosconsole: + debian: + buster: [ros-noetic-rosconsole] +pluginlib: + debian: + buster: [ros-noetic-pluginlib] +qt_gui_cpp: + debian: + buster: [ros-noetic-qt-gui-cpp] +resource_retriever: + debian: + buster: [ros-noetic-resource-retriever] +rosconsole_bridge: + debian: + buster: [ros-noetic-rosconsole-bridge] +roslz4: + debian: + buster: [ros-noetic-roslz4] +rosserial_client: + debian: + buster: [ros-noetic-rosserial-client] +rostest: + debian: + buster: [ros-noetic-rostest] +rqt_action: + debian: + buster: [ros-noetic-rqt-action] +rqt_bag: + debian: + buster: [ros-noetic-rqt-bag] +rqt_bag_plugins: + debian: + buster: [ros-noetic-rqt-bag-plugins] +rqt_common_plugins: + debian: + buster: [ros-noetic-rqt-common-plugins] +rqt_console: + debian: + buster: [ros-noetic-rqt-console] +rqt_dep: + debian: + buster: [ros-noetic-rqt-dep] +rqt_graph: + debian: + buster: [ros-noetic-rqt-graph] +rqt_gui: + debian: + buster: [ros-noetic-rqt-gui] +rqt_logger_level: + debian: + buster: [ros-noetic-rqt-logger-level] +rqt_moveit: + debian: + buster: [ros-noetic-rqt-moveit] +rqt_msg: + debian: + buster: [ros-noetic-rqt-msg] +rqt_nav_view: + debian: + buster: [ros-noetic-rqt-nav-view] +rqt_plot: + debian: + buster: [ros-noetic-rqt-plot] +rqt_pose_view: + debian: + buster: [ros-noetic-rqt-pose-view] +rqt_publisher: + debian: + buster: [ros-noetic-rqt-publisher] +rqt_py_console: + debian: + buster: [ros-noetic-rqt-py-console] +rqt_reconfigure: + debian: + buster: [ros-noetic-rqt-reconfigure] +rqt_robot_dashboard: + debian: + buster: [ros-noetic-rqt-robot-dashboard] +rqt_robot_monitor: + debian: + buster: [ros-noetic-rqt-robot-monitor] +rqt_robot_plugins: + debian: + buster: [ros-noetic-rqt-robot-plugins] +rqt_robot_steering: + debian: + buster: [ros-noetic-rqt-robot-steering] +rqt_runtime_monitor: + debian: + buster: [ros-noetic-rqt-runtime-monitor] +rqt_service_caller: + debian: + buster: [ros-noetic-rqt-service-caller] +rqt_shell: + debian: + buster: [ros-noetic-rqt-shell] +rqt_srv: + debian: + buster: [ros-noetic-rqt-srv] +rqt_tf_tree: + debian: + buster: [ros-noetic-rqt-tf-tree] +rqt_top: + debian: + buster: [ros-noetic-rqt-top] +rqt_topic: + debian: + buster: [ros-noetic-rqt-topic] +rqt_web: + debian: + buster: [ros-noetic-rqt-web] +smach: + debian: + buster: [ros-noetic-smach] +smclib: + debian: + buster: [ros-noetic-smclib] +std_msgs: + debian: + buster: [ros-noetic-std-msgs] +actionlib_msgs: + debian: + buster: [ros-noetic-actionlib-msgs] +bond: + debian: + buster: [ros-noetic-bond] +diagnostic_msgs: + debian: + buster: [ros-noetic-diagnostic-msgs] +geometry_msgs: + debian: + buster: [ros-noetic-geometry-msgs] +eigen_conversions: + debian: + buster: [ros-noetic-eigen-conversions] +kdl_conversions: + debian: + buster: [ros-noetic-kdl-conversions] +nav_msgs: + debian: + buster: [ros-noetic-nav-msgs] +rosbridge_msgs: + debian: + buster: [ros-noetic-rosbridge-msgs] +rosgraph_msgs: + debian: + buster: [ros-noetic-rosgraph-msgs] +rosmsg: + debian: + buster: [ros-noetic-rosmsg] +rqt_py_common: + debian: + buster: [ros-noetic-rqt-py-common] +shape_msgs: + debian: + buster: [ros-noetic-shape-msgs] +smach_msgs: + debian: + buster: [ros-noetic-smach-msgs] +std_srvs: + debian: + buster: [ros-noetic-std-srvs] +tf2_msgs: + debian: + buster: [ros-noetic-tf2-msgs] +tf2: + debian: + buster: [ros-noetic-tf2] +tf2_eigen: + debian: + buster: [ros-noetic-tf2-eigen] +trajectory_msgs: + debian: + buster: [ros-noetic-trajectory-msgs] +control_msgs: + debian: + buster: [ros-noetic-control-msgs] +urdf_parser_plugin: + debian: + buster: [ros-noetic-urdf-parser-plugin] +urdfdom_py: + debian: + buster: [ros-noetic-urdfdom-py] +uuid_msgs: + debian: + buster: [ros-noetic-uuid-msgs] +geographic_msgs: + debian: + buster: [ros-noetic-geographic-msgs] +vision_opencv: + debian: + buster: [ros-noetic-vision-opencv] +visualization_msgs: + debian: + buster: [ros-noetic-visualization-msgs] +visualization_tutorials: + debian: + buster: [ros-noetic-visualization-tutorials] +viz: + debian: + buster: [ros-noetic-viz] +webkit_dependency: + debian: + buster: [ros-noetic-webkit-dependency] +xmlrpcpp: + debian: + buster: [ros-noetic-xmlrpcpp] +roscpp: + debian: + buster: [ros-noetic-roscpp] +bondcpp: + debian: + buster: [ros-noetic-bondcpp] +bondpy: + debian: + buster: [ros-noetic-bondpy] +nodelet: + debian: + buster: [ros-noetic-nodelet] +nodelet_tutorial_math: + debian: + buster: [ros-noetic-nodelet-tutorial-math] +pluginlib_tutorials: + debian: + buster: [ros-noetic-pluginlib-tutorials] +roscpp_tutorials: + debian: + buster: [ros-noetic-roscpp-tutorials] +rosout: + debian: + buster: [ros-noetic-rosout] +camera_calibration: + debian: + buster: [ros-noetic-camera-calibration] +diagnostic_aggregator: + debian: + buster: [ros-noetic-diagnostic-aggregator] +diagnostic_updater: + debian: + buster: [ros-noetic-diagnostic-updater] +diagnostic_common_diagnostics: + debian: + buster: [ros-noetic-diagnostic-common-diagnostics] +dynamic_reconfigure: + debian: + buster: [ros-noetic-dynamic-reconfigure] +filters: + debian: + buster: [ros-noetic-filters] +joint_state_publisher: + debian: + buster: [ros-noetic-joint-state-publisher] +message_filters: + debian: + buster: [ros-noetic-message-filters] +rosauth: + debian: + buster: [ros-noetic-rosauth] +rosbag_storage: + debian: + buster: [ros-noetic-rosbag-storage] +rosnode: + debian: + buster: [ros-noetic-rosnode] +rospy_tutorials: + debian: + buster: [ros-noetic-rospy-tutorials] +rosshow: + debian: + buster: [ros-noetic-rosshow] +rostopic: + debian: + buster: [ros-noetic-rostopic] +rqt_gui_cpp: + debian: + buster: [ros-noetic-rqt-gui-cpp] +rqt_gui_py: + debian: + buster: [ros-noetic-rqt-gui-py] +self_test: + debian: + buster: [ros-noetic-self-test] +smach_ros: + debian: + buster: [ros-noetic-smach-ros] +tf2_py: + debian: + buster: [ros-noetic-tf2-py] +topic_tools: + debian: + buster: [ros-noetic-topic-tools] +rosbag: + debian: + buster: [ros-noetic-rosbag] +actionlib: + debian: + buster: [ros-noetic-actionlib] +actionlib_tutorials: + debian: + buster: [ros-noetic-actionlib-tutorials] +diagnostic_analysis: + debian: + buster: [ros-noetic-diagnostic-analysis] +nodelet_topic_tools: + debian: + buster: [ros-noetic-nodelet-topic-tools] +roswtf: + debian: + buster: [ros-noetic-roswtf] +rqt_launch: + debian: + buster: [ros-noetic-rqt-launch] +sensor_msgs: + debian: + buster: [ros-noetic-sensor-msgs] +camera_calibration_parsers: + debian: + buster: [ros-noetic-camera-calibration-parsers] +cv_bridge: + debian: + buster: [ros-noetic-cv-bridge] +image_geometry: + debian: + buster: [ros-noetic-image-geometry] +image_transport: + debian: + buster: [ros-noetic-image-transport] +camera_info_manager: + debian: + buster: [ros-noetic-camera-info-manager] +compressed_depth_image_transport: + debian: + buster: [ros-noetic-compressed-depth-image-transport] +compressed_image_transport: + debian: + buster: [ros-noetic-compressed-image-transport] +cv_camera: + debian: + buster: [ros-noetic-cv-camera] +image_proc: + debian: + buster: [ros-noetic-image-proc] +image_publisher: + debian: + buster: [ros-noetic-image-publisher] +map_msgs: + debian: + buster: [ros-noetic-map-msgs] +mavros_msgs: + debian: + buster: [ros-noetic-mavros-msgs] +pcl_msgs: + debian: + buster: [ros-noetic-pcl-msgs] +pcl_conversions: + debian: + buster: [ros-noetic-pcl-conversions] +polled_camera: + debian: + buster: [ros-noetic-polled-camera] +rqt_image_view: + debian: + buster: [ros-noetic-rqt-image-view] +stereo_msgs: + debian: + buster: [ros-noetic-stereo-msgs] +image_view: + debian: + buster: [ros-noetic-image-view] +rosbridge_library: + debian: + buster: [ros-noetic-rosbridge-library] +stereo_image_proc: + debian: + buster: [ros-noetic-stereo-image-proc] +tf2_ros: + debian: + buster: [ros-noetic-tf2-ros] +depth_image_proc: + debian: + buster: [ros-noetic-depth-image-proc] +mavros: + debian: + buster: [ros-noetic-mavros] +tf: + debian: + buster: [ros-noetic-tf] +interactive_markers: + debian: + buster: [ros-noetic-interactive-markers] +interactive_marker_tutorials: + debian: + buster: [ros-noetic-interactive-marker-tutorials] +laser_geometry: + debian: + buster: [ros-noetic-laser-geometry] +laser_assembler: + debian: + buster: [ros-noetic-laser-assembler] +laser_filters: + debian: + buster: [ros-noetic-laser-filters] +pcl_ros: + debian: + buster: [ros-noetic-pcl-ros] +tf2_geometry_msgs: + debian: + buster: [ros-noetic-tf2-geometry-msgs] +image_rotate: + debian: + buster: [ros-noetic-image-rotate] +tf2_kdl: + debian: + buster: [ros-noetic-tf2-kdl] +tf2_web_republisher: + debian: + buster: [ros-noetic-tf2-web-republisher] +tf_conversions: + debian: + buster: [ros-noetic-tf-conversions] +theora_image_transport: + debian: + buster: [ros-noetic-theora-image-transport] +turtlesim: + debian: + buster: [ros-noetic-turtlesim] +turtle_actionlib: + debian: + buster: [ros-noetic-turtle-actionlib] +turtle_tf: + debian: + buster: [ros-noetic-turtle-tf] +turtle_tf2: + debian: + buster: [ros-noetic-turtle-tf2] +urdf: + debian: + buster: [ros-noetic-urdf] +kdl_parser: + debian: + buster: [ros-noetic-kdl-parser] +kdl_parser_py: + debian: + buster: [ros-noetic-kdl-parser-py] +mavros_extras: + debian: + buster: [ros-noetic-mavros-extras] +robot_state_publisher: + debian: + buster: [ros-noetic-robot-state-publisher] +rviz: + debian: + buster: [ros-noetic-rviz] +librviz_tutorial: + debian: + buster: [ros-noetic-librviz-tutorial] +rqt_rviz: + debian: + buster: [ros-noetic-rqt-rviz] +rviz_plugin_tutorials: + debian: + buster: [ros-noetic-rviz-plugin-tutorials] +rviz_python_tutorial: + debian: + buster: [ros-noetic-rviz-python-tutorial] +urdf_tutorial: + debian: + buster: [ros-noetic-urdf-tutorial] +usb_cam: + debian: + buster: [ros-noetic-usb-cam] +visualization_marker_tutorials: + debian: + buster: [ros-noetic-visualization-marker-tutorials] +vl53l1x: + debian: + buster: [ros-noetic-vl53l1x] +xacro: + debian: + buster: [ros-noetic-xacro] +ddynamic_reconfigure: + debian: + buster: [ros-noetic-ddynamic-reconfigure] +librealsense2: + debian: + buster: [ros-noetic-librealsense2] +realsense2_camera: + debian: + buster: [ros-noetic-realsense2-camera] +realsense2_description: + debian: + buster: [ros-noetic-realsense2-description] diff --git a/builder/image-ros.sh b/builder/image-ros.sh index 2dd192b6..93a1b2be 100755 --- a/builder/image-ros.sh +++ b/builder/image-ros.sh @@ -49,7 +49,7 @@ echo_stamp() { my_travis_retry() { local result=0 local count=1 - local max_count=50 + local max_count=5 while [ $count -le $max_count ]; do [ $result -ne 0 ] && { echo -e "\nThe command \"$@\" failed. Retrying, $count of $max_count.\n" >&2 From c82490a0c1ffb55a73801c28357ce0fcc4c27358 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 11 Apr 2023 15:27:39 +0300 Subject: [PATCH 17/21] Set terrain_frame_mode to range by default addressing CopterExpress/clover_vm#14 --- clover/launch/clover.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/clover/launch/clover.launch b/clover/launch/clover.launch index c78297d6..613f16ed 100644 --- a/clover/launch/clover.launch +++ b/clover/launch/clover.launch @@ -51,6 +51,7 @@ + From cac6b59a569d5fcdc7f75cab3e6cb9208dd79cb5 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 11 Apr 2023 15:28:38 +0300 Subject: [PATCH 18/21] Set the version to 0.24.0 in ROS packages --- aruco_pose/package.xml | 2 +- clover/package.xml | 2 +- clover_blocks/package.xml | 2 +- clover_description/package.xml | 2 +- clover_simulation/package.xml | 2 +- roswww_static/package.xml | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/aruco_pose/package.xml b/aruco_pose/package.xml index a29c85b2..8ee135c5 100644 --- a/aruco_pose/package.xml +++ b/aruco_pose/package.xml @@ -1,7 +1,7 @@ aruco_pose - 0.23.0 + 0.24.0 Positioning with ArUco markers Oleg Kalachev diff --git a/clover/package.xml b/clover/package.xml index 7ee4c683..98bfbfb3 100644 --- a/clover/package.xml +++ b/clover/package.xml @@ -1,7 +1,7 @@ clover - 0.23.0 + 0.24.0 The Clover package Oleg Kalachev diff --git a/clover_blocks/package.xml b/clover_blocks/package.xml index 384d75f9..0423c456 100644 --- a/clover_blocks/package.xml +++ b/clover_blocks/package.xml @@ -1,7 +1,7 @@ clover_blocks - 0.23.0 + 0.24.0 Blockly programming support for Clover Oleg Kalachev MIT diff --git a/clover_description/package.xml b/clover_description/package.xml index 1e5db705..b5defff3 100644 --- a/clover_description/package.xml +++ b/clover_description/package.xml @@ -1,6 +1,6 @@ clover_description - 0.23.0 + 0.24.0 The clover_description package provides URDF models of the Clover series of quadcopters. Alexey Rogachevskiy diff --git a/clover_simulation/package.xml b/clover_simulation/package.xml index 7ee6983f..65885cc4 100644 --- a/clover_simulation/package.xml +++ b/clover_simulation/package.xml @@ -1,6 +1,6 @@ clover_simulation - 0.23.0 + 0.24.0 The clover_simulation package provides worlds and launch files for Gazebo. Oleg Kalachev diff --git a/roswww_static/package.xml b/roswww_static/package.xml index b98dba6a..09f00159 100644 --- a/roswww_static/package.xml +++ b/roswww_static/package.xml @@ -1,7 +1,7 @@ roswww_static - 0.23.0 + 0.24.0 Static web pages for ROS packages Oleg Kalachev MIT From 28ddbbcdf9ef0469e8c23948be927db20ea448cf Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 11 Apr 2023 19:21:53 +0300 Subject: [PATCH 19/21] docs: add version warnings to camera articles --- docs/en/camera.md | 2 ++ docs/ru/camera.md | 2 ++ 2 files changed, 4 insertions(+) diff --git a/docs/en/camera.md b/docs/en/camera.md index a4079a45..7b22825d 100644 --- a/docs/en/camera.md +++ b/docs/en/camera.md @@ -1,5 +1,7 @@ # Working with the camera +> **Note** The following applies to [image versions](image.md) **0.24** and up. Older documentation is still available for [for version **0.23**](https://github.com/CopterExpress/clover/blob/v0.23/docs/en/camera.md). + Make sure the camera is enabled in the `~/catkin_ws/src/clover/clover/launch/clover.launch` file: ```xml diff --git a/docs/ru/camera.md b/docs/ru/camera.md index ea4082c6..96c6193f 100644 --- a/docs/ru/camera.md +++ b/docs/ru/camera.md @@ -1,5 +1,7 @@ # Работа с камерой +> **Note** Документация для версий [образа](image.md), начиная с версии **0.24**. Для более ранних версий см. [документацию для версии **0.23**](https://github.com/CopterExpress/clover/blob/v0.23/docs/ru/camera.md). + Для работы с основной камерой необходимо убедиться что она включена в файле `~/catkin_ws/src/clover/clover/launch/clover.launch`: From cbba62d16599a511453b9c5907ac9a3d4bbb94c8 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 11 Apr 2023 19:35:00 +0300 Subject: [PATCH 20/21] blocks: add block for reading RC values --- clover_blocks/www/blocks.js | 13 +++++++++++++ clover_blocks/www/index.html | 3 +++ clover_blocks/www/python.js | 6 ++++++ 3 files changed, 22 insertions(+) diff --git a/clover_blocks/www/blocks.js b/clover_blocks/www/blocks.js index f5062b3d..1ddb528f 100644 --- a/clover_blocks/www/blocks.js +++ b/clover_blocks/www/blocks.js @@ -269,6 +269,19 @@ Blockly.Blocks['voltage'] = { } }; +Blockly.Blocks['get_rc'] = { + init: function () { + this.appendDummyInput() + .appendField("RC channel") + this.appendValueInput("CHANNEL") + .setCheck("Number"); + this.setInputsInline(true); + this.setOutput(true, "Number"); + this.setColour(COLOR_STATE); + this.setTooltip("Returns current RC channel value."); + this.setHelpUrl(DOCS_URL + '#' + this.type); + } +} Blockly.Blocks['armed'] = { init: function () { diff --git a/clover_blocks/www/index.html b/clover_blocks/www/index.html index fcd4060f..dfc85186 100644 --- a/clover_blocks/www/index.html +++ b/clover_blocks/www/index.html @@ -100,6 +100,9 @@ + + 0 + diff --git a/clover_blocks/www/python.js b/clover_blocks/www/python.js index a614e5e3..bc0ac0ba 100644 --- a/clover_blocks/www/python.js +++ b/clover_blocks/www/python.js @@ -402,6 +402,12 @@ Blockly.Python.voltage = function(block) { return [code, Blockly.Python.ORDER_FUNCTION_CALL]; } +Blockly.Python.get_rc = function(block) { + Blockly.Python.definitions_['import_rcin'] = 'from mavros_msgs.msg import RCIn'; + var channel = Blockly.Python.valueToCode(block, 'CHANNEL', Blockly.Python.ORDER_NONE); + return [`rospy.wait_for_message('mavros/rc/in', RCIn).channels[${channel}]`, Blockly.Python.ORDER_FUNCTION_CALL] +} + function parseColor(color) { return { r: parseInt(color.substr(2, 2), 16), From d6101dc0a3a875faaec5620a7357b1302092f705 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 12 Apr 2023 01:34:34 +0300 Subject: [PATCH 21/21] ci: add secret variable to temporarily freeze updating docs website --- .github/workflows/docs.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index e819c4e3..ec0402ab 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -81,5 +81,8 @@ jobs: needs: docs steps: - name: Deploy to GitHub Pages + env: + FREEZE_DOCS: ${{ secrets.FREEZE_DOCS }} + if: ${{ !env.FREEZE_DOCS }} id: deployment uses: actions/deploy-pages@v1