diff --git a/P2/src/praktikum2/praktikum2/p2_line_following.py b/P2/src/praktikum2/praktikum2/p2_line_following.py old mode 100644 new mode 100755 index d35c9cb..3d1332a --- a/P2/src/praktikum2/praktikum2/p2_line_following.py +++ b/P2/src/praktikum2/praktikum2/p2_line_following.py @@ -1,6 +1,127 @@ -def main(): - print('Hi from praktikum2.') +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from geometry_msgs.msg import Twist +from cv_bridge import CvBridge +import cv2 +import numpy as np + +class LineFollower(Node): + def __init__(self): + super().__init__('line_following_node') + + # Initialisierungen + self.bridge = CvBridge() + + # Subscriber für die Kamera + self.create_subscription(Image, '/SpotArm/gripper_camera/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) + 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) + self.red_lower2 = np.array([170, 100, 100], dtype="uint8") + self.red_upper2 = np.array([180, 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 + 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) + + # Optional: Rauschen entfernen + # kernel = np.ones((3, 3), np.uint8) + # mask = cv2.erode(mask, kernel, iterations=1) + # mask = cv2.dilate(mask, kernel, iterations=1) + + # 3. Konturen finden + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + 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) + 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 + + # Bewegungslogik + twist_msg.linear.x = 0.2 # Konstante Vorwärtsgeschwindigkeit (m/s) + + # Drehung entgegen dem Fehler + twist_msg.angular.z = -float(error_x) * kp + else: + # Kontur zu klein -> wahrscheinlich Rauschen + twist_msg.linear.x = 0.0 + twist_msg.angular.z = 0.0 + else: + # Keine Linie gefunden -> Stoppen + twist_msg.linear.x = 0.0 + twist_msg.angular.z = 0.0 + + # Befehl senden + self.cmd_vel_pub.publish(twist_msg) + + # 5. 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): + rclpy.init(args=args) + node = LineFollower() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + cv2.destroyAllWindows() if __name__ == '__main__': - main() + main() \ No newline at end of file