diff --git a/P2/src/praktikum2/praktikum2/p2_line_following.py b/P2/src/praktikum2/praktikum2/p2_line_following.py index 3d1332a..321aad8 100755 --- a/P2/src/praktikum2/praktikum2/p2_line_following.py +++ b/P2/src/praktikum2/praktikum2/p2_line_following.py @@ -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 + cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') - # 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) - - # Optional: Rauschen entfernen - # kernel = np.ones((3, 3), np.uint8) - # mask = cv2.erode(mask, kernel, iterations=1) - # mask = cv2.dilate(mask, kernel, iterations=1) + # 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) + + 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) + 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 - # 4. Regelung (Simple P-Controller) - # Fehler berechnen: Differenz zwischen Linienmitte und Bildmitte - error_x = cx - image_center_x + # Bewegungslogik + largest_contour = max(combined_contours, key=cv2.contourArea) + + 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) - # 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 + 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() + rclpy.spin(node) + rclpy.shutdown() -if __name__ == '__main__': + +if __name__ == "__main__": main() \ No newline at end of file