sonlt/APM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
lethanhsonvsp
2025-11-17 15:02:30 +07:00
commit 0a84b9d75e
15481 changed files with 2009655 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
Assets/Scripting/ForkSensor.cs Normal file
View File

@@ -0,0 +1,56 @@
using UnityEngine;
public class ForkSensor : MonoBehaviour
{
public Transform forkLift; // Vị trí animation nâng/hạ
public Animator animator; // Bộ điều khiển animation
public FixedJoint joint; // Joint dùng để gắn pallet
public Rigidbody currentPallet; // Pallet đang tương tác
void Update()
{
if (Input.GetKeyDown(KeyCode.U)) // Up: nâng
{
animator.SetTrigger("Up");
AttachPallet();
}
if (Input.GetKeyDown(KeyCode.D)) // Down: hạ
{
animator.SetTrigger("Down");
DetachPallet();
}
}
void AttachPallet()
{
if (currentPallet == null) return;
// Tạo Joint nếu chưa có
if (joint == null)
{
joint = gameObject.AddComponent<FixedJoint>();
joint.connectedBody = currentPallet;
}
}
void DetachPallet()
{
if (joint != null)
{
Destroy(joint); // Tháo pallet
joint = null;
}
currentPallet = null;
}
// Tùy chọn: phát hiện pallet khi vào vùng sensor
private void OnTriggerEnter(Collider other)
{
if (other.CompareTag("Pallet"))
{
currentPallet = other.attachedRigidbody;
}
}
}

2
Assets/Scripting/ForkSensor.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: c8f579fdb7328274dbd9317be46dba22

8
Assets/Scripting/Khac.meta Normal file
View File

@@ -0,0 +1,8 @@
fileFormatVersion: 2
guid: c9bc5eb8dbf2f4c498bd01769889e03d
folderAsset: yes
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

140
Assets/Scripting/Khac/AnimationControllerAPM.cs Normal file
View File

@@ -0,0 +1,140 @@
using UnityEngine;
using UnityEngine.InputSystem;
using System;
public class AnimationControllerAPM : MonoBehaviour
{
private Animator animator;
private AnimationClip animationUClip;
private AnimationClip animationDClip;
// Tham chiếu đến pallet và fork
public Transform pallet; // Gán GameObject của pallet trong Inspector
public Transform fork; // Gán GameObject của fork trong Inspector
public float AnimationUDuration => animationUClip != null ? animationUClip.length : 0f;
public float AnimationDDuration => animationDClip != null ? animationDClip.length : 0f;
public Action<string, string> OnAnimationStateChanged { get; set; } // ActionType, State (RUNNING/FINISHED)
private string currentActionType; // Theo dõi ActionType hiện tại (pick/drop)
private void Awake()
{
animator = GetComponent<Animator>();
animator.SetBool("U", false);
animator.SetBool("D", false);
InitializeAnimationClips();
}
private void InitializeAnimationClips()
{
if (animator == null || animator.runtimeAnimatorController == null)
{
Debug.LogError("Animator hoặc AnimatorController không được gán!");
return;
}
var controller = animator.runtimeAnimatorController;
var clips = controller.animationClips;
foreach (var clip in clips)
{
if (clip.name.Contains("Up")) // Thay bằng tên thực tế của clip U
{
animationUClip = clip;
}
else if (clip.name.Contains("Down")) // Thay bằng tên thực tế của clip D
{
animationDClip = clip;
}
}
if (animationUClip == null || animationDClip == null)
{
Debug.LogWarning("Không tìm thấy animation clip cho U hoặc D. Kiểm tra tên clip hoặc AnimatorController.");
}
}
// Kích hoạt animation "Down"
public void OnKey1()
{
DetachPallet(); // Tách pallet khỏi fork
animator.SetBool("U", false);
animator.SetBool("D", true);
currentActionType = "drop";
}
// Kích hoạt animation "Up"
public void OnKey2()
{
AttachPalletToFork(); // Gắn pallet vào fork
animator.SetBool("U", true);
animator.SetBool("D", false);
currentActionType = "pick";
}
// Phương thức gắn pallet vào fork
public void AttachPalletToFork()
{
if (pallet != null && fork != null)
{
pallet.SetParent(fork);
Debug.Log("Pallet đã được gắn vào fork.");
}
else
{
Debug.LogWarning("Pallet hoặc fork không được gán trong Inspector.");
}
}
// Phương thức tách pallet khỏi fork
public void DetachPallet()
{
if (pallet != null)
{
pallet.SetParent(null);
Debug.Log("Pallet đã được tách khỏi fork.");
}
else
{
Debug.LogWarning("Pallet không được gán trong Inspector.");
}
}
public float GetAnimationDuration(string actionType)
{
switch (actionType)
{
case "pick":
return AnimationUDuration;
case "drop":
return AnimationDDuration;
default:
Debug.LogWarning($"ActionType {actionType} không được hỗ trợ.");
return 0f;
}
}
// Gọi bởi Animation Event khi animation bắt đầu
public void OnAnimationStart()
{
if (!string.IsNullOrEmpty(currentActionType))
{
Debug.Log($"Animation bắt đầu: {currentActionType}");
OnAnimationStateChanged?.Invoke(currentActionType, "RUNNING");
}
}
// Gọi bởi Animation Event khi animation kết thúc
public void OnAnimationEnd()
{
if (!string.IsNullOrEmpty(currentActionType))
{
Debug.Log($"Animation kết thúc: {currentActionType}");
OnAnimationStateChanged?.Invoke(currentActionType, "FINISHED");
currentActionType = null; // Reset sau khi hoàn tất
}
}
}

2
Assets/Scripting/Khac/AnimationControllerAPM.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: 17549d08c68a63646b3568dd46f952a2

54
Assets/Scripting/Khac/CheckAnimation.cs Normal file
View File

@@ -0,0 +1,54 @@
using UnityEngine;
public class CheckAnimation : MonoBehaviour
{
// Hàm kiểm tra xem GameObject có animation hay không
public bool HasAnimation(GameObject targetObject)
{
// Kiểm tra Animator Component (Mecanim)
Animator animator = targetObject.GetComponent<Animator>();
if (animator != null)
{
// Kiểm tra xem Animator có AnimatorController được gắn không
if (animator.runtimeAnimatorController != null)
{
Debug.Log($"{targetObject.name} có Animator với AnimatorController.");
return true;
}
else
{
Debug.Log($"{targetObject.name} có Animator nhưng không có AnimatorController.");
return false;
}
}
// Kiểm tra Animation Component (Legacy)
Animation animation = targetObject.GetComponent<Animation>();
if (animation != null)
{
// Kiểm tra xem Animation có AnimationClip được gắn không
if (animation.clip != null || animation.GetClipCount() > 0)
{
Debug.Log($"{targetObject.name} có Animation với AnimationClip.");
return true;
}
else
{
Debug.Log($"{targetObject.name} có Animation nhưng không có AnimationClip.");
return false;
}
}
// Nếu không có Animator hoặc Animation
Debug.Log($"{targetObject.name} không có Animator hoặc Animation.");
return false;
}
// Ví dụ sử dụng
void Start()
{
// Kiểm tra chính GameObject mà script này được gắn vào
bool hasAnim = HasAnimation(gameObject);
Debug.Log($"Kết quả kiểm tra: {hasAnim}");
}
}

2
Assets/Scripting/Khac/CheckAnimation.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: c236d6a427a698349af4be427f1bc259

39
Assets/Scripting/Khac/DisplayPosition.cs Normal file
View File

@@ -0,0 +1,39 @@
using UnityEngine;
using TMPro; // Sử dụng TextMeshPro
public class DisplayPosition : MonoBehaviour
{
// Gán TextMeshProUGUI trong Inspector
public TextMeshProUGUI positionText;
void Update()
{
// Lấy vị trí của đối tượng
Vector3 position = transform.position;
float displayEulerY = NormalizeEulerAngle(-transform.eulerAngles.y); // Giữ nguyên hiển thị
float theta = NormalizeAngle(2 * Mathf.PI - transform.eulerAngles.y * Mathf.Deg2Rad); // Công thức mới
// Hiển thị tọa độ x và z
string positionInfo = $"X: {position.x:F2} \n" +
$"Y: {position.z:F2} \n" +
$"Theta: {displayEulerY:F2}" ;
// Cập nhật TextMeshProUGUI
if (positionText != null)
{
positionText.text = positionInfo;
}
}
public float NormalizeEulerAngle(float angleDeg)
{
while (angleDeg > 180f) angleDeg -= 360f;
while (angleDeg < -180f) angleDeg += 360f;
return angleDeg;
}
public float NormalizeAngle(float angleRad)
{
while (angleRad > Mathf.PI) angleRad -= 2 * Mathf.PI;
while (angleRad < -Mathf.PI) angleRad += 2 * Mathf.PI;
return angleRad;
}
}

2
Assets/Scripting/Khac/DisplayPosition.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: 73fb5f1bf8e3fd341baf281a45510492

34
Assets/Scripting/Khac/SceneSetup.cs Normal file
View File

@@ -0,0 +1,34 @@
using UnityEngine;
public class SceneSetup : MonoBehaviour
{
[SerializeField] private Transform groundPlanePrefab; // Transform của Prefab Plane
[SerializeField] private Transform amrPrefab; // Transform của Prefab AMR
void Start()
{
// Khởi tạo Plane
if (groundPlanePrefab != null)
{
GameObject plane = Instantiate(groundPlanePrefab.gameObject, groundPlanePrefab.position, groundPlanePrefab.rotation);
plane.name = "GroundPlane";
Debug.Log($"Đã khởi tạo GroundPlane Prefab tại position: {groundPlanePrefab.position}, rotation: {groundPlanePrefab.rotation.eulerAngles}");
}
else
{
Debug.LogError("GroundPlane Prefab (Transform) chưa được gán trong Inspector!");
}
// Khởi tạo AMR
if (amrPrefab != null)
{
GameObject amr = Instantiate(amrPrefab.gameObject, amrPrefab.position, amrPrefab.rotation);
amr.name = "AMR";
Debug.Log($"Đã khởi tạo AMR Prefab tại position: {amrPrefab.position}, rotation: {amrPrefab.rotation.eulerAngles}");
}
else
{
Debug.LogError("AMR Prefab (Transform) chưa được gán trong Inspector!");
}
}
}

2
Assets/Scripting/Khac/SceneSetup.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: c93a95dd176e6a644b2443303a886c89

8
Assets/Scripting/MQTT.meta Normal file
View File

@@ -0,0 +1,8 @@
fileFormatVersion: 2
guid: 1d25d7d8f591d604b8e47472d06de54b
folderAsset: yes
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

82
Assets/Scripting/MQTT/MQTTBroker.cs Normal file
View File

@@ -0,0 +1,82 @@
using System;
using System.Threading.Tasks;
using UnityEngine;
using MQTTnet;
using MQTTnet.Server;
using MQTTnet.Protocol;
public class MQTTBroker : MonoBehaviour
{
public int port = 1883;
public string username = "";
public string password = "";
public bool allowAnonymousConnections = true;
private MqttServer mqttServer;
private async void Start()
{
var optionsBuilder = new MqttServerOptionsBuilder()
.WithDefaultEndpoint()
.WithDefaultEndpointPort(port);
mqttServer = new MqttFactory().CreateMqttServer(optionsBuilder.Build());
mqttServer.ValidatingConnectionAsync += args =>
{
if (string.IsNullOrEmpty(args.Username))
{
if (allowAnonymousConnections)
{
args.ReasonCode = MqttConnectReasonCode.Success;
//Debug.Log($"[MQTT] Anonymous client connected: {args.ClientId}");
}
else
{
args.ReasonCode = MqttConnectReasonCode.BadUserNameOrPassword;
Debug.LogWarning($"[MQTT] Anonymous not allowed: {args.ClientId}");
}
}
else if (args.Username == username && args.Password == password)
{
args.ReasonCode = MqttConnectReasonCode.Success;
Debug.Log($"[MQTT] Authenticated client: {args.ClientId}");
}
else
{
args.ReasonCode = MqttConnectReasonCode.BadUserNameOrPassword;
Debug.LogWarning($"[MQTT] Invalid credentials: {args.ClientId}");
}
return Task.CompletedTask;
};
mqttServer.ClientConnectedAsync += e =>
{
//Debug.Log($"[MQTT] Client connected: {e.ClientId}");
return Task.CompletedTask;
};
mqttServer.InterceptingPublishAsync += e =>
{
string payload = e.ApplicationMessage?.Payload == null
? ""
: System.Text.Encoding.UTF8.GetString(e.ApplicationMessage.Payload);
//Debug.Log($"[MQTT] Topic: {e.ApplicationMessage.Topic} - Payload: {payload}");
return Task.CompletedTask;
};
await mqttServer.StartAsync();
//Debug.Log($"MQTT broker started on port {port}");
}
private async void OnApplicationQuit()
{
if (mqttServer != null)
{
await mqttServer.StopAsync();
Debug.Log("MQTT broker stopped.");
}
}
}

2
Assets/Scripting/MQTT/MQTTBroker.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: 29bbd3dbef6acad4d853bdb166d95684

134
Assets/Scripting/MQTT/MqttClientManager.cs Normal file
View File

