spheres cheking

unity_robot_controller/fire_extinguisher_emulator.py

@@ -9,6 +9,8 @@ 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):
 
@@ -19,10 +21,19 @@ class FireExtinguisherEmulatorNode(Node):
         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)
@@ -46,6 +57,11 @@ class FireExtinguisherEmulatorNode(Node):
         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.01, self.target_timer_cb)  # 100 Hz
         self.create_subscription(Point, 'target_point', self.target_cb, 10)
@@ -55,12 +71,19 @@ class FireExtinguisherEmulatorNode(Node):
         self.create_service(Trigger, 'fe_burst', self.fe_burst_cb)
 
     def fe_burst_cb(self, req, res):
-        '''
-        TODO successful burst logic
-        '''
 
-        res.success = False
+        intersected_spheres = find_intersected_spheres(
-        res.message = "0" # add ID of source of fire if success
+            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
 
@@ -72,7 +95,7 @@ class FireExtinguisherEmulatorNode(Node):
 
     def cam_info_cb(self, msg):
         """Save camera info and shutdown subscription"""
-        if self.camera_info is None:
+        if self.camera_info is None or self.cam_transform is None:
             self.camera_info = msg
             self.width = msg.width
             self.height = msg.height
@@ -86,6 +109,24 @@ class FireExtinguisherEmulatorNode(Node):
             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)
@@ -135,11 +176,11 @@ class FireExtinguisherEmulatorNode(Node):
                 y /= norm
                 z /= norm
 
-                vector_msg = Vector3()
+                self.vector_msg = Vector3()
-                vector_msg.x = x
+                self.vector_msg.x = x
-                vector_msg.y = y
+                self.vector_msg.y = y
-                vector_msg.z = z
+                self.vector_msg.z = z
-                self.fire_ext_vector_pub.publish(vector_msg)
+                self.fire_ext_vector_pub.publish(self.vector_msg)
 
     def raw_image_cb(self, raw_msg):
         """Draw crosshair on image and publish 3D vector"""
@@ -188,6 +229,79 @@ class FireExtinguisherEmulatorNode(Node):
         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()

unity_robot_controller/unity_robot_controller.py

@@ -54,7 +54,7 @@ class UnityRobotController(RobotController):
             self._node.get_logger().error("Fire extinguisher is out of fuel!")
             return False
 
-        self._node.get_logger().error("Fire extingusher burst is sperforming")
+        self._node.get_logger().error("Fire extingusher burst is performing")
         future = self.fe_burst_srv.call_async(Trigger.Request())
         future.add_done_callback(self._fe_burst_done_cb)
         return True