recharge

.vscode/c_cpp_properties.json

@@ -0,0 +1,16 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c17",
+            "cppStandard": "gnu++17",
+            "intelliSenseMode": "linux-gcc-arm64"
+        }
+    ],
+    "version": 4
+}

src/log.txt

@@ -0,0 +1,882 @@
+[INFO] [1770376017.191192364] [wheelchair_state_machine]: 事件输入处理器已初始化
+[INFO] [1770376017.193594822] [wheelchair_state_machine]: 电池管理器已初始化
+[INFO] [1770376017.193682614] [wheelchair_state_machine]: 电池阈值 - 低电量: 20.0%, 严重低电量: 10.0%
+[INFO] [1770376017.196325698] [wheelchair_state_machine]: 激光扫描控制器已初始化
+[INFO] [1770376017.196447614] [wheelchair_state_machine]: [IpadManager] iPad管理器初始化完成
+[INFO] [1770376017.196629323] [wheelchair_state_machine]: 状态转移表已初始化完成
+[INFO] [1770376017.196822698] [wheelchair_state_machine]: [RotationManager] 旋转管理器初始化完成
+[INFO] [1770376017.196850406] [wheelchair_state_machine]: 旋转管理器已初始化
+[INFO] [1770376017.197190198] [wheelchair_state_machine]: 基站检测订阅者已初始化
+[INFO] [1770376017.197589781] [wheelchair_state_machine]: 工作流管理器已初始化
+[INFO] [1770376017.197899531] [wheelchair_state_machine]: 报告管理器已初始化
+[INFO] [1770376017.197938031] [wheelchair_state_machine]: [IpadManager] 已设置回充启动回调
+[INFO] [1770376017.197954948] [wheelchair_state_machine]: [IpadManager] 已设置回充取消回调
+[INFO] [1770376017.197968073] [wheelchair_state_machine]: 已设置状态更新回调
+[INFO] [1770376017.197985281] [wheelchair_state_machine]: 已设置回充状态回调
+[INFO] [1770376017.198010073] [wheelchair_state_machine]: 已注册事件: ipad_phone_interface_start
+[INFO] [1770376017.198025531] [wheelchair_state_machine]: 已注册事件: ipad_phone_interface_cancel
+[INFO] [1770376017.198079489] [wheelchair_state_machine]: 已注册事件: low_battery_warning
+[INFO] [1770376017.198095823] [wheelchair_state_machine]: 已注册事件: battery_start_charging
+[INFO] [1770376017.198109823] [wheelchair_state_machine]: 已注册事件: battery_stop_charging
+[INFO] [1770376017.198122364] [wheelchair_state_machine]: 已注册事件: battery_full
+[INFO] [1770376017.198141323] [wheelchair_state_machine]: 已注册事件: bluetooth_disconnected
+[INFO] [1770376017.198155323] [wheelchair_state_machine]: 已注册事件: bluetooth_connected
+[INFO] [1770376017.198171656] [wheelchair_state_machine]: 已注册事件: base_station_power_off
+[INFO] [1770376017.198186239] [wheelchair_state_machine]: 已注册事件: lock_vehicle
+[INFO] [1770376017.198201406] [wheelchair_state_machine]: 已注册事件: unlock_vehicle
+[INFO] [1770376017.198233489] [wheelchair_state_machine]: 已注册事件: joystick_pull_back
+[INFO] [1770376017.198247781] [wheelchair_state_machine]: 已注册事件: joystick_stop
+[INFO] [1770376017.198263239] [wheelchair_state_machine]: 已注册事件: push_start
+[INFO] [1770376017.198278406] [wheelchair_state_machine]: 已注册事件: push_stop
+[INFO] [1770376017.198291823] [wheelchair_state_machine]: 已注册事件: base_station_lost
+[INFO] [1770376017.198305823] [wheelchair_state_machine]: 已注册事件: search_30s_timeout
+[INFO] [1770376017.198317198] [wheelchair_state_machine]: 模块回调设置完成
+[INFO] [1770376017.198649114] [wheelchair_state_machine]: 订阅器设置完成
+[INFO] [1770376017.198675656] [wheelchair_state_machine]: 所有模块初始化完成
+[INFO] [1770376017.198786198] [wheelchair_state_machine]: [SYSTEM_STATUS] 系统状态报告已生成
+[INFO] [1770376017.198802823] [wheelchair_state_machine]: 智能轮椅状态机已启动
+[INFO] [1770376024.373299210] [wheelchair_state_machine]: [IpadManager] 启动回充功能
+[INFO] [1770376024.373496669] [wheelchair_state_machine]: iPad启动回充
+[INFO] [1770376024.373639877] [wheelchair_state_machine]: 基站检测状态已重置: false
+[INFO] [1770376024.373703460] [wheelchair_state_machine]: 已重置基站检测状态为false
+[INFO] [1770376024.373761502] [wheelchair_state_machine]: 触发事件: ipad_phone_interface_start
+[INFO] [1770376024.373831794] [wheelchair_state_machine]: 处理iPad启动事件
+[INFO] [1770376024.373938252] [wheelchair_state_machine]: 执行事件: ipad_phone_interface_start
+[INFO] [1770376024.373997460] [wheelchair_state_machine]: 处理iPad启动事件
+[INFO] [1770376024.374315085] [wheelchair_state_machine]: [INFO] IPAD_RECHARGE_STARTED: iPad启动自主回充
+[INFO] [1770376024.374410460] [wheelchair_state_machine]: 状态转移: 就绪中 -> 搜索中
+[INFO] [1770376024.374559502] [wheelchair_state_machine]: [INFO] 搜索中: 状态已更新
+[INFO] [1770376024.374623669] [wheelchair_state_machine]: 导航功能已启动
+[INFO] [1770376024.374675585] [wheelchair_state_machine]: 启动导航控制
+[INFO] [1770376024.374729835] [wheelchair_state_machine]: 开始搜索充电站
+[INFO] [1770376024.374836585] [wheelchair_state_machine]: [INFO] SEARCH_CHARGING_STATION_START:
+[INFO] [1770376024.375047752] [wheelchair_state_machine]: 搜索超时定时器已启动 (30秒)
+[INFO] [1770376024.375160044] [wheelchair_state_machine]: 旋转检查定时器已启动 (10秒后检查)
+[INFO] [1770376024.375237627] [wheelchair_state_machine]: [回充状态] RECHARGE_STARTED: 回充功能已启动，正在搜索充电站
+[INFO] [1770376024.375327460] [wheelchair_state_machine]: [回充状态] RECHARGE_STARTED: 回充功能已启动，正在搜索充电站
+[INFO] [1770376024.375447627] [wheelchair_state_machine]: [INFO] ipad_phone_interface_start: 事件已处理
+[INFO] [1770376034.375274674] [wheelchair_state_machine]: 搜索已持续 10.0 秒，进入旋转状态
+[INFO] [1770376034.375456382] [wheelchair_state_machine]: 状态转移: 搜索中 -> 旋转中
+[INFO] [1770376034.375563715] [wheelchair_state_machine]: [INFO] 旋转中: 状态已更新
+[INFO] [1770376034.375594049] [wheelchair_state_machine]: 进入旋转搜索状态
+[INFO] [1770376034.375694090] [wheelchair_state_machine]: [RotationManager] 开始旋转 - 角速度: 0.150000 rad/s, 持续时间: 40.000000秒
+[INFO] [1770376034.375722382] [wheelchair_state_machine]: 轮椅开始旋转搜索
+[INFO] [1770376034.375755049] [wheelchair_state_machine]: [INFO] WHEELCHAIR_ROTATING: 旋转搜索充电站
+[INFO] [1770376034.375772840] [wheelchair_state_machine]: 进入旋转搜索状态
+[INFO] [1770376034.375795882] [wheelchair_state_machine]: [回充状态] ROTATING_SEARCH: 搜索10秒未找到基站，开始旋转搜索
+[INFO] [1770376034.375813090] [wheelchair_state_machine]: [回充状态] ROTATING_SEARCH: 搜索10秒未找到基站，开始旋转搜索
+[INFO] [1770376034.375839632] [wheelchair_state_machine]: 旋转检查定时器已停止
+[INFO] [1770376034.375959799] [wheelchair_state_machine]: 搜索超时定时器已停止
+[INFO] [1770376034.375977590] [wheelchair_state_machine]: 已停止搜索超时定时器，进入旋转搜索模式
+[INFO] [1770376034.476112184] [wheelchair_state_machine]: [RotationManager] 旋转中 - 已旋转: 0.100378秒, 角速度: 0.150000 rad/s
+[WARN] [1770376035.412585944] [wheelchair_state_machine]: 没有获取到分段
+[WARN] [1770376035.612554256] [wheelchair_state_machine]: 没有获取到分段
+[INFO] [1770376036.476096510] [wheelchair_state_machine]: [RotationManager] 旋转中 - 已旋转: 2.100403秒, 角速度: 0.150000 rad/s
+[INFO] [1770376038.475977336] [wheelchair_state_machine]: [RotationManager] 旋转中 - 已旋转: 4.100319秒, 角速度: 0.150000 rad/s
+[INFO] [1770376040.475954995] [wheelchair_state_machine]: [RotationManager] 旋转中 - 已旋转: 6.100310秒, 角速度: 0.150000 rad/s
+[INFO] [1770376041.170878440] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376041.171026023] [wheelchair_state_machine]: 左直线: y = 0.3718x + -1.0714
+[INFO] [1770376041.171065398] [wheelchair_state_machine]: 右直线: y = 7.3621x + -9.6470
+[INFO] [1770376041.171096023] [wheelchair_state_machine]: 角平分线: y = 1.2496x + -2.1482
+[INFO] [1770376041.171125481] [wheelchair_state_machine]: 交点: (1.2268, -0.6152)
+[INFO] [1770376041.171154648] [wheelchair_state_machine]: 左直线角度: 20.40°
+[INFO] [1770376041.171184690] [wheelchair_state_machine]: 右直线角度: 82.26°
+[INFO] [1770376041.171211815] [wheelchair_state_machine]: 角平分线角度: 51.33°
+[INFO] [1770376041.171240981] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差51.3°)
+[INFO] [1770376041.171350065] [wheelchair_state_machine]: ==================================
+[INFO] [1770376041.171785231] [wheelchair_state_machine]: 旋转中检测到基站，记录检测时间并设置延迟切换
+[INFO] [1770376041.172231190] [wheelchair_state_machine]: 旋转延迟定时器已启动 (2.5秒后切换状态)
+[INFO] [1770376041.172477065] [wheelchair_state_machine]: [INFO] STATION_DETECTED_WHILE_ROTATING: 旋转中检测到基站，继续旋转2.5秒后对接
+[INFO] [1770376041.172535982] [wheelchair_state_machine]: 进入旋转搜索状态
+[INFO] [1770376041.269666491] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376041.269859575] [wheelchair_state_machine]: 左直线: y = 0.3841x + -1.0826
+[INFO] [1770376041.269947075] [wheelchair_state_machine]: 右直线: y = 10.1618x + -13.1514
+[INFO] [1770376041.270032241] [wheelchair_state_machine]: 角平分线: y = 1.3125x + -2.2285
+[INFO] [1770376041.270111866] [wheelchair_state_machine]: 交点: (1.2343, -0.6084)
+[INFO] [1770376041.270191200] [wheelchair_state_machine]: 左直线角度: 21.01°
+[INFO] [1770376041.270268783] [wheelchair_state_machine]: 右直线角度: 84.38°
+[INFO] [1770376041.270342866] [wheelchair_state_machine]: 角平分线角度: 52.70°
+[INFO] [1770376041.270410241] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差52.7°)
+[INFO] [1770376041.270488991] [wheelchair_state_machine]: ==================================
+[INFO] [1770376041.270593991] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376041.370874460] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376041.371064335] [wheelchair_state_machine]: 左直线: y = 0.4212x + -1.1100
+[INFO] [1770376041.371155168] [wheelchair_state_machine]: 右直线: y = 11.0109x + -14.2819
+[INFO] [1770376041.371240335] [wheelchair_state_machine]: 角平分线: y = 1.3676x + -2.2872
+[INFO] [1770376041.371316752] [wheelchair_state_machine]: 交点: (1.2438, -0.5860)
+[INFO] [1770376041.371392002] [wheelchair_state_machine]: 左直线角度: 22.84°
+[INFO] [1770376041.371465502] [wheelchair_state_machine]: 右直线角度: 84.81°
+[INFO] [1770376041.371522668] [wheelchair_state_machine]: 角平分线角度: 53.83°
+[INFO] [1770376041.371622710] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差53.8°)
+[INFO] [1770376041.371700002] [wheelchair_state_machine]: ==================================
+[INFO] [1770376041.371802960] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376041.469572178] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376041.469766720] [wheelchair_state_machine]: 左直线: y = 0.4064x + -1.0833
+[INFO] [1770376041.469828845] [wheelchair_state_machine]: 右直线: y = 14.0679x + -18.0724
+[INFO] [1770376041.469886011] [wheelchair_state_machine]: 角平分线: y = 1.3777x + -2.2911
+[INFO] [1770376041.469948136] [wheelchair_state_machine]: 交点: (1.2436, -0.5779)
+[INFO] [1770376041.470001803] [wheelchair_state_machine]: 左直线角度: 22.12°
+[INFO] [1770376041.470051678] [wheelchair_state_machine]: 右直线角度: 85.93°
+[INFO] [1770376041.470113220] [wheelchair_state_machine]: 角平分线角度: 54.03°
+[INFO] [1770376041.470176220] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差54.0°)
+[INFO] [1770376041.470229303] [wheelchair_state_machine]: ==================================
+[INFO] [1770376041.470355595] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376041.566887105] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376041.567001730] [wheelchair_state_machine]: 左直线: y = 0.4337x + -1.0939
+[INFO] [1770376041.567024480] [wheelchair_state_machine]: 右直线: y = 19.0944x + -24.4184
+[INFO] [1770376041.567044896] [wheelchair_state_machine]: 角平分线: y = 1.4400x + -2.3518
+[INFO] [1770376041.567130355] [wheelchair_state_machine]: 交点: (1.2499, -0.5519)
+[INFO] [1770376041.567149605] [wheelchair_state_machine]: 左直线角度: 23.44°
+[INFO] [1770376041.567165063] [wheelchair_state_machine]: 右直线角度: 87.00°
+[INFO] [1770376041.567180230] [wheelchair_state_machine]: 角平分线角度: 55.22°
+[INFO] [1770376041.567195980] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差55.2°)
+[INFO] [1770376041.567212313] [wheelchair_state_machine]: ==================================
+[INFO] [1770376041.567244688] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376041.668303531] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376041.668563406] [wheelchair_state_machine]: 左直线: y = 0.4292x + -1.0862
+[INFO] [1770376041.668638365] [wheelchair_state_machine]: 右直线: y = 18.9297x + -24.2404
+[INFO] [1770376041.668698156] [wheelchair_state_machine]: 角平分线: y = 1.4336x + -2.3432
+[INFO] [1770376041.668761740] [wheelchair_state_machine]: 交点: (1.2515, -0.5490)
+[INFO] [1770376041.668816281] [wheelchair_state_machine]: 左直线角度: 23.23°
+[INFO] [1770376041.668911073] [wheelchair_state_machine]: 右直线角度: 86.98°
+[INFO] [1770376041.668962698] [wheelchair_state_machine]: 角平分线角度: 55.10°
+[INFO] [1770376041.669012865] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差55.1°)
+[INFO] [1770376041.669074698] [wheelchair_state_machine]: ==================================
+[INFO] [1770376041.669187573] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376041.767454000] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376041.767692583] [wheelchair_state_machine]: 左直线: y = 0.4546x + -1.1040
+[INFO] [1770376041.767765500] [wheelchair_state_machine]: 右直线: y = -5.6779x + 6.7558
+[INFO] [1770376041.767829375] [wheelchair_state_machine]: 角平分线: y = -0.5268x + 0.1539
+[INFO] [1770376041.767894708] [wheelchair_state_machine]: 交点: (1.2817, -0.5213)
+[INFO] [1770376041.767959166] [wheelchair_state_machine]: 左直线角度: 24.45°
+[INFO] [1770376041.768020708] [wheelchair_state_machine]: 右直线角度: -80.01°
+[INFO] [1770376041.768105583] [wheelchair_state_machine]: 角平分线角度: -27.78°
+[INFO] [1770376041.768168000] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-27.8°)
+[INFO] [1770376041.768250250] [wheelchair_state_machine]: ==================================
+[INFO] [1770376041.768348250] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376041.966147520] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376041.966267395] [wheelchair_state_machine]: 左直线: y = 0.5015x + -1.1446
+[INFO] [1770376041.966298895] [wheelchair_state_machine]: 右直线: y = -5.1742x + 6.2007
+[INFO] [1770376041.966322811] [wheelchair_state_machine]: 角平分线: y = -0.4924x + 0.1416
+[INFO] [1770376041.966347020] [wheelchair_state_machine]: 交点: (1.2942, -0.4956)
+[INFO] [1770376041.966370353] [wheelchair_state_machine]: 左直线角度: 26.63°
+[INFO] [1770376041.966393103] [wheelchair_state_machine]: 右直线角度: -79.06°
+[INFO] [1770376041.966415270] [wheelchair_state_machine]: 角平分线角度: -26.21°
+[INFO] [1770376041.966439770] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-26.2°)
+[INFO] [1770376041.966458436] [wheelchair_state_machine]: ==================================
+[INFO] [1770376041.966491103] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376042.064577780] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376042.064784863] [wheelchair_state_machine]: 左直线: y = 0.6214x + -1.2804
+[INFO] [1770376042.064851363] [wheelchair_state_machine]: 右直线: y = -15.5944x + 19.6476
+[INFO] [1770376042.064932738] [wheelchair_state_machine]: 角平分线: y = -0.5148x + 0.1859
+[INFO] [1770376042.065073321] [wheelchair_state_machine]: 交点: (1.2906, -0.4784)
+[INFO] [1770376042.065132530] [wheelchair_state_machine]: 左直线角度: 31.86°
+[INFO] [1770376042.065183571] [wheelchair_state_machine]: 右直线角度: -86.33°
+[INFO] [1770376042.065233155] [wheelchair_state_machine]: 角平分线角度: -27.24°
+[INFO] [1770376042.065285071] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-27.2°)
+[INFO] [1770376042.065342530] [wheelchair_state_machine]: ==================================
+[INFO] [1770376042.065411655] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376042.165104040] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376042.165328915] [wheelchair_state_machine]: 左直线: y = 0.5007x + -1.1315
+[INFO] [1770376042.165402706] [wheelchair_state_machine]: 右直线: y = 3.6297x + -5.0467
+[INFO] [1770376042.165465998] [wheelchair_state_machine]: 角平分线: y = 1.2173x + -2.0281
+[INFO] [1770376042.165591706] [wheelchair_state_machine]: 交点: (1.2513, -0.5049)
+[INFO] [1770376042.165646248] [wheelchair_state_machine]: 左直线角度: 26.60°
+[INFO] [1770376042.165712456] [wheelchair_state_machine]: 右直线角度: 74.60°
+[INFO] [1770376042.165770790] [wheelchair_state_machine]: 角平分线角度: 50.60°
+[INFO] [1770376042.166051373] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差50.6°)
+[INFO] [1770376042.166118748] [wheelchair_state_machine]: ==================================
+[INFO] [1770376042.166208873] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376042.263798300] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376042.263983508] [wheelchair_state_machine]: 左直线: y = 0.4680x + -1.0736
+[INFO] [1770376042.264058758] [wheelchair_state_machine]: 右直线: y = -6.7148x + 8.3198
+[INFO] [1770376042.264125550] [wheelchair_state_machine]: 角平分线: y = -0.5367x + 0.2404
+[INFO] [1770376042.264191758] [wheelchair_state_machine]: 交点: (1.3077, -0.4615)
+[INFO] [1770376042.264260883] [wheelchair_state_machine]: 左直线角度: 25.08°
+[INFO] [1770376042.264327966] [wheelchair_state_machine]: 右直线角度: -81.53°
+[INFO] [1770376042.264402925] [wheelchair_state_machine]: 角平分线角度: -28.22°
+[INFO] [1770376042.264469133] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-28.2°)
+[INFO] [1770376042.264534466] [wheelchair_state_machine]: ==================================
+[INFO] [1770376042.264629550] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376042.362626726] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376042.362819518] [wheelchair_state_machine]: 左直线: y = 0.5844x + -1.2082
+[INFO] [1770376042.362905851] [wheelchair_state_machine]: 右直线: y = -13.3967x + 17.0441
+[INFO] [1770376042.362993935] [wheelchair_state_machine]: 角平分线: y = -0.5253x + 0.2406
+[INFO] [1770376042.363070935] [wheelchair_state_machine]: 交点: (1.3055, -0.4452)
+[INFO] [1770376042.363147060] [wheelchair_state_machine]: 左直线角度: 30.30°
+[INFO] [1770376042.363221143] [wheelchair_state_machine]: 右直线角度: -85.73°
+[INFO] [1770376042.363295518] [wheelchair_state_machine]: 角平分线角度: -27.71°
+[INFO] [1770376042.363355601] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-27.7°)
+[INFO] [1770376042.363428810] [wheelchair_state_machine]: ==================================
+[INFO] [1770376042.363532351] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376042.460788319] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376042.460979069] [wheelchair_state_machine]: 左直线: y = 0.5711x + -1.1807
+[INFO] [1770376042.461054319] [wheelchair_state_machine]: 右直线: y = 3.3039x + -4.6247
+[INFO] [1770376042.461250028] [wheelchair_state_machine]: 角平分线: y = 1.2547x + -2.0422
+[INFO] [1770376042.461342194] [wheelchair_state_machine]: 交点: (1.2602, -0.4610)
+[INFO] [1770376042.461397319] [wheelchair_state_machine]: 左直线角度: 29.73°
+[INFO] [1770376042.461446903] [wheelchair_state_machine]: 右直线角度: 73.16°
+[INFO] [1770376042.461496194] [wheelchair_state_machine]: 角平分线角度: 51.45°
+[INFO] [1770376042.461578444] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差51.4°)
+[INFO] [1770376042.461635611] [wheelchair_state_machine]: ==================================
+[INFO] [1770376042.461718153] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376042.476547988] [wheelchair_state_machine]: [RotationManager] 旋转中 - 已旋转: 8.100883秒, 角速度: 0.150000 rad/s
+[INFO] [1770376042.562213955] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376042.562408788] [wheelchair_state_machine]: 左直线: y = 0.5852x + -1.1905
+[INFO] [1770376042.562496580] [wheelchair_state_machine]: 右直线: y = 3.6765x + -5.1115
+[INFO] [1770376042.562573871] [wheelchair_state_machine]: 角平分线: y = 1.3060x + -2.1048
+[INFO] [1770376042.562649996] [wheelchair_state_machine]: 交点: (1.2683, -0.4483)
+[INFO] [1770376042.562727871] [wheelchair_state_machine]: 左直线角度: 30.33°
+[INFO] [1770376042.562804288] [wheelchair_state_machine]: 右直线角度: 74.78°
+[INFO] [1770376042.562878663] [wheelchair_state_machine]: 角平分线角度: 52.56°
+[INFO] [1770376042.562954205] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差52.6°)
+[INFO] [1770376042.563029746] [wheelchair_state_machine]: ==================================
+[INFO] [1770376042.563131830] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376042.660801714] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376042.661026006] [wheelchair_state_machine]: 左直线: y = 0.6232x + -1.2281
+[INFO] [1770376042.661115548] [wheelchair_state_machine]: 右直线: y = -10.5839x + 13.5674
+[INFO] [1770376042.661212381] [wheelchair_state_machine]: 角平分线: y = -0.4950x + 0.2481
+[INFO] [1770376042.661299006] [wheelchair_state_machine]: 交点: (1.3202, -0.4055)
+[INFO] [1770376042.661380381] [wheelchair_state_machine]: 左直线角度: 31.93°
+[INFO] [1770376042.661457381] [wheelchair_state_machine]: 右直线角度: -84.60°
+[INFO] [1770376042.661534381] [wheelchair_state_machine]: 角平分线角度: -26.34°
+[INFO] [1770376042.661644923] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-26.3°)
+[INFO] [1770376042.661727465] [wheelchair_state_machine]: ==================================
+[INFO] [1770376042.661837715] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376042.757833224] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376042.757929766] [wheelchair_state_machine]: 左直线: y = 0.5954x + -1.1896
+[INFO] [1770376042.757951349] [wheelchair_state_machine]: 右直线: y = -8.8319x + 11.3011
+[INFO] [1770376042.757969724] [wheelchair_state_machine]: 角平分线: y = -0.4961x + 0.2565
+[INFO] [1770376042.757985766] [wheelchair_state_machine]: 交点: (1.3250, -0.4008)
+[INFO] [1770376042.758005599] [wheelchair_state_machine]: 左直线角度: 30.77°
+[INFO] [1770376042.758021058] [wheelchair_state_machine]: 右直线角度: -83.54°
+[INFO] [1770376042.758035933] [wheelchair_state_machine]: 角平分线角度: -26.39°
+[INFO] [1770376042.758051391] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-26.4°)
+[INFO] [1770376042.758065974] [wheelchair_state_machine]: ==================================
+[INFO] [1770376042.758093391] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376042.859882109] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376042.860204984] [wheelchair_state_machine]: 左直线: y = 0.6305x + -1.2180
+[INFO] [1770376042.860297734] [wheelchair_state_machine]: 右直线: y = -8.2456x + 10.5964
+[INFO] [1770376042.860375901] [wheelchair_state_machine]: 角平分线: y = -0.4754x + 0.2540
+[INFO] [1770376042.860449984] [wheelchair_state_machine]: 交点: (1.3310, -0.3788)
+[INFO] [1770376042.860522318] [wheelchair_state_machine]: 左直线角度: 32.23°
+[INFO] [1770376042.860597568] [wheelchair_state_machine]: 右直线角度: -83.09°
+[INFO] [1770376042.860669318] [wheelchair_state_machine]: 角平分线角度: -25.43°
+[INFO] [1770376042.860734651] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-25.4°)
+[INFO] [1770376042.860804068] [wheelchair_state_machine]: ==================================
+[INFO] [1770376042.860915485] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376042.957457494] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376042.957562494] [wheelchair_state_machine]: 左直线: y = 0.6393x + -1.2197
+[INFO] [1770376042.957591953] [wheelchair_state_machine]: 右直线: y = -8.0650x + 10.4008
+[INFO] [1770376042.957616453] [wheelchair_state_machine]: 角平分线: y = -0.4699x + 0.2612
+[INFO] [1770376042.957636578] [wheelchair_state_machine]: 交点: (1.3350, -0.3662)
+[INFO] [1770376042.957656411] [wheelchair_state_machine]: 左直线角度: 32.59°
+[INFO] [1770376042.957675369] [wheelchair_state_machine]: 右直线角度: -82.93°
+[INFO] [1770376042.957694036] [wheelchair_state_machine]: 角平分线角度: -25.17°
+[INFO] [1770376042.957714744] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-25.2°)
+[INFO] [1770376042.957734578] [wheelchair_state_machine]: ==================================
+[INFO] [1770376042.957768119] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376043.059633505] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376043.059930130] [wheelchair_state_machine]: 左直线: y = 0.6988x + -1.2793
+[INFO] [1770376043.060014713] [wheelchair_state_machine]: 右直线: y = -6.4489x + 8.3159
+[INFO] [1770376043.060096088] [wheelchair_state_machine]: 角平分线: y = -0.4269x + 0.2319
+[INFO] [1770376043.060174255] [wheelchair_state_machine]: 交点: (1.3424, -0.3412)
+[INFO] [1770376043.060250380] [wheelchair_state_machine]: 左直线角度: 34.95°
+[INFO] [1770376043.060323588] [wheelchair_state_machine]: 右直线角度: -81.19°
+[INFO] [1770376043.060467380] [wheelchair_state_machine]: 角平分线角度: -23.12°
+[INFO] [1770376043.060547880] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-23.1°)
+[INFO] [1770376043.060610296] [wheelchair_state_machine]: ==================================
+[INFO] [1770376043.060699255] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376043.157603806] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376043.157803598] [wheelchair_state_machine]: 左直线: y = 0.7291x + -1.3083
+[INFO] [1770376043.157901014] [wheelchair_state_machine]: 右直线: y = -5.6821x + 7.3335
+[INFO] [1770376043.157978889] [wheelchair_state_machine]: 角平分线: y = -0.4033x + 0.2180
+[INFO] [1770376043.158045098] [wheelchair_state_machine]: 交点: (1.3479, -0.3256)
+[INFO] [1770376043.158111889] [wheelchair_state_machine]: 左直线角度: 36.09°
+[INFO] [1770376043.158174306] [wheelchair_state_machine]: 右直线角度: -80.02°
+[INFO] [1770376043.158297389] [wheelchair_state_machine]: 角平分线角度: -21.96°
+[INFO] [1770376043.158363889] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-22.0°)
+[INFO] [1770376043.158428056] [wheelchair_state_machine]: ==================================
+[INFO] [1770376043.158521389] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376043.256349524] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376043.256841274] [wheelchair_state_machine]: 左直线: y = 0.7504x + -1.3306
+[INFO] [1770376043.256941316] [wheelchair_state_machine]: 右直线: y = -6.0514x + 7.8450
+[INFO] [1770376043.257014233] [wheelchair_state_machine]: 角平分线: y = -0.4013x + 0.2230
+[INFO] [1770376043.257076358] [wheelchair_state_machine]: 交点: (1.3490, -0.3184)
+[INFO] [1770376043.257138191] [wheelchair_state_machine]: 左直线角度: 36.88°
+[INFO] [1770376043.257198274] [wheelchair_state_machine]: 右直线角度: -80.62°
+[INFO] [1770376043.257256024] [wheelchair_state_machine]: 角平分线角度: -21.87°
+[INFO] [1770376043.257316399] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-21.9°)
+[INFO] [1770376043.257374441] [wheelchair_state_machine]: ==================================
+[INFO] [1770376043.257467774] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376043.357410493] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376043.357761368] [wheelchair_state_machine]: 左直线: y = 0.7320x + -1.2985
+[INFO] [1770376043.358241743] [wheelchair_state_machine]: 右直线: y = -6.6997x + 8.7426
+[INFO] [1770376043.358461951] [wheelchair_state_machine]: 角平分线: y = -0.4173x + 0.2545
+[INFO] [1770376043.358617118] [wheelchair_state_machine]: 交点: (1.3511, -0.3094)
+[INFO] [1770376043.358685076] [wheelchair_state_machine]: 左直线角度: 36.21°
+[INFO] [1770376043.358888660] [wheelchair_state_machine]: 右直线角度: -81.51°
+[INFO] [1770376043.359004743] [wheelchair_state_machine]: 角平分线角度: -22.65°
+[INFO] [1770376043.359129285] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-22.7°)
+[INFO] [1770376043.359194910] [wheelchair_state_machine]: ==================================
+[INFO] [1770376043.359334910] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376043.455917753] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376043.456149919] [wheelchair_state_machine]: 左直线: y = 0.7543x + -1.3047
+[INFO] [1770376043.456235378] [wheelchair_state_machine]: 右直线: y = -5.8690x + 7.6979
+[INFO] [1770376043.456290503] [wheelchair_state_machine]: 角平分线: y = -0.3970x + 0.2602
+[INFO] [1770376043.456344753] [wheelchair_state_machine]: 交点: (1.3592, -0.2794)
+[INFO] [1770376043.456398128] [wheelchair_state_machine]: 左直线角度: 37.03°
+[INFO] [1770376043.456451794] [wheelchair_state_machine]: 右直线角度: -80.33°
+[INFO] [1770376043.456500503] [wheelchair_state_machine]: 角平分线角度: -21.65°
+[INFO] [1770376043.456552128] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-21.7°)
+[INFO] [1770376043.456602003] [wheelchair_state_machine]: ==================================
+[INFO] [1770376043.456696794] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376043.553761137] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376043.553954513] [wheelchair_state_machine]: 左直线: y = 0.7889x + -1.3508
+[INFO] [1770376043.554029179] [wheelchair_state_machine]: 右直线: y = -5.2161x + 6.8186
+[INFO] [1770376043.554082554] [wheelchair_state_machine]: 角平分线: y = -0.3727x + 0.2295
+[INFO] [1770376043.554137388] [wheelchair_state_machine]: 交点: (1.3605, -0.2776)
+[INFO] [1770376043.554214679] [wheelchair_state_machine]: 左直线角度: 38.27°
+[INFO] [1770376043.554284096] [wheelchair_state_machine]: 右直线角度: -79.15°
+[INFO] [1770376043.554348554] [wheelchair_state_machine]: 角平分线角度: -20.44°
+[INFO] [1770376043.554413888] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-20.4°)
+[INFO] [1770376043.554480096] [wheelchair_state_machine]: ==================================
+[INFO] [1770376043.554569638] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376043.654838148] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376043.655060689] [wheelchair_state_machine]: 左直线: y = 0.7904x + -1.3341
+[INFO] [1770376043.655166856] [wheelchair_state_machine]: 右直线: y = -5.2335x + 6.8881
+[INFO] [1770376043.655270398] [wheelchair_state_machine]: 角平分线: y = -0.3725x + 0.2532
+[INFO] [1770376043.655359064] [wheelchair_state_machine]: 交点: (1.3649, -0.2553)
+[INFO] [1770376043.655444814] [wheelchair_state_machine]: 左直线角度: 38.32°
+[INFO] [1770376043.655526481] [wheelchair_state_machine]: 右直线角度: -79.18°
+[INFO] [1770376043.655605231] [wheelchair_state_machine]: 角平分线角度: -20.43°
+[INFO] [1770376043.655686898] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-20.4°)
+[INFO] [1770376043.655763898] [wheelchair_state_machine]: ==================================
+[INFO] [1770376043.655855773] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376043.753236699] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376043.753448741] [wheelchair_state_machine]: 左直线: y = 0.8549x + -1.4022
+[INFO] [1770376043.753543824] [wheelchair_state_machine]: 右直线: y = -4.9961x + 6.6266
+[INFO] [1770376043.753631324] [wheelchair_state_machine]: 角平分线: y = -0.3458x + 0.2455
+[INFO] [1770376043.753715908] [wheelchair_state_machine]: 交点: (1.3722, -0.2291)
+[INFO] [1770376043.753799908] [wheelchair_state_machine]: 左直线角度: 40.53°
+[INFO] [1770376043.753879241] [wheelchair_state_machine]: 右直线角度: -78.68°
+[INFO] [1770376043.753956533] [wheelchair_state_machine]: 角平分线角度: -19.08°
+[INFO] [1770376043.754053074] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-19.1°)
+[INFO] [1770376043.754135908] [wheelchair_state_machine]: ==================================
+[INFO] [1770376043.754240908] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376043.852236209] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376043.852410042] [wheelchair_state_machine]: 左直线: y = 0.8726x + -1.4184
+[INFO] [1770376043.852471584] [wheelchair_state_machine]: 右直线: y = -4.1765x + 5.5140
+[INFO] [1770376043.852513001] [wheelchair_state_machine]: 角平分线: y = -0.3194x + 0.2182
+[INFO] [1770376043.852556751] [wheelchair_state_machine]: 交点: (1.3730, -0.2204)
+[INFO] [1770376043.852598751] [wheelchair_state_machine]: 左直线角度: 41.11°
+[INFO] [1770376043.852640168] [wheelchair_state_machine]: 右直线角度: -76.53°
+[INFO] [1770376043.852679834] [wheelchair_state_machine]: 角平分线角度: -17.71°
+[INFO] [1770376043.852720084] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-17.7°)
+[INFO] [1770376043.852760626] [wheelchair_state_machine]: ==================================
+[INFO] [1770376043.852821584] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376043.953033344] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376043.953240719] [wheelchair_state_machine]: 左直线: y = 0.8650x + -1.3909
+[INFO] [1770376043.953322386] [wheelchair_state_machine]: 右直线: y = -3.7166x + 4.9199
+[INFO] [1770376043.953383636] [wheelchair_state_machine]: 角平分线: y = -0.3065x + 0.2227
+[INFO] [1770376043.953489803] [wheelchair_state_machine]: 交点: (1.3774, -0.1995)
+[INFO] [1770376043.953567386] [wheelchair_state_machine]: 左直线角度: 40.86°
+[INFO] [1770376043.953650803] [wheelchair_state_machine]: 右直线角度: -74.94°
+[INFO] [1770376043.953724011] [wheelchair_state_machine]: 角平分线角度: -17.04°
+[INFO] [1770376043.954088303] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-17.0°)
+[INFO] [1770376043.954163261] [wheelchair_state_machine]: ==================================
+[INFO] [1770376043.954266511] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376044.051756396] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376044.051997313] [wheelchair_state_machine]: 左直线: y = 0.8765x + -1.3958
+[INFO] [1770376044.052090646] [wheelchair_state_machine]: 右直线: y = -3.7033x + 4.9221
+[INFO] [1770376044.052169688] [wheelchair_state_machine]: 角平分线: y = -0.3024x + 0.2306
+[INFO] [1770376044.052233854] [wheelchair_state_machine]: 交点: (1.3795, -0.1866)
+[INFO] [1770376044.052307646] [wheelchair_state_machine]: 左直线角度: 41.23°
+[INFO] [1770376044.052380271] [wheelchair_state_machine]: 右直线角度: -74.89°
+[INFO] [1770376044.052437729] [wheelchair_state_machine]: 角平分线角度: -16.83°
+[INFO] [1770376044.052496354] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-16.8°)
+[INFO] [1770376044.052572479] [wheelchair_state_machine]: ==================================
+[INFO] [1770376044.052684479] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376044.148636989] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376044.148748114] [wheelchair_state_machine]: 左直线: y = 0.9138x + -1.4391
+[INFO] [1770376044.148784864] [wheelchair_state_machine]: 右直线: y = -3.5130x + 4.6796
+[INFO] [1770376044.148812864] [wheelchair_state_machine]: 角平分线: y = -0.2838x + 0.2163
+[INFO] [1770376044.148841739] [wheelchair_state_machine]: 交点: (1.3822, -0.1760)
+[INFO] [1770376044.148869739] [wheelchair_state_machine]: 左直线角度: 42.42°
+[INFO] [1770376044.148897447] [wheelchair_state_machine]: 右直线角度: -74.11°
+[INFO] [1770376044.148924572] [wheelchair_state_machine]: 角平分线角度: -15.84°
+[INFO] [1770376044.148951406] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-15.8°)
+[INFO] [1770376044.148977656] [wheelchair_state_machine]: ==================================
+[INFO] [1770376044.149017614] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376044.250232416] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376044.250434832] [wheelchair_state_machine]: 左直线: y = 0.9446x + -1.4551
+[INFO] [1770376044.250530499] [wheelchair_state_machine]: 右直线: y = -3.0799x + 4.1252
+[INFO] [1770376044.250617999] [wheelchair_state_machine]: 角平分线: y = -0.2553x + 0.2087
+[INFO] [1770376044.250704333] [wheelchair_state_machine]: 交点: (1.3866, -0.1453)
+[INFO] [1770376044.250788916] [wheelchair_state_machine]: 左直线角度: 43.37°
+[INFO] [1770376044.250874374] [wheelchair_state_machine]: 右直线角度: -72.01°
+[INFO] [1770376044.250953124] [wheelchair_state_machine]: 角平分线角度: -14.32°
+[INFO] [1770376044.251035083] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-14.3°)
+[INFO] [1770376044.251114999] [wheelchair_state_machine]: ==================================
+[INFO] [1770376044.251228458] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376044.349997551] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376044.350215718] [wheelchair_state_machine]: 左直线: y = 0.9578x + -1.4579
+[INFO] [1770376044.350305551] [wheelchair_state_machine]: 右直线: y = -2.8827x + 3.8805
+[INFO] [1770376044.350370009] [wheelchair_state_machine]: 角平分线: y = -0.2410x + 0.2085
+[INFO] [1770376044.350435343] [wheelchair_state_machine]: 交点: (1.3901, -0.1266)
+[INFO] [1770376044.350500093] [wheelchair_state_machine]: 左直线角度: 43.76°
+[INFO] [1770376044.350561343] [wheelchair_state_machine]: 右直线角度: -70.87°
+[INFO] [1770376044.350622884] [wheelchair_state_machine]: 角平分线角度: -13.55°
+[INFO] [1770376044.350685593] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-13.6°)
+[INFO] [1770376044.350746259] [wheelchair_state_machine]: ==================================
+[INFO] [1770376044.350833468] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376044.449393186] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376044.449522102] [wheelchair_state_machine]: 左直线: y = 1.0443x + -1.5594
+[INFO] [1770376044.449569644] [wheelchair_state_machine]: 右直线: y = -2.6934x + 3.6392
+[INFO] [1770376044.449606394] [wheelchair_state_machine]: 角平分线: y = -0.2070x + 0.1809
+[INFO] [1770376044.449644311] [wheelchair_state_machine]: 交点: (1.3909, -0.1070)
+[INFO] [1770376044.449681061] [wheelchair_state_machine]: 左直线角度: 46.24°
+[INFO] [1770376044.449715769] [wheelchair_state_machine]: 右直线角度: -69.63°
+[INFO] [1770376044.449747853] [wheelchair_state_machine]: 角平分线角度: -11.70°
+[INFO] [1770376044.449782561] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-11.7°)
+[INFO] [1770376044.449815811] [wheelchair_state_machine]: ==================================
+[INFO] [1770376044.449873269] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376044.476104772] [wheelchair_state_machine]: [RotationManager] 旋转中 - 已旋转: 10.100417秒, 角速度: 0.150000 rad/s
+[INFO] [1770376044.548856821] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376044.549090154] [wheelchair_state_machine]: 左直线: y = 1.0692x + -1.5895
+[INFO] [1770376044.549199821] [wheelchair_state_machine]: 右直线: y = -2.7276x + 3.6893
+[INFO] [1770376044.549296363] [wheelchair_state_machine]: 角平分线: y = -0.2030x + 0.1792
+[INFO] [1770376044.549389696] [wheelchair_state_machine]: 交点: (1.3903, -0.1030)
+[INFO] [1770376044.549483904] [wheelchair_state_machine]: 左直线角度: 46.92°
+[INFO] [1770376044.549577238] [wheelchair_state_machine]: 右直线角度: -69.87°
+[INFO] [1770376044.549665029] [wheelchair_state_machine]: 角平分线角度: -11.48°
+[INFO] [1770376044.549754279] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-11.5°)
+[INFO] [1770376044.549846154] [wheelchair_state_machine]: ==================================
+[INFO] [1770376044.549963404] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376044.645410081] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376044.645509831] [wheelchair_state_machine]: 左直线: y = 1.0508x + -1.5430
+[INFO] [1770376044.645533747] [wheelchair_state_machine]: 右直线: y = -2.7245x + 3.7140
+[INFO] [1770376044.645550081] [wheelchair_state_machine]: 角平分线: y = -0.2073x + 0.2089
+[INFO] [1770376044.645569331] [wheelchair_state_machine]: 交点: (1.3925, -0.0798)
+[INFO] [1770376044.645585664] [wheelchair_state_machine]: 左直线角度: 46.42°
+[INFO] [1770376044.645603456] [wheelchair_state_machine]: 右直线角度: -69.84°
+[INFO] [1770376044.645618914] [wheelchair_state_machine]: 角平分线角度: -11.71°
+[INFO] [1770376044.645635831] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-11.7°)
+[INFO] [1770376044.645651581] [wheelchair_state_machine]: ==================================
+[INFO] [1770376044.645682497] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376044.746807924] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376044.746919049] [wheelchair_state_machine]: 左直线: y = 1.0714x + -1.5603
+[INFO] [1770376044.746951132] [wheelchair_state_machine]: 右直线: y = -2.5499x + 3.4871
+[INFO] [1770376044.746975341] [wheelchair_state_machine]: 角平分线: y = -0.1909x + 0.1991
+[INFO] [1770376044.746999549] [wheelchair_state_machine]: 交点: (1.3938, -0.0669)
+[INFO] [1770376044.747023174] [wheelchair_state_machine]: 左直线角度: 46.98°
+[INFO] [1770376044.747047382] [wheelchair_state_machine]: 右直线角度: -68.59°
+[INFO] [1770376044.747069257] [wheelchair_state_machine]: 角平分线角度: -10.81°
+[INFO] [1770376044.747092882] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-10.8°)
+[INFO] [1770376044.747186799] [wheelchair_state_machine]: ==================================
+[INFO] [1770376044.747225007] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376044.845248726] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376044.845365684] [wheelchair_state_machine]: 左直线: y = 1.1359x + -1.6386
+[INFO] [1770376044.845396601] [wheelchair_state_machine]: 右直线: y = -2.4040x + 3.2989
+[INFO] [1770376044.845421392] [wheelchair_state_machine]: 角平分线: y = -0.1653x + 0.1764
+[INFO] [1770376044.845453476] [wheelchair_state_machine]: 交点: (1.3948, -0.0542)
+[INFO] [1770376044.845477101] [wheelchair_state_machine]: 左直线角度: 48.64°
+[INFO] [1770376044.845501309] [wheelchair_state_machine]: 右直线角度: -67.41°
+[INFO] [1770376044.845522892] [wheelchair_state_machine]: 角平分线角度: -9.39°
+[INFO] [1770376044.845544767] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-9.4°)
+[INFO] [1770376044.845567517] [wheelchair_state_machine]: ==================================
+[INFO] [1770376044.845605726] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376044.944926527] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376044.945194569] [wheelchair_state_machine]: 左直线: y = 1.1924x + -1.6978
+[INFO] [1770376044.945280319] [wheelchair_state_machine]: 右直线: y = -2.3162x + 3.1953
+[INFO] [1770376044.945409819] [wheelchair_state_machine]: 角平分线: y = -0.1462x + 0.1690
+[INFO] [1770376044.945489736] [wheelchair_state_machine]: 交点: (1.3946, -0.0349)
+[INFO] [1770376044.945566736] [wheelchair_state_machine]: 左直线角度: 50.02°
+[INFO] [1770376044.945641694] [wheelchair_state_machine]: 右直线角度: -66.65°
+[INFO] [1770376044.945713444] [wheelchair_state_machine]: 角平分线角度: -8.32°
+[INFO] [1770376044.945782861] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-8.3°)
+[INFO] [1770376044.946086777] [wheelchair_state_machine]: ==================================
+[INFO] [1770376044.946200819] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376045.045446537] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376045.045704371] [wheelchair_state_machine]: 左直线: y = 1.2628x + -1.7717
+[INFO] [1770376045.045791579] [wheelchair_state_machine]: 右直线: y = -2.2215x + 3.0878
+[INFO] [1770376045.045865954] [wheelchair_state_machine]: 角平分线: y = -0.1240x + 0.1625
+[INFO] [1770376045.045938579] [wheelchair_state_machine]: 交点: (1.3947, -0.0105)
+[INFO] [1770376045.046007412] [wheelchair_state_machine]: 左直线角度: 51.62°
+[INFO] [1770376045.046075079] [wheelchair_state_machine]: 右直线角度: -65.77°
+[INFO] [1770376045.046136621] [wheelchair_state_machine]: 角平分线角度: -7.07°
+[INFO] [1770376045.046199329] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-7.1°)
+[INFO] [1770376045.046268162] [wheelchair_state_machine]: ==================================
+[INFO] [1770376045.046371412] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376045.143359339] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376045.143559131] [wheelchair_state_machine]: 左直线: y = 1.2747x + -1.7724
+[INFO] [1770376045.143648089] [wheelchair_state_machine]: 右直线: y = -2.1367x + 2.9879
+[INFO] [1770376045.143727714] [wheelchair_state_machine]: 角平分线: y = -0.1142x + 0.1657
+[INFO] [1770376045.143810839] [wheelchair_state_machine]: 交点: (1.3954, 0.0063)
+[INFO] [1770376045.143889589] [wheelchair_state_machine]: 左直线角度: 51.89°
+[INFO] [1770376045.143970381] [wheelchair_state_machine]: 右直线角度: -64.92°
+[INFO] [1770376045.144047089] [wheelchair_state_machine]: 角平分线角度: -6.52°
+[INFO] [1770376045.144267881] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-6.5°)
+[INFO] [1770376045.144356256] [wheelchair_state_machine]: ==================================
+[INFO] [1770376045.144472047] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376045.242368974] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376045.242564974] [wheelchair_state_machine]: 左直线: y = 1.2931x + -1.7939
+[INFO] [1770376045.242636141] [wheelchair_state_machine]: 右直线: y = -2.1069x + 2.9522
+[INFO] [1770376045.242703224] [wheelchair_state_machine]: 角平分线: y = -0.1080x + 0.1619
+[INFO] [1770376045.242774682] [wheelchair_state_machine]: 交点: (1.3959, 0.0112)
+[INFO] [1770376045.242845266] [wheelchair_state_machine]: 左直线角度: 52.28°
+[INFO] [1770376045.242913516] [wheelchair_state_machine]: 右直线角度: -64.61°
+[INFO] [1770376045.242977391] [wheelchair_state_machine]: 角平分线角度: -6.16°
+[INFO] [1770376045.243040682] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差-6.2°)
+[INFO] [1770376045.243105141] [wheelchair_state_machine]: ==================================
+[INFO] [1770376045.243199349] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376045.441678911] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376045.441877244] [wheelchair_state_machine]: 左直线: y = 1.4379x + -1.9570
+[INFO] [1770376045.441968244] [wheelchair_state_machine]: 右直线: y = -2.0042x + 2.8374
+[INFO] [1770376045.442050202] [wheelchair_state_machine]: 角平分线: y = -0.0726x + 0.1469
+[INFO] [1770376045.442133911] [wheelchair_state_machine]: 交点: (1.3929, 0.0458)
+[INFO] [1770376045.442214411] [wheelchair_state_machine]: 左直线角度: 55.18°
+[INFO] [1770376045.442298411] [wheelchair_state_machine]: 右直线角度: -63.48°
+[INFO] [1770376045.442380369] [wheelchair_state_machine]: 角平分线角度: -4.15°
+[INFO] [1770376045.442458536] [wheelchair_state_machine]: 对准状态: ✓ 已对准基站 (误差-4.2°)
+[INFO] [1770376045.442535827] [wheelchair_state_machine]: ==================================
+[INFO] [1770376045.442642869] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376045.542525004] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376045.542713421] [wheelchair_state_machine]: 左直线: y = 1.4407x + -1.9425
+[INFO] [1770376045.542795962] [wheelchair_state_machine]: 右直线: y = -1.8675x + 2.6680
+[INFO] [1770376045.542870921] [wheelchair_state_machine]: 角平分线: y = -0.0576x + 0.1457
+[INFO] [1770376045.542949379] [wheelchair_state_machine]: 交点: (1.3936, 0.0654)
+[INFO] [1770376045.543024337] [wheelchair_state_machine]: 左直线角度: 55.24°
+[INFO] [1770376045.543098712] [wheelchair_state_machine]: 右直线角度: -61.83°
+[INFO] [1770376045.543167546] [wheelchair_state_machine]: 角平分线角度: -3.30°
+[INFO] [1770376045.543241337] [wheelchair_state_machine]: 对准状态: ✓ 已对准基站 (误差-3.3°)
+[INFO] [1770376045.543310171] [wheelchair_state_machine]: ==================================
+[INFO] [1770376045.543411671] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376045.643702556] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376045.643848389] [wheelchair_state_machine]: 左直线: y = 1.4689x + -1.9677
+[INFO] [1770376045.643879014] [wheelchair_state_machine]: 右直线: y = -1.7913x + 2.5755
+[INFO] [1770376045.643901181] [wheelchair_state_machine]: 角平分线: y = -0.0443x + 0.1410
+[INFO] [1770376045.643923931] [wheelchair_state_machine]: 交点: (1.3935, 0.0793)
+[INFO] [1770376045.643944347] [wheelchair_state_machine]: 左直线角度: 55.75°
+[INFO] [1770376045.643965931] [wheelchair_state_machine]: 右直线角度: -60.83°
+[INFO] [1770376045.643985181] [wheelchair_state_machine]: 角平分线角度: -2.54°
+[INFO] [1770376045.644082306] [wheelchair_state_machine]: 对准状态: ✓ 已对准基站 (误差-2.5°)
+[INFO] [1770376045.644104764] [wheelchair_state_machine]: ==================================
+[INFO] [1770376045.644140639] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376045.839811159] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376045.839952034] [wheelchair_state_machine]: 左直线: y = 1.6556x + -2.1769
+[INFO] [1770376045.840014451] [wheelchair_state_machine]: 右直线: y = -1.7282x + 2.5185
+[INFO] [1770376045.840066367] [wheelchair_state_machine]: 角平分线: y = -0.0094x + 0.1334
+[INFO] [1770376045.840120617] [wheelchair_state_machine]: 交点: (1.3877, 0.1204)
+[INFO] [1770376045.840171659] [wheelchair_state_machine]: 左直线角度: 58.87°
+[INFO] [1770376045.840222117] [wheelchair_state_machine]: 右直线角度: -59.94°
+[INFO] [1770376045.840269367] [wheelchair_state_machine]: 角平分线角度: -0.54°
+[INFO] [1770376045.840315742] [wheelchair_state_machine]: 对准状态: ✓ 已对准基站 (误差-0.5°)
+[INFO] [1770376045.840364159] [wheelchair_state_machine]: ==================================
+[INFO] [1770376045.840429784] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376045.939589294] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376045.940083377] [wheelchair_state_machine]: 左直线: y = 1.6327x + -2.1458
+[INFO] [1770376045.940193336] [wheelchair_state_machine]: 右直线: y = -1.6505x + 2.4173
+[INFO] [1770376045.940276461] [wheelchair_state_machine]: 角平分线: y = -0.0024x + 0.1267
+[INFO] [1770376045.940361336] [wheelchair_state_machine]: 交点: (1.3898, 0.1234)
+[INFO] [1770376045.940446502] [wheelchair_state_machine]: 左直线角度: 58.51°
+[INFO] [1770376045.940529919] [wheelchair_state_machine]: 右直线角度: -58.79°
+[INFO] [1770376045.940610127] [wheelchair_state_machine]: 角平分线角度: -0.14°
+[INFO] [1770376045.940688002] [wheelchair_state_machine]: 对准状态: ✓ 已对准基站 (误差-0.1°)
+[INFO] [1770376045.940767627] [wheelchair_state_machine]: ==================================
+[INFO] [1770376045.941009127] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376046.039515262] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376046.039751512] [wheelchair_state_machine]: 左直线: y = 1.6531x + -2.1559
+[INFO] [1770376046.039858846] [wheelchair_state_machine]: 右直线: y = -1.6410x + 2.4107
+[INFO] [1770376046.039946054] [wheelchair_state_machine]: 角平分线: y = 0.0016x + 0.1336
+[INFO] [1770376046.040033262] [wheelchair_state_machine]: 交点: (1.3863, 0.1358)
+[INFO] [1770376046.040116387] [wheelchair_state_machine]: 左直线角度: 58.83°
+[INFO] [1770376046.040198054] [wheelchair_state_machine]: 右直线角度: -58.64°
+[INFO] [1770376046.040276804] [wheelchair_state_machine]: 角平分线角度: 0.09°
+[INFO] [1770376046.040354971] [wheelchair_state_machine]: 对准状态: ✓ 已对准基站 (误差0.1°)
+[INFO] [1770376046.040432262] [wheelchair_state_machine]: ==================================
+[INFO] [1770376046.040543971] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376046.136817480] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376046.136939980] [wheelchair_state_machine]: 左直线: y = 1.6736x + -2.1682
+[INFO] [1770376046.136971772] [wheelchair_state_machine]: 右直线: y = -2.0387x + 2.9603
+[INFO] [1770376046.136991022] [wheelchair_state_machine]: 角平分线: y = -0.0413x + 0.2009
+[INFO] [1770376046.137012314] [wheelchair_state_machine]: 交点: (1.3815, 0.1439)
+[INFO] [1770376046.137030980] [wheelchair_state_machine]: 左直线角度: 59.14°
+[INFO] [1770376046.137049064] [wheelchair_state_machine]: 右直线角度: -63.87°
+[INFO] [1770376046.137132772] [wheelchair_state_machine]: 角平分线角度: -2.37°
+[INFO] [1770376046.137154064] [wheelchair_state_machine]: 对准状态: ✓ 已对准基站 (误差-2.4°)
+[INFO] [1770376046.137172439] [wheelchair_state_machine]: ==================================
+[INFO] [1770376046.137215022] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376046.236210865] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376046.236373032] [wheelchair_state_machine]: 左直线: y = 1.7896x + -2.3047
+[INFO] [1770376046.236436907] [wheelchair_state_machine]: 右直线: y = -2.0814x + 3.0247
+[INFO] [1770376046.236492324] [wheelchair_state_machine]: 角平分线: y = -0.0308x + 0.2016
+[INFO] [1770376046.236549199] [wheelchair_state_machine]: 交点: (1.3768, 0.1591)
+[INFO] [1770376046.236601407] [wheelchair_state_machine]: 左直线角度: 60.80°
+[INFO] [1770376046.236654782] [wheelchair_state_machine]: 右直线角度: -64.34°
+[INFO] [1770376046.236704074] [wheelchair_state_machine]: 角平分线角度: -1.77°
+[INFO] [1770376046.236742865] [wheelchair_state_machine]: 对准状态: ✓ 已对准基站 (误差-1.8°)
+[INFO] [1770376046.236784574] [wheelchair_state_machine]: ==================================
+[INFO] [1770376046.236851365] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376046.339301667] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376046.339497376] [wheelchair_state_machine]: 左直线: y = 1.8215x + -2.3231
+[INFO] [1770376046.339613167] [wheelchair_state_machine]: 右直线: y = -1.5363x + 2.3082
+[INFO] [1770376046.339755792] [wheelchair_state_machine]: 角平分线: y = 0.0375x + 0.1375
+[INFO] [1770376046.339833959] [wheelchair_state_machine]: 交点: (1.3792, 0.1892)
+[INFO] [1770376046.339911251] [wheelchair_state_machine]: 左直线角度: 61.23°
+[INFO] [1770376046.340027042] [wheelchair_state_machine]: 右直线角度: -56.94°
+[INFO] [1770376046.340101126] [wheelchair_state_machine]: 角平分线角度: 2.15°
+[INFO] [1770376046.340213709] [wheelchair_state_machine]: 对准状态: ✓ 已对准基站 (误差2.1°)
+[INFO] [1770376046.340283709] [wheelchair_state_machine]: ==================================
+[INFO] [1770376046.340380251] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376046.435047760] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376046.435177260] [wheelchair_state_machine]: 左直线: y = 1.9848x + -2.5273
+[INFO] [1770376046.435214885] [wheelchair_state_machine]: 右直线: y = -1.3927x + 2.1257
+[INFO] [1770376046.435245510] [wheelchair_state_machine]: 角平分线: y = 0.0782x + 0.0994
+[INFO] [1770376046.435278469] [wheelchair_state_machine]: 交点: (1.3777, 0.2071)
+[INFO] [1770376046.435310260] [wheelchair_state_machine]: 左直线角度: 63.26°
+[INFO] [1770376046.435340885] [wheelchair_state_machine]: 右直线角度: -54.32°
+[INFO] [1770376046.435369469] [wheelchair_state_machine]: 角平分线角度: 4.47°
+[INFO] [1770376046.435396885] [wheelchair_state_machine]: 对准状态: ✓ 已对准基站 (误差4.5°)
+[INFO] [1770376046.435428969] [wheelchair_state_machine]: ==================================
+[INFO] [1770376046.435475344] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376046.476017889] [wheelchair_state_machine]: [RotationManager] 旋转中 - 已旋转: 12.100364秒, 角速度: 0.150000 rad/s
+[INFO] [1770376046.534345520] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376046.534542979] [wheelchair_state_machine]: 左直线: y = 1.8957x + -2.3801
+[INFO] [1770376046.534643312] [wheelchair_state_machine]: 右直线: y = -1.3568x + 2.0898
+[INFO] [1770376046.534847770] [wheelchair_state_machine]: 角平分线: y = 0.0750x + 0.1220
+[INFO] [1770376046.535006145] [wheelchair_state_machine]: 交点: (1.3743, 0.2251)
+[INFO] [1770376046.535214104] [wheelchair_state_machine]: 左直线角度: 62.19°
+[INFO] [1770376046.535301020] [wheelchair_state_machine]: 右直线角度: -53.61°
+[INFO] [1770376046.535422645] [wheelchair_state_machine]: 角平分线角度: 4.29°
+[INFO] [1770376046.535523562] [wheelchair_state_machine]: 对准状态: ✓ 已对准基站 (误差4.3°)
+[INFO] [1770376046.535676395] [wheelchair_state_machine]: ==================================
+[INFO] [1770376046.535773520] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376046.834271425] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376046.834496884] [wheelchair_state_machine]: 左直线: y = 2.2211x + -2.7442
+[INFO] [1770376046.834570675] [wheelchair_state_machine]: 右直线: y = -1.2116x + 1.9319
+[INFO] [1770376046.834637175] [wheelchair_state_machine]: 角平分线: y = 0.1343x + 0.0985
+[INFO] [1770376046.834704550] [wheelchair_state_machine]: 交点: (1.3622, 0.2814)
+[INFO] [1770376046.834774842] [wheelchair_state_machine]: 左直线角度: 65.76°
+[INFO] [1770376046.834839592] [wheelchair_state_machine]: 右直线角度: -50.47°
+[INFO] [1770376046.834930592] [wheelchair_state_machine]: 角平分线角度: 7.65°
+[INFO] [1770376046.834994759] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差7.6°)
+[INFO] [1770376046.835063592] [wheelchair_state_machine]: ==================================
+[INFO] [1770376046.835159551] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376046.932652852] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376046.932859060] [wheelchair_state_machine]: 左直线: y = 2.1867x + -2.6876
+[INFO] [1770376046.932947144] [wheelchair_state_machine]: 右直线: y = -1.1710x + 1.8833
+[INFO] [1770376046.933013352] [wheelchair_state_machine]: 角平分线: y = 0.1398x + 0.0988
+[INFO] [1770376046.933079269] [wheelchair_state_machine]: 交点: (1.3613, 0.2892)
+[INFO] [1770376046.933143435] [wheelchair_state_machine]: 左直线角度: 65.43°
+[INFO] [1770376046.933206144] [wheelchair_state_machine]: 右直线角度: -49.50°
+[INFO] [1770376046.933267102] [wheelchair_state_machine]: 角平分线角度: 7.96°
+[INFO] [1770376046.933326602] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差8.0°)
+[INFO] [1770376046.933387269] [wheelchair_state_machine]: ==================================
+[INFO] [1770376046.933475060] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376047.030702320] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376047.030886070] [wheelchair_state_machine]: 左直线: y = 2.3579x + -2.8776
+[INFO] [1770376047.030941779] [wheelchair_state_machine]: 右直线: y = -1.1191x + 1.8309
+[INFO] [1770376047.030985529] [wheelchair_state_machine]: 角平分线: y = 0.1656x + 0.0912
+[INFO] [1770376047.031033654] [wheelchair_state_machine]: 交点: (1.3542, 0.3154)
+[INFO] [1770376047.031078279] [wheelchair_state_machine]: 左直线角度: 67.02°
+[INFO] [1770376047.031121737] [wheelchair_state_machine]: 右直线角度: -48.22°
+[INFO] [1770376047.031164904] [wheelchair_state_machine]: 角平分线角度: 9.40°
+[INFO] [1770376047.031203112] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差9.4°)
+[INFO] [1770376047.031242779] [wheelchair_state_machine]: ==================================
+[INFO] [1770376047.031300529] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376047.132182955] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376047.132371372] [wheelchair_state_machine]: 左直线: y = 2.4295x + -2.9492
+[INFO] [1770376047.132451580] [wheelchair_state_machine]: 右直线: y = -1.1595x + 1.8941
+[INFO] [1770376047.132522164] [wheelchair_state_machine]: 角平分线: y = 0.1620x + 0.1107
+[INFO] [1770376047.132703580] [wheelchair_state_machine]: 交点: (1.3495, 0.3293)
+[INFO] [1770376047.132903372] [wheelchair_state_machine]: 左直线角度: 67.63°
+[INFO] [1770376047.133017705] [wheelchair_state_machine]: 右直线角度: -49.22°
+[INFO] [1770376047.133101997] [wheelchair_state_machine]: 角平分线角度: 9.20°
+[INFO] [1770376047.133180747] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差9.2°)
+[INFO] [1770376047.133260372] [wheelchair_state_machine]: ==================================
+[INFO] [1770376047.133366830] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376047.230934007] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376047.231165007] [wheelchair_state_machine]: 左直线: y = 2.4574x + -2.9760
+[INFO] [1770376047.231261840] [wheelchair_state_machine]: 右直线: y = -1.1201x + 1.8460
+[INFO] [1770376047.231345840] [wheelchair_state_machine]: 角平分线: y = 0.1729x + 0.1033
+[INFO] [1770376047.231435965] [wheelchair_state_machine]: 交点: (1.3478, 0.3362)
+[INFO] [1770376047.231516757] [wheelchair_state_machine]: 左直线角度: 67.86°
+[INFO] [1770376047.231656757] [wheelchair_state_machine]: 右直线角度: -48.24°
+[INFO] [1770376047.231735507] [wheelchair_state_machine]: 角平分线角度: 9.81°
+[INFO] [1770376047.231816882] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差9.8°)
+[INFO] [1770376047.231897674] [wheelchair_state_machine]: ==================================
+[INFO] [1770376047.232064215] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376047.329318392] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376047.329524600] [wheelchair_state_machine]: 左直线: y = 2.6583x + -3.2096
+[INFO] [1770376047.329591392] [wheelchair_state_machine]: 右直线: y = -1.3080x + 2.0960
+[INFO] [1770376047.329649725] [wheelchair_state_machine]: 角平分线: y = 0.1475x + 0.1490
+[INFO] [1770376047.329708642] [wheelchair_state_machine]: 交点: (1.3377, 0.3463)
+[INFO] [1770376047.329768142] [wheelchair_state_machine]: 左直线角度: 69.38°
+[INFO] [1770376047.330093642] [wheelchair_state_machine]: 右直线角度: -52.60°
+[INFO] [1770376047.330198350] [wheelchair_state_machine]: 角平分线角度: 8.39°
+[INFO] [1770376047.330267475] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差8.4°)
+[INFO] [1770376047.330341850] [wheelchair_state_machine]: ==================================
+[INFO] [1770376047.330447725] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376047.429493860] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376047.429595944] [wheelchair_state_machine]: 左直线: y = 2.6934x + -3.2356
+[INFO] [1770376047.429618694] [wheelchair_state_machine]: 右直线: y = -1.0603x + 1.7861
+[INFO] [1770376047.429636777] [wheelchair_state_machine]: 角平分线: y = 0.2031x + 0.0961
+[INFO] [1770376047.429656319] [wheelchair_state_machine]: 交点: (1.3378, 0.3677)
+[INFO] [1770376047.429673527] [wheelchair_state_machine]: 左直线角度: 69.63°
+[INFO] [1770376047.429691902] [wheelchair_state_machine]: 右直线角度: -46.68°
+[INFO] [1770376047.429707069] [wheelchair_state_machine]: 角平分线角度: 11.48°
+[INFO] [1770376047.429722819] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差11.5°)
+[INFO] [1770376047.429738860] [wheelchair_state_machine]: ==================================
+[INFO] [1770376047.429769485] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376047.530002370] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376047.530147620] [wheelchair_state_machine]: 左直线: y = 2.9087x + -3.4814
+[INFO] [1770376047.530202162] [wheelchair_state_machine]: 右直线: y = -1.0078x + 1.7286
+[INFO] [1770376047.530257579] [wheelchair_state_machine]: 角平分线: y = 0.2291x + 0.0832
+[INFO] [1770376047.530401370] [wheelchair_state_machine]: 交点: (1.3302, 0.3879)
+[INFO] [1770376047.530459704] [wheelchair_state_machine]: 左直线角度: 71.03°
+[INFO] [1770376047.530511329] [wheelchair_state_machine]: 右直线角度: -45.22°
+[INFO] [1770376047.530551287] [wheelchair_state_machine]: 角平分线角度: 12.90°
+[INFO] [1770376047.530591245] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差12.9°)
+[INFO] [1770376047.530632079] [wheelchair_state_machine]: ==================================
+[INFO] [1770376047.530690120] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376047.629804255] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376047.630244089] [wheelchair_state_machine]: 左直线: y = 2.9301x + -3.5039
+[INFO] [1770376047.630328380] [wheelchair_state_machine]: 右直线: y = -1.3651x + 2.1859
+[INFO] [1770376047.630483547] [wheelchair_state_machine]: 角平分线: y = 0.1528x + 0.1751
+[INFO] [1770376047.630565797] [wheelchair_state_machine]: 交点: (1.3247, 0.3776)
+[INFO] [1770376047.630641047] [wheelchair_state_machine]: 左直线角度: 71.16°
+[INFO] [1770376047.630736714] [wheelchair_state_machine]: 右直线角度: -53.78°
+[INFO] [1770376047.630794172] [wheelchair_state_machine]: 角平分线角度: 8.69°
+[INFO] [1770376047.630849589] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差8.7°)
+[INFO] [1770376047.630906172] [wheelchair_state_machine]: ==================================
+[INFO] [1770376047.630992214] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376047.728032890] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376047.728265932] [wheelchair_state_machine]: 左直线: y = 2.9158x + -3.4592
+[INFO] [1770376047.728344974] [wheelchair_state_machine]: 右直线: y = -1.0860x + 1.8401
+[INFO] [1770376047.728410015] [wheelchair_state_machine]: 角平分线: y = 0.2099x + 0.1241
+[INFO] [1770376047.728476515] [wheelchair_state_machine]: 交点: (1.3242, 0.4020)
+[INFO] [1770376047.728540682] [wheelchair_state_machine]: 左直线角度: 71.07°
+[INFO] [1770376047.728602515] [wheelchair_state_machine]: 右直线角度: -47.36°
+[INFO] [1770376047.728665515] [wheelchair_state_machine]: 角平分线角度: 11.85°
+[INFO] [1770376047.728778390] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差11.9°)
+[INFO] [1770376047.728841557] [wheelchair_state_machine]: ==================================
+[INFO] [1770376047.728926140] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376047.826005400] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376047.826137525] [wheelchair_state_machine]: 左直线: y = 3.2132x + -3.7970
+[INFO] [1770376047.826187983] [wheelchair_state_machine]: 右直线: y = -0.9424x + 1.6697
+[INFO] [1770376047.826230858] [wheelchair_state_machine]: 角平分线: y = 0.2624x + 0.0847
+[INFO] [1770376047.826273733] [wheelchair_state_machine]: 交点: (1.3155, 0.4299)
+[INFO] [1770376047.826316025] [wheelchair_state_machine]: 左直线角度: 72.71°
+[INFO] [1770376047.826358608] [wheelchair_state_machine]: 右直线角度: -43.30°
+[INFO] [1770376047.826397400] [wheelchair_state_machine]: 角平分线角度: 14.71°
+[INFO] [1770376047.826436775] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差14.7°)
+[INFO] [1770376047.826477608] [wheelchair_state_machine]: ==================================
+[INFO] [1770376047.826535358] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376047.928548994] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376047.928742077] [wheelchair_state_machine]: 左直线: y = 3.6417x + -4.3242
+[INFO] [1770376047.928835119] [wheelchair_state_machine]: 右直线: y = -0.9380x + 1.6686
+[INFO] [1770376047.928913577] [wheelchair_state_machine]: 角平分线: y = 0.2818x + 0.0724
+[INFO] [1770376047.929852661] [wheelchair_state_machine]: 交点: (1.3086, 0.4411)
+[INFO] [1770376047.929965536] [wheelchair_state_machine]: 左直线角度: 74.65°
+[INFO] [1770376047.930012494] [wheelchair_state_machine]: 右直线角度: -43.17°
+[INFO] [1770376047.930053036] [wheelchair_state_machine]: 角平分线角度: 15.74°
+[INFO] [1770376047.930092994] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差15.7°)
+[INFO] [1770376047.930134119] [wheelchair_state_machine]: ==================================
+[INFO] [1770376047.930199744] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376048.025243045] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376048.025510503] [wheelchair_state_machine]: 左直线: y = 3.5762x + -4.1958
+[INFO] [1770376048.025585170] [wheelchair_state_machine]: 右直线: y = -0.8446x + 1.5633
+[INFO] [1770376048.025652837] [wheelchair_state_machine]: 角平分线: y = 0.3076x + 0.0623
+[INFO] [1770376048.025717587] [wheelchair_state_machine]: 交点: (1.3027, 0.4630)
+[INFO] [1770376048.025773003] [wheelchair_state_machine]: 左直线角度: 74.38°
+[INFO] [1770376048.025825212] [wheelchair_state_machine]: 右直线角度: -40.18°
+[INFO] [1770376048.025872462] [wheelchair_state_machine]: 角平分线角度: 17.10°
+[INFO] [1770376048.025922628] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差17.1°)
+[INFO] [1770376048.025972504] [wheelchair_state_machine]: ==================================
+[INFO] [1770376048.026057087] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376048.123423513] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376048.123529388] [wheelchair_state_machine]: 左直线: y = 3.8066x + -4.4658
+[INFO] [1770376048.123551847] [wheelchair_state_machine]: 右直线: y = -0.8368x + 1.5568
+[INFO] [1770376048.123569638] [wheelchair_state_machine]: 角平分线: y = 0.3187x + 0.0580
+[INFO] [1770376048.123587430] [wheelchair_state_machine]: 交点: (1.2970, 0.4714)
+[INFO] [1770376048.123603180] [wheelchair_state_machine]: 左直线角度: 75.28°
+[INFO] [1770376048.123620388] [wheelchair_state_machine]: 右直线角度: -39.92°
+[INFO] [1770376048.123636138] [wheelchair_state_machine]: 角平分线角度: 17.68°
+[INFO] [1770376048.123652180] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差17.7°)
+[INFO] [1770376048.123667930] [wheelchair_state_machine]: ==================================
+[INFO] [1770376048.123699430] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376048.225167065] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376048.225363065] [wheelchair_state_machine]: 左直线: y = 4.1503x + -4.8602
+[INFO] [1770376048.225451149] [wheelchair_state_machine]: 右直线: y = -0.7891x + 1.5076
+[INFO] [1770376048.225525815] [wheelchair_state_machine]: 角平分线: y = 0.3460x + 0.0442
+[INFO] [1770376048.225601065] [wheelchair_state_machine]: 交点: (1.2892, 0.4903)
+[INFO] [1770376048.225677190] [wheelchair_state_machine]: 左直线角度: 76.45°
+[INFO] [1770376048.225750982] [wheelchair_state_machine]: 右直线角度: -38.28°
+[INFO] [1770376048.225811649] [wheelchair_state_machine]: 角平分线角度: 19.09°
+[INFO] [1770376048.225882815] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差19.1°)
+[INFO] [1770376048.225955732] [wheelchair_state_machine]: ==================================
+[INFO] [1770376048.226064232] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376048.422721502] [wheelchair_state_machine]: ========== 基站检测信息 ==========
+[INFO] [1770376048.422939377] [wheelchair_state_machine]: 左直线: y = 4.9240x + -5.7524
+[INFO] [1770376048.423018418] [wheelchair_state_machine]: 右直线: y = -0.8679x + 1.6205
+[INFO] [1770376048.423084627] [wheelchair_state_machine]: 角平分线: y = 0.3401x + 0.0827
+[INFO] [1770376048.423275085] [wheelchair_state_machine]: 交点: (1.2730, 0.5156)
+[INFO] [1770376048.423346835] [wheelchair_state_machine]: 左直线角度: 78.52°
+[INFO] [1770376048.423411293] [wheelchair_state_machine]: 右直线角度: -40.96°
+[INFO] [1770376048.423472252] [wheelchair_state_machine]: 角平分线角度: 18.78°
+[INFO] [1770376048.423534085] [wheelchair_state_machine]: 对准状态: ✗ 需要调整 (误差18.8°)
+[INFO] [1770376048.423596502] [wheelchair_state_machine]: ==================================
+[INFO] [1770376048.423688377] [wheelchair_state_machine]: 旋转延迟期间，仅发布检测信号，不执行运动控制
+[INFO] [1770376048.476100382] [wheelchair_state_machine]: [RotationManager] 旋转中 - 已旋转: 14.100405秒, 角速度: 0.150000 rad/s
+[INFO] [1770376050.476063000] [wheelchair_state_machine]: [RotationManager] 旋转中 - 已旋转: 16.100379秒, 角速度: 0.150000 rad/s
+[INFO] [1770376052.476157326] [wheelchair_state_machine]: [RotationManager] 旋转中 - 已旋转: 18.100469秒, 角速度: 0.150000 rad/s
+[WARN] [1770376053.783251749] [wheelchair_state_machine]: 没有获取到分段
+[WARN] [1770376054.078672528] [wheelchair_state_machine]: 没有获取到分段
+[WARN] [1770376054.278764465] [wheelchair_state_machine]: 没有获取到分段
+[WARN] [1770376054.376235183] [wheelchair_state_machine]: 没有获取到分段
+[INFO] [1770376054.476059943] [wheelchair_state_machine]: [RotationManager] 旋转中 - 已旋转: 20.100377秒, 角速度: 0.150000 rad/s
+[WARN] [1770376054.478832235] [wheelchair_state_machine]: 没有获取到分段
+[WARN] [1770376054.575831245] [wheelchair_state_machine]: 没有获取到分段
+[INFO] [1770376056.476004644] [wheelchair_state_machine]: [RotationManager] 旋转中 - 已旋转: 22.100342秒, 角速度: 0.150000 rad/s
+[WARN] [1770376056.860453975] [wheelchair_state_machine]: 没有获取到分段
+[WARN] [1770376056.957486818] [wheelchair_state_machine]: 没有获取到分段
+[WARN] [1770376057.059116911] [wheelchair_state_machine]: 没有获取到分段
+[WARN] [1770376057.159144755] [wheelchair_state_machine]: 没有获取到分段
+[WARN] [1770376057.255384306] [wheelchair_state_machine]: 没有获取到分段
+[WARN] [1770376057.354252899] [wheelchair_state_machine]: 没有获取到分段
+[WARN] [1770376057.455016743] [wheelchair_state_machine]: 没有获取到分段
+[INFO] [1770376058.475966345] [wheelchair_state_machine]: [RotationManager] 旋转中 - 已旋转: 24.100313秒, 角速度: 0.150000 rad/s
+[INFO] [1770376060.476067421] [wheelchair_state_machine]: [RotationManager] 旋转中 - 已旋转: 26.100387秒, 角速度: 0.150000 rad/s
+[INFO] [1770376061.173647825] [wheelchair_state_machine]: [RotationManager] 停止旋转 - 持续时间: 0.000000秒
+[INFO] [1770376061.173898950] [wheelchair_state_machine]: 状态转移: 旋转中 -> 行走中
+[INFO] [1770376061.174115366] [wheelchair_state_machine]: [INFO] 行走中: 状态已更新
+[INFO] [1770376061.174185950] [wheelchair_state_machine]: 进入行走对接状态
+[INFO] [1770376061.174247491] [wheelchair_state_machine]: 轮椅开始行走
+[INFO] [1770376061.174348116] [wheelchair_state_machine]: [INFO] WHEELCHAIR_WALKING:
+[INFO] [1770376061.174406741] [wheelchair_state_machine]: 进入行走对接状态
+[INFO] [1770376061.174500950] [wheelchair_state_machine]: [INFO] ROTATION_DELAY_COMPLETED: 旋转延迟完成，开始对接
+[INFO] [1770376061.174559283] [wheelchair_state_machine]: 进入行走对接状态
+[INFO] [1770376061.174643575] [wheelchair_state_machine]: [回充状态] STATION_DETECTED_AFTER_ROTATION: 旋转检测到基站，延迟2.5秒后开始对接
+[INFO] [1770376061.174703075] [wheelchair_state_machine]: [回充状态] STATION_DETECTED_AFTER_ROTATION: 旋转检测到基站，延迟2.5秒后开始对接
+[WARN] [1770376062.219139846] [wheelchair_state_machine]: 没有获取到分段
+[INFO] [1770376062.665018141] [wheelchair_state_machine]: [IpadManager] 取消回充功能
+[INFO] [1770376062.665121683] [wheelchair_state_machine]: iPad取消回充
+[INFO] [1770376062.665218808] [wheelchair_state_machine]: 触发事件: ipad_phone_interface_cancel
+[INFO] [1770376062.665254391] [wheelchair_state_machine]: 处理iPad取消事件
+[INFO] [1770376062.665300183] [wheelchair_state_machine]: 执行事件: ipad_phone_interface_cancel
+[INFO] [1770376062.665323808] [wheelchair_state_machine]: 处理iPad取消事件
+[INFO] [1770376062.665411016] [wheelchair_state_machine]: [INFO] IPAD_RECHARGE_CANCELLED: iPad取消自主回充
+[INFO] [1770376062.665437558] [wheelchair_state_machine]: 进入行走对接状态
+[INFO] [1770376062.665458849] [wheelchair_state_machine]: 状态转移: 行走中 -> 就绪中
+[INFO] [1770376062.665491808] [wheelchair_state_machine]: [INFO] 就绪中: 状态已更新
+[INFO] [1770376062.665571141] [wheelchair_state_machine]: 停止运动
+[INFO] [1770376062.665678183] [wheelchair_state_machine]: 导航功能已停止
+[INFO] [1770376062.665727183] [wheelchair_state_machine]: 停止运动
+[INFO] [1770376062.665749933] [wheelchair_state_machine]: 停止导航控制
+[INFO] [1770376062.665775308] [wheelchair_state_machine]: 返回到就绪状态 (前状态: 行走中)
+[INFO] [1770376062.665825766] [wheelchair_state_machine]: [INFO] WHEELCHAIR_READY:
+[INFO] [1770376062.665846766] [wheelchair_state_machine]: iPad自主回充已取消，返回到就绪状态
+[INFO] [1770376062.665872433] [wheelchair_state_machine]: [回充状态] RECHARGE_CANCELLED: 用户取消回充
+[INFO] [1770376062.665890516] [wheelchair_state_machine]: [回充状态] RECHARGE_CANCELLED: 用户取消回充
+[INFO] [1770376062.665922891] [wheelchair_state_machine]: [INFO] ipad_phone_interface_cancel: 事件已处理