@@ -0,0 +1,134 @@
using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Text;
using System.Threading.Tasks;
using MQTTnet;
using MQTTnet.Client;
using MQTTnet.Extensions.ManagedClient;
using MQTTnet.Packets;
using UnityEngine;
public class MqttClientManager
{
private readonly string mqttBroker;
private readonly int mqttPort;
private readonly string mqttUsername;
private readonly string mqttPassword;
private readonly string mqttClientId;
private readonly string orderTopic;
private readonly string instantActionsTopic;
private readonly ConcurrentQueue<string> jsonQueue;
private readonly ConcurrentQueue<string> instantActionQueue;
private IManagedMqttClient mqttClient;
public MqttClientManager(string broker, int port, string username, string password, string clientId,
string orderTopic, string instantActionsTopic, ConcurrentQueue<string> jsonQueue,
ConcurrentQueue<string> instantActionQueue)
{
mqttBroker = broker;
mqttPort = port;
mqttUsername = username;
mqttPassword = password;
mqttClientId = clientId;
this.orderTopic = orderTopic;
this.instantActionsTopic = instantActionsTopic;
this.jsonQueue = jsonQueue;
this.instantActionQueue = instantActionQueue;
}
public async Task InitializeAsync()
{
try
{
var mqttFactory = new MqttFactory();
mqttClient = mqttFactory.CreateManagedMqttClient();
var optionsBuilder = new MqttClientOptionsBuilder()
.WithTcpServer(mqttBroker, mqttPort)
.WithClientId(mqttClientId);
if (!string.IsNullOrEmpty(mqttUsername) && !string.IsNullOrEmpty(mqttPassword))
{
optionsBuilder.WithCredentials(mqttUsername, mqttPassword);
}
var managedOptions = new ManagedMqttClientOptionsBuilder()
.WithClientOptions(optionsBuilder.Build())
.WithAutoReconnectDelay(TimeSpan.FromSeconds(5))
.Build();
mqttClient.ApplicationMessageReceivedAsync += async e =>
{
await Task.Run(() =>
{
string topic = e.ApplicationMessage.Topic;
string payload = Encoding.UTF8.GetString(e.ApplicationMessage.Payload ?? Array.Empty<byte>());
if (topic == orderTopic)
{
Debug.Log($"{payload}");
jsonQueue.Enqueue(payload);
}
else if (topic == instantActionsTopic)
{
Debug.Log($"Nhận được JSON từ chủ đề {instantActionsTopic}: {payload}");
instantActionQueue.Enqueue(payload);
}
});
};
mqttClient.ConnectedAsync += async e =>
{
try
{
await mqttClient.SubscribeAsync(new List<MqttTopicFilter>
{
new MqttTopicFilterBuilder().WithTopic(orderTopic).Build(),
new MqttTopicFilterBuilder().WithTopic(instantActionsTopic).Build()
});
}
catch (Exception ex)
{
Debug.LogError($"Lỗi khi kết nối hoặc đăng ký chủ đề: {ex.Message}");
}
};
mqttClient.DisconnectedAsync += async e =>
{
await Task.CompletedTask;
};
await mqttClient.StartAsync(managedOptions);
//Debug.Log("Đã khởi động MQTT client");
}
catch (Exception ex)
{
Debug.LogError($"Không thể khởi tạo MQTT client: {ex.Message}");
}
}
public async Task StopAsync()
{
if (mqttClient != null)
{
await mqttClient.StopAsync();
mqttClient.Dispose();
Debug.Log("Đã dừng và hủy MQTT client");
}
}
public async Task PublishAsync(string topic, string payload)
{
if (mqttClient == null || !mqttClient.IsConnected)
{
return;
}
var message = new MqttApplicationMessageBuilder()
.WithTopic(topic)
.WithPayload(Encoding.UTF8.GetBytes(payload))
.Build();
await mqttClient.EnqueueAsync(message);
}
}

2
Assets/Scripting/MQTT/MqttClientManager.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: 94facf4719dd13944aa270b45bb689cf

428
Assets/Scripting/PlayerInputActions.cs Normal file
View File

@@ -0,0 +1,428 @@
//------------------------------------------------------------------------------
// <auto-generated>
// This code was auto-generated by com.unity.inputsystem:InputActionCodeGenerator
// version 1.13.0
// from Assets/Scripting/PlayerInputActions.inputactions
//
// Changes to this file may cause incorrect behavior and will be lost if
// the code is regenerated.
// </auto-generated>
//------------------------------------------------------------------------------
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine.InputSystem;
using UnityEngine.InputSystem.Utilities;
/// <summary>
/// Provides programmatic access to <see cref="InputActionAsset" />, <see cref="InputActionMap" />, <see cref="InputAction" /> and <see cref="InputControlScheme" /> instances defined in asset "Assets/Scripting/PlayerInputActions.inputactions".
/// </summary>
/// <remarks>
/// This class is source generated and any manual edits will be discarded if the associated asset is reimported or modified.
/// </remarks>
/// <example>
/// <code>
/// using namespace UnityEngine;
/// using UnityEngine.InputSystem;
///
/// // Example of using an InputActionMap named "Player" from a UnityEngine.MonoBehaviour implementing callback interface.
/// public class Example : MonoBehaviour, MyActions.IPlayerActions
/// {
/// private MyActions_Actions m_Actions; // Source code representation of asset.
/// private MyActions_Actions.PlayerActions m_Player; // Source code representation of action map.
///
/// void Awake()
/// {
/// m_Actions = new MyActions_Actions(); // Create asset object.
/// m_Player = m_Actions.Player; // Extract action map object.
/// m_Player.AddCallbacks(this); // Register callback interface IPlayerActions.
/// }
///
/// void OnDestroy()
/// {
/// m_Actions.Dispose(); // Destroy asset object.
/// }
///
/// void OnEnable()
/// {
/// m_Player.Enable(); // Enable all actions within map.
/// }
///
/// void OnDisable()
/// {
/// m_Player.Disable(); // Disable all actions within map.
/// }
///
/// #region Interface implementation of MyActions.IPlayerActions
///
/// // Invoked when "Move" action is either started, performed or canceled.
/// public void OnMove(InputAction.CallbackContext context)
/// {
/// Debug.Log($"OnMove: {context.ReadValue&lt;Vector2&gt;()}");
/// }
///
/// // Invoked when "Attack" action is either started, performed or canceled.
/// public void OnAttack(InputAction.CallbackContext context)
/// {
/// Debug.Log($"OnAttack: {context.ReadValue&lt;float&gt;()}");
/// }
///
/// #endregion
/// }
/// </code>
/// </example>
public partial class @PlayerInputActions: IInputActionCollection2, IDisposable
{
/// <summary>
/// Provides access to the underlying asset instance.
/// </summary>
public InputActionAsset asset { get; }
/// <summary>
/// Constructs a new instance.
/// </summary>
public @PlayerInputActions()
{
asset = InputActionAsset.FromJson(@"{
""name"": ""PlayerInputActions"",
""maps"": [
{
""name"": ""Player"",
""id"": ""82815556-132e-49d6-8fca-b06edd932024"",
""actions"": [
{
""name"": ""R"",
""type"": ""Button"",
""id"": ""01ce2e4e-fc8e-4cc8-9b94-366a22e89c9f"",
""expectedControlType"": """",
""processors"": """",
""interactions"": """",
""initialStateCheck"": false
},
{
""name"": ""P"",
""type"": ""Button"",
""id"": ""9a5aadb3-d54f-4808-a656-07f79f54a44c"",
""expectedControlType"": """",
""processors"": """",
""interactions"": """",
""initialStateCheck"": false
},
{
""name"": ""U"",
""type"": ""Button"",
""id"": ""b7641136-088e-4c90-8036-88a6c96027cf"",
""expectedControlType"": """",
""processors"": """",
""interactions"": """",
""initialStateCheck"": false
},
{
""name"": ""D"",
""type"": ""Button"",
""id"": ""2078562a-bcac-4034-8435-4d09d5662b90"",
""expectedControlType"": """",
""processors"": """",
""interactions"": """",
""initialStateCheck"": false
}
],
""bindings"": [
{
""name"": """",
""id"": ""cac63c8b-ac74-4c56-9ae5-304c4bd98115"",
""path"": ""<Keyboard>/r"",
""interactions"": """",
""processors"": """",
""groups"": """",
""action"": ""R"",
""isComposite"": false,
""isPartOfComposite"": false
},
{
""name"": """",
""id"": ""8d628790-58fe-40a0-9794-cc1385ab0e46"",
""path"": ""<Keyboard>/p"",
""interactions"": """",
""processors"": """",
""groups"": """",
""action"": ""P"",
""isComposite"": false,
""isPartOfComposite"": false
},
{
""name"": """",
""id"": ""6722f789-b46d-494b-b1f5-ac158cdf784c"",
""path"": ""<Keyboard>/u"",
""interactions"": """",
""processors"": """",
""groups"": """",
""action"": ""U"",
""isComposite"": false,
""isPartOfComposite"": false
},
{
""name"": """",
""id"": ""c7afe6a4-4565-46d0-bfe5-e27c8a5b5ac0"",
""path"": ""<Keyboard>/d"",
""interactions"": """",
""processors"": """",
""groups"": """",
""action"": ""D"",
""isComposite"": false,
""isPartOfComposite"": false
}
]
}
],
""controlSchemes"": []
}");
// Player
m_Player = asset.FindActionMap("Player", throwIfNotFound: true);
m_Player_R = m_Player.FindAction("R", throwIfNotFound: true);
m_Player_P = m_Player.FindAction("P", throwIfNotFound: true);
m_Player_U = m_Player.FindAction("U", throwIfNotFound: true);
m_Player_D = m_Player.FindAction("D", throwIfNotFound: true);
}
~@PlayerInputActions()
{
UnityEngine.Debug.Assert(!m_Player.enabled, "This will cause a leak and performance issues, PlayerInputActions.Player.Disable() has not been called.");
}
/// <summary>
/// Destroys this asset and all associated <see cref="InputAction"/> instances.
/// </summary>
public void Dispose()
{
UnityEngine.Object.Destroy(asset);
}
/// <inheritdoc cref="UnityEngine.InputSystem.InputActionAsset.bindingMask" />
public InputBinding? bindingMask
{
get => asset.bindingMask;
set => asset.bindingMask = value;
}
/// <inheritdoc cref="UnityEngine.InputSystem.InputActionAsset.devices" />
public ReadOnlyArray<InputDevice>? devices
{
get => asset.devices;
set => asset.devices = value;
}
/// <inheritdoc cref="UnityEngine.InputSystem.InputActionAsset.controlSchemes" />
public ReadOnlyArray<InputControlScheme> controlSchemes => asset.controlSchemes;
/// <inheritdoc cref="UnityEngine.InputSystem.InputActionAsset.Contains(InputAction)" />
public bool Contains(InputAction action)
{
return asset.Contains(action);
}
/// <inheritdoc cref="UnityEngine.InputSystem.InputActionAsset.GetEnumerator()" />
public IEnumerator<InputAction> GetEnumerator()
{
return asset.GetEnumerator();
}
/// <inheritdoc cref="IEnumerable.GetEnumerator()" />
IEnumerator IEnumerable.GetEnumerator()
{
return GetEnumerator();
}
/// <inheritdoc cref="UnityEngine.InputSystem.InputActionAsset.Enable()" />
public void Enable()
{
asset.Enable();
}
/// <inheritdoc cref="UnityEngine.InputSystem.InputActionAsset.Disable()" />
public void Disable()
{
asset.Disable();
}
/// <inheritdoc cref="UnityEngine.InputSystem.InputActionAsset.bindings" />
public IEnumerable<InputBinding> bindings => asset.bindings;
/// <inheritdoc cref="UnityEngine.InputSystem.InputActionAsset.FindAction(string, bool)" />
public InputAction FindAction(string actionNameOrId, bool throwIfNotFound = false)
{
return asset.FindAction(actionNameOrId, throwIfNotFound);
}
/// <inheritdoc cref="UnityEngine.InputSystem.InputActionAsset.FindBinding(InputBinding, out InputAction)" />
public int FindBinding(InputBinding bindingMask, out InputAction action)
{
return asset.FindBinding(bindingMask, out action);
}
// Player
private readonly InputActionMap m_Player;
private List<IPlayerActions> m_PlayerActionsCallbackInterfaces = new List<IPlayerActions>();
private readonly InputAction m_Player_R;
private readonly InputAction m_Player_P;
private readonly InputAction m_Player_U;
private readonly InputAction m_Player_D;
/// <summary>
/// Provides access to input actions defined in input action map "Player".
/// </summary>
public struct PlayerActions
{
private @PlayerInputActions m_Wrapper;
/// <summary>
/// Construct a new instance of the input action map wrapper class.
/// </summary>
public PlayerActions(@PlayerInputActions wrapper) { m_Wrapper = wrapper; }
/// <summary>
/// Provides access to the underlying input action "Player/R".
/// </summary>
public InputAction @R => m_Wrapper.m_Player_R;
/// <summary>
/// Provides access to the underlying input action "Player/P".
/// </summary>
public InputAction @P => m_Wrapper.m_Player_P;
/// <summary>
/// Provides access to the underlying input action "Player/U".
/// </summary>
public InputAction @U => m_Wrapper.m_Player_U;
/// <summary>
/// Provides access to the underlying input action "Player/D".
/// </summary>
public InputAction @D => m_Wrapper.m_Player_D;
/// <summary>
/// Provides access to the underlying input action map instance.
/// </summary>
public InputActionMap Get() { return m_Wrapper.m_Player; }
/// <inheritdoc cref="UnityEngine.InputSystem.InputActionMap.Enable()" />
public void Enable() { Get().Enable(); }
/// <inheritdoc cref="UnityEngine.InputSystem.InputActionMap.Disable()" />
public void Disable() { Get().Disable(); }
/// <inheritdoc cref="UnityEngine.InputSystem.InputActionMap.enabled" />
public bool enabled => Get().enabled;
/// <summary>
/// Implicitly converts an <see ref="PlayerActions" /> to an <see ref="InputActionMap" /> instance.
/// </summary>
public static implicit operator InputActionMap(PlayerActions set) { return set.Get(); }
/// <summary>
/// Adds <see cref="InputAction.started"/>, <see cref="InputAction.performed"/> and <see cref="InputAction.canceled"/> callbacks provided via <param cref="instance" /> on all input actions contained in this map.
/// </summary>
/// <param name="instance">Callback instance.</param>
/// <remarks>
/// If <paramref name="instance" /> is <c>null</c> or <paramref name="instance"/> have already been added this method does nothing.
/// </remarks>
/// <seealso cref="PlayerActions" />
public void AddCallbacks(IPlayerActions instance)
{
if (instance == null || m_Wrapper.m_PlayerActionsCallbackInterfaces.Contains(instance)) return;
m_Wrapper.m_PlayerActionsCallbackInterfaces.Add(instance);
@R.started += instance.OnR;
@R.performed += instance.OnR;
@R.canceled += instance.OnR;
@P.started += instance.OnP;
@P.performed += instance.OnP;
@P.canceled += instance.OnP;
@U.started += instance.OnU;
@U.performed += instance.OnU;
@U.canceled += instance.OnU;
@D.started += instance.OnD;
@D.performed += instance.OnD;
@D.canceled += instance.OnD;
}
/// <summary>
/// Removes <see cref="InputAction.started"/>, <see cref="InputAction.performed"/> and <see cref="InputAction.canceled"/> callbacks provided via <param cref="instance" /> on all input actions contained in this map.
/// </summary>
/// <remarks>
/// Calling this method when <paramref name="instance" /> have not previously been registered has no side-effects.
/// </remarks>
/// <seealso cref="PlayerActions" />
private void UnregisterCallbacks(IPlayerActions instance)
{
@R.started -= instance.OnR;
@R.performed -= instance.OnR;
@R.canceled -= instance.OnR;
@P.started -= instance.OnP;
@P.performed -= instance.OnP;
@P.canceled -= instance.OnP;
@U.started -= instance.OnU;
@U.performed -= instance.OnU;
@U.canceled -= instance.OnU;
@D.started -= instance.OnD;
@D.performed -= instance.OnD;
@D.canceled -= instance.OnD;
}
/// <summary>
/// Unregisters <param cref="instance" /> and unregisters all input action callbacks via <see cref="PlayerActions.UnregisterCallbacks(IPlayerActions)" />.
/// </summary>
/// <seealso cref="PlayerActions.UnregisterCallbacks(IPlayerActions)" />
public void RemoveCallbacks(IPlayerActions instance)
{
if (m_Wrapper.m_PlayerActionsCallbackInterfaces.Remove(instance))
UnregisterCallbacks(instance);
}
/// <summary>
/// Replaces all existing callback instances and previously registered input action callbacks associated with them with callbacks provided via <param cref="instance" />.
/// </summary>
/// <remarks>
/// If <paramref name="instance" /> is <c>null</c>, calling this method will only unregister all existing callbacks but not register any new callbacks.
/// </remarks>
/// <seealso cref="PlayerActions.AddCallbacks(IPlayerActions)" />
/// <seealso cref="PlayerActions.RemoveCallbacks(IPlayerActions)" />
/// <seealso cref="PlayerActions.UnregisterCallbacks(IPlayerActions)" />
public void SetCallbacks(IPlayerActions instance)
{
foreach (var item in m_Wrapper.m_PlayerActionsCallbackInterfaces)
UnregisterCallbacks(item);
m_Wrapper.m_PlayerActionsCallbackInterfaces.Clear();
AddCallbacks(instance);
}
}
/// <summary>
/// Provides a new <see cref="PlayerActions" /> instance referencing this action map.
/// </summary>
public PlayerActions @Player => new PlayerActions(this);
/// <summary>
/// Interface to implement callback methods for all input action callbacks associated with input actions defined by "Player" which allows adding and removing callbacks.
/// </summary>
/// <seealso cref="PlayerActions.AddCallbacks(IPlayerActions)" />
/// <seealso cref="PlayerActions.RemoveCallbacks(IPlayerActions)" />
public interface IPlayerActions
{
/// <summary>
/// Method invoked when associated input action "R" is either <see cref="UnityEngine.InputSystem.InputAction.started" />, <see cref="UnityEngine.InputSystem.InputAction.performed" /> or <see cref="UnityEngine.InputSystem.InputAction.canceled" />.
/// </summary>
/// <seealso cref="UnityEngine.InputSystem.InputAction.started" />
/// <seealso cref="UnityEngine.InputSystem.InputAction.performed" />
/// <seealso cref="UnityEngine.InputSystem.InputAction.canceled" />
void OnR(InputAction.CallbackContext context);
/// <summary>
/// Method invoked when associated input action "P" is either <see cref="UnityEngine.InputSystem.InputAction.started" />, <see cref="UnityEngine.InputSystem.InputAction.performed" /> or <see cref="UnityEngine.InputSystem.InputAction.canceled" />.
/// </summary>
/// <seealso cref="UnityEngine.InputSystem.InputAction.started" />
/// <seealso cref="UnityEngine.InputSystem.InputAction.performed" />
/// <seealso cref="UnityEngine.InputSystem.InputAction.canceled" />
void OnP(InputAction.CallbackContext context);
/// <summary>
/// Method invoked when associated input action "U" is either <see cref="UnityEngine.InputSystem.InputAction.started" />, <see cref="UnityEngine.InputSystem.InputAction.performed" /> or <see cref="UnityEngine.InputSystem.InputAction.canceled" />.
/// </summary>
/// <seealso cref="UnityEngine.InputSystem.InputAction.started" />
/// <seealso cref="UnityEngine.InputSystem.InputAction.performed" />
/// <seealso cref="UnityEngine.InputSystem.InputAction.canceled" />
void OnU(InputAction.CallbackContext context);
/// <summary>
/// Method invoked when associated input action "D" is either <see cref="UnityEngine.InputSystem.InputAction.started" />, <see cref="UnityEngine.InputSystem.InputAction.performed" /> or <see cref="UnityEngine.InputSystem.InputAction.canceled" />.
/// </summary>
/// <seealso cref="UnityEngine.InputSystem.InputAction.started" />
/// <seealso cref="UnityEngine.InputSystem.InputAction.performed" />
/// <seealso cref="UnityEngine.InputSystem.InputAction.canceled" />
void OnD(InputAction.CallbackContext context);
}
}

