778 lines
29 KiB
C#
778 lines
29 KiB
C#
using System;
|
|
using System.Collections;
|
|
using UnityEngine;
|
|
|
|
public class ActionExecutor
|
|
{
|
|
private readonly MonoBehaviour monoBehaviour;
|
|
private readonly AnimationControllerAPM animationController;
|
|
private readonly ActionStateSender actionStateSender;
|
|
private readonly Transform transform;
|
|
private volatile bool isCancelled;
|
|
private readonly float rotationSpeed = 180f; // Tốc độ xoay (độ/giây)
|
|
private readonly float moveSpeed; // Tốc độ di chuyển từ AMR.defaultSpeed
|
|
private readonly float checkPalletRadius; // Bán kính kiểm tra pallet
|
|
private readonly float checkPalletMaxDistance; // Khoảng cách kiểm tra pallet
|
|
private readonly LayerMask checkPalletTargetMask; // LayerMask kiểm tra pallet
|
|
private readonly bool isDebuggingPalletCheck; // Bật/tắt debug vùng kiểm tra pallet
|
|
|
|
public ActionExecutor(
|
|
MonoBehaviour monoBehaviour,
|
|
AnimationControllerAPM animationController,
|
|
ActionStateSender actionStateSender,
|
|
Transform transform,
|
|
float moveSpeed,
|
|
float checkPalletRadius,
|
|
float checkPalletMaxDistance,
|
|
LayerMask checkPalletTargetMask,
|
|
bool isDebuggingPalletCheck)
|
|
{
|
|
this.monoBehaviour = monoBehaviour;
|
|
this.animationController = animationController;
|
|
this.actionStateSender = actionStateSender;
|
|
this.transform = transform;
|
|
this.moveSpeed = moveSpeed;
|
|
this.checkPalletRadius = checkPalletRadius;
|
|
this.checkPalletMaxDistance = checkPalletMaxDistance;
|
|
this.checkPalletTargetMask = checkPalletTargetMask;
|
|
this.isDebuggingPalletCheck = isDebuggingPalletCheck;
|
|
this.isCancelled = false;
|
|
|
|
if (animationController != null)
|
|
{
|
|
animationController.OnAnimationStateChanged += OnAnimationStateChanged;
|
|
}
|
|
else
|
|
{
|
|
Debug.LogError("AnimationControllerAPM is null in ActionExecutor constructor!");
|
|
}
|
|
}
|
|
|
|
public void CancelActions()
|
|
{
|
|
isCancelled = true;
|
|
Debug.Log("Đã hủy thực thi actions.");
|
|
}
|
|
|
|
public IEnumerator ExecuteNodeActions(Node node)
|
|
{
|
|
if (node == null || node.Actions == null || node.Actions.Length == 0)
|
|
{
|
|
yield break;
|
|
}
|
|
|
|
foreach (var action in node.Actions)
|
|
{
|
|
if (isCancelled)
|
|
{
|
|
Debug.Log($"Hủy thực thi actions tại node {node.NodeDescription} do isCancelled=true.");
|
|
yield break;
|
|
}
|
|
|
|
// Gửi trạng thái WAITING
|
|
yield return actionStateSender.SendActionStateAsync(
|
|
action,
|
|
node.NodeId,
|
|
"NODE",
|
|
"WAITING",
|
|
"Action is waiting to start");
|
|
|
|
// Thực thi hành động
|
|
ExecutionResult execResult = null;
|
|
yield return monoBehaviour.StartCoroutine(ExecuteAction(action, node, result => execResult = result));
|
|
|
|
bool isActionSuccessful = execResult != null && execResult.IsActionSuccessful;
|
|
string actionStatus = execResult != null ? execResult.ActionStatus : "FAILED";
|
|
string resultDescription = execResult != null ? execResult.ResultDescription : "Unknown error";
|
|
|
|
// Gửi trạng thái cuối cùng
|
|
yield return actionStateSender.SendActionStateAsync(
|
|
action,
|
|
node.NodeId,
|
|
"NODE",
|
|
actionStatus,
|
|
resultDescription);
|
|
|
|
// Nếu hành động HARD thất bại, dừng lại
|
|
if (action.BlockingType == "HARD" && actionStatus != "FINISHED")
|
|
{
|
|
Debug.LogError($"Action {action.ActionId} tại node {node.NodeDescription} có blockingType HARD nhưng không hoàn tất (status: {actionStatus}). Bỏ qua các hành động tiếp theo trong node này.");
|
|
if (action.ActionType == "checkPallet")
|
|
{
|
|
isCancelled = true;
|
|
yield return actionStateSender.SendCancelOrder(node, action);
|
|
yield break;
|
|
}
|
|
yield break;
|
|
}
|
|
|
|
// Đợi hành động HARD hoàn tất (pick, drop, rotation, moveBackward, startInPallet)
|
|
if (isActionSuccessful && action.BlockingType == "HARD")
|
|
{
|
|
if (action.ActionType == "rotation")
|
|
{
|
|
float rotationAngle = GetRotationAngle(action);
|
|
if (rotationAngle != 0f)
|
|
{
|
|
yield return monoBehaviour.StartCoroutine(RotateRobot(rotationAngle));
|
|
}
|
|
else
|
|
{
|
|
isActionSuccessful = false;
|
|
actionStatus = "FAILED";
|
|
resultDescription = "Invalid rotation angle";
|
|
}
|
|
}
|
|
else if (action.ActionType == "moveBackward")
|
|
{
|
|
float distance = GetBackwardDistance(action);
|
|
if (distance > 0f)
|
|
{
|
|
yield return monoBehaviour.StartCoroutine(MoveBackward(distance));
|
|
}
|
|
else
|
|
{
|
|
isActionSuccessful = false;
|
|
actionStatus = "FAILED";
|
|
resultDescription = "Invalid or non-positive backward distance";
|
|
}
|
|
}
|
|
else if (action.ActionType == "startInPallet")
|
|
{
|
|
float x = 0f, y = 0f, theta = 0f, speed = moveSpeed;
|
|
bool hasValidParams = ParseStartInPalletParams(action, out x, out y, out theta, out speed);
|
|
if (hasValidParams)
|
|
{
|
|
yield return monoBehaviour.StartCoroutine(MoveToPalletPosition(x, y, theta, speed));
|
|
}
|
|
else
|
|
{
|
|
isActionSuccessful = false;
|
|
actionStatus = "FAILED";
|
|
resultDescription = "Invalid or missing parameters for startInPallet";
|
|
}
|
|
}
|
|
|
|
// Gửi lại trạng thái nếu có lỗi
|
|
if (!isActionSuccessful)
|
|
{
|
|
yield return actionStateSender.SendActionStateAsync(
|
|
action,
|
|
node.NodeId,
|
|
"NODE",
|
|
actionStatus,
|
|
resultDescription);
|
|
}
|
|
}
|
|
|
|
// Kiểm tra trạng thái sau khi thực thi
|
|
if (action.BlockingType == "HARD" && actionStatus != "FINISHED")
|
|
{
|
|
Debug.LogError($"Action {action.ActionType} (ActionId: {action.ActionId}) failed with status {actionStatus}. Stopping further actions in node {node.NodeDescription}.");
|
|
yield break;
|
|
}
|
|
else
|
|
{
|
|
Debug.Log($"Action {action.ActionType} (ActionId: {action.ActionId}) completed with status {actionStatus}. Proceeding to next action.");
|
|
}
|
|
}
|
|
}
|
|
|
|
// Helper class for execution result
|
|
private class ExecutionResult
|
|
{
|
|
public bool IsActionSuccessful { get; set; }
|
|
public string ActionStatus { get; set; }
|
|
public string ResultDescription { get; set; }
|
|
}
|
|
|
|
public IEnumerator ExecuteEdgeAction(Edge edge, ActionData action)
|
|
{
|
|
if (isCancelled)
|
|
{
|
|
Debug.Log($"Hủy thực thi action {action.ActionType} tại edge {edge.EdgeDescription} do isCancelled=true.");
|
|
yield break;
|
|
}
|
|
|
|
// Gửi trạng thái WAITING
|
|
yield return actionStateSender.SendActionStateAsync(
|
|
action,
|
|
edge.EdgeId,
|
|
"EDGE",
|
|
"WAITING",
|
|
"Action is waiting to start");
|
|
|
|
// Gửi trạng thái RUNNING
|
|
yield return actionStateSender.SendActionStateAsync(
|
|
action,
|
|
edge.EdgeId,
|
|
"EDGE",
|
|
"RUNNING",
|
|
"Action is running");
|
|
|
|
string actionStatus = "FINISHED";
|
|
string resultDescription = "Action executed successfully";
|
|
bool needYield = false;
|
|
float x = 0f, y = 0f, theta = 0f, speed = moveSpeed;
|
|
|
|
// Xử lý hành động trong một khối try-catch riêng
|
|
try
|
|
{
|
|
switch (action.ActionType)
|
|
{
|
|
case "backwardNavigation":
|
|
break;
|
|
|
|
case "pick":
|
|
case "drop":
|
|
if (animationController != null)
|
|
{
|
|
if (action.ActionType == "pick")
|
|
animationController.OnKey2(); // Gắn pallet vào fork trong OnKey2
|
|
else
|
|
animationController.OnKey1(); // Tách pallet khỏi fork trong OnKey1
|
|
}
|
|
else
|
|
{
|
|
actionStatus = "FAILED";
|
|
resultDescription = "AnimationControllerAPM not found";
|
|
}
|
|
break;
|
|
|
|
case "startInPallet":
|
|
bool hasValidParams = ParseStartInPalletParams(action, out x, out y, out theta, out speed);
|
|
if (hasValidParams)
|
|
{
|
|
needYield = true; // Đánh dấu cần yield coroutine
|
|
}
|
|
else
|
|
{
|
|
actionStatus = "FAILED";
|
|
resultDescription = "Invalid or missing parameters for startInPallet";
|
|
}
|
|
break;
|
|
|
|
default:
|
|
actionStatus = "FAILED";
|
|
resultDescription = $"Unsupported ActionType: {action.ActionType}";
|
|
break;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
actionStatus = "FAILED";
|
|
resultDescription = $"Action failed: {ex.Message}";
|
|
}
|
|
|
|
// Xử lý yield ngoài khối try-catch
|
|
if (needYield && actionStatus == "FINISHED")
|
|
{
|
|
yield return monoBehaviour.StartCoroutine(MoveToPalletPosition(x, y, theta, speed));
|
|
}
|
|
|
|
// Gửi trạng thái cuối cùng
|
|
yield return actionStateSender.SendActionStateAsync(
|
|
action,
|
|
edge.EdgeId,
|
|
"EDGE",
|
|
actionStatus,
|
|
resultDescription);
|
|
|
|
// Nếu hành động HARD thất bại, hủy di chuyển
|
|
if (action.BlockingType == "HARD" && actionStatus != "FINISHED")
|
|
{
|
|
isCancelled = true;
|
|
}
|
|
}
|
|
private IEnumerator WaitForAnimation(string actionType, float timeout, Action<bool> onComplete)
|
|
{
|
|
bool animationCompleted = false;
|
|
float elapsed = 0f;
|
|
|
|
void OnAnimationState(string type, string state)
|
|
{
|
|
if (type == actionType && state == "FINISHED")
|
|
{
|
|
animationCompleted = true;
|
|
Debug.Log($"Animation {actionType} hoàn tất qua callback.");
|
|
}
|
|
}
|
|
|
|
animationController.OnAnimationStateChanged += OnAnimationState;
|
|
|
|
// Fallback: Kiểm tra trạng thái Animator nếu Animation Events không hoạt động
|
|
Animator animator = animationController?.GetComponent<Animator>();
|
|
string expectedState = actionType == "pick" ? "Up" : "Down";
|
|
|
|
while (!animationCompleted && elapsed < timeout && !isCancelled)
|
|
{
|
|
if (animator != null)
|
|
{
|
|
AnimatorStateInfo stateInfo = animator.GetCurrentAnimatorStateInfo(0);
|
|
if (stateInfo.IsName(expectedState) && stateInfo.normalizedTime >= 1f)
|
|
{
|
|
animationCompleted = true;
|
|
Debug.Log($"Animation {actionType} hoàn tất qua polling.");
|
|
}
|
|
}
|
|
elapsed += Time.deltaTime;
|
|
yield return null;
|
|
}
|
|
|
|
animationController.OnAnimationStateChanged -= OnAnimationState;
|
|
|
|
bool success = animationCompleted && !isCancelled;
|
|
onComplete?.Invoke(success);
|
|
|
|
if (!success)
|
|
{
|
|
Debug.LogWarning($"Animation {actionType} không hoàn tất: completed={animationCompleted}, cancelled={isCancelled}, timeout={timeout}s");
|
|
}
|
|
}
|
|
private IEnumerator ExecuteAction(ActionData action, Node node, Action<ExecutionResult> callback)
|
|
{
|
|
var result = new ExecutionResult
|
|
{
|
|
IsActionSuccessful = true,
|
|
ActionStatus = "FINISHED",
|
|
ResultDescription = "Action executed successfully"
|
|
};
|
|
|
|
bool needToYieldCancelOrder = false;
|
|
bool needToYieldMovement = false;
|
|
bool needToYieldAnimation = false;
|
|
float x = 0f, y = 0f, theta = 0f, speed = moveSpeed;
|
|
|
|
try
|
|
{
|
|
switch (action.ActionType)
|
|
{
|
|
case "pick":
|
|
case "drop":
|
|
if (animationController != null)
|
|
{
|
|
if (action.ActionType == "pick")
|
|
animationController.OnKey2(); // Gắn pallet vào fork
|
|
else
|
|
animationController.OnKey1(); // Tách pallet khỏi fork
|
|
needToYieldAnimation = true; // Đánh dấu cần đợi animation
|
|
}
|
|
else
|
|
{
|
|
result.IsActionSuccessful = false;
|
|
result.ActionStatus = "FAILED";
|
|
result.ResultDescription = "AnimationControllerAPM not found";
|
|
}
|
|
break;
|
|
|
|
case "rotation":
|
|
float rotationAngle = GetRotationAngle(action);
|
|
if (rotationAngle != 0f)
|
|
{
|
|
if (action.BlockingType != "HARD")
|
|
{
|
|
monoBehaviour.StartCoroutine(RotateRobot(rotationAngle));
|
|
}
|
|
}
|
|
else
|
|
{
|
|
result.IsActionSuccessful = false;
|
|
result.ActionStatus = "FAILED";
|
|
result.ResultDescription = "Invalid or missing rotation angle in parameters";
|
|
}
|
|
break;
|
|
|
|
case "moveBackward":
|
|
float distance = GetBackwardDistance(action);
|
|
if (distance > 0f)
|
|
{
|
|
if (action.BlockingType != "HARD")
|
|
{
|
|
monoBehaviour.StartCoroutine(MoveBackward(distance));
|
|
}
|
|
}
|
|
else
|
|
{
|
|
result.IsActionSuccessful = false;
|
|
result.ActionStatus = "FAILED";
|
|
result.ResultDescription = "Invalid or non-positive backward distance in parameters";
|
|
}
|
|
break;
|
|
|
|
case "checkPallet":
|
|
bool palletDetected = CheckPallet(action);
|
|
if (palletDetected)
|
|
{
|
|
result.ActionStatus = "FINISHED";
|
|
result.ResultDescription = "Pallet detected successfully";
|
|
}
|
|
else
|
|
{
|
|
result.IsActionSuccessful = false;
|
|
result.ActionStatus = "FAILED";
|
|
result.ResultDescription = "Không có pallet";
|
|
needToYieldCancelOrder = true;
|
|
}
|
|
break;
|
|
|
|
case "startInPallet":
|
|
bool hasValidParams = ParseStartInPalletParams(action, out x, out y, out theta, out speed);
|
|
if (hasValidParams)
|
|
{
|
|
needToYieldMovement = true; // Đánh dấu cần di chuyển
|
|
}
|
|
else
|
|
{
|
|
result.IsActionSuccessful = false;
|
|
result.ActionStatus = "FAILED";
|
|
result.ResultDescription = "Invalid or missing parameters for startInPallet";
|
|
}
|
|
break;
|
|
|
|
default:
|
|
result.IsActionSuccessful = false;
|
|
result.ActionStatus = "FAILED";
|
|
result.ResultDescription = $"Unsupported ActionType: {action.ActionType}";
|
|
break;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
result.IsActionSuccessful = false;
|
|
result.ActionStatus = "FAILED";
|
|
result.ResultDescription = $"Action failed: {ex.Message}";
|
|
Debug.LogError($"Lỗi khi thực thi action {action.ActionType} tại node {node.NodeDescription}: {ex.Message}");
|
|
}
|
|
|
|
// Xử lý các yield ngoài khối try-catch
|
|
if (needToYieldAnimation && result.ActionStatus == "FINISHED")
|
|
{
|
|
float animationDuration = animationController.GetAnimationDuration(action.ActionType);
|
|
if (animationDuration <= 0f)
|
|
{
|
|
result.IsActionSuccessful = false;
|
|
result.ActionStatus = "FAILED";
|
|
result.ResultDescription = "Invalid animation duration";
|
|
Debug.LogError($"Invalid animation duration for {action.ActionType}. Check AnimationControllerAPM clip names.");
|
|
}
|
|
else
|
|
{
|
|
bool animationSuccess = false;
|
|
yield return monoBehaviour.StartCoroutine(WaitForAnimation(action.ActionType, animationDuration + 1f, success => animationSuccess = success));
|
|
if (!animationSuccess)
|
|
{
|
|
result.IsActionSuccessful = false;
|
|
result.ActionStatus = "FAILED";
|
|
result.ResultDescription = "Animation did not complete within timeout or was cancelled";
|
|
}
|
|
}
|
|
}
|
|
|
|
if (needToYieldMovement && result.ActionStatus == "FINISHED")
|
|
{
|
|
yield return monoBehaviour.StartCoroutine(MoveToPalletPosition(x, y, theta, speed));
|
|
}
|
|
|
|
// Gọi callback với kết quả
|
|
callback?.Invoke(result);
|
|
|
|
// Xử lý hủy order nếu cần
|
|
if (needToYieldCancelOrder)
|
|
{
|
|
yield return actionStateSender.SendCancelOrder(node, action);
|
|
}
|
|
}
|
|
|
|
private bool ParseStartInPalletParams(ActionData action, out float x, out float y, out float theta, out float speed)
|
|
{
|
|
x = 0f;
|
|
y = 0f;
|
|
theta = 0f;
|
|
speed = moveSpeed; // Tốc độ mặc định nếu không được cung cấp
|
|
|
|
if (action.Parameters == null || action.Parameters.Length == 0)
|
|
{
|
|
Debug.LogWarning($"Action parameters are null or empty for startInPallet action (ActionId: {action.ActionId}).");
|
|
return false;
|
|
}
|
|
|
|
bool hasX = false, hasY = false, hasTheta = false;
|
|
|
|
foreach (var param in action.Parameters)
|
|
{
|
|
if (param.Key == "X" && float.TryParse(param.Value.ToString(), out float parsedX))
|
|
{
|
|
x = parsedX;
|
|
hasX = true;
|
|
}
|
|
else if (param.Key == "Y" && float.TryParse(param.Value.ToString(), out float parsedY))
|
|
{
|
|
y = parsedY;
|
|
hasY = true;
|
|
}
|
|
else if (param.Key == "Theta" && float.TryParse(param.Value.ToString(), out float parsedTheta))
|
|
{
|
|
theta = parsedTheta;
|
|
hasTheta = true;
|
|
}
|
|
else if (param.Key == "speed" && float.TryParse(param.Value.ToString(), out float parsedSpeed) && parsedSpeed > 0f)
|
|
{
|
|
speed = parsedSpeed;
|
|
Debug.Log($"Sử dụng tốc độ từ actionParameters: {speed} m/s");
|
|
}
|
|
}
|
|
|
|
if (!hasX || !hasY || !hasTheta)
|
|
{
|
|
Debug.LogWarning($"Missing parameters for startInPallet action (ActionId: {action.ActionId}). Required: X, Y, Theta. Found: X={hasX}, Y={hasY}, Theta={hasTheta}");
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
private IEnumerator MoveToPalletPosition(float x, float y, float theta, float speed)
|
|
{
|
|
if (isCancelled)
|
|
{
|
|
Debug.Log("Hủy di chuyển đến vị trí pallet do isCancelled=true.");
|
|
yield break;
|
|
}
|
|
|
|
// Kiểm tra pallet trước khi di chuyển
|
|
if (!CheckPallet(null)) // Gọi CheckPallet với action null để sử dụng tham số mặc định
|
|
{
|
|
Debug.LogWarning("Không phát hiện pallet trước khi thực hiện startInPallet, nhưng vẫn tiếp tục di chuyển.");
|
|
}
|
|
|
|
Vector3 targetPosition = new Vector3(x, transform.position.y, y);
|
|
float distance = Vector3.Distance(transform.position, targetPosition);
|
|
|
|
if (distance <= 0.01f)
|
|
{
|
|
Debug.LogWarning("Distance to pallet position is too small, skipping movement.");
|
|
yield break;
|
|
}
|
|
|
|
// Di chuyển đến vị trí (x, y)
|
|
Vector3 startPosition = transform.position;
|
|
float elapsed = 0f;
|
|
float duration = distance / speed;
|
|
|
|
while (elapsed < duration && !isCancelled)
|
|
{
|
|
float t = elapsed / duration;
|
|
transform.position = Vector3.Lerp(startPosition, targetPosition, t);
|
|
elapsed += Time.deltaTime;
|
|
yield return null;
|
|
}
|
|
|
|
if (!isCancelled)
|
|
{
|
|
transform.position = targetPosition;
|
|
Debug.Log($"Hoàn thành di chuyển đến vị trí pallet ({x}, {y}) với tốc độ {speed} m/s.");
|
|
}
|
|
else
|
|
{
|
|
Debug.Log("Hủy di chuyển đến vị trí pallet do isCancelled=true.");
|
|
yield break;
|
|
}
|
|
|
|
// Không xoay, giữ nguyên góc hiện tại
|
|
Debug.Log($"Không xoay sau khi di chuyển đến vị trí pallet: giữ góc hiện tại, Unity eulerAngles.y={transform.eulerAngles.y:F2}°");
|
|
}
|
|
|
|
private bool CheckPallet(ActionData action)
|
|
{
|
|
float radius = checkPalletRadius;
|
|
float maxDistance = checkPalletMaxDistance;
|
|
LayerMask targetMask = checkPalletTargetMask;
|
|
|
|
if (action?.Parameters != null)
|
|
{
|
|
foreach (var param in action.Parameters)
|
|
{
|
|
if (param.Key == "radius" && float.TryParse(param.Value.ToString(), out float parsedRadius))
|
|
{
|
|
radius = parsedRadius;
|
|
Debug.Log($"Sử dụng radius từ actionParameters: {radius}");
|
|
}
|
|
else if (param.Key == "distance" && float.TryParse(param.Value.ToString(), out float parsedDistance))
|
|
{
|
|
maxDistance = parsedDistance;
|
|
Debug.Log($"Sử dụng maxDistance từ actionParameters: {maxDistance}");
|
|
}
|
|
}
|
|
}
|
|
|
|
Vector3 origin = transform.position;
|
|
Vector3 direction = transform.right; // Thay đổi từ transform.forward sang transform.right để kiểm tra theo trục X
|
|
Vector3 endPoint = origin + direction * maxDistance;
|
|
|
|
// Hiển thị vùng kiểm tra pallet khi bật debug
|
|
if (isDebuggingPalletCheck)
|
|
{
|
|
// Vẽ đường từ vị trí robot đến điểm kiểm tra (sử dụng Debug.DrawRay để tương thích với runtime)
|
|
Debug.DrawRay(origin, direction * maxDistance, Color.green, 0.1f);
|
|
}
|
|
|
|
Collider[] hits = Physics.OverlapSphere(endPoint, radius, targetMask);
|
|
foreach (var hit in hits)
|
|
{
|
|
if (hit.CompareTag("Pallet") || hit.name.Contains("Pallet"))
|
|
{
|
|
Debug.Log($"Phát hiện pallet tại node: {hit.name} (vị trí: {endPoint})");
|
|
|
|
if (animationController != null)
|
|
{
|
|
animationController.pallet = hit.transform;
|
|
Debug.Log($"Đã gán pallet {hit.name} vào AnimationControllerAPM.pallet.");
|
|
}
|
|
else
|
|
{
|
|
Debug.LogWarning("AnimationControllerAPM là null, không thể gán pallet.");
|
|
}
|
|
|
|
return true; // Phát hiện pallet
|
|
}
|
|
}
|
|
|
|
Debug.Log($"Không phát hiện pallet tại điểm kiểm tra (vị trí: {endPoint})");
|
|
|
|
if (animationController != null)
|
|
{
|
|
animationController.pallet = null;
|
|
Debug.Log("Không tìm thấy pallet, đặt AnimationControllerAPM.pallet thành null.");
|
|
}
|
|
|
|
return false; // Không phát hiện pallet
|
|
}
|
|
private float GetRotationAngle(ActionData action)
|
|
{
|
|
if (action.Parameters == null || action.Parameters.Length == 0)
|
|
{
|
|
Debug.LogWarning($"Action parameters are null or empty for rotation action (ActionId: {action.ActionId}).");
|
|
return 0f;
|
|
}
|
|
|
|
foreach (var param in action.Parameters)
|
|
{
|
|
if (param.Key == "ro")
|
|
{
|
|
Debug.Log($"Found rotation parameter 'ro' with value: {param.Value} for ActionId: {action.ActionId}");
|
|
if (float.TryParse(param.Value.ToString(), out float angle))
|
|
{
|
|
return angle;
|
|
}
|
|
else
|
|
{
|
|
Debug.LogWarning($"Invalid rotation parameter value: {param.Value} for ActionId: {action.ActionId}");
|
|
return 0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
Debug.LogWarning($"Rotation parameter 'ro' not found in action parameters for ActionId: {action.ActionId}.");
|
|
return 0f;
|
|
}
|
|
|
|
private float GetBackwardDistance(ActionData action)
|
|
{
|
|
if (action.Parameters == null || action.Parameters.Length == 0)
|
|
{
|
|
Debug.LogWarning($"Action parameters are null or empty for moveBackward action (ActionId: {action.ActionId}).");
|
|
return 0f;
|
|
}
|
|
|
|
foreach (var param in action.Parameters)
|
|
{
|
|
if (param.Key == "distance")
|
|
{
|
|
if (float.TryParse(param.Value.ToString(), out float distance) && distance > 0f)
|
|
{
|
|
Debug.Log($"Found backward distance parameter 'distance' with value: {distance} for ActionId: {action.ActionId}");
|
|
return distance;
|
|
}
|
|
else
|
|
{
|
|
Debug.LogWarning($"Invalid backward distance value: {param.Value} (must be positive) for ActionId: {action.ActionId}");
|
|
return 0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
Debug.LogWarning($"Backward distance parameter 'distance' not found in action parameters for ActionId: {action.ActionId}.");
|
|
return 0f;
|
|
}
|
|
|
|
private IEnumerator RotateRobot(float angleRad)
|
|
{
|
|
if (Mathf.Approximately(angleRad, 0f))
|
|
{
|
|
Debug.LogWarning("Rotation angle is zero, skipping rotation.");
|
|
yield break;
|
|
}
|
|
|
|
float angleDeg = angleRad * Mathf.Rad2Deg;
|
|
Quaternion startRotation = transform.rotation;
|
|
Quaternion targetRotation = startRotation * Quaternion.Euler(0f, angleDeg, 0f);
|
|
float elapsed = 0f;
|
|
float duration = Mathf.Abs(angleDeg) / rotationSpeed;
|
|
|
|
while (elapsed < duration && !isCancelled)
|
|
{
|
|
float t = elapsed / duration;
|
|
transform.rotation = Quaternion.Slerp(startRotation, targetRotation, t);
|
|
elapsed += Time.deltaTime;
|
|
yield return null;
|
|
}
|
|
|
|
if (!isCancelled)
|
|
{
|
|
transform.rotation = targetRotation;
|
|
Debug.Log($"Hoàn thành xoay {angleRad} rad ({angleDeg} độ).");
|
|
}
|
|
else
|
|
{
|
|
Debug.Log("Hủy xoay do isCancelled=true.");
|
|
}
|
|
}
|
|
|
|
private IEnumerator MoveBackward(float distance)
|
|
{
|
|
if (distance <= 0f)
|
|
{
|
|
Debug.LogWarning("Backward distance is zero or negative, skipping moveBackward.");
|
|
yield break;
|
|
}
|
|
|
|
Vector3 startPosition = transform.position;
|
|
Vector3 direction = -transform.forward; // Hướng ngược lại
|
|
Vector3 targetPosition = startPosition + direction * distance;
|
|
float elapsed = 0f;
|
|
float duration = distance / moveSpeed;
|
|
|
|
while (elapsed < duration && !isCancelled)
|
|
{
|
|
float t = elapsed / duration;
|
|
transform.position = Vector3.Lerp(startPosition, targetPosition, t);
|
|
elapsed += Time.deltaTime;
|
|
yield return null;
|
|
}
|
|
|
|
if (!isCancelled)
|
|
{
|
|
transform.position = targetPosition;
|
|
Debug.Log($"Hoàn thành đi lùi {distance} mét.");
|
|
}
|
|
else
|
|
{
|
|
Debug.Log("Hủy đi lùi do isCancelled=true.");
|
|
}
|
|
}
|
|
|
|
private void OnAnimationStateChanged(string actionType, string state)
|
|
{
|
|
Debug.Log($"Animation state changed: {actionType} -> {state}");
|
|
}
|
|
} |