src/wheelchair_state_machine/include/wheelchair_state_machine/event_input.hpp

@@ -15,7 +15,9 @@
 #include "report.hpp"
 #include <std_msgs/msg/bool.hpp>
 #include <std_msgs/msg/string.hpp>
-
+#include<std_msgs/msg/int32.hpp>
+#include"recharge_status.hpp"
+#include <std_srvs/srv/set_bool.hpp>
 class EventInputHandler
 {
 public:
@@ -71,6 +73,7 @@ private:
 
     // ROS订阅者
     rclcpp::Subscription<std_msgs::msg::String>::SharedPtr cmd_enable_sub_;
+    rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr recharge_service_;
 
     // 事件回调映射
     std::map<std::string, EventCallback> event_callbacks_;

src/wheelchair_state_machine/include/wheelchair_state_machine/lidascan_ctrl.hpp

@@ -11,8 +11,11 @@
 #include <map>
 #include <memory>
 #include <tuple>
+#include <std_msgs/msg/int32.hpp>
 #include <std_msgs/msg/bool.hpp>
-
+#include<std_msgs/msg/string.hpp>
+#include"recharge_status.hpp"
+#include<sstream>
 class LidarScanController
 {
 public:
@@ -25,6 +28,9 @@ public:
     // 初始化
     void initialize();
 
+    // 新增：获取当前检测到的角平分线信息
+    std::pair<double, double> getAngleBisectorInfo() const { return {current_angle_bisector_k_, current_angle_bisector_b_}; }
+
     // 导航控制接口（外部调用）
     void startNavigation();
     void stopNavigation();
@@ -45,6 +51,11 @@ public:
 private:
     rclcpp::Node *node_;
 
+    // 新增：存储当前角平分线信息
+    double current_angle_bisector_k_;
+    double current_angle_bisector_b_;
+    bool has_angle_bisector_;
+
     // ROS2 订阅者和发布者
     rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_ctrl_;
@@ -52,8 +63,10 @@ private:
     rclcpp::TimerBase::SharedPtr navigation_timer_;
     rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_filtered_;
     rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_line_;
-    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr pub_station_detected_; // 新增
-
+    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr pub_station_detected_; 
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_charging_voltage_;
+    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub1_;//对外
+    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub2_;//对外
     // 状态变量
     bool active_;
     ChargingState charging_state_;
@@ -126,6 +139,9 @@ private:
 
     // 控制发布
     void publishControl();
+
+    //判断误差函数
+    bool checkStationAlignment(double dx,double dy,double yaw_diff);
 };
 
 #endif // LIDAR_SCAN_CONTROLLER_HPP

src/wheelchair_state_machine/include/wheelchair_state_machine/recharge_status.hpp

@@ -0,0 +1,25 @@
+#ifndef RECHARGE_STATUS_HPP
+#define RECHARGE_STATUS_HPP
+
+// 回充状态上报枚举 (对应图片上半部分)
+enum RechargeReportStatus
+{
+    RECHARGE_EVENT_OBSTACLE_DETECTED = 1,   // 前往充电站中
+    RECHARGE_EVENT_STATION_NOT_FOUND = 2,   // 未找到充电站
+    RECHARGE_FEEDBACK_SUCCESS_DOCKED = 3      // 成功对接
+    // RECHARGE_EVENT_OBSTACLE_DETECTED = 4,   // 检测到障碍物
+    // RECHARGE_EVENT_START_CHARGING = 5,      // 开始充电
+    // RECHARGE_EVENT_STOP_CHARGING = 6        // 停止充电
+};
+
+// 回充结果反馈枚举 (对应图片下半部分)
+enum RechargeFeedbackResult
+{
+    // RECHARGE_FEEDBACK_SUCCESS_DOCKED = 100,     // 成功-正常对接
+    RECHARGE_FEEDBACK_FAILED_TIMEOUT = 201,     // 停止-找不到基站超时
+    RECHARGE_FEEDBACK_FAILED_OBSTACLE = 202,    // 停止-遇到障碍物
+    RECHARGE_FEEDBACK_FAILED_USER_CANCEL = 203, // 停止-人工操作取消
+    RECHARGE_FEEDBACK_FAILED_OTHER = 299        // 停止-其他故障
+};
+
+#endif // RECHARGE_STATUS_HPP

src/wheelchair_state_machine/include/wheelchair_state_machine/rotation_manager.hpp

@@ -5,6 +5,10 @@
 #include <rclcpp/rclcpp.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 #include <memory>
+#include <std_msgs/msg/string.hpp>
+#include<std_msgs/msg/int32.hpp>
+#include<std_msgs/msg/bool.h>
+#include "recharge_status.hpp"
 
 class RotationManager
 {
@@ -31,7 +35,10 @@ private:
     rclcpp::Node *node_;
     // ROS发布者
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_publisher_;
-
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr rotation_timeout_pub_;
+    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rotation_timeout_pub1_;//对外
+    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rotation_timeout_pub2_;//对外
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub;
     // 定时器
     rclcpp::TimerBase::SharedPtr rotation_timer_;
     rclcpp::TimerBase::SharedPtr rotation_timeout_timer_;
@@ -52,6 +59,7 @@ private:
     void logInfo(const std::string &message);
     void logWarning(const std::string &message);
     void logError(const std::string &message);
+
 };
 
 #endif // ROTATION_MANAGER_HPP

src/wheelchair_state_machine/include/wheelchair_state_machine/workflow.hpp

@@ -14,6 +14,8 @@
 #include <rclcpp/timer.hpp>
 #include "rotation_manager.hpp" // 新增头文件
 #include <std_msgs/msg/bool.hpp>
+#include<std_msgs/msg/int32.hpp>
+#include "recharge_status.hpp"
 
 class WorkflowManager
 {
@@ -82,6 +84,11 @@ public:
 private:
     rclcpp::Node *node_;
 
+    // 新增：基站检测处理相关
+    bool station_detected_while_rotating_ = false;      // 旋转中检测到基站
+    rclcpp::Time station_detected_time_;                // 基站检测时间
+    rclcpp::TimerBase::SharedPtr rotation_delay_timer_; // 旋转延迟定时器
+
     // 状态机状态
     WheelchairState current_state_;
     std::map<std::string, std::map<WheelchairState, bool>> transition_table_;
@@ -93,6 +100,8 @@ private:
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr state_publisher_;
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr recharge_timeout_publisher_;
     rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr station_detected_sub_; // 新增
+    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub1_;//对外
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr rotation_timeout_sub_;
 
     // 定时器
     rclcpp::TimerBase::SharedPtr search_timeout_timer_;
@@ -154,5 +163,13 @@ private:
     void rotationCheckCallback();
 
     void stationDetectedCallback(const std_msgs::msg::Bool::SharedPtr msg);
+
+    // 新增：延迟处理相关方法
+    void startRotationDelayTimer();
+    void stopRotationDelayTimer();
+    void rotationDelayCallback();
+
+    // 修改现有的方法声明
+    void processStationDetectionWhileRotating();
 };
 #endif

src/wheelchair_state_machine/src/event_input.cpp

@@ -2,6 +2,7 @@
 #include "wheelchair_state_machine/event_input.hpp"
 #include <rclcpp/logging.hpp>
 #include <std_msgs/msg/bool.hpp>
+#include <std_srvs/srv/set_bool.hpp>
 using namespace std::chrono_literals;
 
 EventInputHandler::EventInputHandler(rclcpp::Node *node)
@@ -103,6 +104,7 @@ void EventInputHandler::setupModuleCallbacks()
             }
             else if (current_state == WheelchairState::CHARGING)
             {
+                stopNavigationControl();
                 lidar_controller_->stopMotion();
                 lidar_controller_->setChargingVoltage(true);
             }
