joy to common

config/unity.rviz

@@ -5,7 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded: ~
       Splitter Ratio: 0.5
-    Tree Height: 696
+    Tree Height: 652
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -243,10 +243,10 @@ Window Geometry:
     collapsed: false
   Displays:
     collapsed: false
-  Height: 1052
+  Height: 1008
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000035cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004c0000035c000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000002100000035cfc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000004c000001a80000002200fffffffa000000000100000002fb000000060052004100570100000000ffffffff0000005c01000003fb0000000a005600690065007700730000000670000001100000011001000003fb0000001200430072006f00730073006800610069007201000001f5000001b30000002201000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d0065010000000000000780000002bd01000003fb0000000800540069006d00650100000000000004500000000000000000000004040000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000330fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004c00000330000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000021000000330fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000004c000001940000002200fffffffa000000000100000002fb000000060052004100570100000000ffffffff0000005c01000003fb0000000a005600690065007700730000000670000001100000011001000003fb0000001200430072006f00730073006800610069007201000001e10000019b0000002201000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d0065010000000000000780000002bd01000003fb0000000800540069006d00650100000000000004500000000000000000000004040000033000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   RAW:
     collapsed: false
   Selection:
@@ -258,5 +258,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1920
-  X: 0
+  X: 1920
   Y: 0

launch/unity.launch.py

@@ -78,6 +78,21 @@ def generate_launch_description():
         condition=IfCondition(LaunchConfiguration('start_rviz'))
     )
 
+
+    pkg_gen_location_on_mesh = FindPackageShare(package="").find("gen_locations_on_mesh")
+
+    mesh_path = PathJoinSubstitution([pkg_gen_location_on_mesh, "share", "scene_init.ply"])
+
+    cache_path = PathJoinSubstitution([pkg_gen_location_on_mesh, "share", "cache.pkl"])
+
+    start_gen_location_on_mesh_cmd = Node(
+        name='gen_locations_on_mesh',
+        package='gen_locations_on_mesh',
+        executable='gen_locations_on_mesh',
+        parameters=[{'mesh_filename': mesh_path,
+                   'cache_filename': cache_path,
+                   'init_number_of_points': 100}])
+
     return LaunchDescription(
         launch_args
         + [
@@ -85,5 +100,6 @@ def generate_launch_description():
             #joy,
             fe_emulator,
             rviz,
+            start_gen_location_on_mesh_cmd,
         ]
     )

unity_robot_controller/fire_extinguisher_emulator.py

@@ -10,7 +10,8 @@ import cv2
 import numpy as np
 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
 
 class FireExtinguisherEmulatorNode(Node):
 
@@ -26,6 +27,8 @@ class FireExtinguisherEmulatorNode(Node):
 
         self.declare_parameter('show_target', True)
 
+        self.declare_parameter('sof_r', 0.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
@@ -34,8 +37,9 @@ 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
 
-        # Sources of Fire positions in (x, y, z, r)
+        # Sources of Fire positions in (id, x, y, z, r)
         self.sof_poisitions = []
 
         # Publishers
@@ -66,6 +70,9 @@ class FireExtinguisherEmulatorNode(Node):
         self.tf_buffer = Buffer()
         self.tf_listener = TransformListener(self.tf_buffer, self)
 
+        # remove SoF
+        self.remove_sof_srv = self.create_client(RemoveLocation, 'remove_location')
+
         # Timers and subscriptions
         self.target_timer = self.create_timer(0.1, self.target_timer_cb)  # 10 Hz
 
@@ -73,9 +80,14 @@ 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_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))
+
     def fe_burst_cb(self, req, res):
 
         intersected_spheres = find_intersected_spheres(
@@ -91,8 +103,11 @@ class FireExtinguisherEmulatorNode(Node):
             res.success = True
             res.message = str(intersected_spheres[0][0]) # add ID of source of fire if success
 
-        return res
+            remove_req = RemoveLocation.Request()
+            remove_req.location_id = int(intersected_spheres[0][0])
+            future = self.remove_sof_srv.call_async(remove_req) # TODO: check future
 
+        return res
 
     def target_cb(self, msg):
         """Received target point in normalized coordinates [-1, 1]"""
@@ -245,7 +260,7 @@ class FireExtinguisherEmulatorNode(Node):
 
 
 def find_intersected_spheres(
-    spheres: list[tuple[float, float, float, float]],  # [(x, y, z, r)]
+    spheres: list[tuple[int, float, float, float, float]],  # [(id, x, y, z, r)]
     camera_vector: Vector3,                           # единичный вектор в координатах камеры
     camera_vector_length: float,                      # длина луча
     tf_buffer: Buffer,
@@ -291,7 +306,8 @@ def find_intersected_spheres(
     # 4. Проходим по всем сферам
     intersected = []
 
-    for idx, (xs, ys, zs, r) in spheres:
+    for sphere in spheres:
+        idx, xs, ys, zs, r = sphere
         p_s = np.array([xs, ys, zs])  # центр сферы в мире
 
         # k = p₀ - pₛ (вектор от сферы до камеры)

unity_robot_controller/joy_control_node.py

@@ -7,11 +7,11 @@ from sensor_msgs.msg import Joy
 
 class JoyControlNode(Node):
 
-    def __init__(self):
+    def __init__(self, RobotController):
 
         super().__init__('joy_control')
 
-        self.URC = UnityRobotController(self)
+        self.URC = RobotController(self)
 
         self.declare_parameter('speed_x_axis', 0)
         self.declare_parameter('speed_y_axis', 1)
@@ -132,7 +132,8 @@ class JoyControlNode(Node):
 
 def main(args=None):
     rclpy.init(args=args)
-    node = JoyControlNode()
+
+    node = JoyControlNode(UnityRobotController)
     rclpy.spin(node)
     node.destroy_node()
     rclpy.shutdown()