2
Assets/Scripting/PlayerInputActions.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: e4cf5f71e577fe64090e08b4caf7377f

94
Assets/Scripting/PlayerInputActions.inputactions Normal file
View File

@@ -0,0 +1,94 @@
{
"name": "PlayerInputActions",
"maps": [
{
"name": "Player",
"id": "82815556-132e-49d6-8fca-b06edd932024",
"actions": [
{
"name": "R",
"type": "Button",
"id": "01ce2e4e-fc8e-4cc8-9b94-366a22e89c9f",
"expectedControlType": "",
"processors": "",
"interactions": "",
"initialStateCheck": false
},
{
"name": "P",
"type": "Button",
"id": "9a5aadb3-d54f-4808-a656-07f79f54a44c",
"expectedControlType": "",
"processors": "",
"interactions": "",
"initialStateCheck": false
},
{
"name": "U",
"type": "Button",
"id": "b7641136-088e-4c90-8036-88a6c96027cf",
"expectedControlType": "",
"processors": "",
"interactions": "",
"initialStateCheck": false
},
{
"name": "D",
"type": "Button",
"id": "2078562a-bcac-4034-8435-4d09d5662b90",
"expectedControlType": "",
"processors": "",
"interactions": "",
"initialStateCheck": false
}
],
"bindings": [
{
"name": "",
"id": "cac63c8b-ac74-4c56-9ae5-304c4bd98115",
"path": "<Keyboard>/r",
"interactions": "",
"processors": "",
"groups": "",
"action": "R",
"isComposite": false,
"isPartOfComposite": false
},
{
"name": "",
"id": "8d628790-58fe-40a0-9794-cc1385ab0e46",
"path": "<Keyboard>/p",
"interactions": "",
"processors": "",
"groups": "",
"action": "P",
"isComposite": false,
"isPartOfComposite": false
},
{
"name": "",
"id": "6722f789-b46d-494b-b1f5-ac158cdf784c",
"path": "<Keyboard>/u",
"interactions": "",
"processors": "",
"groups": "",
"action": "U",
"isComposite": false,
"isPartOfComposite": false
},
{
"name": "",
"id": "c7afe6a4-4565-46d0-bfe5-e27c8a5b5ac0",
"path": "<Keyboard>/d",
"interactions": "",
"processors": "",
"groups": "",
"action": "D",
"isComposite": false,
"isPartOfComposite": false
}
]
}
],
"controlSchemes": []
}

14
Assets/Scripting/PlayerInputActions.inputactions.meta Normal file
View File

@@ -0,0 +1,14 @@
fileFormatVersion: 2
guid: 46cf0cb27cb69524f8eae00dc3fe8eb0
ScriptedImporter:
internalIDToNameTable: []
externalObjects: {}
serializedVersion: 2
userData:
assetBundleName:
assetBundleVariant:
script: {fileID: 11500000, guid: 8404be70184654265930450def6a9037, type: 3}
generateWrapperCode: 1
wrapperCodePath:
wrapperClassName:
wrapperCodeNamespace:

8
Assets/Scripting/Robot.meta Normal file
View File

@@ -0,0 +1,8 @@
fileFormatVersion: 2
guid: 14f80c6c49fdaff41b0b04cea1c0f448
folderAsset: yes
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

6114
Assets/Scripting/Robot/AMR.cs Normal file
View File

File diff suppressed because it is too large Load Diff

2
Assets/Scripting/Robot/AMR.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: b3ef713dec000114695d1df96efa8e36

778
Assets/Scripting/Robot/ActionExecutor.cs Normal file
View File

@@ -0,0 +1,778 @@
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}");
}
}

2
Assets/Scripting/Robot/ActionExecutor.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: 4aa787d0618948746860649f325e4055

393
Assets/Scripting/Robot/ActionStateSender.cs Normal file
View File

