|
|
|
|
@ -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()
|
|
|
|
|
|
moscowsky
1 month ago