@@ -274,6 +276,29 @@ void EventInputHandler::setupSubscriptions()
         {
             this->ipad_manager_->onCmdEnable(msg);
         });
+    // === 新增：创建回充控制服务 ===
+    recharge_service_ = node_->create_service<std_srvs::srv::SetBool>(
+        "/wheelchair/recharge_control",
+        [this](const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
+               std::shared_ptr<std_srvs::srv::SetBool::Response> response)
+        {
+            if (request->data)
+            {
+                RCLCPP_INFO(node_->get_logger(), "收到外部服务请求: 启动回充");
+                // 复用现有的启动接口，将其推送到工作流
+                this->triggerIpadStart(); 
+                response->success = true;
+                response->message = "启动回充命令已下发";
+            }
+            else
+            {
+                RCLCPP_INFO(node_->get_logger(), "收到外部服务请求: 取消回充");
+                // 复用现有的取消接口，将其推送到工作流
+                this->triggerIpadCancel(); 
+                response->success = true;
+                response->message = "取消回充命令已下发";
+            }
+        });
 
     RCLCPP_INFO(node_->get_logger(), "订阅器设置完成");
 }

src/wheelchair_state_machine/src/ipad_manager.cpp

@@ -13,7 +13,6 @@ IpadManager::IpadManager(rclcpp::Node *node)
 void IpadManager::onCmdEnable(const std_msgs::msg::String::SharedPtr msg)
 {
     std::string new_enable = msg->data;
-    logInfo("收到命令使能: " + new_enable);
     // 状态变化检查
     if (cmd_enable_ == "auxiliary" && new_enable == "recharge")
     {
@@ -27,8 +26,6 @@ void IpadManager::onCmdEnable(const std_msgs::msg::String::SharedPtr msg)
     }
     else
     {
-        // 状态未变化
-        logInfo("命令使能状态未变化");
     }
 }
 

