fire kill in unity works

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
+        X: 200
-        Y: 10
+        Y: 200
         Z: 0
       Plane: XY
-      Plane Cell Count: 6
+      Plane Cell Count: 60
       Reference Frame: <Fixed 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: <Fixed 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
+        X: 5.716884613037109
-        Y: -0.08951294422149658
+        Y: 3.2075774669647217
-        Z: -0.25962093472480774
+        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

launch/joy_control.launch.py

@@ -57,6 +57,7 @@ def generate_launch_description():
             output="both",
             parameters=[
                 ParameterFile(joy_params),
+                {"fire_ext_max_capacity": 100},
             ],
     )
 

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"])
 

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):
+        #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))
+        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.fe_vector = (x, y, z)
-                self.vector_msg.x = x
+
-                self.vector_msg.y = y
+                msg = PoseStamped()
-                self.vector_msg.z = z
+                msg.header.frame_id = self.camera_info.header.frame_id
-                self.fire_ext_vector_pub.publish(self.vector_msg)
+                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)]
+    spheres: list[tuple[int, float, float, float]],  # [(id, x, y, z)]
-    camera_vector: Vector3,                           # единичный вектор в координатах камеры
+    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 (мир)
+        world_frame,           # target frame
-        camera_frame,          # source 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
+        idx, xs, ys, zs = sphere
-        p_s = np.array([xs, ys, zs])  # центр сферы в мире
 
-        # k = p₀ - pₛ (вектор от сферы до камеры)
+        dx = p0[0] - xs
-        k = p0 - p_s
+        dy = p0[1] - ys
+        D2 = (dx)**2 + (dy)**2
 
-        # Коэффициенты квадратного уравнения
+        if D2 <= l2:
-        # t² + 2*b*t + c = 0, где a = 1 (у нас u единичный)
+            dz = p0[2] - zs
-        b = np.dot(k, u)
-        c = np.dot(k, k) - r**2
 
-        # Дискриминант
+            def cos_sim(a, b):
-        D = b**2 - c
+                return np.dot(a, b) / (np.linalg.norm(a) * np.linalg.norm(b))
 
-        if D >= 0:  # есть пересечение (D=0 — касание, D>0 — 2 точки)
+            sim = cos_sim(u, -np.array((dx, dy, dz)))
-            sqrt_D = np.sqrt(D)
-            t1 = -b - sqrt_D
-            t2 = -b + sqrt_D
 
-            # Проверяем, что хотя бы одна точка пересечения на луче [0, length]
+            if sim >= sof_cos_sim:
-            if (0 <= t1 <= camera_vector_length) or (0 <= t2 <= camera_vector_length):
+                intersected.append((idx, np.sqrt(D2), sim))
-                intersected.append((idx, t1, t2))
 
-    return intersected
+    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):

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):