diff --git a/unity_robot_controller/fire_extinguisher_emulator.py b/unity_robot_controller/fire_extinguisher_emulator.py index bd504a6..ec12cd6 100644 --- a/unity_robot_controller/fire_extinguisher_emulator.py +++ b/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 - res.message = "0" # add ID of source of fire if success + 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 @@ -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() - vector_msg.x = x - vector_msg.y = y - vector_msg.z = z - self.fire_ext_vector_pub.publish(vector_msg) + 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""" @@ -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() diff --git a/unity_robot_controller/unity_robot_controller.py b/unity_robot_controller/unity_robot_controller.py index c2d0b90..1dd84f6 100644 --- a/unity_robot_controller/unity_robot_controller.py +++ b/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