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
from turtlesim.srv import SetPen
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.pen_client = self.create_client(SetPen, '/turtle1/set_pen')
        
        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.current_color = None
        self.traveled = 0.0
        self.grid_step = 0.5
        
        # Границы подарка
        self.gift_x_min = None
        self.gift_x_max = None
        self.gift_y_min = None
        self.gift_y_max = None
        
        self.get_logger().info('Сервер запущен')

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

    def color_cb(self, msg):
        self.current_color = msg

    def is_on_gift(self):
        if self.current_color is None:
            return False
        return (self.current_color.r == 0 and 
                self.current_color.g == 255 and 
                self.current_color.b == 0)

    def update_gift_bounds(self):
        if self.pose is None:
            return
        
        x, y = self.pose.x, self.pose.y
        
        if self.gift_x_min is None:
            self.gift_x_min = self.gift_x_max = x
            self.gift_y_min = self.gift_y_max = y
        else:
            self.gift_x_min = min(self.gift_x_min, x)
            self.gift_x_max = max(self.gift_x_max, x)
            self.gift_y_min = min(self.gift_y_min, y)
            self.gift_y_max = max(self.gift_y_max, y)

    def get_gift_center(self):
        if self.gift_x_min is None:
            return None
        center_x = (self.gift_x_min + self.gift_x_max) / 2
        center_y = (self.gift_y_min + self.gift_y_max) / 2
        return (center_x, center_y)

    def gift_found(self):
        return self.gift_x_min is not None

    def set_pen(self, off):
        if self.pen_client.wait_for_service(timeout_sec=1.0):
            req = SetPen.Request()
            req.off = 1 if off else 0
            self.pen_client.call_async(req)

    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()
        self.pub.publish(twist)
        time.sleep(0.03)

    def rotate_to(self, target_angle):
        """Поворот к заданному углу."""
        target_angle = self.norm_angle(target_angle)
        twist = Twist()
        max_iterations = 500
        iterations = 0
        
        while rclpy.ok() and iterations < max_iterations:
            iterations += 1
            
            error = self.norm_angle(target_angle - self.pose.theta)
            
            if abs(error) < 0.02:
                break
            
            speed = 2.0 * error
            speed = max(-1.5, min(1.5, speed))
            
            if 0 < speed < 0.15:
                speed = 0.15
            elif -0.15 < speed < 0:
                speed = -0.15
            
            twist.angular.z = speed
            self.pub.publish(twist)
            time.sleep(0.02)
        
        self.stop()

    def go_to(self, x, y):
        """Быстрое перемещение к точке."""
        if not self.wait_pose():
            return
        
        start_x, start_y = self.pose.x, self.pose.y
        
        dx = x - self.pose.x
        dy = y - self.pose.y
        dist = math.sqrt(dx * dx + dy * dy)
        
        if dist < 0.05:
            return
        
        angle = math.atan2(dy, dx)
        self.rotate_to(angle)
        
        twist = Twist()
        twist.linear.x = 5.0  # Быстрое движение
        
        while rclpy.ok():
            dx = x - self.pose.x
            dy = y - self.pose.y
            remaining = math.sqrt(dx * dx + dy * dy)
            
            if remaining < 0.05:
                break
            
            # Замедление при приближении
            if remaining < 0.5:
                twist.linear.x = max(1.0, remaining * 4)
            
            self.pub.publish(twist)
            time.sleep(0.01)
        
        self.stop()
        
        end_x, end_y = self.pose.x, self.pose.y
        self.traveled += math.sqrt((end_x - start_x)**2 + (end_y - start_y)**2)

    def trace_gift_boundary(self):
        """Обходит границу подарка."""
        self.get_logger().info('Обхожу границу подарка...')
        self.update_gift_bounds()
        
        directions = [0, math.pi/2, math.pi, -math.pi/2]
        for direction in directions:
            self.trace_one_direction(direction)
        
        self.get_logger().info(
            f'Границы: x=[{self.gift_x_min:.2f}, {self.gift_x_max:.2f}], '
            f'y=[{self.gift_y_min:.2f}, {self.gift_y_max:.2f}]'
        )

    def trace_one_direction(self, angle):
        """Движется в направлении до края подарка."""
        self.rotate_to(angle)
        time.sleep(0.03)
        
        twist = Twist()
        twist.linear.x = 2.0  # Ускорено (было 0.3)
        max_steps = 500
        steps = 0
        
        while rclpy.ok() and self.is_on_gift() and steps < max_steps:
            self.update_gift_bounds()
            self.pub.publish(twist)
            time.sleep(0.01)
            steps += 1
        
        self.stop()
        
        # Возврат назад
        twist.linear.x = -1.0  # Ускорено (было -0.2)
        steps = 0
        
        while rclpy.ok() and not self.is_on_gift() and steps < 100:
            self.pub.publish(twist)
            time.sleep(0.01)
            steps += 1
        
        self.stop()
        
        if self.is_on_gift():
            self.update_gift_bounds()

    def search(self, area, goal_handle):
        """Быстрый поиск змейкой."""
        self.set_pen(off=True)
        time.sleep(0.05)
        
        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
        moving_right = True
        
        while y <= y_max and rclpy.ok():
            target_x = x_max if moving_right else x_min
            
            if moving_right:
                self.rotate_to(0)
            else:
                self.rotate_to(math.pi)
            
            start_line_x = self.pose.x
            twist = Twist()
            twist.linear.x = 5.0  # Быстрое сканирование (было 1.0)
            
            while rclpy.ok():
                if moving_right and self.pose.x >= target_x:
                    break
                if not moving_right and self.pose.x <= target_x:
                    break
                
                if self.is_on_gift():
                    self.stop()
                    self.trace_gift_boundary()
                    
                    fb = FindHiddenGift.Feedback()
                    fb.gift_ever_detected = True
                    fb.traveled_distance = self.traveled
                    goal_handle.publish_feedback(fb)
                    return
                
                self.pub.publish(twist)
                time.sleep(0.01)  # Чаще проверяем (было 0.02)
            
            self.stop()
            self.traveled += abs(self.pose.x - start_line_x)
            
            fb = FindHiddenGift.Feedback()
            fb.gift_ever_detected = self.gift_found()
            fb.traveled_distance = self.traveled
            goal_handle.publish_feedback(fb)
            
            y += self.grid_step
            moving_right = not moving_right
            
            if y <= y_max:
                self.rotate_to(math.pi / 2)
                
                twist = Twist()
                twist.linear.x = 3.0  # Ускорено (было 0.5)
                start_y = self.pose.y
                
                while rclpy.ok() and self.pose.y < y:
                    if self.is_on_gift():
                        self.stop()
                        self.trace_gift_boundary()
                        
                        fb = FindHiddenGift.Feedback()
                        fb.gift_ever_detected = True
                        fb.traveled_distance = self.traveled
                        goal_handle.publish_feedback(fb)
                        return
                    
                    self.pub.publish(twist)
                    time.sleep(0.01)
                
                self.stop()
                self.traveled += abs(self.pose.y - start_y)

    def execute_callback(self, goal_handle):
        self.get_logger().info('Начинаю поиск')
        
        self.gift_x_min = None
        self.gift_x_max = None
        self.gift_y_min = None
        self.gift_y_max = 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()
        center = self.get_gift_center()
        
        if center:
            result.gift_found = True
            result.gift_coordinates.x = center[0]
            result.gift_coordinates.y = center[1]
            self.get_logger().info(f'Центр: ({center[0]:.3f}, {center[1]:.3f})')
        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()