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.callback_group = ReentrantCallbackGroup()
        
        self._action_server = ActionServer(
            self, FindHiddenGift, 'find_hidden_gift',
            self.execute_callback, callback_group=self.callback_group
        )
        
        self.cmd_vel_publisher = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)
        
        self.create_subscription(
            Pose, '/turtle1/pose', self.pose_callback, 10,
            callback_group=self.callback_group
        )
        
        self.create_subscription(
            Color, '/turtle1/color_sensor', self.color_callback, 10,
            callback_group=self.callback_group
        )

        self.current_pose = None
        self.gift_found = False
        self.gift_coordinates = None
        self.traveled_distance = 0.0
        
        self.linear_speed = 2.0
        self.grid_step = 0.5
        self.angle_tolerance = 0.02
        self.position_tolerance = 0.05
        
        self.get_logger().info('Action Server запущен')

    def pose_callback(self, msg):
        self.current_pose = msg

    def color_callback(self, msg):
        if msg.r == 0 and msg.g == 255 and msg.b == 0:
            if not self.gift_found and self.current_pose:
                self.gift_found = True
                self.gift_coordinates = (self.current_pose.x, self.current_pose.y)
                self.get_logger().info(
                    f'🎁 Подарок найден: ({self.gift_coordinates[0]:.2f}, {self.gift_coordinates[1]:.2f})'
                )

    def wait_for_pose(self, timeout=5.0):
        start = time.time()
        while self.current_pose is None and rclpy.ok():
            if time.time() - start > timeout:
                return False
            time.sleep(0.05)
        return True

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

    def stop(self):
        twist = Twist()
        self.cmd_vel_publisher.publish(twist)

    def rotate_to_angle(self, target_angle):
        if not self.wait_for_pose():
            return
        
        Kp = 4.0
        min_speed, max_speed = 0.1, 2.0
        twist = Twist()
        
        while rclpy.ok() and not self.gift_found:
            error = self.normalize_angle(target_angle - self.current_pose.theta)
            
            if abs(error) < self.angle_tolerance:
                break
            
            speed = max(min_speed, min(max_speed, abs(Kp * error)))
            twist.angular.z = speed if error > 0 else -speed
            self.cmd_vel_publisher.publish(twist)
            time.sleep(0.02)
        
        self.stop()

    def move_to_point(self, target_x, target_y):
        if not self.wait_for_pose():
            return False
        
        start_x, start_y = self.current_pose.x, self.current_pose.y
        
        target_angle = math.atan2(target_y - start_y, target_x - start_x)
        self.rotate_to_angle(target_angle)
        
        if self.gift_found:
            return False
        
        twist = Twist()
        
        while rclpy.ok() and not self.gift_found:
            dx = target_x - self.current_pose.x
            dy = target_y - self.current_pose.y
            distance = math.sqrt(dx*dx + dy*dy)
            
            if distance < self.position_tolerance:
                break
            
            target_angle = math.atan2(dy, dx)
            angle_error = self.normalize_angle(target_angle - self.current_pose.theta)
            
            twist.linear.x = min(self.linear_speed, max(0.3, self.linear_speed * distance / 0.5))
            twist.angular.z = 2.0 * angle_error
            self.cmd_vel_publisher.publish(twist)
            time.sleep(0.02)
        
        self.stop()
        
        dx = self.current_pose.x - start_x
        dy = self.current_pose.y - start_y
        self.traveled_distance += math.sqrt(dx*dx + dy*dy)
        
        return not self.gift_found

    def search_snake_pattern(self, search_area, goal_handle):
        x_min = search_area.bottom_left.x + self.grid_step / 2
        x_max = search_area.top_right.x - self.grid_step / 2
        y_min = search_area.bottom_left.y + self.grid_step / 2
        y_max = search_area.top_right.y - self.grid_step / 2
        
        self.move_to_point(x_min, y_min)
        
        current_y = y_min
        moving_right = True
        
        while current_y <= y_max and rclpy.ok() and not self.gift_found:
            target_x = x_max if moving_right else x_min
            
            if not self.move_to_point(target_x, current_y):
                return
            
            feedback = FindHiddenGift.Feedback()
            feedback.gift_ever_detected = self.gift_found
            feedback.traveled_distance = self.traveled_distance
            goal_handle.publish_feedback(feedback)
            
            current_y += self.grid_step
            moving_right = not moving_right
            
            if current_y <= y_max and not self.gift_found:
                if not self.move_to_point(self.current_pose.x, current_y):
                    return

    def execute_callback(self, goal_handle):
        self.get_logger().info('Получен запрос на поиск подарка')
        
        self.gift_found = False
        self.gift_coordinates = None
        self.traveled_distance = 0.0
        
        search_area = goal_handle.request.search_area
        self.get_logger().info(
            f'Область: ({search_area.bottom_left.x:.1f}, {search_area.bottom_left.y:.1f}) - '
            f'({search_area.top_right.x:.1f}, {search_area.top_right.y:.1f})'
        )
        
        if not self.wait_for_pose():
            goal_handle.abort()
            result = FindHiddenGift.Result()
            result.gift_found = False
            return result
        
        self.search_snake_pattern(search_area, goal_handle)
        
        result = FindHiddenGift.Result()
        
        if self.gift_found:
            result.gift_found = True
            result.gift_coordinates.x = self.gift_coordinates[0]
            result.gift_coordinates.y = self.gift_coordinates[1]
            self.get_logger().info(f'Подарок: ({self.gift_coordinates[0]:.2f}, {self.gift_coordinates[1]:.2f})')
        else:
            result.gift_found = False
            self.get_logger().info('Подарок не найден')
        
        self.get_logger().info(f'Пройдено: {self.traveled_distance:.2f} м')
        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()