src/wheelchair_state_machine/src/lidascan_ctrl.cpp

@@ -14,7 +14,9 @@ using namespace std::chrono_literals;
 using std::placeholders::_1;
 
 LidarScanController::LidarScanController(rclcpp::Node *node)
-    : node_(node), active_(false), charging_state_(ChargingState::IDLE), detect_charging_voltage_(false), frame_index_(0), station_detected_(false)
+    : node_(node), active_(false), charging_state_(ChargingState::IDLE), detect_charging_voltage_(false), frame_index_(0), station_detected_(false), current_angle_bisector_k_(0.0),
+      current_angle_bisector_b_(0.0), // 新增
+      has_angle_bisector_(false)      // 新增
 {
 }
 
@@ -61,9 +63,12 @@ void LidarScanController::initialize()
 
     // 创建基站检测状态发布者（使用 transient_local 确保新订阅者能收到最后一条消息）
     pub_station_detected_ = node_->create_publisher<std_msgs::msg::Bool>(
-        "station/detected",
+        "/station/detected",
         rclcpp::QoS(10).transient_local()); // 修改为 transient_local
-
+    pub_charging_voltage_ = node_->create_publisher<std_msgs::msg::String>(
+        "/charging/voltage_detected", 10);
+    pub1_=node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/state",10);
+    pub2_=node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/feedback",10);
     // 初始化控制消息
     ctrl_twist_.linear.x = 0.0;
     ctrl_twist_.angular.z = 0.0;
