From 57e146b78212886a02187d40e82bb061422b61c8 Mon Sep 17 00:00:00 2001 From: moscowsky Date: Wed, 24 Jun 2026 18:10:55 +0300 Subject: [PATCH] light and fall recovery --- config/T-29.yaml | 2 ++ config/unity.rviz | 18 +++++----- launch/unity.launch.py | 7 ++-- unity_robot_controller/joy_control_node.py | 10 ++++++ unity_robot_controller/robot_controller | 2 +- .../unity_robot_controller.py | 35 +++++++++++++++++-- 6 files changed, 57 insertions(+), 17 deletions(-) diff --git a/config/T-29.yaml b/config/T-29.yaml index 951403b..fe9032b 100644 --- a/config/T-29.yaml +++ b/config/T-29.yaml @@ -10,6 +10,8 @@ sirius_bot/joy_control: fe_burst_btn: 2 + lights_btn: 1 + fall_reset_btn: 3 crosshair_plus_x_btn: -1 crosshair_minus_x_btn: -1 diff --git a/config/unity.rviz b/config/unity.rviz index 4373717..f0b9676 100644 --- a/config/unity.rviz +++ b/config/unity.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 652 + Tree Height: 348 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -217,25 +217,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/ThirdPersonFollower - Distance: 72.5179443359375 + Distance: 11.205231666564941 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 5.716884613037109 - Y: 3.2075774669647217 - Z: -3.7333018553908914e-05 - Focal Shape Fixed Size: true + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5053315162658691 + Pitch: 0.5403984189033508 Target Frame: base_link Value: ThirdPersonFollower (rviz_default_plugins) - Yaw: 3.1497514247894287 + Yaw: 2.730400323867798 Saved: - Class: rviz_default_plugins/ThirdPersonFollower Distance: 2.978720188140869 @@ -285,7 +285,7 @@ Window Geometry: Height: 1008 Hide Left Dock: true Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000330fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000004c00000330000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000039500000330fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000004c000001980000000000fffffffaffffffff0100000002fb000000060052004100570000000000ffffffff0000005c01000003fb0000000a005600690065007700730000000670000001100000011001000003fb0000001200430072006f007300730068006100690072010000004c000003300000002201000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d0065010000000000000780000002bd01000003fb0000000800540069006d00650100000000000004500000000000000000000003ea0000033000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000330fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000004c00000330000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000039500000330fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000004c000001100000000000fffffffaffffffff0100000002fb000000060052004100570000000000ffffffff0000005c01000003fb0000000a005600690065007700730000000670000001100000011001000003fb0000001200430072006f007300730068006100690072010000004c000003300000002201000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d0065010000000000000780000002bd01000003fb0000000800540069006d00650100000000000004500000000000000000000003ea0000033000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RAW: collapsed: false Selection: diff --git a/launch/unity.launch.py b/launch/unity.launch.py index c7227ec..396eae4 100644 --- a/launch/unity.launch.py +++ b/launch/unity.launch.py @@ -83,18 +83,15 @@ 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", "sirius_scene_floor.ply"]) - - cache_path = PathJoinSubstitution([pkg_gen_location_on_mesh, "share", "cache.pkl"]) + mesh_path = PathJoinSubstitution([pkg_gen_location_on_mesh, "share", "sirius_scene_floor_and_objs.ply"]) 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, - 'max_z_coord': 3.}]) + 'max_z_coord': 10.}]) return LaunchDescription( launch_args diff --git a/unity_robot_controller/joy_control_node.py b/unity_robot_controller/joy_control_node.py index 7721c39..4f12603 100644 --- a/unity_robot_controller/joy_control_node.py +++ b/unity_robot_controller/joy_control_node.py @@ -23,6 +23,8 @@ class JoyControlNode(Node): self.declare_parameter('crosshair_minus_y_btn', 4) self.declare_parameter('fe_burst_btn', 5) + self.declare_parameter('lights_btn', 6) + self.declare_parameter('fall_reset_btn', 7) self.declare_parameter('crosshair_step', 0.01) @@ -40,6 +42,8 @@ class JoyControlNode(Node): self.crosshair_minus_y_btn = self.get_parameter('crosshair_minus_y_btn').value self.fe_burst_btn = self.get_parameter('fe_burst_btn').value + self.lights_btn = self.get_parameter('lights_btn').value + self.fall_reset_btn = self.get_parameter('fall_reset_btn').value self.crosshair_x_axis = self.get_parameter('crosshair_x_axis').value self.crosshair_y_axis = self.get_parameter('crosshair_y_axis').value @@ -128,7 +132,13 @@ class JoyControlNode(Node): if self.was_button_pressed(msg, self.fe_burst_btn): self.URC.send_fire_ext_burst_cmd() + # lights control + if self.was_button_pressed(msg, self.lights_btn): + self.URC.send_lights_cmd() + # fall reset + if self.was_button_pressed(msg, self.fall_reset_btn): + self.URC.send_fall_reset_cmd() def main(args=None): rclpy.init(args=args) diff --git a/unity_robot_controller/robot_controller b/unity_robot_controller/robot_controller index b34711e..730c241 160000 --- a/unity_robot_controller/robot_controller +++ b/unity_robot_controller/robot_controller @@ -1 +1 @@ -Subproject commit b34711ebc690cf9199b15bc3074e08f258303003 +Subproject commit 730c2417dd700d72506ba9dfdb153649e279e18b diff --git a/unity_robot_controller/unity_robot_controller.py b/unity_robot_controller/unity_robot_controller.py index 60fe947..aa4cf15 100644 --- a/unity_robot_controller/unity_robot_controller.py +++ b/unity_robot_controller/unity_robot_controller.py @@ -24,7 +24,7 @@ class UnityRobotController(RobotController): self._node.declare_parameter('fire_ext_pixel_speed', 1.0) self._node.declare_parameter('fire_ext_max_capacity', 5) - self._node.declare_parameter('max_tilt_deg', 15.) + self._node.declare_parameter('max_tilt_deg', 90.) self._lin_max = self._node.get_parameter('max_linear_speed').value self._ang_max = self._node.get_parameter('max_angular_speed').value @@ -32,6 +32,8 @@ class UnityRobotController(RobotController): self._max_tilt = np.deg2rad(self._node.get_parameter('max_tilt_deg').value) self._FALL = False + self._LIGHTS = False + fire_ext_max_capacity = self._node.get_parameter('fire_ext_max_capacity').value super().__init__(fire_ext_max_capacity = fire_ext_max_capacity) @@ -46,6 +48,9 @@ class UnityRobotController(RobotController): self.fe_burst_srv = self._node.create_client(Trigger, 'fe_burst') + self.lights_srv = self._node.create_client(Trigger, '/unity/light_trigger') + self.fall_reset_srv = self._node.create_client(Trigger, '/unity/robot_reset_trigger') + self.start_time = self.get_time_seconds() # TODO save it when first cmd is given # TODO create collision subscriber from Unity @@ -92,7 +97,7 @@ class UnityRobotController(RobotController): self._node.get_logger().error("Fire extinguisher is out of fuel!") return False - self._node.get_logger().error("Fire extingusher burst is performing") + self._node.get_logger().info("Fire extingusher burst is performing") future = self.fe_burst_srv.call_async(Trigger.Request()) future.add_done_callback(self._fe_burst_done_cb) return True @@ -153,6 +158,32 @@ class UnityRobotController(RobotController): self.pub_status() self._FALL = fall + def send_lights_cmd(self): + future = self.lights_srv.call_async(Trigger.Request()) + future.add_done_callback(self._lights_done_cb) + + + def _lights_done_cb(self, future): + result = future.result() + self._LIGHTS = not self._LIGHTS + self._node.get_logger().info(f"Lights is {'ON' if self._LIGHTS else 'OFF'}") + + def send_fall_reset_cmd(self): + if self._FALL: + self._node.get_logger().info("Trying to reset robot...") + future = self.fall_reset_srv.call_async(Trigger.Request()) + future.add_done_callback(self._fall_reset_done_cb) + else: + self._node.get_logger().error("Robot didnot fall yet") + + + def _fall_reset_done_cb(self, future): + result = future.result() + self._FALL = False + self._node.get_logger().info(f"Fall reset done {result.success} {result.message}") + + +