@@ -0,0 +1,393 @@
using Newtonsoft.Json;
using System;
using System.Collections;
using System.Linq;
using System.Threading.Tasks;
using UnityEngine;
public class ActionStateSender
{
private readonly MqttClientManager mqttClientManager;
private readonly OrderProcessor orderProcessor;
private readonly AnimationControllerAPM animationController;
private readonly string stateTopic;
private readonly string instantActionsTopic;
private readonly string serialNumber;
private readonly string currentMapId;
private readonly PathMover pathMover;
private uint headerIdCounter;
public ActionStateSender(
MqttClientManager mqttClientManager,
OrderProcessor orderProcessor,
AnimationControllerAPM animationController,
string stateTopic,
string instantActionsTopic,
string serialNumber,
string currentMapId,
PathMover pathMover)
{
this.mqttClientManager = mqttClientManager;
this.orderProcessor = orderProcessor;
this.animationController = animationController;
this.stateTopic = stateTopic;
this.instantActionsTopic = instantActionsTopic;
this.serialNumber = serialNumber;
this.currentMapId = currentMapId;
this.pathMover = pathMover;
this.headerIdCounter = 0;
}
public async void ProcessInstantActionJson(string json)
{
try
{
InstantActionData instantAction = await Task.Run(() => JsonConvert.DeserializeObject<InstantActionData>(json));
if (instantAction.Actions != null)
{
foreach (var action in instantAction.Actions)
{
if (action == null) continue;
Debug.Log($"Nhận được instant action: ActionType={action.ActionType}, ActionId={action.ActionId ?? "null"}, SerialNumber={serialNumber}");
if (instantAction.SerialNumber != serialNumber)
{
Debug.LogWarning($"Action có serialNumber ({instantAction.SerialNumber}) không khớp với serialNumber của robot ({serialNumber}). Bỏ qua.");
continue;
}
switch (action.ActionType)
{
case "cancelOrder":
Debug.Log($"Nhận được lệnh cancelOrder, ActionId: {(action.ActionId ?? "null")}, SerialNumber: {serialNumber}, HeaderId: {instantAction.HeaderId}");
//await SendActionConfirmation(instantAction, action, "FINISHED", "Hủy nhiệm vụ thành công");
UnityMainThreadDispatcher.Enqueue(() => orderProcessor.CancelOrder());
break;
case "pick":
if (animationController != null)
{
UnityMainThreadDispatcher.Enqueue(() =>
{
animationController.OnKey2();
Debug.Log($"Đã kích hoạt animation U cho instant action: {action.ActionType}");
});
//await SendActionConfirmation(instantAction, action, "FINISHED", "Animation U triggered successfully");
}
else
{
//await SendActionConfirmation(instantAction, action, "FAILED", "AnimationControllerAPM not found");
}
break;
case "drop":
if (animationController != null)
{
UnityMainThreadDispatcher.Enqueue(() =>
{
animationController.OnKey1();
Debug.Log($"Đã kích hoạt animation D cho instant action: {action.ActionType}");
});
//await SendActionConfirmation(instantAction, action, "FINISHED", "Animation D triggered successfully");
}
else
{
//await SendActionConfirmation(instantAction, action, "FAILED", "AnimationControllerAPM not found");
}
break;
default:
Debug.LogWarning($"Instant ActionType không được hỗ trợ: {action.ActionType}, SerialNumber: {serialNumber}");
//await SendActionConfirmation(instantAction, action, "FAILED", $"Unsupported ActionType: {action.ActionType}");
break;
}
}
}
}
catch (Exception ex)
{
Debug.LogError($"Lỗi khi phân tích JSON instant action cho robot với serialNumber {serialNumber}: {ex.Message}");
}
}
public async Task SendActionStateAsync(
ActionData action,
string referenceId,
string referenceType,
string actionStatus,
string resultDescription)
{
try
{
// Cập nhật trạng thái hành động trong ActionData
action.ActionStatus = actionStatus;
action.ActionDescription = resultDescription;
var actionState = new ActionState
{
ActionId = string.IsNullOrEmpty(action.ActionId) ? Guid.NewGuid().ToString() : action.ActionId,
ActionType = action.ActionType,
ActionDescription = action.ActionDescription ?? "",
ActionStatus = actionStatus,
};
var stateData = new SendState
{
HeaderId = headerIdCounter++,
Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
Version = "1.0.0",
Manufacturer = "phenikaaX",
SerialNumber = serialNumber,
Maps = new Map[]
{
new Map
{
MapId = currentMapId,
MapVersion = "1.0",
MapDescription = "Default map",
MapStatus = "ENABLED"
}
},
OrderId = orderProcessor?.OrderId ?? "",
OrderUpdateId = orderProcessor?.OrderId != null ? 1 : 0,
ZoneSetId = "",
LastNodeId = orderProcessor?.LastNodeId ?? "",
LastNodeSequenceId = orderProcessor?.LastNodeSequenceId ?? 0,
NodeStates = orderProcessor?.CurrentOrder?.Nodes?.Select(n => new NodeState
{
NodeId = n.NodeId,
SequenceId = n.SequenceId,
NodeDescription = n.NodeDescription ?? "",
Released = n.Released,
NodePosition = new NodePosition
{
X = n.NodePosition.X,
Y = n.NodePosition.Y,
Theta = n.NodePosition.Theta,
AllowedDeviationXY = n.NodePosition.AllowedDeviationXY,
AllowedDeviationTheta = n.NodePosition.AllowedDeviationTheta,
MapId = n.NodePosition.MapId,
MapDescription = n.NodePosition.MapDescription ?? ""
}
}).ToArray() ?? new NodeState[0],
EdgeStates = orderProcessor?.CurrentOrder?.Edges?.Select(e => new EdgeState
{
EdgeId = e.EdgeId,
SequenceId = e.SequenceId,
EdgeDescription = e.EdgeDescription ?? "",
Released = e.Released,
Trajectory = e.Trajectory != null ? new Trajectory
{
Degree = e.Trajectory.Degree,
KnotVector = e.Trajectory.KnotVector,
ControlPoints = e.Trajectory.ControlPoints?.Select(cp => new ControlPoint
{
X = cp.X,
Y = cp.Y,
Weight = cp.Weight
}).ToArray()
} : null
}).ToArray() ?? new EdgeState[0],
Driving = pathMover?.IsMoving ?? false,
Paused = !(pathMover?.IsMoving ?? false),
NewBaseRequest = !(pathMover?.IsMoving ?? false),
DistanceSinceLastNode = 1.531483174223427f,
AgvPosition = new AgvPosition
{
X = 0f, // Will be updated by VisualizationSender
Y = 0f,
Theta = 0f,
MapId = currentMapId,
MapDescription = "",
PositionInitialized = true,
LocalizationScore = 0.7f,
DeviationRange = 0.1f
},
Velocity = new Velocity
{
Vx = 0f,
Vy = 0f,
Omega = 0f
},
Loads = new Load[0],
ActionStates = new[] { actionState },
BatteryState = new BatteryState
{
BatteryCharge = 80f,
BatteryVoltage = 48f,
BatteryHealth = 100f,
Charging = false,
Reach = 1000f
},
OperatingMode = "AUTOMATIC",
Errors = new Error[0],
Information = new Information[0],
SafetyState = new SafetyState
{
EStop = "NONE",
FieldViolation = false
}
};
string json = JsonConvert.SerializeObject(stateData, Formatting.None);
await mqttClientManager.PublishAsync(stateTopic, json);
Debug.Log($"Đã gửi trạng thái action {action.ActionType} ({referenceType} {referenceId}): {actionStatus} - {resultDescription}");
}
catch (Exception)
{
}
}
private async Task SendActionConfirmation(InstantActionData instantAction, ActionData action, string actionStatus, string resultDescription)
{
try
{
var confirmation = new CancelConfirmationData
{
HeaderId = instantAction.HeaderId,
Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
Version = instantAction.Version ?? "0.0.1",
Manufacturer = instantAction.Manufacturer ?? "phenikaaX",
SerialNumber = instantAction.SerialNumber ?? serialNumber,
ActionStates = new[]
{
new ActionState
{
ActionId = string.IsNullOrEmpty(action.ActionId) ? Guid.NewGuid().ToString() : action.ActionId,
ActionType = action.ActionType,
ActionStatus = actionStatus,
ResultDescription = resultDescription
}
}
};
string confirmationJson = JsonConvert.SerializeObject(confirmation, Formatting.None);
await mqttClientManager.PublishAsync(stateTopic, confirmationJson);
Debug.Log($"Đã gửi xác nhận action {action.ActionType} lên topic {stateTopic}: {confirmationJson}");
}
catch (Exception ex)
{
Debug.LogError($"Lỗi khi gửi xác nhận action {action.ActionType}: {ex.Message}");
}
}
public IEnumerator SendCancelOrder(Node node, ActionData action)
{
if (node is null)
{
yield break;
}
if (action is null)
{
yield break;
}
// Kiểm tra orderProcessor
if (orderProcessor == null)
{
// Gửi thông báo hủy qua MQTT mà không cần orderProcessor
var cancelOrderDataWithoutProcessor = new
{
HeaderId = headerIdCounter++,
Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
Version = "0.0.1",
Manufacturer = "phenikaaX",
SerialNumber = serialNumber ?? "unknown",
Actions = new[]
{
new
{
ActionType = "cancelOrder",
ActionId = Guid.NewGuid().ToString(),
ActionDescription = $"Không có pallet tại node {node.NodeDescription}",
BlockingType = "NONE",
ActionParameters = new[]
{
new { Key = "ORDER_ID", Value = "unknown" },
new { Key = "REASON", Value = "Không có pallet" }
}
}
}
};
string cancelJson = JsonConvert.SerializeObject(cancelOrderDataWithoutProcessor, Formatting.None);
if (mqttClientManager != null)
{
Task publishTask = mqttClientManager.PublishAsync(instantActionsTopic, cancelJson);
while (!publishTask.IsCompleted)
{
yield return null;
}
if (publishTask.IsFaulted)
{
Debug.LogError($"Lỗi khi gửi JSON cancelOrder: {publishTask.Exception?.Message}");
}
else
{
Debug.Log($"Đã gửi JSON cancelOrder lên topic instantActions: {cancelJson}");
}
}
else
{
Debug.LogError("MqttClientManager is null. Cannot send cancel order message.");
}
yield break;
}
// Hủy order ngay lập tức
UnityMainThreadDispatcher.Enqueue(() => orderProcessor.CancelOrder());
Debug.Log($"Đã hủy order với OrderId: {orderProcessor.OrderId ?? "N/A"} do không có pallet tại node {node.NodeDescription}");
// Tạo JSON hủy order
var cancelOrderDataWithProcessor = new
{
HeaderId = headerIdCounter++,
Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
Version = "0.0.1",
Manufacturer = "phenikaaX",
SerialNumber = orderProcessor.SerialNumber ?? "unknown",
Actions = new[]
{
new
{
ActionType = "cancelOrder",
ActionId = Guid.NewGuid().ToString(),
ActionDescription = $"Không có pallet tại node {node.NodeDescription}",
BlockingType = "NONE",
ActionParameters = new[]
{
new { Key = "ORDER_ID", Value = orderProcessor.OrderId ?? "" },
new { Key = "REASON", Value = "Không có pallet" }
}
}
}
};
// Gửi JSON qua MQTT
if (mqttClientManager != null)
{
string cancelJson = JsonConvert.SerializeObject(cancelOrderDataWithProcessor, Formatting.None);
Task publishTask = mqttClientManager.PublishAsync(instantActionsTopic, cancelJson);
while (!publishTask.IsCompleted)
{
yield return null;
}
if (publishTask.IsFaulted)
{
Debug.LogError($"Lỗi khi gửi JSON cancelOrder: {publishTask.Exception?.Message}");
}
else
{
Debug.Log($"Đã gửi JSON cancelOrder lên topic instantActions: {cancelJson}");
}
}
else
{
Debug.LogError("MqttClientManager is null. Cannot send cancel order message.");
}
}
}

2
Assets/Scripting/Robot/ActionStateSender.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: c2b26dc08b2cc7249a81b521ea27a641

381
Assets/Scripting/Robot/GameObjectTimeline.playable Normal file
View File

@@ -0,0 +1,381 @@
%YAML 1.1
%TAG !u! tag:unity3d.com,2011:
--- !u!114 &-8995895463826435607
MonoBehaviour:
m_ObjectHideFlags: 1
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 0}
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: 05acc715f855ced458d76ee6f8ac6c61, type: 3}
m_Name: Cinemachine Track
m_EditorClassIdentifier:
m_Version: 3
m_AnimClip: {fileID: 0}
m_Locked: 0
m_Muted: 0
m_CustomPlayableFullTypename:
m_Curves: {fileID: 0}
m_Parent: {fileID: 11400000}
m_Children: []
m_Clips:
- m_Version: 1
m_Start: 0
m_ClipIn: 0
m_Asset: {fileID: 2769284456360264383}
m_Duration: 200
m_TimeScale: 1
m_ParentTrack: {fileID: -8995895463826435607}
m_EaseInDuration: 0
m_EaseOutDuration: 0
m_BlendInDuration: -1
m_BlendOutDuration: -1
m_MixInCurve:
serializedVersion: 2
m_Curve:
- serializedVersion: 3
time: 0
value: 0
inSlope: 0
outSlope: 0
tangentMode: 0
weightedMode: 0
inWeight: 0
outWeight: 0
- serializedVersion: 3
time: 1
value: 1
inSlope: 0
outSlope: 0
tangentMode: 0
weightedMode: 0
inWeight: 0
outWeight: 0
m_PreInfinity: 2
m_PostInfinity: 2
m_RotationOrder: 4
m_MixOutCurve:
serializedVersion: 2
m_Curve:
- serializedVersion: 3
time: 0
value: 1
inSlope: 0
outSlope: 0
tangentMode: 0
weightedMode: 0
inWeight: 0
outWeight: 0
- serializedVersion: 3
time: 1
value: 0
inSlope: 0
outSlope: 0
tangentMode: 0
weightedMode: 0
inWeight: 0
outWeight: 0
m_PreInfinity: 2
m_PostInfinity: 2
m_RotationOrder: 4
m_BlendInCurveMode: 0
m_BlendOutCurveMode: 0
m_ExposedParameterNames: []
m_AnimationCurves: {fileID: 0}
m_Recordable: 0
m_PostExtrapolationMode: 0
m_PreExtrapolationMode: 0
m_PostExtrapolationTime: 0
m_PreExtrapolationTime: 0
m_DisplayName: CinemachineShot
m_Markers:
m_Objects: []
TrackPriority: 0
--- !u!114 &-4820503243207489472
MonoBehaviour:
m_ObjectHideFlags: 1
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 0}
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: f1e95aa6d658d694785bfde37c857fff, type: 3}
m_Name: RecorderClip
m_EditorClassIdentifier:
settings: {fileID: 8427519837969805819}
--- !u!114 &-4667555978659030408
MonoBehaviour:
m_ObjectHideFlags: 1
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 0}
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: 0e6cf5671577b7344ba25c25b4346ce4, type: 3}
m_Name: Recorder Track
m_EditorClassIdentifier:
m_Version: 3
m_AnimClip: {fileID: 0}
m_Locked: 0
m_Muted: 0
m_CustomPlayableFullTypename:
m_Curves: {fileID: 0}
m_Parent: {fileID: 11400000}
m_Children: []
m_Clips:
- m_Version: 1
m_Start: 0
m_ClipIn: 0
m_Asset: {fileID: -4820503243207489472}
m_Duration: 200
m_TimeScale: 1
m_ParentTrack: {fileID: -4667555978659030408}
m_EaseInDuration: 0
m_EaseOutDuration: 0
m_BlendInDuration: 0
m_BlendOutDuration: 0
m_MixInCurve:
serializedVersion: 2
m_Curve:
- serializedVersion: 3
time: 0
value: 0
inSlope: 0
outSlope: 0
tangentMode: 0
weightedMode: 0
inWeight: 0
outWeight: 0
- serializedVersion: 3
time: 1
value: 1
inSlope: 0
outSlope: 0
tangentMode: 0
weightedMode: 0
inWeight: 0
outWeight: 0
m_PreInfinity: 2
m_PostInfinity: 2
m_RotationOrder: 4
m_MixOutCurve:
serializedVersion: 2
m_Curve:
- serializedVersion: 3
time: 0
value: 1
inSlope: 0
outSlope: 0
tangentMode: 0
weightedMode: 0
inWeight: 0
outWeight: 0
- serializedVersion: 3
time: 1
value: 0
inSlope: 0
outSlope: 0
tangentMode: 0
weightedMode: 0
inWeight: 0
outWeight: 0
m_PreInfinity: 2
m_PostInfinity: 2
m_RotationOrder: 4
m_BlendInCurveMode: 0
m_BlendOutCurveMode: 0
m_ExposedParameterNames: []
m_AnimationCurves: {fileID: 0}
m_Recordable: 0
m_PostExtrapolationMode: 0
m_PreExtrapolationMode: 0
m_PostExtrapolationTime: 0
m_PreExtrapolationTime: 0
m_DisplayName: RecorderClip
m_Markers:
m_Objects: []
--- !u!114 &11400000
MonoBehaviour:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 0}
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: bfda56da833e2384a9677cd3c976a436, type: 3}
m_Name: GameObjectTimeline
m_EditorClassIdentifier:
m_Version: 0
m_Tracks:
- {fileID: -8995895463826435607}
- {fileID: -4667555978659030408}
m_FixedDuration: 0
m_EditorSettings:
m_Framerate: 60
m_ScenePreview: 1
m_DurationMode: 0
m_MarkerTrack: {fileID: 0}
--- !u!114 &2769284456360264383
MonoBehaviour:
m_ObjectHideFlags: 1
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 0}
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: 90fb794a295e73545af71bcdb7375791, type: 3}
m_Name: CinemachineShot
m_EditorClassIdentifier:
DisplayName:
VirtualCamera:
exposedName:
defaultValue: {fileID: 0}
--- !u!114 &8427519837969805819
MonoBehaviour:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 0}
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: 6fde0a8ac3e6b482c95fa602e65ab045, type: 3}
m_Name: RecorderClip
m_EditorClassIdentifier:
enabled: 1
take: 7
captureEveryNthFrame: 1
fileNameGenerator:
m_Path:
m_Root: 0
m_Leaf: Recordings
m_ForceAssetFolder: 0
m_AbsolutePath:
m_FileName: <Recorder>_<Take>
encoderSettings:
rid: 5465612107755290723
captureAlpha: 0
captureAudio: 1
m_ImageInputSelector:
m_Selected: gameViewInputSettings
gameViewInputSettings:
m_OutputResolution:
m_CustomWidth: 1024
m_CustomHeight: 1024
imageHeight: 0
maxSupportedHeight: 2160
m_AspectRatio:
m_CustomAspectX: 1
m_CustomAspectY: 1
m_ImageAspect: 1
flipFinalOutput: 0
cameraInputSettings:
m_OutputResolution:
m_CustomWidth: 1024
m_CustomHeight: 1024
imageHeight: 0
maxSupportedHeight: 4320
m_AspectRatio:
m_CustomAspectX: 1
m_CustomAspectY: 1
m_ImageAspect: 1
source: 2
cameraTag:
flipFinalOutput: 0
captureUI: 0
camera360InputSettings:
source: 2
cameraTag:
flipFinalOutput: 0
renderStereo: 1
stereoSeparation: 0.065
mapSize: 1024
m_OutputWidth: 1024
m_OutputHeight: 2048
renderTextureInputSettings:
renderTexture: {fileID: 0}
flipFinalOutput: 0
renderTextureSamplerSettings:
source: 1
m_RenderWidth: 1280
m_RenderHeight: 720
m_OutputWidth: 1280
m_OutputHeight: 720
outputAspectRatio:
m_CustomAspectX: 1
m_CustomAspectY: 1
m_ImageAspect: 1
superSampling: 1
superKernelPower: 16
superKernelScale: 1
cameraTag:
colorSpace: 0
flipFinalOutput: 0
m_AudioInputSettings:
preserveAudio: 1
_accumulationSettings:
rid: 5465612107755290724
outputFormat: 0
encodingQuality: 2
containerFormatSelected: 0
encoderSelected: 0
encoderPresetSelected: 0
encoderPresetSelectedName:
encoderPresetSelectedOptions:
encoderPresetSelectedSuffixes:
encoderColorDefinitionSelected: 0
encoderCustomOptions:
m_MovieRecorderVersion: 1
references:
version: 2
RefIds:
- rid: 5465612107755290723
type: {class: CoreEncoderSettings, ns: UnityEditor.Recorder.Encoder, asm: Unity.Recorder.Editor}
data:
targetBitRate: 8
gopSize: 25
numConsecutiveBFrames: 2
encodingProfile: 2
keyframeDistance: 25
codec: 0
encodingQuality: 2
- rid: 5465612107755290724
type: {class: AccumulationSettings, ns: , asm: Unity.Recorder.Editor}
data:
captureAccumulation: 0
samples: 1
shutterInterval: 1
shutterProfileType: 0
shutterProfileCurve:
serializedVersion: 2
m_Curve:
- serializedVersion: 3
time: 0
value: 1
inSlope: 0
outSlope: 0
tangentMode: 0
weightedMode: 0
inWeight: 0
outWeight: 0
- serializedVersion: 3
time: 1
value: 1
inSlope: 0
outSlope: 0
tangentMode: 0
weightedMode: 0
inWeight: 0
outWeight: 0
m_PreInfinity: 2
m_PostInfinity: 2
m_RotationOrder: 4
shutterFullyOpen: 0.25
shutterBeginsClosing: 0.75
useSubPixelJitter: 1

