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.gift_found = False
        self.gift_coords = None
        self.traveled = 0.0
        self.grid_step = 0.5
        
        self.get_logger().info('Сервер запущен')

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

    def color_cb(self, msg):
        if msg.r == 0 and msg.g == 255 and msg.b == 0:
            if not self.gift_found and self.pose:
                self.gift_found = True
                self.gift_coords = (self.pose.x, self.pose.y)
                self.get_logger().info(f'🎁 Найден: ({self.pose.x:.2f}, {self.pose.y:.2f})')

    def wait_pose(self):
        for _ in range(100):  # 5 сек максимум
            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):
        self.pub.publish(Twist())

    def rotate_to(self, target_angle):
        """Поворот к заданному углу. Только угловая скорость."""
        twist = Twist()
        
        while rclpy.ok() and not self.gift_found:
            error = self.norm_angle(target_angle - self.pose.theta)
            
            if abs(error) < 0.03:  # ~1.7 градуса
                break
            
            # Простой P-регулятор с ограничением
            twist.angular.z = max(-2.0, min(2.0, 3.0 * error))
            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
        
        while rclpy.ok() and not self.gift_found:
            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() or self.gift_found:
            return
        
        dx, dy = x - self.pose.x, y - self.pose.y
        dist = math.sqrt(dx*dx + dy*dy)
        
        if dist < 0.1:  # Уже на месте
            return
        
        # Шаг 1: поворачиваемся к цели
        angle = math.atan2(dy, dx)
        self.rotate_to(angle)
        
        if self.gift_found:
            return
        
        # Шаг 2: едем прямо
        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() and not self.gift_found:
            # Горизонтальная линия
            target_x = x_max if right else x_min
            self.go_to(target_x, y)
            
            # Feedback
            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 and not self.gift_found:
                self.go_to(self.pose.x, y)

    def execute_callback(self, goal_handle):
        self.get_logger().info('Начинаю поиск')
        
        self.gift_found = False
        self.gift_coords = 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_found:
            result.gift_found = True
            result.gift_coordinates.x = self.gift_coords[0]
            result.gift_coordinates.y = self.gift_coords[1]
        else:
            result.gift_found = False
        
        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()