Untitled

import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from turtlesim_contest_interface.action import FindHiddenGift
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose, Color
import math
import time


class GiftFinderActionServer(Node):

    def __init__(self):
        super().__init__('gift_finder_action_server')
        
        self.cb_group = ReentrantCallbackGroup()
        
        self._action_server = ActionServer(
            self, FindHiddenGift, 'find_hidden_gift',
            self.execute_callback, callback_group=self.cb_group
        )
        
        self.pub = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)
        
        self.create_subscription(
            Pose, '/turtle1/pose', self.pose_cb, 10, callback_group=self.cb_group
        )
        self.create_subscription(
            Color, '/turtle1/color_sensor', self.color_cb, 10, callback_group=self.cb_group
        )

        self.pose = None
        self.traveled = 0.0
        self.grid_step = 0.5
        
        # Для поиска центра подарка
        self.on_gift = False           # Сейчас на зелёном?
        self.gift_points = []          # Все точки на подарке
        self.gift_center = None        # Вычисленный центр
        
        self.get_logger().info('Сервер запущен')

    def pose_cb(self, msg):
        self.pose = msg

    def color_cb(self, msg):
        is_green = (msg.r == 0 and msg.g == 255 and msg.b == 0)
        
        if is_green and self.pose:
            if not self.on_gift:
                # Въехали на подарок
                self.on_gift = True
                self.get_logger().info(f'Въезд на подарок: ({self.pose.x:.2f}, {self.pose.y:.2f})')
            # Собираем все точки на подарке
            self.gift_points.append((self.pose.x, self.pose.y))
        else:
            if self.on_gift:
                # Выехали с подарка — вычисляем центр
                self.on_gift = False
                self.calculate_center()

    def calculate_center(self):
        """Вычисляет центр по собранным точкам."""
        if not self.gift_points:
            return
        
        # Находим границы
        xs = [p[0] for p in self.gift_points]
        ys = [p[1] for p in self.gift_points]
        
        # Центр = середина между крайними точками
        center_x = (min(xs) + max(xs)) / 2
        center_y = (min(ys) + max(ys)) / 2
        
        # Обновляем центр (берём среднее с предыдущим, если был)
        if self.gift_center:
            old_x, old_y = self.gift_center
            self.gift_center = ((old_x + center_x) / 2, (old_y + center_y) / 2)
        else:
            self.gift_center = (center_x, center_y)
        
        self.get_logger().info(f'Центр подарка: ({self.gift_center[0]:.2f}, {self.gift_center[1]:.2f})')
        
        # Очищаем точки для следующего прохода
        self.gift_points = []

    def gift_found(self):
        """Проверяет, найден ли подарок."""
        return self.gift_center is not None

    def wait_pose(self):
        for _ in range(100):
            if self.pose:
                return True
            time.sleep(0.05)
        return False

    def norm_angle(self, a):
        while a > math.pi: a -= 2*math.pi
        while a < -math.pi: a += 2*math.pi
        return a

    def stop(self):
        twist = Twist()
        for _ in range(3):
            self.pub.publish(twist)
        time.sleep(0.15)

    def rotate_to(self, target_angle):
        twist = Twist()
        
        while rclpy.ok():
            error = self.norm_angle(target_angle - self.pose.theta)
            
            if abs(error) < 0.015:  # ~0.9 градуса
                break
            
            # Уменьшенный Kp и ограничение скорости
            speed = 1.5 * error
            speed = max(-0.8, min(0.8, speed))
            
            # Минимальная скорость, чтобы не застрять
            if 0 < abs(speed) < 0.15:
                speed = 0.15 if speed > 0 else -0.15
            
            twist.angular.z = speed
            self.pub.publish(twist)
            time.sleep(0.02)
        
        self.stop()

    def move_forward(self, distance):
        start_x, start_y = self.pose.x, self.pose.y
        twist = Twist()
        twist.linear.x = 1.5
        traveled = 0.0
        
        while rclpy.ok():
            dx = self.pose.x - start_x
            dy = self.pose.y - start_y
            traveled = math.sqrt(dx*dx + dy*dy)
            
            if traveled >= distance:
                break
            
            self.pub.publish(twist)
            time.sleep(0.02)
        
        self.stop()
        self.traveled += traveled

    def go_to(self, x, y):
        if not self.wait_pose():
            return
        
        dx, dy = x - self.pose.x, y - self.pose.y
        dist = math.sqrt(dx*dx + dy*dy)
        
        if dist < 0.1:
            return
        
        angle = math.atan2(dy, dx)
        self.rotate_to(angle)
        self.move_forward(dist)

    def search(self, area, goal_handle):
        x_min = area.bottom_left.x + 0.3
        x_max = area.top_right.x - 0.3
        y_min = area.bottom_left.y + 0.3
        y_max = area.top_right.y - 0.3
        
        self.go_to(x_min, y_min)
        
        y = y_min
        right = True
        
        while y <= y_max and rclpy.ok():
            target_x = x_max if right else x_min
            self.go_to(target_x, y)
            
            fb = FindHiddenGift.Feedback()
            fb.gift_ever_detected = self.gift_found()
            fb.traveled_distance = self.traveled
            goal_handle.publish_feedback(fb)
            
            y += self.grid_step
            right = not right
            
            if y <= y_max:
                self.go_to(self.pose.x, y)
        
        # Финальная обработка, если закончили на подарке
        if self.on_gift and self.gift_points:
            self.calculate_center()

    def execute_callback(self, goal_handle):
        self.get_logger().info('Начинаю поиск')
        
        self.on_gift = False
        self.gift_points = []
        self.gift_center = None
        self.traveled = 0.0
        
        area = goal_handle.request.search_area
        
        if not self.wait_pose():
            goal_handle.abort()
            return FindHiddenGift.Result()
        
        self.search(area, goal_handle)
        
        result = FindHiddenGift.Result()
        if self.gift_center:
            result.gift_found = True
            result.gift_coordinates.x = self.gift_center[0]
            result.gift_coordinates.y = self.gift_center[1]
            self.get_logger().info(f'Центр подарка: ({self.gift_center[0]:.2f}, {self.gift_center[1]:.2f})')
        else:
            result.gift_found = False
            self.get_logger().info('Подарок не найден')
        
        self.get_logger().info(f'Пройдено: {self.traveled:.1f} м')
        goal_handle.succeed()
        return result


def main(args=None):
    rclpy.init(args=args)
    node = GiftFinderActionServer()
    
    executor = MultiThreadedExecutor()
    executor.add_node(node)
    
    try:
        executor.spin()
    except KeyboardInterrupt:
        pass
    finally:
        node.stop()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()