8
Assets/Scripting/Robot/GameObjectTimeline.playable.meta Normal file
View File

@@ -0,0 +1,8 @@
fileFormatVersion: 2
guid: 1db705b096781be4594bd90ce844c93f
NativeFormatImporter:
externalObjects: {}
mainObjectFileID: 11400000
userData:
assetBundleName:
assetBundleVariant:

9
Assets/Scripting/Robot/MainThreadDispatcher.cs Normal file
View File

@@ -0,0 +1,9 @@
using UnityEngine;
public class MainThreadDispatcher : MonoBehaviour
{
private void Update()
{
UnityMainThreadDispatcher.Update();
}
}

2
Assets/Scripting/Robot/MainThreadDispatcher.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: 2a549a9912ffa1f4b9d7e5cade52a964

145
Assets/Scripting/Robot/OrderProcessor.cs Normal file
View File

@@ -0,0 +1,145 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading.Tasks;
using Newtonsoft.Json;
using UnityEngine;
public class OrderProcessor
{
private readonly PathVisualizer pathVisualizer;
private readonly PathMover pathMover;
private readonly Transform transform;
private string currentMapId;
private readonly string serialNumber;
private string lastNodeId;
private string orderId;
private int lastNodeSequenceId;
private OrderData currentOrder;
private readonly Queue<OrderData> orderQueue;
private bool isProcessingOrder;
public OrderProcessor(PathVisualizer pathVisualizer, PathMover pathMover, Transform transform,
string initialMapId, string serialNumber)
{
this.pathVisualizer = pathVisualizer;
this.pathMover = pathMover;
this.transform = transform;
this.currentMapId = initialMapId;
this.serialNumber = serialNumber;
this.lastNodeId = null;
this.orderId = null;
this.lastNodeSequenceId = 0;
this.currentOrder = null;
this.orderQueue = new Queue<OrderData>();
this.isProcessingOrder = false;
if (pathMover != null)
{
pathMover.OnMoveCompleted += OnMoveCompleted;
}
}
public string CurrentMapId => currentMapId;
public string LastNodeId => lastNodeId;
public string OrderId => orderId;
public int LastNodeSequenceId => lastNodeSequenceId;
public OrderData CurrentOrder => currentOrder;
public string SerialNumber => serialNumber;
public async void ProcessOrderJson(string json)
{
try
{
OrderData order = await Task.Run(() => JsonConvert.DeserializeObject<OrderData>(json));
if (order.SerialNumber != serialNumber)
{
Debug.LogWarning($"SerialNumber trong JSON ({order.SerialNumber}) không khớp với serialNumber của robot ({serialNumber}).");
return;
}
lock (orderQueue)
{
orderQueue.Enqueue(order);
}
if (!isProcessingOrder)
{
UnityMainThreadDispatcher.Enqueue(() => ProcessNextOrder());
}
}
catch (Exception ex)
{
Debug.LogError($"Lỗi khi phân tích JSON: {ex.Message}, SerialNumber: {serialNumber}");
}
}
private void ProcessNextOrder()
{
if (isProcessingOrder)
{
Debug.Log($"Đang xử lý order, không thể bắt đầu order mới. SerialNumber: {serialNumber}");
return;
}
lock (orderQueue)
{
if (orderQueue.Count == 0)
{
Debug.Log($"Hàng đợi order trống. SerialNumber: {serialNumber}");
return;
}
isProcessingOrder = true;
currentOrder = orderQueue.Dequeue();
}
orderId = currentOrder.OrderId;
var lastNode = currentOrder.Nodes.OrderByDescending(n => n.SequenceId).FirstOrDefault();
lastNodeId = lastNode?.NodeId ?? "";
lastNodeSequenceId = lastNode?.SequenceId ?? 0;
string newMapId = currentOrder.Nodes.FirstOrDefault()?.NodePosition.MapId;
if (!string.IsNullOrEmpty(newMapId))
{
currentMapId = newMapId;
}
pathVisualizer.DrawPath(currentOrder.Nodes, currentOrder.Edges);
pathMover.StartMove(currentOrder.Nodes, currentOrder.Edges);
}
private void OnMoveCompleted()
{
Debug.Log($"Hoàn thành order. OrderId: {orderId}, SerialNumber: {serialNumber}");
lastNodeId = "";
orderId = "";
lastNodeSequenceId = 0;
currentOrder = null;
isProcessingOrder = false;
ProcessNextOrder();
}
public void CancelOrder()
{
pathMover.CancelMove();
pathVisualizer.ClearPath();
lastNodeId = "";
orderId = "";
lastNodeSequenceId = 0;
currentOrder = null;
lock (orderQueue)
{
orderQueue.Clear();
Debug.Log($"Đã hủy order hiện tại và xóa hàng đợi. SerialNumber: {serialNumber}");
}
isProcessingOrder = false;
Debug.Log($"Đã hủy nhiệm vụ. SerialNumber: {serialNumber}");
}
}

2
Assets/Scripting/Robot/OrderProcessor.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: 9461eea78d7ab1b42a75393b8636e54c

446
Assets/Scripting/Robot/PathMover.cs Normal file
View File

