#!/usr/bin/env python3 import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry from sensor_msgs.msg import LaserScan import math import time import sys import numpy as np try: import pygame except ImportError: print("Błąd: Brak biblioteki pygame. Zainstaluj wpisując: pip3 install pygame") sys.exit(1) class HarvesterStateMachine(Node): def __init__(self): super().__init__('harvester_sm_node') # --- KONFIGURACJA TEMATÓW --- odom_topic_name = '/odom' cmd_vel_topic_name = '/cmd_vel' qos_profile = QoSProfile( reliability=ReliabilityPolicy.BEST_EFFORT, history=HistoryPolicy.KEEP_LAST, depth=10 ) # --- PUBLISHERS & SUBSCRIBERS --- self.cmd_vel_pub = self.create_publisher(Twist, cmd_vel_topic_name, 10) self.odom_sub = self.create_subscription(Odometry, odom_topic_name, self.odom_callback, qos_profile) # Subskrypcja dwóch krzyżujących się LiDARów zgodnie z Twoim planem self.lidar_left_sub = self.create_subscription(LaserScan, '/lidar_left', self.lidar_left_callback, qos_profile) self.lidar_right_sub = self.create_subscription(LaserScan, '/lidar_right', self.lidar_right_callback, qos_profile) # --- MASZYNA STANÓW --- self.STATE_INITIALIZING = "INITIALIZING" self.STATE_GPS_DRIVE = "GPS_DRIVE" self.STATE_LIDAR_DRIVE = "LIDAR_DRIVE" self.STATE_HARVESTING = "HARVESTING" self.STATE_MISSION_DONE = "MISSION_DONE" self.current_state = self.STATE_INITIALIZING # --- ZMIENNE ODOMETRII --- self.robot_x, self.robot_y, self.robot_yaw = 0.0, 0.0, 0.0 self.has_odom = False self.start_offset_x, self.start_offset_y = 0.0, 0.0 # --- CHMURY PUNKTÓW PO FUZJI (W UKŁADZIE ROBOTA) --- self.pts_L = ([], []) # Ściana lewa (zlewana z obu laserów) self.pts_R = ([], []) # Ściana prawa (zlewana z obu laserów) self.avg_left = 0.0 self.avg_right = 0.0 self.end_of_row_detected = False # Bufory na surowe wiadomości ROS2 self.latest_left_scan = None self.latest_right_scan = None # --- LOGIKA PRZEJAZDU --- self.lidar_start_x, self.lidar_start_y = 0.0, 0.0 self.lidar_enter_time = 0.0 self.harvesting_done_in_this_row = False self.harvest_trigger_distance = 6.0 # --- PUNKTY DOCELOWE --- self.global_waypoints = [{"x": -3.0, "y": 0.8}, {"x": -3.0, "y": 2.4}] self.local_waypoints = [] self.current_wp_idx = 0 self.position_reached = False # --- PARAMETRY REGULACJI --- self.target_pos_tolerance = 0.4 self.target_heading_tolerance = 0.05 self.lidar_speed = 0.25 self.lidar_kp = 0.35 self.deadzone = 0.08 self.control_timer = self.create_timer(0.1, self.control_loop) # --- PYGAME WIZUALIZACJA --- pygame.init() self.win_size = 600 self.screen = pygame.display.set_mode((self.win_size, self.win_size)) pygame.display.set_caption("Kombajn: Lokalna Fuzja Układu Współrzędnych (Cross-Eye)") def odom_callback(self, msg: Odometry): self.robot_x = msg.pose.pose.position.x self.robot_y = msg.pose.pose.position.y q = msg.pose.pose.orientation siny_cosp = 2 * (q.w * q.z + q.x * q.y) cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z) self.robot_yaw = math.atan2(siny_cosp, cosy_cosp) if not self.has_odom: self.start_offset_x, self.start_offset_y = self.robot_x, self.robot_y gazebo_spawn_x, gazebo_spawn_y = -10.0, -10.0 for wp in self.global_waypoints: self.local_waypoints.append({ "x": self.start_offset_x + (wp["x"] - gazebo_spawn_x), "y": self.start_offset_y + (wp["y"] - gazebo_spawn_y) }) self.has_odom = True def lidar_left_callback(self, msg: LaserScan): self.latest_left_scan = msg def lidar_right_callback(self, msg: LaserScan): self.latest_right_scan = msg def process_lidar_fusion(self): if not self.latest_left_scan or not self.latest_right_scan: return x_left_wall, y_left_wall = [], [] x_right_wall, y_right_wall = [], [] # --- SENSOR 1: LEWY LIDAR (Nowe pozycje!) --- l_msg = self.latest_left_scan l_x, l_y, l_yaw = -1.0, 0.5, -0.785398 # <--- TUTAJ ZMIANA # ... (reszta pętli dla lewego lasera bez zmian) ... for i, distance in enumerate(l_msg.ranges): if math.isinf(distance) or math.isnan(distance) or distance < 3.0 or distance > l_msg.range_max: continue angle = l_msg.angle_min + i * l_msg.angle_increment # Pozycja punktu względem samego sensora xs = distance * math.cos(angle) ys = distance * math.sin(angle) # Rzutowanie (Transformacja macierzowa) na układ robota (środek ramy) xr = l_x + (xs * math.cos(l_yaw) - ys * math.sin(l_yaw)) yr = l_y + (xs * math.sin(l_yaw) + ys * math.cos(l_yaw)) # Segregacja: jeśli punkt wylądował po lewej stronie robota (Yr > 0), to lewa ściana, inaczej prawa if yr > 0.0: x_left_wall.append(xr) y_left_wall.append(yr) else: x_right_wall.append(xr) y_right_wall.append(yr) r_msg = self.latest_right_scan r_x, r_y, r_yaw = -1.0, -0.5, 0.785398 # <--- TUTAJ ZMIANA # ... (reszta pętli dla prawego lasera bez zmian) ... for i, distance in enumerate(r_msg.ranges): if math.isinf(distance) or math.isnan(distance) or distance < 3.0 or distance > r_msg.range_max: continue angle = r_msg.angle_min + i * r_msg.angle_increment xs = distance * math.cos(angle) ys = distance * math.sin(angle) # Rzutowanie na układ robota xr = r_x + (xs * math.cos(r_yaw) - ys * math.sin(r_yaw)) yr = r_y + (xs * math.sin(r_yaw) + ys * math.cos(r_yaw)) if yr > 0.0: x_left_wall.append(xr) y_left_wall.append(yr) else: x_right_wall.append(xr) y_right_wall.append(yr) self.pts_L = (x_left_wall, y_left_wall) self.pts_R = (x_right_wall, y_right_wall) # Wyliczanie średnich odległości bocznych korytarza dla regulatora PID min_points = 4 if len(y_left_wall) < min_points and len(y_right_wall) < min_points: self.end_of_row_detected = True self.avg_left, self.avg_right = 1.4, 1.4 else: self.end_of_row_detected = False # Średnia odległość punktów od osi podłużnej robota (Y=0) self.avg_left = sum(y_left_wall) / len(y_left_wall) if y_left_wall else 1.4 self.avg_right = abs(sum(y_right_wall) / len(y_right_wall)) if y_right_wall else 1.4 def control_loop(self): for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.quit() sys.exit() # Dynamiczny start w oparciu o zebrany komplet tematów z mostka if self.current_state == self.STATE_INITIALIZING: if self.has_odom and self.latest_left_scan is not None and self.latest_right_scan is not None: self.current_state = self.STATE_GPS_DRIVE self.draw_pygame_window() return # Wykonaj fuzję współrzędnych lokalnych przed podjęciem decyzji o ruchu self.process_lidar_fusion() if self.current_state == self.STATE_GPS_DRIVE: self.execute_gps_drive() elif self.current_state == self.STATE_LIDAR_DRIVE: self.execute_lidar_drive() elif self.current_state == self.STATE_HARVESTING: self.execute_harvesting() elif self.current_state == self.STATE_MISSION_DONE: self.stop_robot() self.draw_pygame_window() def execute_gps_drive(self): if self.current_wp_idx >= len(self.local_waypoints): self.current_state = self.STATE_MISSION_DONE return target = self.local_waypoints[self.current_wp_idx] cmd = Twist() if not self.position_reached: dx, dy = target["x"] - self.robot_x, target["y"] - self.robot_y distance = math.sqrt(dx**2 + dy**2) desired_yaw = math.atan2(dy, dx) yaw_error = math.atan2(math.sin(desired_yaw - self.robot_yaw), math.cos(desired_yaw - self.robot_yaw)) if distance < self.target_pos_tolerance: self.stop_robot() self.position_reached = True return if abs(yaw_error) > 0.15: cmd.angular.z = max(min(0.6 * yaw_error, 0.5), -0.5) else: cmd.linear.x = 0.4 cmd.angular.z = 0.8 * yaw_error self.cmd_vel_pub.publish(cmd) else: yaw_error = math.atan2(math.sin(0.0 - self.robot_yaw), math.cos(0.0 - self.robot_yaw)) if abs(yaw_error) < self.target_heading_tolerance: self.stop_robot() self.position_reached = False self.lidar_start_x, self.lidar_start_y = self.robot_x, self.robot_y self.lidar_enter_time = time.time() self.harvesting_done_in_this_row = False self.current_state = self.STATE_LIDAR_DRIVE return cmd.angular.z = max(min(0.5 * yaw_error, 0.4), -0.4) if 0 < cmd.angular.z < 0.15: cmd.angular.z = 0.15 if -0.15 < cmd.angular.z < 0: cmd.angular.z = -0.15 self.cmd_vel_pub.publish(cmd) def execute_lidar_drive(self): cmd = Twist() time_in_lidar = time.time() - self.lidar_enter_time if self.end_of_row_detected and time_in_lidar > 6.0: self.stop_robot() self.get_logger().info("Wyjazd z korytarza. Powrót do nawigacji GPS.") self.current_wp_idx += 1 self.current_state = self.STATE_GPS_DRIVE return dist_traveled = math.sqrt((self.robot_x - self.lidar_start_x)**2 + (self.robot_y - self.lidar_start_y)**2) if dist_traveled >= self.harvest_trigger_distance and not self.harvesting_done_in_this_row: self.stop_robot() self.current_state = self.STATE_HARVESTING return # Logika centrowania oparta o zunifikowane odległości boczne Y po fuzji diff = self.avg_left - self.avg_right cmd.linear.x = self.lidar_speed if abs(diff) < self.deadzone: cmd.angular.z = 0.0 else: cmd.angular.z = self.lidar_kp * diff cmd.angular.z = max(min(cmd.angular.z, 0.2), -0.2) self.cmd_vel_pub.publish(cmd) def execute_harvesting(self): self.stop_robot() time.sleep(2.0) self.harvesting_done_in_this_row = True self.current_state = self.STATE_LIDAR_DRIVE def stop_robot(self): cmd = Twist() cmd.linear.x, cmd.angular.z = 0.0, 0.0 self.cmd_vel_pub.publish(cmd) def draw_pygame_window(self): """Wizualizacja zrzutowanych punktów w lokalnej macierzy robota""" self.screen.fill((12, 16, 24)) # Środek okna reprezentuje geometryczny środek kombajnu cx, cy = self.win_size // 2, self.win_size // 2 + 50 scale = 55.0 # Siatka radarowa (okręgi co 1 metr) for r in range(1, 8): pygame.draw.circle(self.screen, (25, 35, 45), (cx, cy), int(r * scale), 1) # Rysowanie obrysu kombajnu (SDF: długość 4.0, szerokość 3.2) robot_w = int(3.2 * scale) robot_h = int(4.0 * scale) rx = cx - robot_w // 2 ry = cy - robot_h // 2 pygame.draw.rect(self.screen, (55, 65, 80), pygame.Rect(rx, ry, robot_w, robot_h), 2) # Pozycja montażowa fizyczna dwóch wewnętrznych LiDARów (X=-2.0, Y= +/- 1.5) pygame.draw.circle(self.screen, (0, 255, 0), (cx - int(1.5*scale), cy + int(2.0*scale)), 5) # Lewy pygame.draw.circle(self.screen, (255, 150, 0), (cx - int(-1.5*scale), cy + int(2.0*scale)), 5) # Prawy font = pygame.font.SysFont('Monospace', 13, bold=True) if self.current_state == self.STATE_INITIALIZING: status_text = "INITIALIZING: Oczekiwanie na" if not self.has_odom: status_text += " [ODOM]" if self.latest_left_scan is None: status_text += " [LIDAR_L]" if self.latest_right_scan is None: status_text += " [LIDAR_R]" self.screen.blit(font.render(status_text, True, (255, 140, 0)), (15, 15)) pygame.display.flip() return # Rysowanie przeliczonych chmur punktów (Lewa ściana = zielona, Prawa = pomarańczowa) for x, y in zip(self.pts_L[0], self.pts_L[1]): px = cx - int(y * scale) py = cy - int(x * scale) if 0 <= px < self.win_size and 0 <= py < self.win_size: pygame.draw.circle(self.screen, (100, 255, 100), (px, py), 2) for x, y in zip(self.pts_R[0], self.pts_R[1]): px = cx - int(y * scale) py = cy - int(x * scale) if 0 <= px < self.win_size and 0 <= py < self.win_size: pygame.draw.circle(self.screen, (255, 165, 0), (px, py), 2) # Wyświetlanie tekstów diagnostycznych self.screen.blit(font.render(f"STAN: {self.current_state}", True, (255, 255, 255)), (15, 15)) self.screen.blit(font.render(f"DYSTANS OD LEWEJ ŚCIANY: {self.avg_left:.2f}m", True, (100, 255, 100)), (15, 35)) self.screen.blit(font.render(f"DYSTANS OD PRAWEJ ŚCIANY: {self.avg_right:.2f}m", True, (255, 165, 0)), (15, 55)) pygame.display.flip() def main(args=None): rclpy.init(args=args) node = HarvesterStateMachine() try: rclpy.spin(node) except KeyboardInterrupt: pass finally: node.stop_robot() node.destroy_node() rclpy.shutdown() pygame.quit() if __name__ == '__main__': main()