From 3afd2a97ff30ab1f88a3c503b633b8867102da22 Mon Sep 17 00:00:00 2001
From: tanjianbin <632190820@qq.com>
Date: Tue, 14 Apr 2026 11:09:13 +0800
Subject: [PATCH] =?UTF-8?q?feat(=E5=AF=BC=E8=88=AA):=20=E5=A2=9E=E5=8A=A0?=
 =?UTF-8?q?=E6=8E=A5=E5=BE=85=E4=BB=BB=E5=8A=A1=E4=B8=AD=E6=9C=BA=E5=99=A8?=
 =?UTF-8?q?=E4=BA=BA=E6=9C=9D=E5=90=91=E4=BF=9D=E6=8C=81=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

在接待任务中，机器人到达接待位置并检测到人时记录当前朝向作为锚点角度。
当检测状态变为空闲时，自动恢复记录的朝向，避免机器人因微小移动而偏离接待方向。
同时添加角度归一化函数处理角度计算，并确保仅在接待任务相关场景触发此功能。
---
 .../lzwcai_terminal_temi/MainActivity.kt      | 63 +++++++++++++++++++
 1 file changed, 63 insertions(+)

diff --git a/app/src/main/java/com/example/lzwcai_terminal_temi/MainActivity.kt b/app/src/main/java/com/example/lzwcai_terminal_temi/MainActivity.kt
index 86a9f36..0208ac0 100644
--- a/app/src/main/java/com/example/lzwcai_terminal_temi/MainActivity.kt
+++ b/app/src/main/java/com/example/lzwcai_terminal_temi/MainActivity.kt
@@ -97,6 +97,8 @@ class MainActivity : AppCompatActivity(), OnRobotReadyListener, TtsListener, OnG
     private var blinkJob: Job? = null
     private var networkErrorJob: Job? = null
     private var autoRechargeJob: Job? = null
+    private var latestYaw: Float? = null
+    private var receptionAnchorYaw: Float? = null
     private lateinit var telemetryManager: TelemetryManager
     private lateinit var taskController: TaskController
 
@@ -322,6 +324,7 @@ class MainActivity : AppCompatActivity(), OnRobotReadyListener, TtsListener, OnG
     }
 
     override fun onCurrentPositionChanged(position: Position) {
+        latestYaw = position.yaw
         telemetryManager.onCurrentPositionChanged(position)
     }
 
@@ -355,6 +358,14 @@ class MainActivity : AppCompatActivity(), OnRobotReadyListener, TtsListener, OnG
         if (taskController.currentTask == "patrol") {
             taskController.handlePatrolArrival(location)
         }
+        if (taskController.currentTask == "reception" &&
+            location.equals(taskController.getReceptionLocation(), ignoreCase = true)
+        ) {
+            captureReceptionAnchorYawIfNeeded()
+        }
+        if (taskController.currentTask != "reception") {
+            receptionAnchorYaw = null
+        }
         if (taskController.handleNotificationArrival(location)) {
             return
         }
@@ -406,6 +417,13 @@ class MainActivity : AppCompatActivity(), OnRobotReadyListener, TtsListener, OnG
     private fun handleStableDetectionStateChanged(state: Int) {
         Log.i("MainActivity", "Stable detection state: $state")
         liveKitManager?.setDetectionActive(state == DETECTED)
+        if (taskController.currentTask == "reception") {
+            if (state == DETECTED) {
+                captureReceptionAnchorYawIfNeeded()
+            } else if (state == IDLE) {
+                recoverReceptionFacingDirection()
+            }
+        }
         if (taskController.handleDetectionStateChanged(state)) {
             Log.i("MainActivity", "what the f**k")
             return
@@ -490,6 +508,9 @@ class MainActivity : AppCompatActivity(), OnRobotReadyListener, TtsListener, OnG
         if (task.trim().isNotEmpty()) {
             cancelAutoRecharge("task_set:$task")
         }
+        if (!task.equals("reception", ignoreCase = true)) {
+            receptionAnchorYaw = null
+        }
         taskController.setCurrentTask(task)
     }
 
@@ -762,6 +783,48 @@ class MainActivity : AppCompatActivity(), OnRobotReadyListener, TtsListener, OnG
         autoRechargeJob = null
     }
 
+    private fun captureReceptionAnchorYawIfNeeded() {
+        if (receptionAnchorYaw != null) {
+            return
+        }
+        val yaw = latestYaw ?: return
+        receptionAnchorYaw = yaw
+        Log.i("MainActivity", "Reception anchor yaw captured: $yaw")
+    }
+
+    private fun recoverReceptionFacingDirection() {
+        if (taskController.currentTask != "reception") {
+            return
+        }
+        val atReceptionLocation = lastArrivalLocation?.equals(taskController.getReceptionLocation(), ignoreCase = true) == true
+        if (!atReceptionLocation) {
+            return
+        }
+        val anchorYaw = receptionAnchorYaw ?: return
+        val currentYaw = latestYaw ?: return
+        val delta = normalizeAngle(anchorYaw - currentYaw)
+        // Ignore tiny drift to avoid jitter.
+        if (kotlin.math.abs(delta) < 8f) {
+            return
+        }
+        val turn = delta.toInt().coerceIn(-45, 45)
+        if (turn == 0) {
+            return
+        }
+        navCon.turnBy(turn, 0.35f)
+        Log.i("MainActivity", "Reception facing recovered by $turn degrees (delta=$delta).")
+    }
+
+    private fun normalizeAngle(angle: Float): Float {
+        var normalized = angle % 360f
+        if (normalized > 180f) {
+            normalized -= 360f
+        } else if (normalized < -180f) {
+            normalized += 360f
+        }
+        return normalized
+    }
+
     private fun updateConnectionIndicator() {
         val colorRes = when {
             !isLiveKitConnected && !isMqttConnected -> android.R.color.holo_red_dark