From 24a8411ad8eb8a1cdceecb63755b2531ed211852 Mon Sep 17 00:00:00 2001 From: moscowsky Date: Tue, 23 Jun 2026 15:02:52 +0300 Subject: [PATCH] fix fall --- config/unity.rviz | 2 +- launch/unity.launch.py | 2 +- unity_robot_controller/unity_robot_controller.py | 7 ++++++- 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/config/unity.rviz b/config/unity.rviz index 9e36f10..c602c0d 100644 --- a/config/unity.rviz +++ b/config/unity.rviz @@ -285,7 +285,7 @@ Window Geometry: Height: 1008 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000330fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004c00000330000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000021000000330fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000004c000001940000002200fffffffa000000000100000002fb000000060052004100570100000000ffffffff0000005c01000003fb0000000a005600690065007700730000000670000001100000011001000003fb0000001200430072006f00730073006800610069007201000001e10000019b0000002201000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d0065010000000000000780000002bd01000003fb0000000800540069006d00650100000000000004500000000000000000000004040000033000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000330fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004c00000330000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000021000000330fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000004c000001980000002200fffffffa000000000100000002fb000000060052004100570100000000ffffffff0000005c01000003fb0000000a005600690065007700730000000670000001100000011001000003fb0000001200430072006f00730073006800610069007201000001e5000001970000002201000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d0065010000000000000780000002bd01000003fb0000000800540069006d00650100000000000004500000000000000000000004040000033000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RAW: collapsed: false Selection: diff --git a/launch/unity.launch.py b/launch/unity.launch.py index 581af6d..9310d58 100644 --- a/launch/unity.launch.py +++ b/launch/unity.launch.py @@ -58,7 +58,7 @@ def generate_launch_description(): parameters=[ {"use_sim_time": LaunchConfiguration("use_sim_time")}, {"crosshair_alpha": 1.0}, - {"sof_cos_sim": 0.999}, + {"sof_cos_sim": 0.99}, {"fe_distance": 5.0}, ], remappings=[ diff --git a/unity_robot_controller/unity_robot_controller.py b/unity_robot_controller/unity_robot_controller.py index 21e970a..60fe947 100644 --- a/unity_robot_controller/unity_robot_controller.py +++ b/unity_robot_controller/unity_robot_controller.py @@ -146,7 +146,12 @@ class UnityRobotController(RobotController): cos_angle = float(np.dot(z_axis_map, down)) cos_thresh = np.cos(self._max_tilt) - self._FALL = cos_angle >= cos_thresh + fall = cos_angle >= cos_thresh + if not self._FALL and fall: + self._FALL = fall + self._register_fall(self.get_relative_time()) + self.pub_status() + self._FALL = fall