@@ -136,14 +141,14 @@ void LidarScanController::publishStationDetected(bool detected)
     }
     last_station_publish_time_ = now;
 
-    if (detected)
-    {
-        RCLCPP_INFO(node_->get_logger(), "发布基站检测成功信号: true");
-    }
-    else
-    {
-        RCLCPP_DEBUG(node_->get_logger(), "发布基站检测信号: false");
-    }
+    // if (detected)
+    // {
+    //     RCLCPP_INFO(node_->get_logger(), "发布基站检测成功信号: true");
+    // }
+    // else
+    // {
+    //     RCLCPP_DEBUG(node_->get_logger(), "发布基站检测信号: false");
+    // }
 }
 
 void LidarScanController::stopMotion()
@@ -210,7 +215,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
     auto sections = getSections(begin_index, end_index, msg, xy_list);
     if (sections.empty())
     {
-        RCLCPP_WARN(node_->get_logger(), "没有获取到分段");
+        // RCLCPP_WARN(node_->get_logger(), "没有获取到分段");
         return false;
     }
 
@@ -270,7 +275,6 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
     if (xy_list_full.size() < 2)
     {
         // publishStationDetected(false);
-        RCLCPP_ERROR(node_->get_logger(), "xy坐标系没有转换成功");
         return false;
     }
 
