Safak/Robotik
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

P2.2 final

Browse Source
This commit is contained in:
Safak Hazinedar
2025-12-07 21:16:51 +01:00
parent 6246e56b8b
commit 6f0849e5bd
1 changed files with 57 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

117
P2/src/praktikum2/praktikum2/p2_line_following.py
View File

@@ -15,113 +15,100 @@ class LineFollower(Node):
# Initialisierungen
self.bridge = CvBridge()
# Subscriber für die Kamera
self.create_subscription(Image, '/SpotArm/gripper_camera/image_color', self.camera_callback, 10)
# /Spot/kinect_color/image_color
# /SpotArm/gripper_camera/image_color
self.create_subscription(
Image,
'/Spot/kinect_color/image_color',
self.camera_callback,
10
)
# Publisher für Bewegungsbefehle (cmd_vel)
self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
self.get_logger().info("Line Follower Node gestartet.")
# HSV-Werte für ROT
# WICHTIG: Rot liegt im HSV-Raum an beiden Enden des Spektrums (0-10 UND 170-180).
# Wir definieren beide Bereiche, um robust gegen leichte Farbverschiebungen zu sein.
# Bereich 1: Unteres Ende (0-10)
# Rot liegt im HSV-Raum an beiden Enden des Spektrums (0-10 & 170-180).
# Bereich 1: Rot unteres Ende (0-10)
self.red_lower1 = np.array([0, 100, 100], dtype="uint8")
self.red_upper1 = np.array([10, 255, 255], dtype="uint8")
# Bereich 2: Oberes Ende (170-180)
# Bereich 2: Rot oberes Ende (170-180)
self.red_lower2 = np.array([170, 100, 100], dtype="uint8")
self.red_upper2 = np.array([180, 255, 255], dtype="uint8")
# Bereich 3: Gelb (20-30)
self.yellow_lower = np.array([20, 100, 100], dtype="uint8")
self.yellow_upper = np.array([30, 255, 255], dtype="uint8")
def camera_callback(self, msg):
try:
cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
except Exception as e:
self.get_logger().error(f"Bildkonvertierung fehlgeschlagen: {e}")
return
# Bildgröße für Berechnungen
height, width, _ = cv_image.shape
image_center_x = width // 2
# 1. Bildverarbeitung: BGR -> HSV
# 1. Konvertierung: BGR -> HSV
hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
# 2. Maske erstellen (Rote Bereiche filtern)
# Wir erstellen zwei Masken und kombinieren sie mit bitwise_or
mask1 = cv2.inRange(hsv, self.red_lower1, self.red_upper1)
mask2 = cv2.inRange(hsv, self.red_lower2, self.red_upper2)
mask = cv2.bitwise_or(mask1, mask2)
# 2. Maske erstellen
mask_red_1 = cv2.inRange(hsv, self.red_lower1, self.red_upper1)
mask_red_2 = cv2.inRange(hsv, self.red_lower2, self.red_upper2)
mask_red = cv2.bitwise_or(mask_red_1, mask_red_2)
# Optional: Rauschen entfernen
# kernel = np.ones((3, 3), np.uint8)
# mask = cv2.erode(mask, kernel, iterations=1)
# mask = cv2.dilate(mask, kernel, iterations=1)
mask_yellow = cv2.inRange(hsv, self.yellow_lower, self.yellow_upper)
mask_combined = cv2.bitwise_or(mask_red, mask_yellow)
# 3. Konturen finden
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
red_contours = cv2.findContours(mask_red, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
red_contours = red_contours[0] if len(red_contours) == 2 else red_contours[1]
yellow_contours = cv2.findContours(mask_yellow, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
yellow_contours = yellow_contours[0] if len(yellow_contours) == 2 else yellow_contours[1]
combined_contours = cv2.findContours(mask_combined, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
combined_contours = combined_contours[0] if len(combined_contours) == 2 else combined_contours[1]
twist_msg = Twist()
if contours:
# Größte Kontur finden (angenommen, das ist die Linie)
largest_contour = max(contours, key=cv2.contourArea)
# Überprüfen, ob die Kontur groß genug ist, um kein Rauschen zu sein
if cv2.contourArea(largest_contour) > 100:
# Bounding Rect verwenden, um Mitte zu finden
x, y, w, h = cv2.boundingRect(largest_contour)
# 4. Befehl bestimmen
if combined_contours:
# alle Konturen visualisieren
for contour in combined_contours:
# Mitte des Rechtecks der größten Kontur
x, y, w, h = cv2.boundingRect(contour)
cx = x + w // 2
cy = y + h // 2
# Zeichnen zur Visualisierung (Grüne Box um die Linie)
cv2.rectangle(cv_image, (x, y), (x + w, y + h), (0, 255, 0), 2)
cv2.circle(cv_image, (cx, cy), 5, (255, 0, 0), -1)
# 4. Regelung (Simple P-Controller)
# Fehler berechnen: Differenz zwischen Linienmitte und Bildmitte
error_x = cx - image_center_x
# Verstärkungsfaktor (kp)
kp = 0.005
cv2.rectangle(cv_image, (x, y), (x + w, y + h), (0, 255, 0), 2) # grüne Box um den Pfad
cv2.circle(cv_image, (cx, cy), 5, (255, 0, 0), -1) # baluer Punkt in der Mitte
# Bewegungslogik
twist_msg.linear.x = 0.2 # Konstante Vorwärtsgeschwindigkeit (m/s)
largest_contour = max(combined_contours, key=cv2.contourArea)
# Drehung entgegen dem Fehler
twist_msg.angular.z = -float(error_x) * kp
else:
# Kontur zu klein -> wahrscheinlich Rauschen
if cv2.contourArea(largest_contour) > 50: # Rauschen ignorieren
error_x = cx - (cv_image.shape[1] // 2) # Fehler berechnen: Differenz zwischen Linienmitte und Bildmitte
twist_msg.angular.z = -float(error_x) * 0.01 # entegegen dem Fehler drehen
twist_msg.linear.x = 0.1 # Konstante Vorwärtsgeschwindigkeit (m/s)
else: # Kontur zu klein -> wahrscheinlich Rauschen
twist_msg.linear.x = 0.0
twist_msg.angular.z = 0.0
else:
# Keine Linie gefunden -> Stoppen
else: # Keine Linie gefunden -> Stoppen
twist_msg.linear.x = 0.0
twist_msg.angular.z = 0.0
# Befehl senden
# 5. Befehl senden
self.cmd_vel_pub.publish(twist_msg)
# 5. Anzeige
# 6. Anzeige
cv2.imshow("Line Follower View", cv_image)
# Debugging-Maske anzeigen (optional, falls es immer noch Probleme gibt)
# cv2.imshow("Mask Debug", mask)
cv2.waitKey(1)
def main(args=None):
def main(args = None):
rclpy.init(args=args)
node = LineFollower()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
cv2.destroyAllWindows()
if __name__ == '__main__':
if __name__ == "__main__":
main()