diff --git a/.gitmodules b/.gitmodules index a05f8ce..4cd2d19 100644 --- a/.gitmodules +++ b/.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 diff --git a/unity_robot_controller/fire_extinguisher_emulator.py b/unity_robot_controller/fire_extinguisher_emulator.py index 2f819a8..012625b 100644 --- a/unity_robot_controller/fire_extinguisher_emulator.py +++ b/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) diff --git a/unity_robot_controller/gesture_rec b/unity_robot_controller/gesture_rec new file mode 160000 index 0000000..266adb8 --- /dev/null +++ b/unity_robot_controller/gesture_rec @@ -0,0 +1 @@ +Subproject commit 266adb892ad59d5fdc70f6202480c77b98d94cdc diff --git a/unity_robot_controller/robot_controller b/unity_robot_controller/robot_controller index 9f65622..b34711e 160000 --- a/unity_robot_controller/robot_controller +++ b/unity_robot_controller/robot_controller @@ -1 +1 @@ -Subproject commit 9f65622b633900e07ee1f9dbe9e33a5add3167ae +Subproject commit b34711ebc690cf9199b15bc3074e08f258303003 diff --git a/unity_robot_controller/unity_robot_controller.py b/unity_robot_controller/unity_robot_controller.py index 5c076f5..21e970a 100644 --- a/unity_robot_controller/unity_robot_controller.py +++ b/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 + + +