@@ -281,7 +285,6 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
     if (filtered_segments.empty())
     {
         // publishStationDetected(false);
-        RCLCPP_DEBUG(node_->get_logger(), "未检测到有效的分段");
         return false;
     }
 
@@ -336,6 +339,7 @@ void LidarScanController::procData(RechargeResult &result)
     if (result.left_points.size() < 2 || result.right_points.size() < 2)
     {
         RCLCPP_WARN(node_->get_logger(), "点数量不足，跳过拟合");
+        has_angle_bisector_ = false;
         return;
     }
 
@@ -355,20 +359,38 @@ void LidarScanController::procData(RechargeResult &result)
         result.middle_line = StraightLine(0.0, 0.0);
         result.left_line = StraightLine(k1, b1);
         result.right_line = StraightLine(k2, b2);
+        has_angle_bisector_ = false;
         return;
     }
 
     // 计算交点
     double x = (b2 - b1) / (k1 - k2);
     double y = k1 * x + b1;
-    RCLCPP_DEBUG(node_->get_logger(), "交点: (%.4f, %.4f)", x, y);
 
     // 更新中间点
     result.middle_point = IndexPoint(x, y, 0);
 
     // 计算角平分线
     auto [k3, b3] = RechargeTool::calculateAngleBisector(k1, b1, k2, b2);
