falls

.gitmodules

@@ -4,3 +4,6 @@
 [submodule "unity_robot_controller/robot_controller"]
 	path = unity_robot_controller/robot_controller
 	url = https://git.robofob.ru/sirius/robot_controller
+[submodule "unity_robot_controller/gesture_rec"]
+	path = unity_robot_controller/gesture_rec
+	url = https://git.robofob.ru/sirius/gesture_rec

unity_robot_controller/fire_extinguisher_emulator.py

@@ -310,7 +310,7 @@ def find_intersected_spheres(
         transform.transform.translation.y,
         transform.transform.translation.z
     ])
-    print_f(f"{p0}")
+    #print_f(f"{p0}")
 
     # 3. Преобразуем вектор камеры в координаты мира
     # Вектор как точка (с нулевой координатой w)
@@ -353,33 +353,6 @@ def find_intersected_spheres(
     return sorted(intersected, key=lambda x: x[1])
 
 
-
-    # for sphere in spheres:
-    #     idx, xs, ys, zs, r = sphere
-    #     p_s = np.array([xs, ys, zs])  # центр сферы в мире
-    #
-    #     # k = p₀ - pₛ (вектор от сферы до камеры)
-    #     k = p0 - p_s
-    #
-    #     # Коэффициенты квадратного уравнения
-    #     # t² + 2*b*t + c = 0, где a = 1 (у нас u единичный)
-    #     b = np.dot(k, u)
-    #     c = np.dot(k, k) - r**2
-    #
-    #     # Дискриминант
-    #     D = b**2 - c
-    #
-    #     if D >= 0:  # есть пересечение (D=0 — касание, D>0 — 2 точки)
-    #         sqrt_D = np.sqrt(D)
-    #         t1 = -b - sqrt_D
-    #         t2 = -b + sqrt_D
-    #
-    #         # Проверяем, что хотя бы одна точка пересечения на луче [0, length]
-    #         if (0 <= t1 <= camera_vector_length) or (0 <= t2 <= camera_vector_length):
-    #             intersected.append((idx, t1, t2))
-    #
-    # return intersected
-
 def quaternion_from_two_vectors(a, b):
     a = np.array(a, dtype=float)
     b = np.array(b, dtype=float)

unity_robot_controller/unity_robot_controller.py

@@ -6,6 +6,9 @@ from unity_robot_controller.robot_controller.robot_controller import RobotContro
 from geometry_msgs.msg import Twist, Point
 from builtin_interfaces.msg import Time
 from std_srvs.srv import Trigger
+from std_msgs.msg import Bool, String
+import numpy as np
+from tf2_ros import Buffer, TransformListener
 
 
 class UnityRobotController(RobotController):
@@ -21,22 +24,52 @@ 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._lin_max = self._node.get_parameter('max_linear_speed').value
         self._ang_max = self._node.get_parameter('max_angular_speed').value
 
+        self._max_tilt = np.deg2rad(self._node.get_parameter('max_tilt_deg').value)
+        self._FALL = False
+
         fire_ext_max_capacity = self._node.get_parameter('fire_ext_max_capacity').value
 
         super().__init__(fire_ext_max_capacity = fire_ext_max_capacity)
 
+        self._tf_buffer = Buffer()
+        self._tf_listener = TransformListener(self._tf_buffer, self._node)
+
         self._twist_pub = self._node.create_publisher(Twist, 'cmd_vel', 10)
         self._target_point_pub = self._node.create_publisher(Point, 'target_point', 10)
 
+        self._status_str_pub = self._node.create_publisher(String, 'status', 10)
+
         self.fe_burst_srv = self._node.create_client(Trigger, 'fe_burst')
 
         self.start_time = self.get_time_seconds() # TODO save it when first cmd is given
 
         # 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)
+
+    def pub_status(self):
+        msg = String()
+        status = self.get_str_status()
+        if self._FALL:
+            status += f"\n - Robot is fallen"
+        msg.data = status
+        self._status_str_pub.publish(msg)
+        self._node.get_logger().info(msg.data)
+
+    def collision_cb(self, msg):
+        if msg.data and not self._prev_collision:
+            self._register_collision(self.get_relative_time())
+            self.pub_status()
+        self._prev_collision = msg.data
+
+
     def get_time_seconds(self):
         current_time = self._node.get_clock().now()
         return current_time.nanoseconds / 1e9
@@ -68,7 +101,7 @@ class UnityRobotController(RobotController):
         result = future.result()
         if result.success:
             self._register_exted_sof(self.get_relative_time(), {'id': result.message})
-
+            self.pub_status()
 
     def send_fire_ext_pose_cmd(self, horisontal_pose, vertical_pose):
         horisontal_pose, vertical_pose = super(UnityRobotController, self).send_fire_ext_pose_cmd(horisontal_pose, vertical_pose)
@@ -79,6 +112,44 @@ class UnityRobotController(RobotController):
 
         self._target_point_pub.publish(point_msg)
 
+    def send_unfall_cmd(self):
+        self._FALL = False
+
+    def fall_timer_cb(self):
+        try:
+            robot_transform = self._tf_buffer.lookup_transform(
+                "map",           # target frame
+                "base_link",        # source frame
+                rclpy.time.Time()           # текущее время (latest transform)
+            )
+
+        except (Exception) as e:
+            self._node.get_logger().warning(f"Failed to lookup transform from map to base_link: {e}")
+            return
+
+        q = robot_transform.transform.rotation
+        t = robot_transform.transform.translation
+
+        # Quaternion to rotation matrix
+        x, y, z, w = q.x, q.y, q.z, q.w
+        R = np.array([
+            [1 - 2*(y*y + z*z),     2*(x*y - z*w),     2*(x*z + y*w)],
+            [    2*(x*y + z*w), 1 - 2*(x*x + z*z),     2*(y*z - x*w)],
+            [    2*(x*z - y*w),     2*(y*z + x*w), 1 - 2*(x*x + y*y)]
+        ], dtype=float)
+
+        # Local +Z axis expressed in map frame
+        z_axis_map = R @ np.array([0.0, 0.0, 1.0])
+
+        # Compare with map down direction
+        down = np.array([0.0, 0.0, -1.0])
+        cos_angle = float(np.dot(z_axis_map, down))
+        cos_thresh = np.cos(self._max_tilt)
+
+        self._FALL = cos_angle >= cos_thresh
+
+
+