#!/usr/bin/env python3 import rclpy from rclpy.node import Node from rclpy.executors import SingleThreadedExecutor, MultiThreadedExecutor from geometry_msgs.msg import Point, Vector3 from sensor_msgs.msg import Image, CameraInfo from cv_bridge import CvBridge import cv2 import numpy as np from std_srvs.srv import Trigger from tf2_ros import Buffer, TransformListener class FireExtinguisherEmulatorNode(Node): def __init__(self): super().__init__('fire_extinguisher_emulator') self.declare_parameter('crosshair_alpha', 0.5) self.declare_parameter('crosshair_speed', 500.) # pixel/sec self.declare_parameter('crosshair_color', [255, 0, 0]) # BGR (blue) self.declare_parameter('robot_frame', "base_link") self.declare_parameter('fe_distance', 5.) self.crosshair_alpha = self.get_parameter('crosshair_alpha').value self.crosshair_speed = self.get_parameter('crosshair_speed').value self.crosshair_color = self.get_parameter('crosshair_color').value self.fe_distance = self.get_parameter('fe_distance').value self.robot_frame = self.get_parameter('robot_frame').value # Sources of Fire positions in (x, y, z, r) self.sof_poisitions = [] # Publishers self.fire_ext_image_pub = self.create_publisher(Image, 'crosshair_image', 10) self.fire_ext_vector_pub = self.create_publisher(Vector3, 'crosshair_vector', 10) # State variables self.target_x = 0.0 self.target_y = 0.0 self.current_x = None self.current_y = None # Camera info self.camera_info = None self.width = 0 self.height = 0 self.fx = 0.0 self.fy = 0.0 self.cx = 0.0 self.cy = 0.0 # Image buffer self.current_image = None self.bridge = CvBridge() # TF2 setup self.cam_transform = None self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) # Timers and subscriptions self.target_timer = self.create_timer(0.1, self.target_timer_cb) # 10 Hz self.create_subscription(Point, 'target_point', self.target_cb, 10) self.sub_camera_info = self.create_subscription(CameraInfo, 'camera_info', self.cam_info_cb, 10) self.create_subscription(Image, 'raw_image', self.raw_image_cb, 10) self.create_service(Trigger, 'fe_burst', self.fe_burst_cb) def fe_burst_cb(self, req, res): intersected_spheres = find_intersected_spheres( spheres=self.sof_poisitions, camera_vector=self.vector_msg, # ваш Vector3 с направлением camera_vector_length=self.fe_distance, # длина луча tf_buffer=self.tf_buffer, camera_frame=self.camera_info.header.frame_id, # из CameraInfo world_frame="map" ) if len(intersected_spheres): res.success = True res.message = str(intersected_spheres[0][0]) # add ID of source of fire if success return res def target_cb(self, msg): """Received target point in normalized coordinates [-1, 1]""" self.target_x = min(max(-1.0, msg.x), 1.0) self.target_y = min(max(-1.0, msg.y), 1.0) def cam_info_cb(self, msg): """Save camera info and shutdown subscription""" if self.camera_info is None or self.cam_transform is None: self.camera_info = msg self.width = msg.width self.height = msg.height self.fx = msg.k[0] # fx self.fy = msg.k[4] # fy self.cx = msg.k[2] # cx self.cy = msg.k[5] # cy self.get_logger().info(f"Camera info received: {self.width}x{self.height}, fx={self.fx}, fy={self.fy}") self.current_x = (self.target_x + 1.0) * 0.5 * self.width self.current_y = (self.target_y + 1.0) * 0.5 * self.height try: self.cam_transform = self.tf_buffer.lookup_transform( self.robot_frame, # target frame msg.header.frame_id, # source frame (из CameraInfo) rclpy.time.Time() # текущее время (latest transform) ) self.get_logger().info( f"Transform from {msg.header.frame_id} to {self.robot_frame}: " f"xyz=({self.cam_transform.transform.translation.x}, " f"{self.cam_transform.transform.translation.y}, " f"{self.cam_transform.transform.translation.z})" ) except (Exception) as e: self.get_logger().warning( f"Failed to lookup transform from {msg.header.frame_id} to {self.robot_frame}: {e}" ) # Remove the subscription after getting camera info if hasattr(self, 'sub_camera_info'): self.destroy_subscription(self.sub_camera_info) def target_timer_cb(self): """Smooth movement of crosshair towards target with speed limit""" if self.camera_info is None: self.get_logger().warning(f"Camera info stil not recieved!") return # Calculate target pixels target_pixel_x = (self.target_x + 1.0) * 0.5 * self.width target_pixel_y = self.height - (self.target_y + 1.0) * 0.5 * self.height # Calculate distance to target dx = target_pixel_x - self.current_x dy = target_pixel_y - self.current_y distance = np.sqrt(dx**2 + dy**2) # Move towards target with speed limit if distance > 0.1: speed_pixels = self.crosshair_speed # pixels per second dt = 0.01 # 100 Hz timer max_step = speed_pixels * dt if distance > max_step: step_x = max_step * dx / distance step_y = max_step * dy / distance else: step_x = dx step_y = dy self.current_x += step_x self.current_y += step_y #self.get_logger().info(f"Target {self.target_x} {self.target_y} current {self.current_x} {self.current_y}") # Calculate 3D unit vector from camera frame if self.fx > 0 and self.fy > 0: px = self.current_x py = self.current_y x = (px - self.cx) / self.fx y = (py - self.cy) / self.fy z = 1.0 norm = np.sqrt(x**2 + y**2 + z**2) if norm > 0: x /= norm y /= norm z /= norm self.vector_msg = Vector3() self.vector_msg.x = x self.vector_msg.y = y self.vector_msg.z = z self.fire_ext_vector_pub.publish(self.vector_msg) def raw_image_cb(self, raw_msg): """Draw crosshair on image and publish 3D vector""" if self.camera_info is None: return try: cv_image = self.bridge.imgmsg_to_cv2(raw_msg, desired_encoding='bgr8') except Exception as e: self.get_logger().error(f"Failed to convert image: {e}") return crosshair_size = 20 crosshair_thickness = 2 color = tuple(self.crosshair_color) x1 = int(self.current_x - crosshair_size) x2 = int(self.current_x + crosshair_size) y1 = int(self.current_y) y2 = int(self.current_y) cv2.line(cv_image, (x1, y1), (x2, y2), color, crosshair_thickness) x1 = int(self.current_x) x2 = int(self.current_x) y1 = int(self.current_y - crosshair_size) y2 = int(self.current_y + crosshair_size) cv2.line(cv_image, (x1, y1), (x2, y2), color, crosshair_thickness) cv2.circle(cv_image, (int(self.current_x), int(self.current_y)), 5, color, -1) if self.crosshair_alpha < 1.0: overlay = cv_image.copy() cv_image = cv2.addWeighted(cv_image, 1.0, overlay, self.crosshair_alpha, 0) try: ros_image = self.bridge.cv2_to_imgmsg(cv_image, encoding='bgr8') ros_image.header = raw_msg.header self.fire_ext_image_pub.publish(ros_image) except Exception as e: self.get_logger().error(f"Failed to publish image: {e}") def destroy_node(self): """Clean up subscriptions""" if hasattr(self, 'sub_camera_info'): self.sub_camera_info.close() super().destroy_node() def find_intersected_spheres( spheres: list[tuple[float, float, float, float]], # [(x, y, z, r)] camera_vector: Vector3, # единичный вектор в координатах камеры camera_vector_length: float, # длина луча tf_buffer: Buffer, camera_frame: str, world_frame: str = "world" ) -> list[tuple[int, float, float]]: """ Находит все сферы, пересекаемые лучом. Returns: список (index сферы, t1, t2) — параметры пересечения вдоль луча """ # 1. Получаем преобразование камеры -> мир transform = tf_buffer.lookup_transform( world_frame, # target frame (мир) camera_frame, # source frame (камера) rclpy.time.Time() # latest transform ) # 2. Позиция камеры в координатах мира (p₀) p0 = np.array([ transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z ]) # 3. Преобразуем вектор камеры в координаты мира # Вектор как точка (с нулевой координатой w) camera_vec = np.array([camera_vector.x, camera_vector.y, camera_vector.z]) # Матрица вращения из transform (кватернион -> матрица) q = transform.transform.rotation R = np.array([ [1 - 2*q.y**2 - 2*q.z**2, 2*q.x*q.y - 2*q.z*q.w, 2*q.x*q.z + 2*q.y*q.w], [2*q.x*q.y + 2*q.z*q.w, 1 - 2*q.x**2 - 2*q.z**2, 2*q.y*q.z - 2*q.x*q.w], [2*q.x*q.z - 2*q.y*q.w, 2*q.y*q.z + 2*q.x*q.w, 1 - 2*q.x**2 - 2*q.y**2] ]) # Вектор направления в координатах мира (u) — должен быть единичным u = R @ camera_vec u = u / np.linalg.norm(u) # нормализация # 4. Проходим по всем сферам intersected = [] for idx, (xs, ys, zs, r) in spheres: p_s = np.array([xs, ys, zs]) # центр сферы в мире # k = p₀ - pₛ (вектор от сферы до камеры) k = p0 - p_s # Коэффициенты квадратного уравнения # t² + 2*b*t + c = 0, где a = 1 (у нас u единичный) b = np.dot(k, u) c = np.dot(k, k) - r**2 # Дискриминант D = b**2 - c if D >= 0: # есть пересечение (D=0 — касание, D>0 — 2 точки) sqrt_D = np.sqrt(D) t1 = -b - sqrt_D t2 = -b + sqrt_D # Проверяем, что хотя бы одна точка пересечения на луче [0, length] if (0 <= t1 <= camera_vector_length) or (0 <= t2 <= camera_vector_length): intersected.append((idx, t1, t2)) return intersected def main(args=None): rclpy.init(args=args) node = FireExtinguisherEmulatorNode() executor = SingleThreadedExecutor() #executor = MultiThreadedExecutor(num_threads=4) executor.add_node(node) try: executor.spin() #rclpy.spin(node, executor) #rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()