240 lines
8.1 KiB
C#
240 lines
8.1 KiB
C#
using System;
|
|
using System.Collections;
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
|
|
public class RobotManager : MonoBehaviour
|
|
{
|
|
public static RobotManager Instance { get; private set; }
|
|
|
|
UIManager uiManager => UIManager.Instance;
|
|
|
|
[Header("MQTT Settings")]
|
|
[SerializeField] private MqttManager mqttManager;
|
|
|
|
[Header("Robot Config")]
|
|
[SerializeField] private TextAsset robotIDconfig;
|
|
[SerializeField] private TextAsset robotActionTopics;
|
|
[SerializeField] private HybridInverseKinematicsNode hybridIKNode;
|
|
|
|
[Header("UI Settings")]
|
|
[Tooltip("각 로봇에 대해 생성할 UI 프리팹")]
|
|
[SerializeField] private GameObject robotUIPrefab;
|
|
[Tooltip("생성된 UI들이 위치할 부모 Canvas")]
|
|
[SerializeField] private Transform uiParentCanvas;
|
|
|
|
// --- MQTT Topic 관리 ---
|
|
private RobotIDConfig idConfigData;
|
|
private RobotActionTopics actionTopicsData;
|
|
|
|
// --- 로봇 관리 ---
|
|
private readonly Dictionary<string, RobotController> robotRegistry = new Dictionary<string, RobotController>();// 로봇 ID를 키로 하여 관리
|
|
private string targetRobotId; // 현재 조작 중인 로봇 ID (가상 = 현실)
|
|
private Dictionary<string, GameObject> robotUIs = new Dictionary<string, GameObject>();
|
|
|
|
// --- 로봇 선택 및 기록 관리 ---
|
|
private RobotController selectedRobot; // 현재 선택된 로봇
|
|
private Transform RecordingHandleTarget; // Handle(잡는 부분)의 Transform
|
|
public static event Action<RobotController> OnRobotSelected; // 로봇 선택 이벤트
|
|
|
|
private void Awake()
|
|
{
|
|
if (Instance == null)
|
|
{
|
|
Instance = this;
|
|
DontDestroyOnLoad(gameObject);
|
|
}
|
|
else
|
|
{
|
|
Destroy(gameObject);
|
|
}
|
|
|
|
InitializeRobotRegistry();
|
|
ManageConfig();
|
|
}
|
|
|
|
// 이벤트 구독 관리
|
|
private void OnEnable()
|
|
{
|
|
MqttManager.OnMessageReceived += HandleMessage; // 메시지 수신 이벤트 구독
|
|
UIController.OnTransformSendUIClicked += HandlePublishTransformCommand;
|
|
UIController.OnKeyframesSendUIClicked += HandlePublishKeyframeListCommand;
|
|
}
|
|
|
|
private void OnDisable()
|
|
{
|
|
MqttManager.OnMessageReceived -= HandleMessage;
|
|
UIController.OnTransformSendUIClicked -= HandlePublishTransformCommand;
|
|
UIController.OnKeyframesSendUIClicked -= HandlePublishKeyframeListCommand;
|
|
}
|
|
|
|
void Start()
|
|
{
|
|
InstantiateRobotUIs();
|
|
}
|
|
|
|
private void InitializeRobotRegistry()
|
|
{
|
|
RobotController[] allRobots = FindObjectsByType<RobotController>(FindObjectsSortMode.None); // 비활성화 된 건 빼고 찾기
|
|
foreach (var robot in allRobots)
|
|
{
|
|
if (!robotRegistry.ContainsKey(robot.robotId))
|
|
{
|
|
robotRegistry.Add(robot.robotId, robot);
|
|
}
|
|
else
|
|
{
|
|
Debug.LogWarning($"로봇 ID {robot.robotId}가 중복되었습니다. 첫 번째 로봇만 등록됩니다.");
|
|
}
|
|
}
|
|
}
|
|
|
|
private void InstantiateRobotUIs()
|
|
{
|
|
if (robotUIPrefab == null || uiParentCanvas == null)
|
|
{
|
|
Debug.LogError("Robot UI Prefab 또는 UI Parent Canvas가 설정되지 않았습니다.");
|
|
return;
|
|
}
|
|
foreach (var entry in robotRegistry)
|
|
{
|
|
RobotController robot = entry.Value;
|
|
// '가상 로봇 (트윈)'에 대해서만 UI를 생성
|
|
if (!robot.isLocallyControlled)
|
|
{
|
|
GameObject uiInstance = Instantiate(robotUIPrefab, uiParentCanvas);
|
|
uiInstance.name = $"UI_{robot.robotId}";
|
|
|
|
// 기존 UI Prefab은 비활성화
|
|
robotUIPrefab.SetActive(false);
|
|
|
|
UIController uiController = uiInstance.GetComponent<UIController>();
|
|
if (uiController != null)
|
|
{
|
|
uiController.Initialize(this, uiManager);
|
|
robotUIs.Add(robot.robotId, uiInstance);
|
|
}
|
|
uiInstance.SetActive(false);
|
|
}
|
|
}
|
|
}
|
|
|
|
public void SelectRobot(string robotId)
|
|
{
|
|
if (robotRegistry.TryGetValue(robotId, out RobotController robot))
|
|
{
|
|
selectedRobot = robot;
|
|
targetRobotId = robotId; // 현재 조작 중인 로봇 ID 설정
|
|
Debug.Log($"로봇 '{robotId}' 선택됨. 현재 조작 중인 로봇 ID로 설정.");
|
|
|
|
// 기록할 Handle Target 설정
|
|
RecordingHandleTarget = GameObject.FindWithTag("Handle")?.transform;
|
|
|
|
// 모든 UI 비활성화
|
|
foreach (var ui in robotUIs.Values) ui.SetActive(false);
|
|
|
|
// 선택된 로봇의 UI만 활성화
|
|
if (robotUIs.TryGetValue(robotId, out GameObject selectedUI))
|
|
{
|
|
selectedUI.SetActive(true);
|
|
}
|
|
|
|
OnRobotSelected?.Invoke(selectedRobot); // UIManager의 HandleRobotSelected 이벤트 호출
|
|
SubscribeToTopic(new string[] { $"robots/{targetRobotId}/ {actionTopicsData.Topic1}" }); // Topic1: state
|
|
|
|
HandlePublishTransformCommand(); // 선택 시 한 번 동기화
|
|
}
|
|
}
|
|
|
|
#region Publishing Logic
|
|
|
|
private void ManageConfig()
|
|
{
|
|
idConfigData = JsonUtility.FromJson<RobotIDConfig>(robotIDconfig.text);
|
|
actionTopicsData = JsonUtility.FromJson<RobotActionTopics>(robotActionTopics.text);
|
|
}
|
|
|
|
// 가상 로봇을 조작하여 현실 로봇에게 명령 발행 (UI 사용해서)
|
|
public void HandlePublishTransformCommand()
|
|
{
|
|
Debug.Log("TransformSendUI 이벤트 실행되어 HandlePublishTransformCommand 불러옴");
|
|
Vector3 commandPosition = RecordingHandleTarget.transform.localPosition;
|
|
List<Vector3> commandJoints = hybridIKNode.GetCurrentJointPositions();
|
|
List<Quaternion> commandRotations = hybridIKNode.GetCurrentJointRotations();
|
|
|
|
string topic = $"robots/{targetRobotId}/{actionTopicsData.Topic2}"; // Topic2: command
|
|
Debug.Log($"topic: {topic}");
|
|
|
|
IKData data = new IKData
|
|
{
|
|
robotId = targetRobotId, // 로봇 ID (가상 = 현실)
|
|
position = commandPosition,
|
|
nodesPosition = new List<Vector3>(commandJoints),
|
|
nodesRotation = new List<Quaternion>(commandRotations)
|
|
};
|
|
string json = JsonUtility.ToJson(data, true);
|
|
mqttManager.PublishMessage(topic, json);
|
|
Debug.Log($"위치 동기화 명령 발행: {json}");
|
|
}
|
|
|
|
public void HandlePublishKeyframeListCommand()
|
|
{
|
|
Debug.Log("ListSendUI 이벤트 실행되어 HandlePublishKeyframeListCommand 불러옴");
|
|
IReadOnlyList<IKData> keyframePositions = UIManager.Instance.KeyframePositions;
|
|
|
|
|
|
//List<Vector3> commandJoints = hybridIKNode.GetJointPositionsForKeyframe();
|
|
// 리스트를 메시지로 publish
|
|
if (keyframePositions.Count > 0)
|
|
{
|
|
string topic = $"robots/{targetRobotId}/{actionTopicsData.Topic2}"; // Topic2: command
|
|
Debug.Log($"topic: {topic}");
|
|
|
|
IKDataListWrapper wrapper = new IKDataListWrapper
|
|
{
|
|
robotId = targetRobotId,
|
|
keyFrameList = new List<IKData>(keyframePositions),
|
|
//nodesList = commandJoints
|
|
};
|
|
string json = JsonUtility.ToJson(wrapper, true);
|
|
mqttManager.PublishMessage(topic, json);
|
|
Debug.Log($"키프레임 리스트 명령 발행: {json}");
|
|
}
|
|
}
|
|
|
|
private void HandleMessage(string topic, string message)
|
|
{
|
|
string[] topicLevels = topic.Split('/');
|
|
if (topicLevels.Length < 3) return;
|
|
|
|
string robotId = topicLevels[1];
|
|
string messageType = topicLevels[2];
|
|
|
|
// ID에 해당하는 로봇이 있는지 확인
|
|
if (!robotRegistry.TryGetValue(robotId, out RobotController targetRobot))
|
|
{
|
|
return;
|
|
}
|
|
|
|
//HandleStateUpdate(targetRobot, message); // TODO. 가상 트윈의 위치 업데이트 -> 실제 로봇에서 버튼 클릭하면 동기화 하기로.
|
|
}
|
|
|
|
private void HandleStateUpdate(RobotController twinRobot, string message) // 가상 <- 현실
|
|
{
|
|
IKData data = JsonUtility.FromJson<IKData>(message);
|
|
twinRobot.transform.localPosition = data.position;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Subscribing Logic
|
|
|
|
private void SubscribeToTopic(string[] topics)
|
|
{
|
|
// MqttManager에서 구독 관리
|
|
mqttManager.SubscribeToTopics(topics);
|
|
}
|
|
|
|
#endregion
|
|
}
|