@@ -0,0 +1,446 @@
using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;
public class PathMover
{
private readonly Transform transform;
private readonly MonoBehaviour monoBehaviour;
private readonly float defaultSpeed;
private readonly float rotationSpeed;
private readonly int trajectoryResolution;
private readonly ActionExecutor actionExecutor;
private bool isMoving;
private volatile bool isCancelled;
private Coroutine moveCoroutine;
private float currentTheta;
private float currentVdaTheta;
private readonly float rotationThreshold = 0.05f; // Ngưỡng xoay 0.1 rad (~5.7°)
private bool isBackward = false; // Trạng thái đi lùi
public event Action OnMoveCompleted;
public PathMover(
Transform transform,
MonoBehaviour monoBehaviour,
float defaultSpeed,
int trajectoryResolution,
ActionExecutor actionExecutor,
float rotationSpeed = 180f)
{
this.transform = transform;
this.monoBehaviour = monoBehaviour;
this.defaultSpeed = defaultSpeed;
this.rotationSpeed = rotationSpeed;
this.trajectoryResolution = trajectoryResolution;
this.actionExecutor = actionExecutor;
isMoving = false;
isCancelled = false;
currentTheta = 0f;
currentVdaTheta = 0f;
isBackward = false;
}
public bool IsMoving => isMoving;
public float CurrentTheta => currentTheta;
public float CurrentVdaTheta => currentVdaTheta;
public void SetTheta(float vdaTheta)
{
currentVdaTheta = vdaTheta;
currentTheta = NormalizeTheta(-vdaTheta * Mathf.Rad2Deg);
transform.eulerAngles = new Vector3(0, -vdaTheta * Mathf.Rad2Deg, 0);
}
public void UpdateVdaThetaFromUnity(float eulerYDegrees)
{
currentVdaTheta = NormalizeAngle(-eulerYDegrees * Mathf.Deg2Rad);
currentTheta = NormalizeTheta(eulerYDegrees);
}
public void StartMove(Node[] nodes, Edge[] edges)
{
if (!isMoving)
{
isCancelled = false;
isBackward = false; // Reset trạng thái đi lùi
moveCoroutine = monoBehaviour.StartCoroutine(MoveAlongPath(nodes, edges));
Debug.Log("Bắt đầu di chuyển mới.");
}
else
{
Debug.LogWarning("AMR đang di chuyển, bỏ qua lệnh mới.");
}
}
public void CancelMove()
{
if (moveCoroutine != null)
{
monoBehaviour.StopCoroutine(moveCoroutine);
moveCoroutine = null;
Debug.Log("Đã dừng coroutine MoveAlongPath.");
}
isMoving = false;
isCancelled = true;
actionExecutor.CancelActions();
isBackward = false; // Reset trạng thái đi lùi
Debug.Log("Đã hủy di chuyển.");
}
private float RoundThetaToStandardAngle(float thetaRad)
{
float thetaDeg = thetaRad * Mathf.Rad2Deg;
float[] standardAngles = { 0f, 90f, 180f, 270f };
float roundingThreshold = 10f; // Ngưỡng làm tròn ±5°
float closestAngle = standardAngles.OrderBy(a => Mathf.Abs(Mathf.DeltaAngle(thetaDeg, a))).First();
if (Mathf.Abs(Mathf.DeltaAngle(thetaDeg, closestAngle)) <= roundingThreshold)
{
return closestAngle * Mathf.Deg2Rad;
}
return thetaRad;
}
private IEnumerator MoveAlongPath(Node[] nodes, Edge[] edges)
{
isMoving = true;
var sortedEdges = edges.OrderBy(e => e.SequenceId).ToList();
var sortedNodes = nodes.OrderBy(n => n.SequenceId).ToList();
bool isLastEdge = false;
// Xác định edge cong gần node cuối
var curveEdges = sortedEdges.Where(e => e.Trajectory.Degree > 1).ToList();
var lastCurveEdge = curveEdges.OrderByDescending(e => e.SequenceId).FirstOrDefault();
int lastCurveSequenceId = lastCurveEdge != null ? lastCurveEdge.SequenceId : -1;
// Xác định các node giao giữa đường thẳng và đường cong
HashSet<string> intersectionNodeIds = new();
for (int i = 0; i < sortedEdges.Count - 1; i++)
{
var currentEdge = sortedEdges[i];
var nextEdge = sortedEdges[i + 1];
bool isCurrentStraight = currentEdge.Trajectory.Degree == 1;
bool isNextCurve = nextEdge.Trajectory.Degree > 1;
bool isCurrentCurve = currentEdge.Trajectory.Degree > 1;
bool isNextStraight = nextEdge.Trajectory.Degree == 1;
if ((isCurrentStraight && isNextCurve) || (isCurrentCurve && isNextStraight))
{
intersectionNodeIds.Add(currentEdge.EndNodeId);
}
}
// Xử lý node đầu tiên
var firstNode = sortedNodes.FirstOrDefault(n => n.SequenceId == 0);
if (firstNode != null)
{
// Xoay theo theta của node đầu tiên
float vdaTheta = firstNode.NodePosition.Theta;
float thetaDiff = Mathf.Abs(NormalizeAngle(vdaTheta - currentVdaTheta));
if (thetaDiff > rotationThreshold)
{
currentVdaTheta = vdaTheta;
float unityEulerY = -vdaTheta * Mathf.Rad2Deg;
Quaternion targetRotation = Quaternion.Euler(0, unityEulerY, 0);
while (Quaternion.Angle(transform.rotation, targetRotation) > 1f)
{
transform.rotation = Quaternion.Slerp(transform.rotation, targetRotation, rotationSpeed * Time.deltaTime / 180f);
currentTheta = NormalizeTheta(Mathf.Atan2(transform.right.x, transform.forward.z));
yield return null;
}
transform.rotation = targetRotation;
}
// Thực thi hành động tại node đầu tiên
if (firstNode.Actions != null && firstNode.Actions.Length > 0)
{
yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteNodeActions(firstNode));
if (isCancelled) { isMoving = false; yield break; }
}
}
for (int edgeIndex = 0; edgeIndex < sortedEdges.Count; edgeIndex++)
{
var edge = sortedEdges[edgeIndex];
isLastEdge = (edgeIndex == sortedEdges.Count - 1);
if (isCancelled) { isMoving = false; yield break; }
if (edge.Trajectory?.ControlPoints == null || edge.Trajectory.ControlPoints.Length == 0)
continue;
var startNode = sortedNodes.FirstOrDefault(n => n.NodeId == edge.StartNodeId);
var endNode = sortedNodes.FirstOrDefault(n => n.NodeId == edge.EndNodeId);
// Thực thi hành động tại startNode
if (startNode != null && startNode.Actions != null && startNode.Actions.Length > 0)
{
yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteNodeActions(startNode));
if (isCancelled) { isMoving = false; yield break; }
}
// Kích hoạt đi lùi tại điểm bắt đầu của edge cong cuối
if (!isBackward && edge.SequenceId == lastCurveSequenceId)
{
isBackward = true;
}
// Thực thi các hành động của edge
foreach (var action in edge.Actions)
{
yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteEdgeAction(edge, action));
if (isCancelled) { isMoving = false; yield break; }
}
float speed = edge.MaxSpeed > 0 ? edge.MaxSpeed : defaultSpeed;
List<Vector3> points;
bool isCurve = edge.Trajectory.Degree > 1;
if (!isCurve)
{
points = new List<Vector3>
{
new Vector3(edge.Trajectory.ControlPoints[0].X, transform.position.y, edge.Trajectory.ControlPoints[0].Y),
new Vector3(edge.Trajectory.ControlPoints[1].X, transform.position.y, edge.Trajectory.ControlPoints[1].Y)
};
}
else
{
points = GenerateBSplinePoints(edge.Trajectory, trajectoryResolution);
if (points.Count == 0) continue;
var lastControlPoint = edge.Trajectory.ControlPoints.Last();
points[points.Count - 1] = new Vector3(lastControlPoint.X, transform.position.y, lastControlPoint.Y);
}
// Tính tổng quãng đường của edge
float totalDistance = 0f;
for (int i = 0; i < points.Count - 1; i++)
{
totalDistance += Vector3.Distance(points[i], points[i + 1]);
}
// Tính góc mục tiêu tại endNode, làm tròn ngay từ đầu cho edge cong
float endTheta = endNode != null ? endNode.NodePosition.Theta : currentVdaTheta;
if (isCurve && endNode != null)
{
endTheta = RoundThetaToStandardAngle(endTheta);
Debug.Log($"Bắt đầu edge cong {edge.EdgeId}: Tổng quãng đường={totalDistance:F2}m, " +
$"Góc chuẩn mục tiêu tại endNode={endTheta:F2} rad (eulerY={-endTheta * Mathf.Rad2Deg:F2}°)");
}
Quaternion endRotation = Quaternion.Euler(0, -endTheta * Mathf.Rad2Deg, 0);
for (int i = 0; i < points.Count - 1; i++)
{
if (isCancelled) { isMoving = false; yield break; }
Vector3 currentPoint = points[i];
Vector3 nextPoint = points[i + 1];
Vector3 forward = (nextPoint - currentPoint).normalized;
if (isBackward)
{
forward = -forward;
}
float distanceToNext = Vector3.Distance(transform.position, nextPoint);
float step = speed * Time.deltaTime;
while (distanceToNext > 0.01f)
{
if (isCancelled) { isMoving = false; yield break; }
transform.position = Vector3.MoveTowards(transform.position, nextPoint, step);
if (isCurve && forward != Vector3.zero)
{
// Tính tiến độ di chuyển trên toàn bộ edge
float remainingDistance = 0f;
for (int j = i; j < points.Count - 1; j++)
{
remainingDistance += Vector3.Distance(points[j], points[j + 1]);
}
float progress = 1f - (remainingDistance / totalDistance);
if (progress < 0) progress = 0;
if (progress > 1) progress = 1;
// Tính góc hiện tại từ vector forward
Quaternion tangentRotation = Quaternion.LookRotation(forward, Vector3.up);
tangentRotation *= Quaternion.Euler(0, -90, 0);
// Tính góc cần xoay và thời gian còn lại
float angleToTarget = Quaternion.Angle(tangentRotation, endRotation);
float timeRemaining = remainingDistance / speed;
float adjustedRotationSpeed = timeRemaining > 0 ? (angleToTarget / timeRemaining) : rotationSpeed;
adjustedRotationSpeed = Mathf.Max(adjustedRotationSpeed, rotationSpeed);
// Xoay dần về góc mục tiêu tại endNode (đã làm tròn)
Quaternion targetRotation = Quaternion.Slerp(tangentRotation, endRotation, progress);
float currentAngleToTarget = Quaternion.Angle(transform.rotation, targetRotation);
if (currentAngleToTarget > 0.1f)
{
transform.rotation = Quaternion.Slerp(transform.rotation, targetRotation, adjustedRotationSpeed * Time.deltaTime / 180f);
currentTheta = NormalizeTheta(Mathf.Atan2(transform.right.x, transform.forward.z));
currentVdaTheta = NormalizeAngle(-transform.eulerAngles.y * Mathf.Deg2Rad);
}
}
distanceToNext = Vector3.Distance(transform.position, nextPoint);
yield return null;
}
transform.position = nextPoint;
}
// Kiểm tra khớp góc tại endNode của edge cong
if (isCurve && endNode != null)
{
float roundedTheta = endTheta; // Đã làm tròn từ đầu
float thetaDiff = Mathf.Abs(NormalizeAngle(roundedTheta - currentVdaTheta));
}
// Xử lý endNode
if (endNode != null)
{
bool shouldRotate = false;
string rotateReason = "";
// Kiểm tra xem endNode có cần xoay không
if (isLastEdge)
{
shouldRotate = true;
rotateReason = $"node cuối của edge cuối {edge.EdgeId}";
}
else if (intersectionNodeIds.Contains(endNode.NodeId))
{
shouldRotate = true;
rotateReason = $"node giao giữa đường thẳng và đường cong, NodeId={endNode.NodeId}";
}
if (shouldRotate)
{
float vdaTheta = endNode.NodePosition.Theta;
float roundedTheta = RoundThetaToStandardAngle(vdaTheta); // Làm tròn theta cho node giao hoặc node cuối
float thetaDiffEnd = Mathf.Abs(NormalizeAngle(roundedTheta - currentVdaTheta));
if (thetaDiffEnd > rotationThreshold)
{
currentVdaTheta = roundedTheta;
float unityEulerY = -roundedTheta * Mathf.Rad2Deg;
Quaternion targetRotation = Quaternion.Euler(0, unityEulerY, 0);
while (Quaternion.Angle(transform.rotation, targetRotation) > 0.1f)
{
transform.rotation = Quaternion.Slerp(transform.rotation, targetRotation, 0.07f);
currentTheta = NormalizeTheta(Mathf.Atan2(transform.right.x, transform.forward.z));
yield return null;
}
transform.rotation = targetRotation;
}
}
// Thực thi hành động tại endNode
if (endNode.Actions != null && endNode.Actions.Length > 0)
{
yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteNodeActions(endNode));
if (isCancelled) { isMoving = false; yield break; }
}
}
// Tắt chế độ đi lùi
if (isBackward)
{
isBackward = false;
Debug.Log("Tắt chế độ đi lùi khi kết thúc order: isBackward=false");
}
}
isMoving = false;
OnMoveCompleted?.Invoke();
}
private float NormalizeTheta(float eulerYDegrees)
{
float theta = eulerYDegrees * Mathf.Deg2Rad;
while (theta > Mathf.PI) theta -= 2 * Mathf.PI;
while (theta < -Mathf.PI) theta += 2 * Mathf.PI;
return theta;
}
private float NormalizeAngle(float angleRad)
{
while (angleRad > Mathf.PI) angleRad -= 2 * Mathf.PI;
while (angleRad < -Mathf.PI) angleRad += 2 * Mathf.PI;
return angleRad;
}
private List<Vector3> GenerateBSplinePoints(Trajectory trajectory, int resolution)
{
List<Vector3> points = new();
int degree = trajectory.Degree;
float[] knots = trajectory.KnotVector;
ControlPoint[] controlPoints = trajectory.ControlPoints;
if (controlPoints == null || controlPoints.Length < degree + 1 || knots == null || knots.Length < controlPoints.Length + degree + 1)
return points;
float minU = knots[degree];
float maxU = knots[controlPoints.Length];
if (minU >= maxU) return points;
float minDistance = 0.1f;
Vector3? lastPoint = null;
for (int i = 0; i <= resolution; i++)
{
float u = minU + (maxU - minU) * i / (float)resolution;
Vector2 point = CalculateBSplinePoint(u, degree, knots, controlPoints);
if (point != Vector2.zero)
{
Vector3 pos = new(point.x, transform.position.y, point.y);
if (lastPoint == null || Vector3.Distance(pos, lastPoint.Value) >= minDistance)
{
points.Add(pos);
lastPoint = pos;
}
}
}
return points;
}
private Vector2 CalculateBSplinePoint(float u, int degree, float[] knots, ControlPoint[] controlPoints)
{
Vector2 point = Vector2.zero;
int n = controlPoints.Length - 1;
for (int i = 0; i <= n; i++)
{
float basis = CalculateBasisFunction(i, degree, u, knots);
point += new Vector2(controlPoints[i].X, controlPoints[i].Y) * basis;
}
return point;
}
private float CalculateBasisFunction(int i, int degree, float u, float[] knots)
{
if (degree == 0)
return (u >= knots[i] && u < knots[i + 1]) ? 1f : 0f;
float left = 0f;
float right = 0f;
float denom1 = knots[i + degree] - knots[i];
if (denom1 > 0)
left = ((u - knots[i]) / denom1) * CalculateBasisFunction(i, degree - 1, u, knots);
float denom2 = knots[i + degree + 1] - knots[i + 1];
if (denom2 > 0)
right = ((knots[i + degree + 1] - u) / denom2) * CalculateBasisFunction(i + 1, degree - 1, u, knots);
return left + right;
}
}

2
Assets/Scripting/Robot/PathMover.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: 0c6c40a66accdaf4ab1f9f45e95413fc

183
Assets/Scripting/Robot/PathVisualizer.cs Normal file
View File

