docs: add redirect from /red_circle to camera article

* 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/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()

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

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
+			}
+		}
+	}
+}

clover_simulation/models/red_circle/model.config

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<model>
+    <name>Red Circle</name>
+    <version>1.0</version>
+    <sdf version="1.5">red_circle.sdf</sdf>
+    <author>
+        <name>Oleg Kalachev</name>
+        <email>okalachev@gmail.com</email>
+    </author>
+    <description>
+        Red circle of size 40 cm on the floor for recognizing by a drone
+    </description>
+</model>

clover_simulation/models/red_circle/red_circle.sdf

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<sdf version="1.5">
+    <model name="red_circle">
+        <static>true</static>
+        <link name="red_circle_link">
+            <pose>0 0 1e-3 0 0 0</pose>
+            <visual name="red_circle_texture">
+                <cast_shadows>false</cast_shadows>
+                <geometry>
+                    <box>
+                        <size>0.4 0.4 1e-3</size>
+                    </box>
+                </geometry>
+                <material>
+                    <script>
+                        <uri>model://red_circle/materials/scripts</uri>
+                        <uri>model://red_circle/materials/textures</uri>
+                        <name>red_circle</name>
+                    </script>
+                </material>
+            </visual>
+        </link>
+    </model>
+</sdf>

clover_simulation/models/red_circle/red_circle.svg

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<svg xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
+	<title>
+		red_circle
+	</title><g fill="red">
+	<circle cx="10.05" cy="10.05" r="9.9"/>
+</g></svg>

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||

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||

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" },