From ef7d32ad8e6ab9fe95b95fae2810cc6af6e252c1 Mon Sep 17 00:00:00 2001
From: Nathan Beaud <nathan.beaud@ecam.fr>
Date: Mon, 18 Mar 2024 11:28:24 +0100
Subject: [PATCH] Adding owner following code

---
 owner_following.py | 109 +++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 109 insertions(+)
 create mode 100644 owner_following.py

diff --git a/owner_following.py b/owner_following.py
new file mode 100644
index 0000000..dd97472
--- /dev/null
+++ b/owner_following.py
@@ -0,0 +1,109 @@
+#!/usr/bin/env python3
+
+import rospy
+import cv2
+import numpy as np
+from pyzbar.pyzbar import decode
+from sensor_msgs.msg import Image, LaserScan
+from geometry_msgs.msg import Twist
+from cv_bridge import CvBridge, CvBridgeError
+import random
+
+TARGET_DISTANCE = 0.2  # Meters
+ANGULAR_SPEED = 0.5
+LINEAR_SPEED = 0.2
+
+class QRCodeFollower:
+    def __init__(self):
+        rospy.init_node('qr_code_follower', anonymous=False)
+        self.bridge = CvBridge()
+        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+        self.image_sub = rospy.Subscriber("/camera/image", Image, self.image_callback)
+        self.laser_sub = rospy.Subscriber("/scan", LaserScan, self.laser_callback)
+        
+        self.qr_code_found = False
+        self.distance_to_owner = None
+
+    def rotate_360(self):
+        twist = Twist()
+        twist.angular.z = ANGULAR_SPEED
+        duration = 7.85 / ANGULAR_SPEED  # Time to complete a full rotation
+        start_time = rospy.Time.now().to_sec()
+        while rospy.Time.now().to_sec() - start_time < duration:
+            self.cmd_vel_pub.publish(twist)
+        twist.angular.z = 0
+        self.cmd_vel_pub.publish(twist)
+
+    def move_random_direction(self):
+        twist = Twist()
+        twist.linear.x = LINEAR_SPEED
+        duration = random.uniform(1, 3)  # Move for a random duration
+        start_time = rospy.Time.now().to_sec()
+        while rospy.Time.now().to_sec() - start_time < duration:
+            self.cmd_vel_pub.publish(twist)
+        twist.linear.x = 0
+        self.cmd_vel_pub.publish(twist)
+
+    def detect_qr_code(self, image):
+        decoded_objects = decode(image)
+        for obj in decoded_objects:
+            if obj.data.decode() == "Owner":
+                self.qr_code_found = True
+                
+                points = obj.polygon
+                if len(points) > 4: 
+                    hull = cv2.convexHull(np.array([point for point in points], dtype=np.float32))
+                    points = hull
+                n = len(points)
+                for j in range(n):
+                    cv2.line(image, points[j], points[(j+1) % n], (255,0,0), 3)
+                
+                cv2.imshow("QR Code Detection", image)
+                cv2.waitKey(1)
+                return True
+        return False
+
+    def image_callback(self, msg):
+        try:
+            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+        except CvBridgeError as e:
+            rospy.logerr(e)
+            return
+        
+        if not self.detect_qr_code(cv_image) and not self.qr_code_found:
+            self.rotate_360()
+            self.move_random_direction()
+            self.rotate_360()
+
+    def laser_callback(self, msg):
+        self.distance_to_owner = min(msg.ranges)
+
+    def follow_qr_code(self):
+        twist = Twist()
+        if self.qr_code_found and self.distance_to_owner > TARGET_DISTANCE:
+            twist.linear.x = LINEAR_SPEED
+            rospy.loginfo(f"Distance to owner: {self.distance_to_owner:.2f} meters")
+        else:
+            twist.linear.x = 0
+            rospy.loginfo("Reached the target distance to owner.")
+        self.cmd_vel_pub.publish(twist)
+
+    def run(self):
+        rate = rospy.Rate(10)  # 10 Hz
+        while not rospy.is_shutdown():
+            if self.qr_code_found:
+                self.follow_qr_code()
+            rate.sleep()
+
+    def shutdown(self):
+        rospy.loginfo("Stopping QRCodeFollower...")
+        self.cmd_vel_pub.publish(Twist())
+        cv2.destroyAllWindows()
+
+if __name__ == '__main__':
+    qr_follower = QRCodeFollower()
+    rospy.on_shutdown(qr_follower.shutdown)
+    try:
+        qr_follower.run()
+    except rospy.ROSInterruptException:
+        pass