diff --git a/config/unity.rviz b/config/unity.rviz index 3f8f0aa..f864b86 100644 --- 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 diff --git a/launch/unity.launch.py b/launch/unity.launch.py index ab4a13d..e733159 100644 --- 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, ] ) diff --git a/unity_robot_controller/fire_extinguisher_emulator.py b/unity_robot_controller/fire_extinguisher_emulator.py index ad40ec9..a1b25ab 100644 --- 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ₛ (вектор от сферы до камеры) diff --git a/unity_robot_controller/joy_control_node.py b/unity_robot_controller/joy_control_node.py index f94ed52..7721c39 100644 --- 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()