-    RCLCPP_DEBUG(node_->get_logger(), "角平分线: y = %.4fx + %.4f", k3, b3);
+    current_angle_bisector_k_ = k3;
+    current_angle_bisector_b_ = b3;
+    has_angle_bisector_ = true;
+
+    // 详细打印角平分线信息（用于测试对齐）
+    // RCLCPP_INFO(node_->get_logger(), "========== 基站检测信息 ==========");
+    // RCLCPP_INFO(node_->get_logger(), "左直线: y = %.4fx + %.4f", k1, b1);
+    // RCLCPP_INFO(node_->get_logger(), "右直线: y = %.4fx + %.4f", k2, b2);
+    // RCLCPP_INFO(node_->get_logger(), "角平分线: y = %.4fx + %.4f", k3, b3);
+    // RCLCPP_INFO(node_->get_logger(), "交点: (%.4f, %.4f)", x, y);
+
+    // 计算角度信息
+    double left_angle = atan(k1) * 180.0 / M_PI;
+    double right_angle = atan(k2) * 180.0 / M_PI;
+    double bisector_angle = atan(k3) * 180.0 / M_PI;
+
+    // RCLCPP_INFO(node_->get_logger(), "左直线角度: %.2f°", left_angle);
+    // RCLCPP_INFO(node_->get_logger(), "右直线角度: %.2f°", right_angle);
+    // RCLCPP_INFO(node_->get_logger(), "角平分线角度: %.2f°", bisector_angle);
 
     // 更新结果
     result.middle_line = StraightLine(k3, b3);
@@ -384,7 +406,25 @@ void LidarScanController::procData(RechargeResult &result)
 
 void LidarScanController::controlRechargeMotion(const RechargeResult &result)
 {
+    if (!active_) return;
     publishStationDetected(true);
+    bool in_rotation_delay = node_->get_parameter("in_rotation_delay").as_bool();
+    if (in_rotation_delay)
+    {
+        RCLCPP_INFO(node_->get_logger(), "旋转延迟期间，仅发布检测信号，不执行运动控制");
+        // 清除控制命令，避免定时器发布旧命令
+        // 修改：不再清除控制命令，而是继续旋转
+        // 只清除线性速度，保持角速度
+        double angular_velocity = 0.15; // 与RotationManager中的角速度一致
+
+        ctrl_twist_.linear.x = 0.0;               // 线速度保持0
+        ctrl_twist_.angular.z = angular_velocity; // 保持旋转速度
+        ctrl_twist_time_ = node_->now();
+
+        // 发布旋转命令
+        pub_ctrl_->publish(ctrl_twist_);
+        return;
+    }
     // 如果检测到充电电压，停止移动
     if (getChargingVoltage())
     {
@@ -392,6 +432,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         ctrl_twist_.linear.x = 0.0;
         ctrl_twist_.angular.z = 0.0;
         ctrl_twist_time_ = node_->now();
+        stopNavigation();
         return;
     }
 
@@ -406,15 +447,133 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
     RCLCPP_DEBUG(node_->get_logger(), "调整后 dy: %.4f", dy);
 
     // 计算偏航角误差
-    double yaw_error = atan(k);
-    RCLCPP_DEBUG(node_->get_logger(), "偏航角误差: %.4f rad (%.2f 度)",
-                 yaw_error, yaw_error * 180.0 / M_PI);
+    double yaw_diff = atan(k);
+    RCLCPP_INFO(node_->get_logger(), "偏航角误差: %.4f rad (%.2f 度)",
+                yaw_diff, yaw_diff * 180.0 / M_PI);
+
+    if (checkStationAlignment(dx, dy, yaw_diff))
+    {
+        // 停止行车中，准备向前移动，再校准回充
+        RCLCPP_WARN(node_->get_logger(), "===== 开始对准误差调整 =====");
+        RCLCPP_WARN(node_->get_logger(), "当前误差 - 夹角:%.2f°, dy:%.3fm",
+                    yaw_diff * 180.0 / M_PI, dy);
+
+        // 1. 停止当前运动
+        ctrl_twist_.linear.x = 0.0;
+        ctrl_twist_.angular.z = 0.0;
+        ctrl_twist_time_ = node_->now();
+        pub_ctrl_->publish(ctrl_twist_);
+        rclcpp::sleep_for(std::chrono::milliseconds(300));
+
+        // 2. 根据误差类型进行旋转调整
+        double adjust_duration = 0.0;
+        double adjust_angular = 0.0;
+        std::string adjust_type;
+
+        const double ANGLE_8DEG = 5.0 * M_PI / 180.0; // 5度阈值
+                                                      // const double DY_THRESHOLD = 0.05;             // 5cm阈值
+
+        // 优先调整角度
+        if (fabs(yaw_diff) > ANGLE_8DEG)
+        {
+            adjust_type = "角度调整";
+            // 根据夹角大小计算调整时间
+            adjust_duration = fabs(yaw_diff) / 0.15; // 角速度0.25 rad/s
+            adjust_angular = (yaw_diff > 0) ? -0.15 : 0.15;
+
+            RCLCPP_WARN(node_->get_logger(), "旋转 %.2f 秒", adjust_duration);
+        }
+        // 角度满足要求后调整dy
+        // else
+        // {
+        //     adjust_type = "横向偏移调整";
+        //     // dy为正(偏右) -> 向左转；dy为负(偏左) -> 向右转
+        //     adjust_duration = fabs(dy) * 20.0;  // 每5cm误差旋转1秒
+        //     adjust_angular = (dy > 0) ? 0.10 : -0.10;
+
+        //     RCLCPP_WARN(node_->get_logger(), "旋转 %.2f 秒",adjust_duration);
+        // }
+
+        // 3. 执行旋转调整
+        if (active_)
+        {
+            if (adjust_duration > 0.1)
+            {
+                geometry_msgs::msg::Twist adjust_cmd;
+                adjust_cmd.linear.x = 0.0;
+                adjust_cmd.angular.z = adjust_angular;
+
+                // 按计算的时间连续旋转
+                rclcpp::Time adjust_start = node_->now();
+                int publish_count = 0;
+
+                while (rclcpp::ok() && active_ &&(node_->now() - adjust_start).seconds() < adjust_duration)
+                {
+                    pub_ctrl_->publish(adjust_cmd);
+                    publish_count++;
+                    rclcpp::sleep_for(std::chrono::milliseconds(50));
+                }
+
+                RCLCPP_WARN(node_->get_logger(), "%s完成: 发布%d次命令, 实际旋转%.2f秒",
+                            adjust_type.c_str(), publish_count, adjust_duration);
+
+                // 4. 停止旋转
+                adjust_cmd.angular.z = 0.0;
+                pub_ctrl_->publish(adjust_cmd);
+                rclcpp::sleep_for(std::chrono::milliseconds(500));
+            }
+
+            // 5. 向前走一小段距离（新增功能）
+            const double FORWARD_DISTANCE = 0.80; // 向前走150cm
+            const double FORWARD_SPEED = 0.09;    // 前进速度 0.08 m/s
+
+            // 计算前进所需时间
+            double forward_duration = FORWARD_DISTANCE / FORWARD_SPEED;
+
+            RCLCPP_WARN(node_->get_logger(), "开始向前调整: 距离=%.2fm, 速度=%.2fm/s, 时间=%.2fs",
+                        FORWARD_DISTANCE, FORWARD_SPEED, forward_duration);
+
+            // 发送前进命令
+            geometry_msgs::msg::Twist forward_cmd;
+            forward_cmd.linear.x = FORWARD_SPEED;
+            forward_cmd.angular.z = 0.0;
+
+            rclcpp::Time forward_start = node_->now();
+            int forward_count = 0;
+
+            while (rclcpp::ok() && active_ &&(node_->now() - forward_start).seconds() < forward_duration)
+            {
+                pub_ctrl_->publish(forward_cmd);
+                forward_count++;
+                rclcpp::sleep_for(std::chrono::milliseconds(50));
+            }
+
+            // 停止前进
+            forward_cmd.linear.x = 0.0;
+            pub_ctrl_->publish(forward_cmd);
 
+            RCLCPP_INFO(node_->get_logger(), "前进调整完成: 发布%d次命令, 实际前进%.2f秒",
+                        forward_count, forward_duration);
+        }
+
+        // 6. 重要：等待新一帧激光数据并更新result
+        // 等待激光数据更新（约100ms足够接收新数据）
+        rclcpp::sleep_for(std::chrono::milliseconds(450));
+
+        // 注意：这里无法直接更新result，但函数返回后，下一帧激光数据会触发新的processSegmentData
+        // 从而得到新的result，重新进入controlRechargeMotion
+
+        RCLCPP_WARN(node_->get_logger(), "===== 对准误差调整完成 =====");
+
+        // 7. 返回，等待下一帧激光数据重新计算
+        // 不继续执行后面的速度计算，避免使用旧数据
+        return;
+    }
     // 计算控制速度
-    auto [linear_velocity, angular_velocity] = RechargeTool::computeDockingVelocity(dx - 0.3, dy, yaw_error);
+    auto [linear_velocity, angular_velocity] = RechargeTool::computeDockingVelocity(dx - 0.3, dy, yaw_diff);
     RCLCPP_DEBUG(node_->get_logger(), "基础控制速度 - 线速度: %.4f, 角速度: %.4f",
                  linear_velocity, angular_velocity);
-
+    RCLCPP_INFO(node_->get_logger(), "dx=%.4f,dy=%.4f", dx, dy);
     // 限制角速度
     if (angular_velocity > 0.0)
     {
@@ -487,12 +646,46 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         ctrl_twist_.linear.x = 0.0;
         ctrl_twist_.angular.z = 0.0;
         ctrl_twist_time_ = node_->now();
+        auto msg =std_msgs::msg::Int32();
+        msg.data=RECHARGE_FEEDBACK_SUCCESS_DOCKED;
+        pub1_->publish(msg);
+        stopNavigation();
 
         // 模拟检测到充电电压
         setChargingVoltage(true);
     }
 }
 
+bool LidarScanController::checkStationAlignment(double dx, double dy, double yaw_diff)
+{
+    bool isstop = false;
+    const double ALIGNMENT_DISTANCE_THRESHOLD = 0.80;            // 80cm
+    const double ALIGNMENT_ANGLE_THRESHOLD = 5.0 * M_PI / 180.0; // 5度
+
+    if (dx <= ALIGNMENT_DISTANCE_THRESHOLD)
+    {
+        RCLCPP_INFO(node_->get_logger(), "进入对准区域 - 距离基站: %.4f m, 夹角: %.2f°",
+                    dx, yaw_diff * 180.0 / M_PI);
+
+        // 判断是否对准（夹角小于10度）
+        if (fabs(yaw_diff) <= ALIGNMENT_ANGLE_THRESHOLD)
+        {
+            RCLCPP_INFO(node_->get_logger(), "✓ 已对准基站，继续行驶 - 夹角: %.2f°,dx=%.2f,dy=%.2f",
+                        yaw_diff * 180.0 / M_PI, dx, dy);
+
+            RCLCPP_DEBUG(node_->get_logger(), "对准行驶 - 线速度: %.4f", ctrl_twist_.linear.x);
+        }
+        else
+        {
+            RCLCPP_WARN(node_->get_logger(), "✗ 未对准基站，停止行驶 - 夹角: %.2f° > 5°,dx=%.2f,dy=%.2f",
+                        yaw_diff * 180.0 / M_PI, dx, dy);
+            isstop = true;
+            // RCLCPP_INFO(node_->get_logger(), "自动调整方向 - 旋转角速度: %.4f", adjust_angular);
+        }
+    }
+    return isstop;
+}
+
 // ==================== 过滤函数实现 ====================
 void LidarScanController::filterScanModify0(LaserScanSharedPtr msg)
 {
@@ -1097,15 +1290,24 @@ void LidarScanController::setChargingVoltage(bool value)
 {
     detect_charging_voltage_ = value;
     detect_charging_voltage_time_ = node_->now();
+    auto msg = std_msgs::msg::String();
+    std::stringstream ss;
+    ss << (value ? "CHARGING_VOLTAGE_DETECTED" : "CHARGING_VOLTAGE_LOST")
+       << ":timestamp=" << node_->now().seconds()
+       << ",voltage_status=" << (value ? "true" : "false");
 
     if (value)
     {
+        ss << ",message=充电电压已检测到，开始充电流程";
         RCLCPP_INFO(node_->get_logger(), "检测到充电电压");
     }
     else
     {
+        ss << ",message=充电电压已断开";
         RCLCPP_INFO(node_->get_logger(), "充电电压已断开");
     }
+     msg.data = ss.str();
+    pub_charging_voltage_->publish(msg);
 }
 
 bool LidarScanController::getChargingVoltage() const
@@ -1114,9 +1316,10 @@ bool LidarScanController::getChargingVoltage() const
     auto diff = now - detect_charging_voltage_time_;
 
     // 如果检测到充电电压且在0.1秒内，认为有效
-    if (detect_charging_voltage_ && diff.seconds() < 0.1)
-    {
-        return true;
-    }
-    return false;
+    // if (detect_charging_voltage_ && diff.seconds() < 0.1)
+    // {
+    //     return true;
+    // }
+    // return false;
+    return detect_charging_voltage_; 
 }

