joy to common

1 month ago · be6398ba68
parent b7c0bac66a
commit be6398ba68
4 changed files with 45 additions and 12 deletions
--- a/config/unity.rviz
+++ b/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
--- a/launch/unity.launch.py
+++ b/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,
        ]
    )
--- a/unity_robot_controller/fire_extinguisher_emulator.py
+++ b/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ₛ (вектор от сферы до камеры)
--- a/unity_robot_controller/joy_control_node.py
+++ b/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()