From a32b2951f303830f336e00f42945df14b4fb31ca Mon Sep 17 00:00:00 2001 From: moscowsky Date: Tue, 23 Jun 2026 11:57:26 +0300 Subject: [PATCH] fire kill in unity works --- config/unity.rviz | 59 ++++-- launch/joy_control.launch.py | 1 + launch/unity.launch.py | 6 +- .../fire_extinguisher_emulator.py | 168 +++++++++++++----- .../unity_robot_controller.py | 9 +- 5 files changed, 182 insertions(+), 61 deletions(-) diff --git a/config/unity.rviz b/config/unity.rviz index f864b86..9e36f10 100644 --- a/config/unity.rviz +++ b/config/unity.rviz @@ -28,7 +28,7 @@ Visualization Manager: Class: "" Displays: - Alpha: 0.5 - Cell Size: 50 + Cell Size: 10 Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true @@ -38,11 +38,11 @@ Visualization Manager: Name: Grid Normal Cell Count: 0 Offset: - X: 20 - Y: 10 + X: 200 + Y: 200 Z: 0 Plane: XY - Plane Cell Count: 6 + Plane Cell Count: 60 Reference Frame: Value: true - Class: rviz_default_plugins/Image @@ -132,6 +132,45 @@ Visualization Manager: {} Update Interval: 0 Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: CrossHair Vector + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sirius_bot/crosshair_vector + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /locations_markers + Value: true + - Class: rviz_default_plugins/Axes + Enabled: false + Length: 10 + Name: Axes + Radius: 1 + Reference Frame: + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 @@ -178,25 +217,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/ThirdPersonFollower - Distance: 61.71200180053711 + Distance: 72.5179443359375 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.47304797172546387 - Y: -0.08951294422149658 - Z: -0.25962093472480774 + X: 5.716884613037109 + Y: 3.2075774669647217 + Z: -3.7333018553908914e-05 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5053984522819519 + Pitch: 0.5053315162658691 Target Frame: base_link Value: ThirdPersonFollower (rviz_default_plugins) - Yaw: 3.5853965282440186 + Yaw: 3.1497514247894287 Saved: - Class: rviz_default_plugins/ThirdPersonFollower Distance: 2.978720188140869 diff --git a/launch/joy_control.launch.py b/launch/joy_control.launch.py index 7e95e45..03fef36 100644 --- a/launch/joy_control.launch.py +++ b/launch/joy_control.launch.py @@ -57,6 +57,7 @@ def generate_launch_description(): output="both", parameters=[ ParameterFile(joy_params), + {"fire_ext_max_capacity": 100}, ], ) diff --git a/launch/unity.launch.py b/launch/unity.launch.py index e733159..581af6d 100644 --- a/launch/unity.launch.py +++ b/launch/unity.launch.py @@ -57,7 +57,9 @@ def generate_launch_description(): namespace=LaunchConfiguration("namespace"), parameters=[ {"use_sim_time": LaunchConfiguration("use_sim_time")}, - {"crosshair_alpha": 1.0} + {"crosshair_alpha": 1.0}, + {"sof_cos_sim": 0.999}, + {"fe_distance": 5.0}, ], remappings=[ ("raw_image", "/sirius_bot/camera/image_view"), @@ -81,7 +83,7 @@ def generate_launch_description(): pkg_gen_location_on_mesh = FindPackageShare(package="").find("gen_locations_on_mesh") - mesh_path = PathJoinSubstitution([pkg_gen_location_on_mesh, "share", "scene_init.ply"]) + mesh_path = PathJoinSubstitution([pkg_gen_location_on_mesh, "share", "sirius_scene_floor.ply"]) cache_path = PathJoinSubstitution([pkg_gen_location_on_mesh, "share", "cache.pkl"]) diff --git a/unity_robot_controller/fire_extinguisher_emulator.py b/unity_robot_controller/fire_extinguisher_emulator.py index a1b25ab..2f819a8 100644 --- a/unity_robot_controller/fire_extinguisher_emulator.py +++ b/unity_robot_controller/fire_extinguisher_emulator.py @@ -3,7 +3,7 @@ import rclpy from rclpy.node import Node from rclpy.executors import SingleThreadedExecutor, MultiThreadedExecutor -from geometry_msgs.msg import Point, Vector3 +from geometry_msgs.msg import Point, Vector3, PoseStamped from sensor_msgs.msg import Image, CameraInfo from cv_bridge import CvBridge import cv2 @@ -12,6 +12,9 @@ from std_srvs.srv import Trigger from tf2_ros import Buffer, TransformListener from gen_locations_on_mesh_msgs.msg import LocationsOnMesh from gen_locations_on_mesh_msgs.srv import RemoveLocation +from scipy.spatial.transform import Rotation +from visualization_msgs.msg import MarkerArray + class FireExtinguisherEmulatorNode(Node): @@ -27,7 +30,7 @@ class FireExtinguisherEmulatorNode(Node): self.declare_parameter('show_target', True) - self.declare_parameter('sof_r', 0.5) + self.declare_parameter('sof_cos_sim', 0.5) self.crosshair_alpha = self.get_parameter('crosshair_alpha').value self.crosshair_speed = self.get_parameter('crosshair_speed').value @@ -37,14 +40,14 @@ class FireExtinguisherEmulatorNode(Node): self.robot_frame = self.get_parameter('robot_frame').value self.show_target = self.get_parameter('show_target').value - self.sof_r = self.get_parameter('sof_r').value + self.sof_cos_sim = self.get_parameter('sof_cos_sim').value # Sources of Fire positions in (id, 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) + self.fire_ext_vector_pub = self.create_publisher(PoseStamped, 'crosshair_vector', 10) # State variables self.target_x = 0.0 @@ -71,7 +74,9 @@ class FireExtinguisherEmulatorNode(Node): self.tf_listener = TransformListener(self.tf_buffer, self) # remove SoF - self.remove_sof_srv = self.create_client(RemoveLocation, 'remove_location') + self.remove_sof_srv = self.create_client(RemoveLocation, '/remove_location') + + self.fe_vector = None # Timers and subscriptions self.target_timer = self.create_timer(0.1, self.target_timer_cb) # 10 Hz @@ -80,25 +85,35 @@ class FireExtinguisherEmulatorNode(Node): 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_subscription(LocationsOnMesh, 'locations', self.locations_cb, 10) + self.create_subscription(MarkerArray, '/locations_markers', self.locations_cb, 10) self.create_service(Trigger, 'fe_burst', self.fe_burst_cb) def locations_cb(self, msg): - for idx, x, y, z in zip(msg.location_id, msg.x, msg.y, msg.z): - self.sof_poisitions.append((idx, x, y, z, self.sof_r)) + #for idx, x, y, z in zip(msg.location_id, msg.x, msg.y, msg.z): + self.sof_poisitions = [] + for marker in msg.markers: + self.sof_poisitions.append((marker.id, + marker.pose.position.x, + marker.pose.position.y, + marker.pose.position.z, + )) def fe_burst_cb(self, req, res): intersected_spheres = find_intersected_spheres( spheres=self.sof_poisitions, - camera_vector=self.vector_msg, # ваш Vector3 с направлением + camera_vector=self.fe_vector, # ваш Vector3 с направлением camera_vector_length=self.fe_distance, # длина луча tf_buffer=self.tf_buffer, camera_frame=self.camera_info.header.frame_id, # из CameraInfo - world_frame="map" + world_frame="map", + sof_cos_sim=self.sof_cos_sim, + print_f = self.get_logger().info ) + self.get_logger().info(f"{intersected_spheres}") + if len(intersected_spheres): res.success = True res.message = str(intersected_spheres[0][0]) # add ID of source of fire if success @@ -184,8 +199,6 @@ class FireExtinguisherEmulatorNode(Node): 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 @@ -200,11 +213,20 @@ class FireExtinguisherEmulatorNode(Node): 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) + self.fe_vector = (x, y, z) + + msg = PoseStamped() + msg.header.frame_id = self.camera_info.header.frame_id + msg.header.stamp = self.get_clock().now().to_msg() + + q = quaternion_from_two_vectors([1.0, 0.0, 0.0], self.fe_vector) + msg.pose.orientation.x = float(q[0]) + msg.pose.orientation.y = float(q[1]) + msg.pose.orientation.z = float(q[2]) + msg.pose.orientation.w = float(q[3]) + + self.fire_ext_vector_pub.publish(msg) + def raw_image_cb(self, raw_msg): """Draw crosshair on image and publish 3D vector""" @@ -260,12 +282,14 @@ class FireExtinguisherEmulatorNode(Node): def find_intersected_spheres( - spheres: list[tuple[int, float, float, float, float]], # [(id, x, y, z, r)] - camera_vector: Vector3, # единичный вектор в координатах камеры + spheres: list[tuple[int, float, float, float]], # [(id, x, y, z)] + camera_vector: (float, float, float), # единичный вектор в координатах камеры camera_vector_length: float, # длина луча tf_buffer: Buffer, camera_frame: str, - world_frame: str = "world" + world_frame: str, + sof_cos_sim, + print_f = print ) -> list[tuple[int, float, float]]: """ Находит все сферы, пересекаемые лучом. @@ -275,8 +299,8 @@ def find_intersected_spheres( # 1. Получаем преобразование камеры -> мир transform = tf_buffer.lookup_transform( - world_frame, # target frame (мир) - camera_frame, # source frame (камера) + world_frame, # target frame + camera_frame, # source frame rclpy.time.Time() # latest transform ) @@ -286,10 +310,11 @@ def find_intersected_spheres( transform.transform.translation.y, transform.transform.translation.z ]) + print_f(f"{p0}") # 3. Преобразуем вектор камеры в координаты мира # Вектор как точка (с нулевой координатой w) - camera_vec = np.array([camera_vector.x, camera_vector.y, camera_vector.z]) + camera_vec = np.array(camera_vector) # Матрица вращения из transform (кватернион -> матрица) q = transform.transform.rotation @@ -306,31 +331,80 @@ def find_intersected_spheres( # 4. Проходим по всем сферам intersected = [] + l2 = camera_vector_length**2 for sphere in spheres: - idx, xs, ys, zs, r = sphere - 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 + idx, xs, ys, zs = sphere + + dx = p0[0] - xs + dy = p0[1] - ys + D2 = (dx)**2 + (dy)**2 + + if D2 <= l2: + dz = p0[2] - zs + + def cos_sim(a, b): + return np.dot(a, b) / (np.linalg.norm(a) * np.linalg.norm(b)) + + sim = cos_sim(u, -np.array((dx, dy, dz))) + + if sim >= sof_cos_sim: + intersected.append((idx, np.sqrt(D2), sim)) + + return sorted(intersected, key=lambda x: x[1]) + + + + # for sphere in spheres: + # idx, xs, ys, zs, r = sphere + # 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 quaternion_from_two_vectors(a, b): + a = np.array(a, dtype=float) + b = np.array(b, dtype=float) + a /= np.linalg.norm(a) + b /= np.linalg.norm(b) + + v = np.cross(a, b) + c = np.dot(a, b) + + if c < -0.999999: + axis = np.array([1.0, 0.0, 0.0]) + if abs(a[0]) > 0.9: + axis = np.array([0.0, 1.0, 0.0]) + v = np.cross(a, axis) + v /= np.linalg.norm(v) + q = np.array([v[0], v[1], v[2], 0.0]) + return q + + s = np.sqrt((1.0 + c) * 2.0) + invs = 1.0 / s + qx = v[0] * invs + qy = v[1] * invs + qz = v[2] * invs + qw = 0.5 * s + return np.array([qx, qy, qz, qw], dtype=float) def main(args=None): diff --git a/unity_robot_controller/unity_robot_controller.py b/unity_robot_controller/unity_robot_controller.py index b8355c8..5c076f5 100644 --- a/unity_robot_controller/unity_robot_controller.py +++ b/unity_robot_controller/unity_robot_controller.py @@ -11,7 +11,7 @@ from std_srvs.srv import Trigger class UnityRobotController(RobotController): def __init__(self, node): - super().__init__() + self._node = node @@ -19,10 +19,15 @@ class UnityRobotController(RobotController): self._node.declare_parameter('max_angular_speed', 1.5) self._node.declare_parameter('fire_ext_pixel_speed', 1.0) + self._node.declare_parameter('fire_ext_max_capacity', 5) self._lin_max = self._node.get_parameter('max_linear_speed').value self._ang_max = self._node.get_parameter('max_angular_speed').value + fire_ext_max_capacity = self._node.get_parameter('fire_ext_max_capacity').value + + super().__init__(fire_ext_max_capacity = fire_ext_max_capacity) + self._twist_pub = self._node.create_publisher(Twist, 'cmd_vel', 10) self._target_point_pub = self._node.create_publisher(Point, 'target_point', 10) @@ -62,7 +67,7 @@ class UnityRobotController(RobotController): def _fe_burst_done_cb(self, future): result = future.result() if result.success: - self._register_exted_sof(self.get_relative_time(), {'id': success.message}) + self._register_exted_sof(self.get_relative_time(), {'id': result.message}) def send_fire_ext_pose_cmd(self, horisontal_pose, vertical_pose):