diff --git a/ros/launch/marker_follower.launch b/ros/launch/marker_follower.launch
index 5fb3595..f7ba14b 100644
--- a/ros/launch/marker_follower.launch
+++ b/ros/launch/marker_follower.launch
@@ -9,11 +9,13 @@
-
+
+
+
-
@@ -21,8 +23,13 @@
+
-
+
+
+
+
+
diff --git a/ros/launch/simple_solution.launch b/ros/launch/simple_solution.launch
index 1ce5dc1..8ecb7a6 100644
--- a/ros/launch/simple_solution.launch
+++ b/ros/launch/simple_solution.launch
@@ -27,4 +27,7 @@
+
+
+
diff --git a/ros/requirements.txt b/ros/requirements.txt
index e076f65..3a72c9a 100644
--- a/ros/requirements.txt
+++ b/ros/requirements.txt
@@ -1,4 +1,5 @@
pyyaml
-numpy
-opencv-python
-opencv-contrib-python
+numpy>=1.20
+opencv-python==4.5.5.64
+opencv-contrib-python==4.6.0.66
+mediapipe==0.9.1
diff --git a/ros/tello_ros/package.xml b/ros/tello_ros/package.xml
index db4c24b..8b452f9 100644
--- a/ros/tello_ros/package.xml
+++ b/ros/tello_ros/package.xml
@@ -9,6 +9,7 @@
rclpy
geometry_msgs
+ sensor_msgs
ament_copyright
ament_flake8
diff --git a/ros/tello_ros/setup.py b/ros/tello_ros/setup.py
index 5c08d0a..02845c8 100644
--- a/ros/tello_ros/setup.py
+++ b/ros/tello_ros/setup.py
@@ -24,7 +24,8 @@
'image_processing = tello_ros.image_processing:main',
'marker_follower = tello_ros.aruco_marker_follower:main',
'manager_node = tello_ros.manager_node:main',
- 'middleware_joy_node = tello_ros.middleware_joy_node:main'
+ 'middleware_joy_node = tello_ros.middleware_joy_node:main',
+ 'tello_joy_node = tello_ros.tello_joy_node:main',
],
},
)
diff --git a/ros/tello_ros/tello_ros/aruco_marker_follower.py b/ros/tello_ros/tello_ros/aruco_marker_follower.py
index d246c9c..7cd1bd5 100644
--- a/ros/tello_ros/tello_ros/aruco_marker_follower.py
+++ b/ros/tello_ros/tello_ros/aruco_marker_follower.py
@@ -3,11 +3,22 @@
import rclpy
import cv2
import numpy as np
+import mediapipe as mp
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from tello_msgs.msg import FlightData
from tello_ros.config import mtx, dist, MARKER_LEN, MARKER_NUMBER, ARUCO_DICT, ARUCO_PARAMS
from tello_ros.basic_tello_solution_node import BasicTelloSolutionNode
+from enum import Enum, auto
+
+
+class Action(Enum):
+ STOP = auto()
+ MOVE = auto()
+ FLIP = auto()
+ BACK = auto()
+ NONE = auto()
+
class ArUcoMarkerFollower(BasicTelloSolutionNode):
@@ -16,7 +27,15 @@ class ArUcoMarkerFollower(BasicTelloSolutionNode):
FONT_COLOR = (0, 255, 0)
FONT_STOKE = 2
LINE_STEP = 30
- TIMEOUT = 0.8
+ TIMEOUT = 0.05
+ EPS = 0.02
+ mp_drawing = mp.solutions.drawing_utils
+ mp_drawing_styles = mp.solutions.drawing_styles
+ mp_hands = mp.solutions.hands
+ hands = mp_hands.Hands(
+ static_image_mode=True,
+ max_num_hands=1,
+ min_detection_confidence=0.5)
def __init__(self):
super().__init__("aruco_marker_follower")
@@ -34,34 +53,41 @@ def __init__(self):
self.out_img_pub = self.create_publisher(Image, "output_images", 10)
self.last_marker_time = time.process_time()
self.draw_marker = True
- self.speed_coeff = 1.0
- self.height = 40
- self.distance = 100
+ self.speed_coeff = 0.3
+
+ self.height = 20
+ self.distance = 90
self.decrease_rotation_coeff = 2.0
self.rotation_speed = 90 / self.decrease_rotation_coeff
+ self.flip_timeout = 0.5
def process_image(self, msg):
- img = self.cv_bridge.imgmsg_to_cv2(msg, desired_encoding="rgb8")
- res = self.find_marker(img)
- if res is not None and self.draw_marker:
- img = cv2.aruco.drawDetectedMarkers(img, np.array([res[2]]), np.array([res[3]]))
- if self.active:
- if res is not None:
- self.fly_to_marker(res[0], res[1])
- self.last_marker_time = time.process_time()
- elif time.process_time() - self.last_marker_time > self.TIMEOUT:
- self.send_twist_command(0.0, 0.0, 0.0, 0.0)
- self.last_marker_time = time.process_time()
- self.write_status_on_image(img)
- new_img_msg = self.cv_bridge.cv2_to_imgmsg(img, encoding="rgb8")
- self.out_img_pub.publish(new_img_msg)
+ try:
+ img = self.cv_bridge.imgmsg_to_cv2(msg, desired_encoding="rgb8")
+ # res = self.find_marker(img)
+ res = self.find_hand(img)
+ if res is not None and self.draw_marker:
+ # img = cv2.aruco.drawDetectedMarkers(img, np.array([res[2]]), np.array([res[3]]))
+ img = self.draw_hand(img, res)
+ if self.active:
+ if res is not None:
+ # self.fly_to_marker(res[0], res[1])
+ self.action_from_hand(img, res)
+ self.last_marker_time = time.process_time()
+ elif time.process_time() - self.last_marker_time > self.TIMEOUT:
+ self.send_twist_command(0.0, 0.0, 0.0, 0.0)
+ self.last_marker_time = time.process_time()
+ self.write_status_on_image(img)
+ new_img_msg = self.cv_bridge.cv2_to_imgmsg(img, encoding="rgb8")
+ self.out_img_pub.publish(new_img_msg)
+ except Exception as e:
+ self.get_logger().info(f"Ошибка {e}")
def write_status_on_image(self, img):
pos = [0, self.LINE_STEP]
for key, val in self.cur_status.items():
cv2.putText(img, f"{key}: {val}", pos, self.FONT, self.FONT_SIZE, self.FONT_COLOR, self.FONT_STOKE)
pos[1] += self.LINE_STEP
-
def update_status(self, msg):
new_status = {
"battery(%)": msg.bat,
@@ -81,6 +107,114 @@ def detect_marker_on_image(self, image):
return (ids[i], corners[i])
return None
+ def find_hand(self, image):
+ image.flags.writeable = False
+ # image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
+ results = self.hands.process(image)
+
+ # Draw the hand annotations on the image.
+ image.flags.writeable = True
+ # image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
+ return results.multi_hand_landmarks
+
+ def check_ok(self, results):
+ for hand_landmarks in results:
+ landmark = hand_landmarks.landmark
+ v = np.array([landmark[4].x - landmark[8].x, landmark[4].y - landmark[8].y])
+ d = np.linalg.norm(v)
+ if d < self.EPS / 2:
+ self.send_tello_action_async("land")
+ return True
+ return False
+ return False
+
+
+ def action_from_hand(self, img, results):
+ if results:
+ for hand_landmarks in results:
+ # if self.check_ok(results):
+ # cv2.putText(img, f"Landing", (0, self.LINE_STEP * (len(self.cur_status) + 1)), self.FONT, self.FONT_SIZE, self.FONT_COLOR, self.FONT_STOKE)
+ # return
+ vector = self.get_fingers_vectors(hand_landmarks.landmark)
+ count = 0
+ for y in vector:
+ count += 1 if y < -self.EPS else 0
+ # if count >= 4:
+ # action = Action.STOP
+ # self.get_logger().info(f"Flying with speeds (0.0, 0.0, 0.0, 0.0)")
+ # self.send_twist_command(0.0, 0.0, 0.0, 0.0)
+ if count == 1:
+ # action = Action.BACK
+ # self.get_logger().info(f"Flying with speeds ({-self.speed_coeff}, 0.0, 0.0, 0.0)")
+ # self.send_twist_command(-self.speed_coeff, 0.0, 0.0, 0.0)
+ action = Action.BACK
+ self.get_logger().info(f"Flying with speeds ({-self.speed_coeff}, 0.0, 0.0, 0.0)")
+ self.send_twist_command(-self.speed_coeff, 0.0, 0.0, 0.0)
+ elif count == 2:
+ # action = Action.FLIP
+ # if time.process_time() - self.last_marker_time > self.flip_timeout:
+ # self.send_tello_action_async("flip b")
+ action = Action.MOVE
+ self.get_logger().info(f"Flying with speeds ({self.speed_coeff}, 0.0, 0.0, 0.0)")
+ self.send_twist_command(self.speed_coeff, 0.0, 0.0, 0.0)
+ elif count == 0:
+ action = Action.STOP
+ self.get_logger().info(f"Flying with speeds (0.0, 0.0, 0.0, 0.0)")
+ self.send_twist_command(0.0, 0.0, 0.0, 0.0)
+ else:
+ action = Action.NONE
+ cv2.putText(img, f"Action is {action}(fingers {count})", (0, self.LINE_STEP * (len(self.cur_status) + 1)), self.FONT, self.FONT_SIZE, self.FONT_COLOR, self.FONT_STOKE)
+
+
+ def draw_hand(self, image, results):
+ for hand_landmarks in results:
+ self.mp_drawing.draw_landmarks(
+ image,
+ hand_landmarks,
+ self.mp_hands.HAND_CONNECTIONS,
+ self.mp_drawing_styles.get_default_hand_landmarks_style(),
+ self.mp_drawing_styles.get_default_hand_connections_style())
+ return image
+
+ def get_fingers_vectors(self, landmarks):
+ # h, w = img_shape
+ fingers_indecies = [
+ (5, 8),
+ (9, 12),
+ (13, 16),
+ (17, 20)
+ ]
+ return [
+ landmarks[j].y - landmarks[i].y
+ for i, j in fingers_indecies
+ ]
+
+
+ def hand_process_image(image: "np.ndarray"):
+ image.flags.writeable = False
+ image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
+ results = hands.process(image)
+
+ # Draw the hand annotations on the image.
+ image.flags.writeable = True
+ image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
+ if results.multi_hand_landmarks:
+ for hand_landmarks in results.multi_hand_landmarks:
+ # for land in hand_landmarks.landmark:
+ # print(land.x, land.y)
+ mp_drawing.draw_landmarks(
+ image,
+ hand_landmarks,
+ mp_hands.HAND_CONNECTIONS,
+ mp_drawing_styles.get_default_hand_landmarks_style(),
+ mp_drawing_styles.get_default_hand_connections_style())
+ vector = get_fingers_vectors(hand_landmarks.landmark)
+ count = 0
+ # print(vector)
+ for y in vector:
+ count += 1 if y < -EPS else 0
+ cv2.putText(image, f"fingers up {count}", (30, 30), cv2.FONT_HERSHEY_TRIPLEX, 1, (0, 255, 0), 2)
+
def find_marker(self, image):
res = self.detect_marker_on_image(image)
if res is None:
diff --git a/ros/tello_ros/tello_ros/basic_tello_solution_node.py b/ros/tello_ros/tello_ros/basic_tello_solution_node.py
index d0187a7..13f40a9 100644
--- a/ros/tello_ros/tello_ros/basic_tello_solution_node.py
+++ b/ros/tello_ros/tello_ros/basic_tello_solution_node.py
@@ -50,6 +50,11 @@ def send_twist_command(self, x, y, z, rotation):
msg.angular.z = rotation
self.cmd_vel_pub.publish(msg)
+ def send_tello_action_async(self, cmd):
+ request = TelloAction.Request()
+ request.cmd = cmd
+ self.srv_action.call_async(request)
+
def send_tello_action(self, cmd):
if self.received_response is not None:
return False
diff --git a/ros/tello_ros/tello_ros/tello_joy_node.py b/ros/tello_ros/tello_ros/tello_joy_node.py
new file mode 100644
index 0000000..5cdba86
--- /dev/null
+++ b/ros/tello_ros/tello_ros/tello_joy_node.py
@@ -0,0 +1,82 @@
+from enum import Enum, auto
+import rclpy
+from rclpy.node import Node
+from tello_msgs.srv import TelloAction
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Joy
+from std_msgs.msg import Bool
+
+
+class JoyState:
+
+ def __init__(self, msg: Joy) -> None:
+ self.ButtonA = msg.buttons[0] == 1
+ self.ButtonB = msg.buttons[1] == 1
+ self.ButtonX = msg.buttons[2] == 1
+ self.ButtonY = msg.buttons[3] == 1
+ self.ButtonBack = msg.buttons[6] == 1
+ self.ButtonStart = msg.buttons[7] == 1
+ self.LStickH = msg.axes[0]
+ self.LStickV = msg.axes[1]
+ self.RStickH = msg.axes[3]
+ self.RStickV = msg.axes[4]
+ self.ButtonCrossL = msg.axes[6] == -1
+ self.ButtonCrossR = msg.axes[6] == 1
+ self.ButtonCrossD = msg.axes[7] == -1
+ self.ButtonCrossU = msg.axes[7] == 1
+
+ def convert_to_twist(self) -> Twist:
+ msg = Twist()
+ msg.linear.x = self.LStickV
+ msg.linear.y = self.LStickH
+ msg.linear.z = self.RStickV
+ msg.angular.z = self.RStickH
+ return msg
+
+
+class TelloJoyNode(Node):
+
+ def __init__(self):
+ super().__init__("tello_joy_node")
+ self.publisher = self.create_publisher(Bool, "/control", 10)
+ self.publisher_twist = self.create_publisher(Twist, "cmd_vel", 10)
+ self.joy_subscriber = self.create_subscription(Joy, "/joy", self.joy_callback, 10)
+ self.srv_action = self.create_client(TelloAction, "srv_tello_action")
+ self.get_logger().info("Started tello_joy_node")
+
+ def send_control_command(self, cmd: bool):
+ msg = Bool()
+ msg.data = cmd
+ self.publisher.publish(msg)
+
+ def send_action_command(self, cmd: str):
+ request = TelloAction.Request()
+ request.cmd = cmd
+ self.srv_action.call_async(request)
+
+ def joy_callback(self, msg):
+ state = JoyState(msg)
+ if state.ButtonStart:
+ self.send_action_command("takeoff")
+ if state.ButtonBack:
+ self.send_action_command("land")
+ if state.ButtonA:
+ self.send_control_command(True)
+ if state.ButtonB:
+ self.send_control_command(False)
+ self.publisher_twist.publish(state.convert_to_twist())
+ # self.get_logger().info("Joy CB")
+ # self.get_logger().info(str(msg))
+ # self.get_logger().info(str(msg.axes))
+ # self.get_logger().info(str(msg.buttons))
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = TelloJoyNode()
+ rclpy.spin(node)
+ node.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == "__main__":
+ main()