Merge branch 'main' of https://git.robofob.ru/sirius/unity_robot_controller into main

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: <Fixed 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:

unity_robot_controller/fire_extinguisher_emulator.py

@@ -112,7 +112,7 @@ class FireExtinguisherEmulatorNode(Node):
             print_f = self.get_logger().info
             )
 
-        self.get_logger().info(f"{intersected_spheres}")
+        #self.get_logger().info(f"{intersected_spheres}")
 
         if len(intersected_spheres):
             res.success = True

unity_robot_controller/joy_control_node.py

@@ -144,10 +144,19 @@ def main(args=None):
     rclpy.init(args=args)
 
     node = JoyControlNode(UnityRobotController)
-    rclpy.spin(node)
-    node.destroy_node()
-    rclpy.shutdown()
 
+    try:
+        # Keep the node running until an interrupt occurs
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        # Catch Ctrl+C and log a clean exit message
+        node.get_logger().info('KeyboardInterrupt received, shutting down gracefully...')
+        path = node.URC.dump_stats()
+        node.get_logger().info(f"Saved data to {path}")
+    finally:
+        # Clean up resources safely
+        node.destroy_node()
+        rclpy.shutdown()
 
 if __name__ == "__main__":
     main()

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):
@@ -26,9 +29,16 @@ class UnityRobotController(RobotController):
 
         self._node.declare_parameter('max_tilt_deg', 90.)
 
+        self._node.declare_parameter('discharge_speed_base', 0.0)
+        self._node.declare_parameter('discharge_speed_lights', 0.5)
+
         self._lin_max = self._node.get_parameter('max_linear_speed').value
         self._ang_max = self._node.get_parameter('max_angular_speed').value
 
+        self._discharge_speed_base = self._node.get_parameter('discharge_speed_base').value
+        self._discharge_speed_lights = self._node.get_parameter('discharge_speed_lights').value
+
+
         self._max_tilt = np.deg2rad(self._node.get_parameter('max_tilt_deg').value)
         self._FALL = False
 
@@ -38,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)
 
@@ -45,19 +57,60 @@ 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')
 
         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
+        #self.start_time = self.get_time_seconds() # TODO save it when first cmd is given
+        self.t_prev = self.get_time_seconds()
 
-        # TODO create collision subscriber from Unity
 
         self._prev_collision = False
         self._node.create_subscription(Bool, 'collision_detection', self.collision_cb, 10)
-        self.fall_timer = self._node.create_timer(0.1, self.fall_timer_cb)
+        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()
@@ -70,7 +123,7 @@ class UnityRobotController(RobotController):
 
     def collision_cb(self, msg):
         if msg.data and not self._prev_collision:
-            self._register_collision(self.get_relative_time())
+            self._register_collision()#self.get_relative_time())
             self.pub_status()
         self._prev_collision = msg.data
 
@@ -105,7 +158,7 @@ class UnityRobotController(RobotController):
     def _fe_burst_done_cb(self, future):
         result = future.result()
         if result.success:
-            self._register_exted_sof(self.get_relative_time(), {'id': result.message})
+            self._register_exted_sof({'id': result.message})
             self.pub_status()
 
     def send_fire_ext_pose_cmd(self, horisontal_pose, vertical_pose):
@@ -120,7 +173,19 @@ class UnityRobotController(RobotController):
     def send_unfall_cmd(self):
         self._FALL = False
 
-    def fall_timer_cb(self):
+    def proc_timer_cb(self):
+
+        # LIGHT + CHARGE PROCESS
+        t = self.get_time_seconds()
+        discharge_speed = self._discharge_speed_base
+        if self._LIGHTS:
+            discharge_speed += self._discharge_speed_lights
+        self._decrease_charge(discharge_speed, t - self.t_prev)
+        self.t_prev = t
+        if self._charge_points == 0:
+            self._node.get_logger().info('Robot is dischadged!', throttle_duration_sec=2.5)
+
+        # FALL PROCESS
         try:
             robot_transform = self._tf_buffer.lookup_transform(
                 "map",           # target frame
@@ -154,11 +219,12 @@ class UnityRobotController(RobotController):
         fall = cos_angle >= cos_thresh
         if not self._FALL and fall:
             self._FALL = fall
-            self._register_fall(self.get_relative_time())
+            self._register_fall()#self.get_relative_time())
             self.pub_status()
         self._FALL = fall
 
     def send_lights_cmd(self):
+        super(UnityRobotController, self).send_lights_cmd()
         future = self.lights_srv.call_async(Trigger.Request())
         future.add_done_callback(self._lights_done_cb)
 
@@ -169,6 +235,7 @@ class UnityRobotController(RobotController):
         self._node.get_logger().info(f"Lights is {'ON' if self._LIGHTS else 'OFF'}")
 
     def send_fall_reset_cmd(self):
+        super(UnityRobotController, self).send_fall_reset_cmd()
         if self._FALL:
             self._node.get_logger().info("Trying to reset robot...")
             future = self.fall_reset_srv.call_async(Trigger.Request())