@@ -0,0 +1,183 @@
using System.Collections.Generic;
using System.Linq;
using UnityEngine;
public class PathVisualizer
{
private readonly Material lineMaterial;
private readonly Material nodeMaterial;
private readonly Transform transform;
private readonly List<GameObject> nodeObjects = new();
private GameObject lineRendererObject;
private readonly int trajectoryResolution;
public PathVisualizer(Material lineMaterial, Material nodeMaterial, Transform transform, int trajectoryResolution)
{
this.lineMaterial = lineMaterial;
this.nodeMaterial = nodeMaterial;
this.transform = transform;
this.trajectoryResolution = trajectoryResolution;
}
public void DrawPath(Node[] nodes, Edge[] edges)
{
if (lineRendererObject != null)
{
Object.Destroy(lineRendererObject);
}
foreach (var node in nodeObjects)
{
if (node != null) Object.Destroy(node); // Kiểm tra null để an toàn
}
nodeObjects.Clear();
lineRendererObject = new GameObject("PathLineRenderer");
LineRenderer lineRenderer = lineRendererObject.AddComponent<LineRenderer>();
lineRenderer.material = lineMaterial != null ? lineMaterial : new Material(Shader.Find("Sprites/Default"));
lineRenderer.startWidth = 0.1f;
lineRenderer.endWidth = 0.1f;
lineRenderer.startColor = Color.green;
lineRenderer.endColor = Color.green;
var sortedEdges = edges.OrderBy(e => e.SequenceId).ToList();
List<Vector3> pathPoints = new();
foreach (var edge in sortedEdges)
{
if (edge.Trajectory == null || edge.Trajectory.ControlPoints == null || edge.Trajectory.ControlPoints.Length == 0)
{
continue;
}
if (edge.Trajectory.Degree == 1)
{
foreach (var point in edge.Trajectory.ControlPoints)
{
Vector3 pos = new(point.X, transform.position.y, point.Y);
pathPoints.Add(pos);
}
}
else
{
var points = GenerateBSplinePoints(edge.Trajectory, trajectoryResolution);
if (points.Count > 0)
{
var lastControlPoint = edge.Trajectory.ControlPoints.Last();
points[^1] = new Vector3(lastControlPoint.X, transform.position.y, lastControlPoint.Y);
pathPoints.AddRange(points);
}
}
}
pathPoints.RemoveAll(p => p.x == 0 && p.z == 0);
if (pathPoints.Count == 0)
{
Debug.LogError("Không có điểm hợp lệ để vẽ đường đi!");
lineRenderer.positionCount = 0;
return;
}
lineRenderer.positionCount = pathPoints.Count;
lineRenderer.SetPositions(pathPoints.ToArray());
foreach (var node in nodes.OrderBy(n => n.SequenceId))
{
GameObject nodeObject = GameObject.CreatePrimitive(PrimitiveType.Sphere);
nodeObject.transform.position = new Vector3(node.NodePosition.X, transform.position.y, node.NodePosition.Y);
nodeObject.transform.localScale = Vector3.one * 0.1f;
nodeObject.name = $"Node_{node.NodeDescription}";
nodeObject.GetComponent<Renderer>().material = nodeMaterial != null ? nodeMaterial : new Material(Shader.Find("Sprites/Default"));
nodeObject.GetComponent<Renderer>().material.color = Color.red;
nodeObjects.Add(nodeObject);
}
}
public void ClearPath()
{
if (lineRendererObject != null)
{
Object.Destroy(lineRendererObject);
lineRendererObject = null;
Debug.Log("Đã xóa lineRendererObject.");
}
foreach (var node in nodeObjects)
{
if (node != null) Object.Destroy(node); // Khôi phục lệnh xóa node
}
nodeObjects.Clear();
}
private List<Vector3> GenerateBSplinePoints(Trajectory trajectory, int resolution)
{
List<Vector3> points = new();
int degree = trajectory.Degree;
float[] knots = trajectory.KnotVector;
ControlPoint[] controlPoints = trajectory.ControlPoints;
if (controlPoints == null || controlPoints.Length < degree + 1 || knots == null || knots.Length < controlPoints.Length + degree + 1)
{
Debug.LogError($"Dữ liệu Trajectory không hợp lệ: Degree={degree}, ControlPoints={controlPoints?.Length}, Knots={knots?.Length}");
return points;
}
float minU = knots[degree];
float maxU = knots[controlPoints.Length];
if (minU >= maxU)
{
Debug.LogError($"KnotVector không hợp lệ: minU={minU}, maxU={maxU}");
return points;
}
for (int i = 0; i <= resolution; i++)
{
float u = minU + (maxU - minU) * i / (float)resolution;
Vector2 point = CalculateBSplinePoint(u, degree, knots, controlPoints);
if (point != Vector2.zero)
{
Vector3 pos = new(point.x, transform.position.y, point.y);
points.Add(pos);
}
}
return points;
}
private Vector2 CalculateBSplinePoint(float u, int degree, float[] knots, ControlPoint[] controlPoints)
{
Vector2 point = Vector2.zero;
int n = controlPoints.Length - 1;
for (int i = 0; i <= n; i++)
{
float basis = CalculateBasisFunction(i, degree, u, knots);
point += new Vector2(controlPoints[i].X, controlPoints[i].Y) * basis;
}
return point;
}
private float CalculateBasisFunction(int i, int degree, float u, float[] knots)
{
if (degree == 0)
{
return (u >= knots[i] && u < knots[i + 1]) ? 1f : 0f;
}
float left = 0f;
float right = 0f;
float denom1 = knots[i + degree] - knots[i];
if (denom1 > 0)
{
left = ((u - knots[i]) / denom1) * CalculateBasisFunction(i, degree - 1, u, knots);
}
float denom2 = knots[i + degree + 1] - knots[i + 1];
if (denom2 > 0)
{
right = ((knots[i + degree + 1] - u) / denom2) * CalculateBasisFunction(i + 1, degree - 1, u, knots);
}
return left + right;
}
}

2
Assets/Scripting/Robot/PathVisualizer.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: dacf3fe04ef90744e9393ad1c190e91e

20
Assets/Scripting/Robot/UnityMainThreadDispatcher.cs Normal file
View File

@@ -0,0 +1,20 @@
using System;
using System.Collections.Concurrent;
public static class UnityMainThreadDispatcher
{
private static readonly ConcurrentQueue<Action> _executionQueue = new();
public static void Update()
{
while (_executionQueue.TryDequeue(out var action))
{
action?.Invoke();
}
}
public static void Enqueue(Action action)
{
_executionQueue.Enqueue(action);
}
}

2
Assets/Scripting/Robot/UnityMainThreadDispatcher.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: 3d4079869a4ee5d42a7ece88bf10b684

251
Assets/Scripting/Robot/VisualizationSender.cs Normal file
View File

@@ -0,0 +1,251 @@
using Newtonsoft.Json;
using System;
using System.Linq;
using System.Threading.Tasks;
using UnityEngine;
public class VisualizationSender
{
private readonly MqttClientManager mqttClientManager;
private readonly OrderProcessor orderProcessor;
public readonly Transform transform;
private readonly string visualizationTopic;
private readonly string stateTopic;
public readonly string serialNumber;
private readonly string currentMapId;
private readonly PathMover pathMover;
public Vector3 velocity;
public float angularVelocity;
public VisualizationSender(
MqttClientManager mqttClientManager,
Transform transform,
OrderProcessor orderProcessor,
string visualizationTopic,
string stateTopic,
string serialNumber,
string currentMapId,
PathMover pathMover)
{
this.mqttClientManager = mqttClientManager;
this.orderProcessor = orderProcessor;
this.transform = transform;
this.visualizationTopic = visualizationTopic;
this.stateTopic = stateTopic;
this.serialNumber = serialNumber;
this.currentMapId = currentMapId;
this.pathMover = pathMover;
velocity = Vector3.zero;
angularVelocity = 0f;
//Debug.Log($"Khởi tạo VisualizationSender cho robot với serialNumber: {serialNumber}");
}
public void SetVelocity(Vector3 velocity, float angularVelocity)
{
this.velocity = velocity;
this.angularVelocity = angularVelocity;
}
// Hàm chuẩn hóa góc trong khoảng [-π, π]
public float NormalizeAngle(float angleRad)
{
while (angleRad > Mathf.PI) angleRad -= 2 * Mathf.PI;
while (angleRad < -Mathf.PI) angleRad += 2 * Mathf.PI;
return angleRad;
}
// Hàm chuẩn hóa góc độ trong khoảng [-180°, 180°]
public float NormalizeEulerAngle(float angleDeg)
{
while (angleDeg > 180f) angleDeg -= 360f;
while (angleDeg < -180f) angleDeg += 360f;
return angleDeg;
}
public async void SendVisualization()
{
try
{
if (transform == null)
{
Debug.LogError("Transform is null in SendVisualization.");
return;
}
Vector3 position = transform.position;
float theta = NormalizeAngle(2 * Mathf.PI - transform.eulerAngles.y * Mathf.Deg2Rad); // Công thức mới
Vector3 vel = velocity;
float omega = angularVelocity;
float displayEulerY = NormalizeEulerAngle(transform.eulerAngles.y); // Giữ nguyên hiển thị
string json = await Task.Run(() =>
{
var visualizationData = new VisualizationData
{
HeaderId = 1,
Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
Version = "1.0.0",
Manufacturer = "phenikaaX",
SerialNumber = serialNumber,
MapId = currentMapId,
MapDescription = string.IsNullOrEmpty(currentMapId) ? "" : "Default map",
AgvPosition = new AgvPosition
{
X = position.x,
Y = position.z,
MapId = currentMapId,
Theta = theta,
PositionInitialized = true,
LocalizationScore = 0.7f,
DeviationRange = 0.1f
},
Velocity = new Velocity
{
Vx = vel.x,
Vy = vel.z,
Omega = omega
}
};
return JsonConvert.SerializeObject(visualizationData, Formatting.None);
});
await mqttClientManager.PublishAsync(visualizationTopic, json);
}
catch (Exception ex)
{
Debug.LogError($"Lỗi khi gửi visualization cho robot với serialNumber {serialNumber}: {ex.Message}");
}
}
public async void SendState()
{
try
{
if (transform == null)
{
Debug.LogError("Transform is null in SendState.");
return;
}
Vector3 position = transform.position;
float theta = NormalizeAngle(2 * Mathf.PI - transform.eulerAngles.y * Mathf.Deg2Rad); // Công thức mới
Vector3 vel = velocity;
float omega = angularVelocity;
float displayEulerY = NormalizeEulerAngle(transform.eulerAngles.y); // Giữ nguyên hiển thị
string json = await Task.Run(() =>
{
var order = orderProcessor.CurrentOrder;
var nodeStates = order?.Nodes?.Select(n => new NodeState
{
NodeId = n.NodeId,
SequenceId = n.SequenceId,
NodeDescription = n.NodeDescription ?? "",
Released = n.Released,
NodePosition = new NodePosition
{
X = n.NodePosition.X,
Y = n.NodePosition.Y,
Theta = n.NodePosition.Theta, // Giữ nguyên theta từ JSON
AllowedDeviationXY = n.NodePosition.AllowedDeviationXY,
AllowedDeviationTheta = n.NodePosition.AllowedDeviationTheta,
MapId = n.NodePosition.MapId,
MapDescription = n.NodePosition.MapDescription ?? ""
}
}).ToArray() ?? new NodeState[0];
var edgeStates = order?.Edges?.Select(e => new EdgeState
{
EdgeId = e.EdgeId,
SequenceId = e.SequenceId,
EdgeDescription = e.EdgeDescription ?? "",
Released = e.Released,
Trajectory = e.Trajectory != null ? new Trajectory
{
Degree = e.Trajectory.Degree,
KnotVector = e.Trajectory.KnotVector,
ControlPoints = e.Trajectory.ControlPoints?.Select(cp => new ControlPoint
{
X = cp.X,
Y = cp.Y,
Weight = cp.Weight
}).ToArray()
} : null
}).ToArray() ?? new EdgeState[0];
var stateData = new SendState
{
HeaderId = 1,
Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
Version = "1.0.0",
Manufacturer = "PhenikaaX",
SerialNumber = serialNumber,
Maps = new Map[]
{
new()
{
MapId = currentMapId,
MapVersion = "1.0",
MapDescription = "Default map",
MapStatus = "ENABLED"
}
},
OrderId = orderProcessor.OrderId ?? "",
OrderUpdateId = orderProcessor.OrderId != null ? 1 : 0,
ZoneSetId = "",
LastNodeId = orderProcessor.LastNodeId ?? "",
LastNodeSequenceId = orderProcessor.LastNodeSequenceId,
NodeStates = nodeStates,
EdgeStates = edgeStates,
Driving = pathMover.IsMoving,
Paused = !pathMover.IsMoving,
NewBaseRequest = !pathMover.IsMoving,
DistanceSinceLastNode = 1.531483174223427f,
AgvPosition = new AgvPosition
{
X = position.x,
Y = position.z,
Theta = theta,
MapId = currentMapId,
MapDescription = "",
PositionInitialized = true,
LocalizationScore = 0.7f,
DeviationRange = 0.1f
},
Velocity = new Velocity
{
Vx = vel.x,
Vy = vel.z,
Omega = omega
},
Loads = new Load[0],
ActionStates = new ActionState[0],
BatteryState = new BatteryState
{
BatteryCharge = 80f,
BatteryVoltage = 48f,
BatteryHealth = 100f,
Charging = false,
Reach = 1000f
},
OperatingMode = "AUTOMATIC",
Errors = new Error[0],
Information = new Information[0],
SafetyState = new SafetyState
{
EStop = "NONE",
FieldViolation = false
}
};
return JsonConvert.SerializeObject(stateData, Formatting.None);
});
await mqttClientManager.PublishAsync(stateTopic, json);
}
catch (Exception ex)
{
Debug.LogError($"Lỗi khi gửi state cho robot với serialNumber {serialNumber}: {ex.Message}");
}
}
}

2
Assets/Scripting/Robot/VisualizationSender.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: 6ea3c60c5aa873b45806756378bf9466

8
Assets/Scripting/VDA5050.meta Normal file
View File

@@ -0,0 +1,8 @@
fileFormatVersion: 2
guid: e0187b5dc258a304d9a68d8809ba9bcf
folderAsset: yes
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

71
Assets/Scripting/VDA5050/InstantActionData.cs Normal file
View File