src/wheelchair_state_machine/src/rotation_manager.cpp

@@ -5,7 +5,7 @@
 using namespace std::chrono_literals;
 
 RotationManager::RotationManager(rclcpp::Node *node)
-    : node_(node), is_rotating_(false), rotation_timeout_(20.0) // 旋转20秒超时
+    : node_(node), is_rotating_(false), rotation_timeout_(40.0) // 旋转40秒超时
       ,
       angular_velocity_(0.15) // 角速度0.3 rad/s
       ,
@@ -14,7 +14,11 @@ RotationManager::RotationManager(rclcpp::Node *node)
     // 创建控制命令发布者
     cmd_vel_publisher_ = node_->create_publisher<geometry_msgs::msg::Twist>(
         "/cmd_vel_raw", 10);
-
+    rotation_timeout_pub1_ = node_->create_publisher<std_msgs::msg::Int32>(
+        "/vlago/recharge/state", 10);
+    rotation_timeout_pub2_=node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/feedback",10);
+    rotation_timeout_pub_ = node_->create_publisher<std_msgs::msg::String>(
+        "/wheelchair/rotation_timeout", 10);
     logInfo("旋转管理器初始化完成");
 }
 
@@ -43,7 +47,16 @@ void RotationManager::startRotation()
         [this]()
         {
             logInfo("旋转超时，自动停止旋转");
+             auto timeout_msg = std_msgs::msg::String();
+            timeout_msg.data = "rotation_timeout";
+            rotation_timeout_pub_->publish(timeout_msg);
             stopRotation();
+            auto timeout_msg1 = std_msgs::msg::Int32();
+            timeout_msg1.data = RECHARGE_EVENT_STATION_NOT_FOUND;
+            rotation_timeout_pub1_->publish(timeout_msg1); 
+            auto timeout_msg2 = std_msgs::msg::Int32();
+            timeout_msg2.data = RECHARGE_FEEDBACK_FAILED_TIMEOUT;
+            rotation_timeout_pub2_->publish(timeout_msg2);
         });
 
     logInfo("开始旋转 - 角速度: " + std::to_string(angular_velocity_) +

src/wheelchair_state_machine/src/workflow.cpp

@@ -8,7 +8,7 @@
 using namespace std::chrono_literals;
 
 WorkflowManager::WorkflowManager(rclcpp::Node *node)
-    : node_(node), current_state_(WheelchairState::READY)
+    : node_(node), current_state_(WheelchairState::READY), station_detected_while_rotating_(false)
 {
     // 初始化状态转移表
     initializeTransitionTable();
@@ -20,11 +20,12 @@ WorkflowManager::WorkflowManager(rclcpp::Node *node)
 
     // 创建状态发布者
     state_publisher_ = node_->create_publisher<std_msgs::msg::String>(
-        "wheelchair/state", 10);
+        "/wheelchair/state", 10);
 
     // 创建回充超时报告发布者
     recharge_timeout_publisher_ = node_->create_publisher<std_msgs::msg::String>(
-        "wheelchair/recharge_timeout", 10);
+        "/wheelchair/recharge_timeout", 10);
+    pub1_ = node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/state", 10);
 
     RCLCPP_INFO(node_->get_logger(), "工作流管理器已初始化");
 }
@@ -32,13 +33,33 @@ void WorkflowManager::initializeSubscriptions()
 {
     // 创建基站检测订阅者
     station_detected_sub_ = node_->create_subscription<std_msgs::msg::Bool>(
-        "station/detected", 10,
+        "/station/detected", 10,
         [this](const std_msgs::msg::Bool::SharedPtr msg)
         {
             this->stationDetectedCallback(msg);
         });
+    rotation_timeout_sub_ = node_->create_subscription<std_msgs::msg::String>(
+        "/wheelchair/rotation_timeout", 10,
+        [this](const std_msgs::msg::String::SharedPtr msg)
+        {
+            if (msg->data == "rotation_timeout")
+            {
+                RCLCPP_INFO(node_->get_logger(), "收到旋转超时信号，返回就绪状态");
+                
+                // 如果当前是旋转状态，处理超时
+                if (current_state_ == WheelchairState::ROTATING)
+                {
+                    // 发布回充失败通知
+                    publishRechargeStatus("RECHARGE_FAILED", "旋转搜索超时，未检测到基站");
+                    
+                    // 返回就绪状态
+                    transitionToReady();
+                }
+            }
+        });
 
     RCLCPP_INFO(node_->get_logger(), "基站检测订阅者已初始化");
+    node_->declare_parameter<bool>("in_rotation_delay", false);
 }
 
 void WorkflowManager::stationDetectedCallback(const std_msgs::msg::Bool::SharedPtr msg)
@@ -53,15 +74,35 @@ void WorkflowManager::stationDetectedCallback(const std_msgs::msg::Bool::SharedP
                      station_detected ? "true" : "false",
                      current_state_str.c_str());
 
-        // 只有在搜索或旋转状态下，且检测到基站时，才考虑状态转移
-        if (station_detected &&
-            (current_state_ == WheelchairState::SEARCHING ||
-             current_state_ == WheelchairState::ROTATING))
+        // 处理旋转中检测到基站的情况
+        if (station_detected && current_state_ == WheelchairState::ROTATING)
+        {
+            RCLCPP_INFO(node_->get_logger(),
+                        "旋转中检测到基站，记录检测时间并设置延迟切换");
+
+            // 记录检测时间和状态
+            station_detected_while_rotating_ = true;
+            station_detected_time_ = node_->now();
+            // 通知激光控制器进入旋转延迟模式
+            node_->set_parameter(rclcpp::Parameter("in_rotation_delay", true));
+
+            // 启动延迟定时器（2秒后切换状态）
+            startRotationDelayTimer();
+
+            // 发布状态信息
+            publishState("STATION_DETECTED_WHILE_ROTATING",
+                         "旋转中检测到基站，继续旋转2秒后对接");
+
+            return; // 直接返回，不立即切换状态
+        }
+
+        // 搜索中检测到基站，立即切换状态（保持原有逻辑）
+        if (station_detected && current_state_ == WheelchairState::SEARCHING)
         {
             RCLCPP_INFO(node_->get_logger(),
                         "搜索中检测到基站，切换到行走状态进行对接");
 
-            // 从搜索/旋转状态切换到行走状态
+            // 从搜索状态切换到行走状态
             transitionToWalking();
 
             // 发布状态更新
@@ -73,12 +114,20 @@ void WorkflowManager::stationDetectedCallback(const std_msgs::msg::Bool::SharedP
         }
         else if (!station_detected)
         {
-            // 基站丢失处理（如果之前正在行走对接，但现在基站丢失）
+            // 基站丢失处理
             if (current_state_ == WheelchairState::WALKING)
             {
-                // 可以根据需要处理基站丢失的情况
                 RCLCPP_DEBUG(node_->get_logger(), "行走对接过程中基站信号丢失");
             }
+
+            // 如果之前旋转中检测到基站但现在丢失了，重置状态
+            if (station_detected_while_rotating_)
+            {
+                station_detected_while_rotating_ = false;
+                stopRotationDelayTimer();
+                RCLCPP_WARN(node_->get_logger(),
+                            "旋转中检测到的基站信号丢失，重置检测状态");
+            }
         }
     }
     catch (const std::exception &e)
@@ -87,6 +136,91 @@ void WorkflowManager::stationDetectedCallback(const std_msgs::msg::Bool::SharedP
     }
 }
 
+void WorkflowManager::startRotationDelayTimer()
+{
+    try
+    {
+        // 如果已有定时器，先停止
+        stopRotationDelayTimer();
+
+        // 创建0.5秒延迟定时器
+        rotation_delay_timer_ = node_->create_wall_timer(
+            std::chrono::milliseconds(1500),
+            [this]()
+            {
+                this->rotationDelayCallback();
+            });
+
+        RCLCPP_INFO(node_->get_logger(),
+                    "旋转延迟定时器已启动 (0秒后切换状态)");
+    }
+    catch (const std::exception &e)
+    {
+        handleException("启动旋转延迟定时器", e);
+    }
+}
+
+void WorkflowManager::stopRotationDelayTimer()
+{
+    try
+    {
+        if (rotation_delay_timer_)
+        {
+            rotation_delay_timer_->cancel();
+            rotation_delay_timer_.reset();
+            RCLCPP_DEBUG(node_->get_logger(), "旋转延迟定时器已停止");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("停止旋转延迟定时器", e);
+    }
+}
+
+void WorkflowManager::rotationDelayCallback()
+{
+    try
+    {
+        if (station_detected_while_rotating_ &&
+            current_state_ == WheelchairState::ROTATING)
+        {
+            node_->set_parameter(rclcpp::Parameter("in_rotation_delay", false));
+            // 计算实际旋转时间
+            double rotation_time = (node_->now() - station_detected_time_).seconds();
+            // 停止旋转并切换到行走状态
+            if (rotation_manager_ && rotation_manager_->isRotating())
+            {
+                rotation_manager_->stopRotation();
+            }
+
+            transitionToWalking();
+
+            // 重置状态
+            station_detected_while_rotating_ = false;
+            stopRotationDelayTimer();
+
+            // 发布状态更新
+            publishState("ROTATION_DELAY_COMPLETED",
+                         "旋转延迟完成，开始对接");
+
+            publishRechargeStatus("STATION_DETECTED_AFTER_ROTATION",
+                                  "旋转检测到基站，延迟2.5秒后开始对接");
+        }
+        else
+        {
+            // 状态已改变或检测已失效，清理定时器
+            station_detected_while_rotating_ = false;
+            stopRotationDelayTimer();
+            RCLCPP_DEBUG(node_->get_logger(),
+                         "旋转延迟定时器触发，但状态已变化或检测失效");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("旋转延迟定时器回调", e);
+    }
+}
+
 void WorkflowManager::initializeRotationManager()
 {
     rotation_manager_ = std::make_unique<RotationManager>(node_);
@@ -254,7 +388,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> search_timeout_perms = {
         {WheelchairState::READY, false},
         {WheelchairState::WALKING, false},
-        {WheelchairState::ROTATING, false}, // 旋转中❌
+        {WheelchairState::ROTATING, true},
         {WheelchairState::SEARCHING, true},
         {WheelchairState::CHARGING, false}};
     transition_table_["search_30s_timeout"] = search_timeout_perms;
@@ -570,7 +704,6 @@ void WorkflowManager::transitionToRotating()
                 rotation_manager_->startRotation();
                 RCLCPP_INFO(node_->get_logger(), "轮椅开始旋转搜索");
                 publishState("WHEELCHAIR_ROTATING", "旋转搜索充电站");
-
                 // 发布回充状态
                 publishRechargeStatus("ROTATING_SEARCH",
                                       "搜索10秒未找到基站，开始旋转搜索");
@@ -579,13 +712,10 @@ void WorkflowManager::transitionToRotating()
             {
                 RCLCPP_ERROR(node_->get_logger(), "旋转管理器未初始化");
             }
-
             // 停止旋转检查定时器
             stopRotationTimer();
-
             // 关键修复：进入旋转状态时，停止搜索超时定时器
             stopSearchTimeoutTimer();
-
             RCLCPP_INFO(node_->get_logger(), "已停止搜索超时定时器，进入旋转搜索模式");
         }
         else
@@ -610,6 +740,11 @@ void WorkflowManager::transitionToWalking()
         {
             rotation_manager_->stopRotation();
         }
+        //  新增：停止延迟定时器
+        stopRotationDelayTimer();
+
+        // 新增：重置检测标志
+        station_detected_while_rotating_ = false;
         if (
             current_state_ == WheelchairState::SEARCHING ||
             current_state_ == WheelchairState::ROTATING)
@@ -620,6 +755,9 @@ void WorkflowManager::transitionToWalking()
             stopRotationTimer();
             RCLCPP_INFO(node_->get_logger(), "轮椅开始行走");
             publishState("WHEELCHAIR_WALKING");
+            auto msg = std_msgs::msg::Int32();
+            msg.data = RECHARGE_EVENT_OBSTACLE_DETECTED;
+            pub1_->publish(msg);
         }
     }
     catch (const std::exception &e)
@@ -695,6 +833,10 @@ void WorkflowManager::transitionToReady()
         // 停止所有定时器
         stopSearchTimeoutTimer();
         stopRotationTimer();
+        stopRotationDelayTimer();
+
+        // 重置检测状态
+        station_detected_while_rotating_ = false;
 
         // 如果正在旋转，停止旋转
         if (rotation_manager_ && rotation_manager_->isRotating())
@@ -750,6 +892,10 @@ void WorkflowManager::processIpadCancelEvent()
         // 发布状态更新
         publishState("IPAD_RECHARGE_CANCELLED", "iPad取消自主回充");
 
+        // 清理旋转延迟相关状态
+        station_detected_while_rotating_ = false;
+        stopRotationDelayTimer();
+
         // 根据当前状态决定是否取消回充
         if (current_state_ == WheelchairState::SEARCHING ||
             current_state_ == WheelchairState::WALKING || current_state_ == WheelchairState::ROTATING)
@@ -1118,7 +1264,6 @@ void WorkflowManager::handleBaseStationLostEvent()
         {
             RCLCPP_WARN(node_->get_logger(), "基站丢失，退出回充流程");
             transitionToReady();
-
             // 发布回充失败通知
             publishRechargeStatus("RECHARGE_FAILED", "基站丢失导致回充失败");
         }
@@ -1143,7 +1288,7 @@ void WorkflowManager::handleSearchTimeoutEvent()
         RCLCPP_WARN(node_->get_logger(), "搜索30秒超时，回充失败");
 
         // 发布回充失败通知
-        publishRechargeStatus("RECHARGE_FAILED", "搜索充电站30秒超时");
+        publishRechargeStatus("RECHARGE_FAILED", "搜索充电站秒超时");
 
         // 退出回充流程
         transitionToReady();
@@ -1241,6 +1386,14 @@ void WorkflowManager::emergencyTransitionToReady(const std::string &reason)
             rotation_check_timer_->cancel();
             rotation_check_timer_.reset();
         }
+        if (rotation_delay_timer_) // 新增：停止延迟定时器
+        {
+            rotation_delay_timer_->cancel();
+            rotation_delay_timer_.reset();
+        }
+
+        // 重置检测状态
+        station_detected_while_rotating_ = false;
 
         // 停止旋转
         if (rotation_manager_ && rotation_manager_->isRotating())

src/wheelchair_state_machine/src/第一种调到垂线范围（角度误差在5度）

@@ -0,0 +1,27 @@
+第一种调到垂线范围（角度误差在5度）
+第二种种调到比垂线角度过大（角度误差比5度要大）
+
+
+录雷达
+/scan/back
+/scan/out
+/scan/out_line
+/cmd_vel_raw
+/cmd_vel
+/cmd_vel_nav
+/wheel/twist
+/scene_services/mode
+/sensor/getcharging
+
+
+
+
+
+发布输出：
+主题名	                              消息类型	                          发布者                                                        	描述
+/cmd_vel_raw	             geometry_msgs/msg/Twist	           LidarScanController,RotationManager	                          轮椅运动控制命令
+station/detected	         std_msgs/msg/Bool	                   LidarScanController	                                          充电基站检测状态
+wheelchair/state	             std_msgs/msg/String	           WorkflowManager	                                                轮椅当前状态
+wheelchair/report	              std_msgs/msg/String	            ReportManager	                                                 系统状态报告
+wheelchair/recharge_timeout	      std_msgs/msg/String	             WorkflowManager	                                            回充超时/状态报告
+charging/voltage_detected        std_msgs/msg/String               LidarScanController                                              检测到充电电压