|
|
#!/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()
|