mirror of
https://github.com/CopterExpress/clover.git
synced 2026-06-03 08:29:32 +00:00
Compare commits
58 Commits
v0.24-alph
...
simple-off
| Author | SHA1 | Date | |
|---|---|---|---|
|
50b9ad3ad8 | ||
|
|
b0021583c2 | ||
|
|
7cbd7e5d3f | ||
|
|
34dd5d6c60 | ||
|
|
87de227553 | ||
|
|
55b613da23 | ||
|
|
d66eda5cd2 | ||
|
|
90620d148f | ||
|
|
d73b627579 | ||
|
|
66d4839f81 | ||
|
|
5a98e1ab78 | ||
|
|
84b085352e | ||
|
|
da4c722a70 | ||
|
|
6474a61544 | ||
|
|
df6ecdb594 | ||
|
|
d4f1706e0c | ||
|
|
e2536395e7 | ||
|
|
56db20c87d | ||
|
|
0b6dc17c4b | ||
|
|
62e3954805 | ||
|
|
288667a08d | ||
|
|
26d65407fe | ||
|
|
5fa1f35e42 | ||
|
|
758fd204f6 | ||
|
|
f767ae0068 | ||
|
|
041560d971 | ||
|
|
794fa264b4 | ||
|
|
7e3ffc9626 | ||
|
|
323c1da325 | ||
|
|
c004165eb0 | ||
|
|
86ad467460 | ||
|
|
716e50ae02 | ||
|
|
629ad72f8f | ||
|
|
5d9a20497d | ||
|
|
b99eee81ad | ||
|
|
ffac7a721a | ||
|
|
22542dab2f | ||
|
|
00c35aff05 | ||
|
|
288bd3d06b | ||
|
|
7cd8f2501e | ||
|
|
b36b1de5e4 | ||
|
|
6899093c43 | ||
|
|
3ebb5faa2a | ||
|
|
a1c7976ee3 | ||
|
|
3c21c5ea71 | ||
|
|
2a74940140 | ||
|
|
68677fccdc | ||
|
|
051d094f37 | ||
|
|
db293545df | ||
|
|
325d1c317b | ||
|
|
7980773d95 | ||
|
|
aa595f799e | ||
|
|
6a0753b1d2 | ||
|
|
d1c39bb379 | ||
|
|
7afeff0633 | ||
|
|
201a20fe71 | ||
|
|
944cd28dba | ||
|
|
27f6836ca8 |
@@ -1,97 +0,0 @@
|
||||
# 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()
|
||||
@@ -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 3 # 0 = baro, 1 = gps, 2 = range, 3 = vision
|
||||
param set-default EKF2_HGT_MODE 2 # 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
|
||||
|
||||
@@ -1,16 +0,0 @@
|
||||
material red_circle
|
||||
{
|
||||
technique
|
||||
{
|
||||
pass
|
||||
{
|
||||
scene_blend alpha_blend
|
||||
texture_unit
|
||||
{
|
||||
texture red_circle.png
|
||||
filtering none
|
||||
scale 1.0 1.0
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 7.9 KiB |
@@ -1,13 +0,0 @@
|
||||
<?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>
|
||||
@@ -1,24 +0,0 @@
|
||||
<?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>
|
||||
@@ -1,7 +0,0 @@
|
||||
<?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>
|
||||
|
Before Width: | Height: | Size: 221 B |
@@ -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`|3 (*Vision*)|If the [rangefinder](laser.md) is present and flying over horizontal floor – 2 (*Range sensor*)|
|
||||
|`EKF2_HGT_MODE`|2 (*Range sensor*)|If the [rangefinder](laser.md) is present and flying over horizontal floor|
|
||||
|`EKF2_EVA_NOISE`|0.1||
|
||||
|`EKF2_EVP_NOISE`|0.1||
|
||||
|`EKF2_EV_DELAY`|0||
|
||||
|
||||
@@ -44,7 +44,7 @@
|
||||
|`EKF2_OF_QMIN`|10||
|
||||
|`EKF2_OF_N_MIN`|0.05||
|
||||
|`EKF2_OF_N_MAX`|0.2||
|
||||
|`EKF2_HGT_MODE`|3 (*Vision*)|При наличии [дальномера](laser.md) и полете над ровным полом — 2 (*Range sensor*)|
|
||||
|`EKF2_HGT_MODE`|2 (*Range sensor*)|При наличии [дальномера](laser.md) и полете над ровным полом|
|
||||
|`EKF2_EVA_NOISE`|0.1||
|
||||
|`EKF2_EVP_NOISE`|0.1||
|
||||
|`EKF2_EV_DELAY`|0||
|
||||
|
||||
@@ -65,7 +65,6 @@
|
||||
{ "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" },
|
||||
|
||||
Reference in New Issue
Block a user