From 1519f2cb2915a457c629a1c9d41baa631e90ea54 Mon Sep 17 00:00:00 2001 From: moscowsky Date: Thu, 25 Jun 2026 15:40:17 +0300 Subject: [PATCH] status image --- config/unity.rviz | 24 ++++++++-- .../unity_robot_controller.py | 46 +++++++++++++++++++ 2 files changed, 66 insertions(+), 4 deletions(-) diff --git a/config/unity.rviz b/config/unity.rviz index f0b9676..b79b2dc 100644 --- a/config/unity.rviz +++ b/config/unity.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 348 + Tree Height: 652 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -60,7 +60,7 @@ Visualization Manager: Value: /sirius_bot/camera/image_view Value: false - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -72,7 +72,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /sirius_bot/crosshair_image - Value: true + Value: false - Class: rviz_default_plugins/TF Enabled: true Filter (blacklist): "" @@ -171,6 +171,20 @@ Visualization Manager: Radius: 1 Reference Frame: Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: STATUS + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sirius_bot/status_image + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -285,9 +299,11 @@ Window Geometry: Height: 1008 Hide Left Dock: true Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000330fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000004c00000330000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000039500000330fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000004c000001100000000000fffffffaffffffff0100000002fb000000060052004100570000000000ffffffff0000005c01000003fb0000000a005600690065007700730000000670000001100000011001000003fb0000001200430072006f007300730068006100690072010000004c000003300000002201000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d0065010000000000000780000002bd01000003fb0000000800540069006d00650100000000000004500000000000000000000003ea0000033000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000330fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000004c00000330000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000039500000330fc0200000005fb0000000c005300540041005400550053010000004c000003300000002201000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000004c000001100000000000fffffffaffffffff0100000002fb000000060052004100570000000000ffffffff0000005c01000003fb0000000a005600690065007700730000000670000001100000011001000003fb0000001200430072006f007300730068006100690072000000004c000003300000002201000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d0065010000000000000780000002bd01000003fb0000000800540069006d00650100000000000004500000000000000000000003ea0000033000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RAW: collapsed: false + STATUS: + collapsed: false Selection: collapsed: false Time: diff --git a/unity_robot_controller/unity_robot_controller.py b/unity_robot_controller/unity_robot_controller.py index bbc658a..6537913 100644 --- a/unity_robot_controller/unity_robot_controller.py +++ b/unity_robot_controller/unity_robot_controller.py @@ -9,6 +9,9 @@ from std_srvs.srv import Trigger from std_msgs.msg import Bool, String import numpy as np from tf2_ros import Buffer, TransformListener +from cv_bridge import CvBridge +from sensor_msgs.msg import Image +import cv2 class UnityRobotController(RobotController): @@ -45,6 +48,8 @@ class UnityRobotController(RobotController): super().__init__(fire_ext_max_capacity = fire_ext_max_capacity) + self._bridge = CvBridge() + self._tf_buffer = Buffer() self._tf_listener = TransformListener(self._tf_buffer, self._node) @@ -52,6 +57,7 @@ class UnityRobotController(RobotController): self._target_point_pub = self._node.create_publisher(Point, 'target_point', 10) self._status_str_pub = self._node.create_publisher(String, 'status', 10) + self._status_image_pub = self._node.create_publisher(Image, 'status_image', 10) self.fe_burst_srv = self._node.create_client(Trigger, 'fe_burst') @@ -66,6 +72,46 @@ class UnityRobotController(RobotController): self._node.create_subscription(Bool, 'collision_detection', self.collision_cb, 10) self.proc_timer = self._node.create_timer(0.1, self.proc_timer_cb) + self._node.create_subscription(Image, 'crosshair_image', self.ch_image_cb, 10) + + def ch_image_cb(self, msg): + try: + cv_image = self._bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') + except Exception as e: + self._node.get_logger().error(f"Failed to convert image: {e}") + return + + fh = 22 + scale = 0.5 + + cv2.putText(cv_image, f"CHARGE: {round(self._charge_points/self._start_charge * 100 ,1)}%", (1, int(5 + fh * scale)), cv2.FONT_HERSHEY_SIMPLEX, scale, (255, 255, 255), thickness=None, lineType=None, bottomLeftOrigin=None) + + cv2.putText(cv_image, f"HP: {round(self._hit_points/self._start_hp * 100 ,1)}%", (int(cv_image.shape[1]/4), int(5 + fh * scale)), cv2.FONT_HERSHEY_SIMPLEX, scale, (255, 255, 255), thickness=None, lineType=None, bottomLeftOrigin=None) + + cv2.putText(cv_image, f"SCORE: {round(self.get_score(),1)}", (int(cv_image.shape[1]/4*2), int(5 + fh * scale)), cv2.FONT_HERSHEY_SIMPLEX, scale, (255, 255, 255), thickness=None, lineType=None, bottomLeftOrigin=None) + + cv2.putText(cv_image, f"F.E. left: {self._fire_ext_capacity}", (int(cv_image.shape[1]/4*3), int(5 + fh * scale)), cv2.FONT_HERSHEY_SIMPLEX, scale, (255, 255, 255), thickness=None, lineType=None, bottomLeftOrigin=None) + + if self._FALL: + text = "FALLEN" + scale = 5 + + (text_w, text_h), _ = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, scale, None) + + x = int((cv_image.shape[1] - text_w) / 2) + y = int((cv_image.shape[0] + text_h) / 2) + + cv2.putText(cv_image, "FALLEN", (x, y), cv2.FONT_HERSHEY_SIMPLEX, scale, (0, 0, 255), thickness=None, lineType=None, bottomLeftOrigin=None) + + try: + ros_image = self._bridge.cv2_to_imgmsg(cv_image, encoding='bgr8') + ros_image.header = msg.header + self._status_image_pub.publish(ros_image) + except Exception as e: + self.get_logger().error(f"Failed to publish image: {e}") + + + def pub_status(self): msg = String() status = self.get_str_status()