@@ -0,0 +1,71 @@
using Newtonsoft.Json;
public class InstantActionData
{
[JsonProperty("headerId")]
public int HeaderId { get; set; }
[JsonProperty("timestamp")]
public string Timestamp { get; set; }
[JsonProperty("version")]
public string Version { get; set; }
[JsonProperty("manufacturer")]
public string Manufacturer { get; set; }
[JsonProperty("serialNumber")]
public string SerialNumber { get; set; }
[JsonProperty("orderId")]
public string OrderId { get; set; }
[JsonProperty("orderUpdateId")]
public int OrderUpdateId { get; set; }
[JsonProperty("zoneSetId")]
public string ZoneSetId { get; set; }
[JsonProperty("actions")]
public ActionData[] Actions { get; set; }
}
public class ActionData
{
[JsonProperty("actionId")]
public string ActionId { get; set; }
[JsonProperty("actionType")]
public string ActionType { get; set; }
[JsonProperty("actionStatus")]
public string ActionStatus { get; set; }
[JsonProperty("blockingType")]
public string BlockingType { get; set; }
[JsonProperty("resultDescription")]
public string ActionDescription { get; set; }
[JsonProperty("actionParameters")]
public Parameter[] Parameters { get; set; } // Sửa thành actionParameters
}
public class Parameter
{
[JsonProperty("key")]
public string Key { get; set; }
[JsonProperty("value")]
public object Value { get; set; }
}
// Helper class for result passing
public class ExecutionResult
{
public bool ActionResult { get; set; }
public string ActionStatus { get; set; }
public string ResultDescription { get; set; }
}
public class NodePosition
{
[JsonProperty("x")]
public float X { get; set; }
[JsonProperty("y")]
public float Y { get; set; }
[JsonProperty("theta")]
public float Theta { get; set; }
[JsonProperty("allowedDeviationXY")]
public float AllowedDeviationXY { get; set; }
[JsonProperty("allowedDeviationTheta")]
public float AllowedDeviationTheta { get; set; }
[JsonProperty("mapId")]
public string MapId { get; set; }
[JsonProperty("mapDescription")]
public string MapDescription { get; set; }
}

2
Assets/Scripting/VDA5050/InstantActionData.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: e2a442f6c19ccd84ea7c06ada19f8861

73
Assets/Scripting/VDA5050/OrderData.cs Normal file
View File

@@ -0,0 +1,73 @@
using Newtonsoft.Json;
public class OrderData
{
public uint HeaderId { get; set; }
public string Timestamp { get; set; }
public string Version { get; set; }
public string Manufacturer { get; set; }
public string SerialNumber { get; set; }
public string OrderId { get; set; }
public int OrderUpdateId { get; set; }
public string ZoneSetId { get; set; }
[JsonProperty("nodes")]
public Node[] Nodes { get; set; }
[JsonProperty("edges")]
public Edge[] Edges { get; set; }
}
public class Node
{
[JsonProperty("nodeId")]
public string NodeId { get; set; }
[JsonProperty("sequenceId")]
public int SequenceId { get; set; }
[JsonProperty("nodeDescription")]
public string NodeDescription { get; set; }
[JsonProperty("released")]
public bool Released { get; set; }
[JsonProperty("nodePosition")]
public NodePosition NodePosition { get; set; }
[JsonProperty("actions")]
public ActionData[] Actions { get; set; }
}
public class Edge
{
[JsonProperty("edgeId")]
public string EdgeId { get; set; }
[JsonProperty("sequenceId")]
public int SequenceId { get; set; }
[JsonProperty("edgeDescription")]
public string EdgeDescription { get; set; }
[JsonProperty("released")]
public bool Released { get; set; }
[JsonProperty("startNodeId")]
public string StartNodeId { get; set; }
[JsonProperty("endNodeId")]
public string EndNodeId { get; set; }
[JsonProperty("maxSpeed")]
public float MaxSpeed { get; set; }
[JsonProperty("minSpeed")]
public float MaxHeight { get; set; }
[JsonProperty("minHeight")]
public float MinHeight { get; set; }
[JsonProperty("orientation")]
public float Orientation { get; set; }
[JsonProperty("orientationType")]
public string OrientationType { get; set; }
[JsonProperty("direction")]
public string Direction { get; set; }
[JsonProperty("rotationAllowed")]
public bool RotationAllowed { get; set; }
[JsonProperty("maxRotationSpeed")]
public float MaxRotationSpeed { get; set; }
[JsonProperty("length")]
public float Length { get; set; }
[JsonProperty("trajectory")]
public Trajectory Trajectory { get; set; }
[JsonProperty("actions")]
public ActionData[] Actions { get; set; }
}

2
Assets/Scripting/VDA5050/OrderData.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: 6e0c7846a41d5bc4092689d19463ff58

288
Assets/Scripting/VDA5050/RobotVDA5050State.cs Normal file
View File

@@ -0,0 +1,288 @@
using Newtonsoft.Json;
using System;
public class SendState
{
[JsonProperty("headerId")]
public uint HeaderId { get; set; }
[JsonProperty("timestamp")]
public string Timestamp { get; set; }
[JsonProperty("version")]
public string Version { get; set; }
[JsonProperty("manufacturer")]
public string Manufacturer { get; set; }
[JsonProperty("serialNumber")]
public string SerialNumber { get; set; }
[JsonProperty("maps")]
public Map[] Maps { get; set; }
[JsonProperty("orderId")]
public string OrderId { get; set; }
[JsonProperty("orderUpdateId")]
public int OrderUpdateId { get; set; }
[JsonProperty("zoneSetId")]
public string ZoneSetId { get; set; }
[JsonProperty("lastNodeId")]
public string LastNodeId { get; set; }
[JsonProperty("lastNodeSequenceId")]
public int LastNodeSequenceId { get; set; }
[JsonProperty("nodeStates")]
public NodeState[] NodeStates { get; set; }
[JsonProperty("edgeStates")]
public EdgeState[] EdgeStates { get; set; }
[JsonProperty("driving")]
public bool Driving { get; set; }
[JsonProperty("paused")]
public bool Paused { get; set; }
[JsonProperty("newBaseRequest")]
public bool NewBaseRequest { get; set; }
[JsonProperty("distanceSinceLastNode")]
public float DistanceSinceLastNode { get; set; }
[JsonProperty("agvPosition")]
public AgvPosition AgvPosition { get; set; }
[JsonProperty("velocity")]
public Velocity Velocity { get; set; }
[JsonProperty("loads")]
public Load[] Loads { get; set; }
[JsonProperty("actionStates")]
public ActionState[] ActionStates { get; set; }
[JsonProperty("batteryState")]
public BatteryState BatteryState { get; set; }
[JsonProperty("operatingMode")]
public string OperatingMode { get; set; }
[JsonProperty("errors")]
public Error[] Errors { get; set; }
[JsonProperty("information")]
public Information[] Information { get; set; }
[JsonProperty("safetyState")]
public SafetyState SafetyState { get; set; }
}
public class CancelConfirmationData
{
[JsonProperty("headerId")]
public int HeaderId { get; set; }
[JsonProperty("timestamp")]
public string Timestamp { get; set; }
[JsonProperty("version")]
public string Version { get; set; }
[JsonProperty("manufacturer")]
public string Manufacturer { get; set; }
[JsonProperty("serialNumber")]
public string SerialNumber { get; set; }
[JsonProperty("actionStates")]
public ActionState[] ActionStates { get; set; }
}
public class Map
{
[JsonProperty("mapId")]
public string MapId { get; set; }
[JsonProperty("mapVersion")]
public string MapVersion { get; set; }
[JsonProperty("mapDescription")]
public string MapDescription { get; set; }
[JsonProperty("mapStatus")]
public string MapStatus { get; set; }
}
public class NodeState
{
[JsonProperty("nodeId")]
public string NodeId { get; set; }
[JsonProperty("sequenceId")]
public int SequenceId { get; set; }
[JsonProperty("nodeDescription")]
public string NodeDescription { get; set; }
[JsonProperty("released")]
public bool Released { get; set; }
[JsonProperty("nodeStatus")]
public NodePosition NodePosition { get; set; }
}
public class EdgeState
{
[JsonProperty("edgeId")]
public string EdgeId { get; set; }
[JsonProperty("sequenceId")]
public int SequenceId { get; set; }
[JsonProperty("edgeDescription")]
public string EdgeDescription { get; set; }
[JsonProperty("released")]
public bool Released { get; set; }
[JsonProperty("trajectory")]
public Trajectory Trajectory { get; set; }
}
public class AgvPosition
{
[JsonProperty("x")]
public float X { get; set; }
[JsonProperty("y")]
public float Y { get; set; }
[JsonProperty("theta")]
public float Theta { get; set; }
[JsonProperty("mapId")]
public string MapId { get; set; }
[JsonProperty("mapDescription")]
public string MapDescription { get; set; }
[JsonProperty("positionInitialized")]
public bool PositionInitialized { get; set; }
[JsonProperty("positionStatus")]
public float LocalizationScore { get; set; }
[JsonProperty("deviationRange")]
public float DeviationRange { get; set; }
}
public class Velocity
{
[JsonProperty("vx")]
public float Vx { get; set; }
[JsonProperty("vy")]
public float Vy { get; set; }
[JsonProperty("omega")]
public float Omega { get; set; }
}
public class Load
{
[JsonProperty("loadId")]
public string LoadId { get; set; }
[JsonProperty("loadType")]
public string LoadType { get; set; }
[JsonProperty("loadPosition")]
public string LoadPosition { get; set; }
[JsonProperty("boundingBoxReference")]
public BoundingBoxReference BoundingBoxReference { get; set; }
[JsonProperty("loadDimensions")]
public LoadDimensions LoadDimensions { get; set; }
[JsonProperty("weight")]
public float Weight { get; set; }
}
public class BoundingBoxReference
{
[JsonProperty("x")]
public float X { get; set; }
[JsonProperty("y")]
public float Y { get; set; }
[JsonProperty("z")]
public float Z { get; set; }
[JsonProperty("theta")]
public float Theta { get; set; }
}
public class LoadDimensions
{
[JsonProperty("length")]
public float Length { get; set; }
[JsonProperty("width")]
public float Width { get; set; }
[JsonProperty("height")]
public float Height { get; set; }
}
public class ActionState
{
[JsonProperty("actionId")]
public string ActionId { get; set; }
[JsonProperty("actionType")]
public string ActionType { get; set; }
[JsonProperty("actionDescription")]
public string ActionDescription { get; set; }
[JsonProperty("actionStatus")]
public string ActionStatus { get; set; }
[JsonProperty("resultDescription")]
public string ResultDescription { get; set; }
}
public class BatteryState
{
[JsonProperty("batteryCharge")]
public float BatteryCharge { get; set; }
[JsonProperty("batteryVoltage")]
public float BatteryVoltage { get; set; }
[JsonProperty("batteryHealth")]
public float BatteryHealth { get; set; }
[JsonProperty("charging")]
public bool Charging { get; set; }
[JsonProperty("reach")]
public float Reach { get; set; }
}
public class Error
{
[JsonProperty("errorType")]
public string ErrorType { get; set; }
[JsonProperty("errorReferences")]
public ErrorReference[] ErrorReferences { get; set; }
[JsonProperty("errorDescription")]
public string ErrorDescription { get; set; }
[JsonProperty("errorHint")]
public string ErrorHint { get; set; }
[JsonProperty("errorLevel")]
public string ErrorLevel { get; set; }
}
public class ErrorReference
{
[JsonProperty("referenceKey")]
public string ReferenceKey { get; set; }
[JsonProperty("referenceValue")]
public string ReferenceValue { get; set; }
}
public class Information
{
[JsonProperty("infoType")]
public string InfoType { get; set; }
[JsonProperty("infoReferences")]
public InfoReference[] InfoReferences { get; set; }
[JsonProperty("infoDescription")]
public string InfoDescription { get; set; }
[JsonProperty("infoLevel")]
public string InfoLevel { get; set; }
}
public class InfoReference
{
[JsonProperty("referenceKey")]
public string ReferenceKey { get; set; }
[JsonProperty("referenceValue")]
public string ReferenceValue { get; set; }
}
public class SafetyState
{
[JsonProperty("eStop")]
public string EStop { get; set; }
[JsonProperty("fieldViolation")]
public bool FieldViolation { get; set; }
}
public class Trajectory
{
[JsonProperty("degree")]
public int Degree { get; set; }
[JsonProperty("knotVector")]
public float[] KnotVector { get; set; }
[JsonProperty("controlPoints")]
public ControlPoint[] ControlPoints { get; set; }
}
public class ControlPoint
{
[JsonProperty("x")]
public float X { get; set; }
[JsonProperty("y")]
public float Y { get; set; }
[JsonProperty("weight")]
public float Weight { get; set; }
}

2
Assets/Scripting/VDA5050/RobotVDA5050State.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: f0c532063e68c354f81c477aaedb7155

23
Assets/Scripting/VDA5050/VisualizationData.cs Normal file
View File

@@ -0,0 +1,23 @@
using Newtonsoft.Json;
public class VisualizationData
{
[JsonProperty("headerId")]
public uint HeaderId { get; set; }
[JsonProperty("timestamp")]
public string Timestamp { get; set; }
[JsonProperty("version")]
public string Version { get; set; }
[JsonProperty("manufacturer")]
public string Manufacturer { get; set; }
[JsonProperty("serialNumber")]
public string SerialNumber { get; set; }
[JsonProperty("mapId")]
public string MapId { get; set; }
[JsonProperty("mapDescription")]
public string MapDescription { get; set; }
[JsonProperty("agvPosition")]
public AgvPosition AgvPosition { get; set; }
[JsonProperty("velocity")]
public Velocity Velocity { get; set; }
}

2
Assets/Scripting/VDA5050/VisualizationData.cs.meta Normal file
View File

@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: e53ab94bcee9ea449a2d99fa130300c6