110 lines
3.6 KiB
Python
110 lines
3.6 KiB
Python
#!/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
|