diff --git a/Assets/IK/InverseKinematicsController.cs b/Assets/IK/InverseKinematicsController.cs index 5cb9318..8c3df7c 100644 --- a/Assets/IK/InverseKinematicsController.cs +++ b/Assets/IK/InverseKinematicsController.cs @@ -198,7 +198,7 @@ public class InverseKinematicsController : MonoBehaviour } joint.currentAngle = desiredAngle; - joint.servo.SetAngle(desiredAngle); + joint.servo.SetAngleAndSendControlLibrary(desiredAngle, 1f); endEffectorPos = endEffector.position; diff --git a/Assets/Module/DiscoveryButton.cs b/Assets/Module/DiscoveryButton.cs index 1265e75..e9698fe 100644 --- a/Assets/Module/DiscoveryButton.cs +++ b/Assets/Module/DiscoveryButton.cs @@ -3,6 +3,8 @@ using UnityEngine; public class DiscoveryButton : MonoBehaviour { public TopologyBuilder topologyBuilder; + [Tooltip("If set, camera focuses on GeneratedTopology after Discover. If null, will FindObjectOfType.")] + public UserCameraControl cameraController; void Start() { @@ -21,6 +23,7 @@ public class DiscoveryButton : MonoBehaviour { Debug.Log("Discovery button pressed."); topologyBuilder.BuildTopologyFromJson(); + FocusCameraOnTopology(); } else { @@ -28,6 +31,13 @@ public class DiscoveryButton : MonoBehaviour } } + void FocusCameraOnTopology() + { + UserCameraControl cam = cameraController != null ? cameraController : FindObjectOfType(); + if (cam != null) + cam.FindModuleStructure(); + } + void OnDestroy() { Debug.Log("Cleaning up native resources"); diff --git a/Assets/Module/TopologyBuilder.cs b/Assets/Module/TopologyBuilder.cs index d81ad4d..c814ec0 100644 --- a/Assets/Module/TopologyBuilder.cs +++ b/Assets/Module/TopologyBuilder.cs @@ -4,59 +4,122 @@ using System; using System.Linq; using Frontend; - +[System.Serializable] +public class JsonTopologyGraph { public List Modules; public List Connections; } +[System.Serializable] +public class JsonModuleData { public string Id; public string Type; public float Degree; } +[System.Serializable] +public class JsonConnection { public string FromModuleId; public string ToModuleId; public string FromSocket; public string ToSocket; public int Orientation; } public class TopologyBuilder : MonoBehaviour { public ModuleSpawner spawner; + [Header("Topology Selection")] + [Tooltip("Set to true to load from JSON file (for local testing). Set to false to use ControlLibrary.")] + public bool useJsonFile = false; + [Tooltip("When true, angle commands are not sent to native ControlLibrary - use only for local testing without hardware.")] + public bool skipControlLibraryCalls = false; + [Tooltip("JSON file name (without .json) in Resources folder. e.g. 'mockData' or 'mockDataSimple'")] + public string jsonFileName = "mockData"; + public static Dictionary idToInstance = new(); + public static bool _skipControlLibraryCalls = false; + + public static bool SkipControlLibraryCalls => _skipControlLibraryCalls; public void BuildTopologyFromJson() { - - // todo: this is some really bad temporary code TopologyGraph graph = new TopologyGraph(); - RobotConfiguration config = ControlLibrary.getRobotConfiguration(); - Dictionary idToType = new(); - int moduleCount = config.ModulesLength; - for (int i = 0; i < moduleCount; i++) + + if (useJsonFile) { - var n_module = config.Modules(i); - if (n_module != null) + TextAsset jsonAsset = Resources.Load(jsonFileName); + if (jsonAsset == null) { Debug.LogError($"JSON not found: Resources/{jsonFileName}.json"); return; } + JsonTopologyGraph jsonGraph = JsonUtility.FromJson(jsonAsset.text); + if (jsonGraph?.Modules == null || jsonGraph.Modules.Count == 0) { Debug.LogError("JSON has no modules"); return; } + + Dictionary stringToIntId = new Dictionary(); + int nextIntId = 1; + foreach (var m in jsonGraph.Modules) { - var module = n_module.Value; - Debug.Log("Adding module " + module.Id); - graph.Modules.Add(new ModuleData - { - Id = module.Id, - Type = module.ModuleType.ToString(), - Degree = module.ConfigurationAsMotorState().Angle - }); - idToType.Add(module.Id, module.ModuleType); + if (!stringToIntId.ContainsKey(m.Id)) stringToIntId[m.Id] = nextIntId++; + int intId = stringToIntId[m.Id]; + ModuleType mt = ParseModuleType(m.Type); + idToType[intId] = mt; + graph.Modules.Add(new ModuleData { Id = intId, Type = mt.ToString(), Degree = m.Degree }); } + if (jsonGraph.Connections != null) + foreach (var c in jsonGraph.Connections) + { + if (!stringToIntId.TryGetValue(c.FromModuleId, out int fromId) || !stringToIntId.TryGetValue(c.ToModuleId, out int toId)) continue; + Orientation o = c.Orientation == 90 ? Orientation.Deg90 : c.Orientation == 180 ? Orientation.Deg180 : c.Orientation == 270 ? Orientation.Deg270 : Orientation.Deg0; + graph.Connections.Add(new Connection { FromModuleId = fromId, ToModuleId = toId, FromSocket = c.FromSocket ?? "MaleSocket", ToSocket = c.ToSocket ?? "FemaleSocket", Orientation = o }); + } + Debug.Log($"[TopologyBuilder] Built from JSON: {jsonFileName}"); + _skipControlLibraryCalls = skipControlLibraryCalls; + } + else + { + RobotConfiguration config = ControlLibrary.getRobotConfiguration(); + int moduleCount = config.ModulesLength; + for (int i = 0; i < moduleCount; i++) + { + var n_module = config.Modules(i); + if (n_module != null) + { + var module = n_module.Value; + Debug.Log("Adding module " + module.Id); + graph.Modules.Add(new ModuleData + { + Id = module.Id, + Type = module.ModuleType.ToString(), + Degree = module.ConfigurationAsMotorState().Angle + }); + idToType.Add(module.Id, module.ModuleType); + } + } + int connectionCount = config.ConnectionsLength; + for (int i = 0; i < connectionCount; i++) + { + var n_connection = config.Connections(i); + if (n_connection != null) + { + var connection = n_connection.Value; + Debug.Log("orientation: " + connection.Orientation); + graph.Connections.Add(new Connection + { + FromModuleId = connection.FromModuleId, + FromSocket = idToType[connection.FromModuleId] == ModuleType.SPLITTER ? "MaleSocket" + (connection.FromSocket == 0 ? "" : connection.FromSocket) : "MaleSocket", + ToModuleId = connection.ToModuleId, + ToSocket = "FemaleSocket", + Orientation = connection.Orientation + }); + } + } + _skipControlLibraryCalls = false; } - int connectionCount = config.ConnectionsLength; - for (int i = 0; i < connectionCount; i++) - { - var n_connection = config.Connections(i); - if (n_connection != null) - { - var connection = n_connection.Value; - Debug.Log("orientation: " + connection.Orientation); - graph.Connections.Add(new Connection - { - FromModuleId = connection.FromModuleId, - FromSocket = idToType[connection.FromModuleId] == ModuleType.SPLITTER ? "MaleSocket" + (connection.FromSocket == 0 ? "" : connection.FromSocket) : "MaleSocket", - ToModuleId = connection.ToModuleId, - ToSocket = "FemaleSocket", - Orientation = connection.Orientation - }); - } - } + BuildTopologyFromGraph(graph); + } + private static ModuleType ParseModuleType(string typeString) + { + if (string.IsNullOrEmpty(typeString)) return ModuleType.SPLITTER; + switch (typeString) + { + case "Servo1": return ModuleType.SERVO_1; + case "Servo2": return ModuleType.SERVO_2; + case "DC": return ModuleType.DC_MOTOR; + case "Hub": return ModuleType.SPLITTER; + case "Battery": return ModuleType.BATTERY; + default: return ModuleType.SPLITTER; + } + } + + private void BuildTopologyFromGraph(TopologyGraph graph) + { // Destroy previous topology root if it exists GameObject oldRoot = GameObject.Find("GeneratedTopology"); if (oldRoot != null) @@ -101,10 +164,11 @@ public class TopologyBuilder : MonoBehaviour if (baseScript != null) { baseScript.moduleID = module.Id.ToString(); - if (module.Type.Contains("Servo") && module.Degree != null) + ModuleType parsedType = Enum.Parse(module.Type); + if ((parsedType == ModuleType.SERVO_1 || parsedType == ModuleType.SERVO_2)) { ServoMotorModule servo; - if (module.Type == "Servo1") + if (parsedType == ModuleType.SERVO_1) { servo = instance.GetComponent(); } @@ -112,8 +176,6 @@ public class TopologyBuilder : MonoBehaviour { servo = instance.GetComponent(); } - //ServoBendModule servo = instance.GetComponent(); - // servo.SetInitialAngle(module.Degree); servo.InitialSetAngle(module.Degree); } } diff --git a/Assets/ModuleBase.cs b/Assets/ModuleBase.cs index 9b84223..96b1b96 100644 --- a/Assets/ModuleBase.cs +++ b/Assets/ModuleBase.cs @@ -37,6 +37,10 @@ public abstract class ModuleBase : MonoBehaviour }); } Debug.Log($"[ControlLibrary] Sending command: {json}"); + if (TopologyBuilder.SkipControlLibraryCalls) + { + return; + } if (0 != ControlLibrary.send_angle_control(Int32.Parse(moduleID), (int)currentAngle)) { Debug.Log("Control library exited with error"); diff --git a/Assets/Resources/mockData.json b/Assets/Resources/mockData.json index 337d5cc..633764c 100644 --- a/Assets/Resources/mockData.json +++ b/Assets/Resources/mockData.json @@ -1,80 +1 @@ -{ - "Modules": [ - { "Id": "servo1", "Type": "Servo1", "Degree": 34 }, - { "Id": "servo2", "Type": "Servo1", "Degree": 130 }, - { "Id": "dc1", "Type": "DC", "Degree": null }, - { "Id": "dc2", "Type": "DC", "Degree": null }, - { "Id": "hub1", "Type": "Hub", "Degree": null }, - { "Id": "hub2", "Type": "Hub", "Degree": null }, - { "Id": "battery1", "Type": "Battery", "Degree": null }, - { "Id": "battery2", "Type": "Battery", "Degree": null }, - { "Id": "servo3", "Type": "Servo2", "Degree": 32 }, - { "Id": "battery3", "Type": "Battery", "Degree": null } - ], - "Connections": [ - { - "FromModuleId": "servo1", - "ToModuleId": "servo2", - "FromSocket": "MaleSocket", - "ToSocket": "FemaleSocket", - "Orientation": 90 - }, - { - "FromModuleId": "servo2", - "ToModuleId": "dc1", - "FromSocket": "MaleSocket", - "ToSocket": "FemaleSocket", - "Orientation": 180 - }, - { - "FromModuleId": "hub1", - "ToModuleId": "battery1", - "FromSocket": "MaleSocket2", - "ToSocket": "FemaleSocket", - "Orientation": 0 - }, - { - "FromModuleId": "hub1", - "ToModuleId": "battery2", - "FromSocket": "MaleSocket3", - "ToSocket": "FemaleSocket", - "Orientation": 90 - }, - { - "FromModuleId": "hub1", - "ToModuleId": "dc2", - "FromSocket": "MaleSocket1", - "ToSocket": "FemaleSocket", - "Orientation": 0 - }, - { - "FromModuleId": "servo1", - "ToModuleId": "battery1", - "FromSocket": "BodySocket", - "ToSocket": "MaleSocket", - "Orientation": 0 - }, - { - "FromModuleId": "servo3", - "ToModuleId": "battery3", - "FromSocket": "MaleSocket", - "ToSocket": "FemaleSocket", - "Orientation": 90 - }, - { - "FromModuleId": "hub1", - "ToModuleId": "battery3", - "FromSocket": "FemaleSocket", - "ToSocket": "MaleSocket", - "Orientation": 0 - }, - { - "FromModuleId": "servo3", - "ToModuleId": "hub2", - "FromSocket": "FemaleSocket", - "ToSocket": "MaleSocket1", - "Orientation": 90 - } - ] - } - \ No newline at end of file +{"Modules":[{"Id":"battery1","Type":"Battery","Degree":null},{"Id":"servo1","Type":"Servo1","Degree":45},{"Id":"servo2","Type":"Servo2","Degree":60},{"Id":"servo3","Type":"Servo1","Degree":90},{"Id":"servo4","Type":"Servo2","Degree":45},{"Id":"servo5","Type":"Servo1","Degree":120},{"Id":"servo6","Type":"Servo2","Degree":30},{"Id":"endModule","Type":"DC","Degree":null}],"Connections":[{"FromModuleId":"battery1","ToModuleId":"servo1","FromSocket":"MaleSocket","ToSocket":"BodySocket","Orientation":0},{"FromModuleId":"servo1","ToModuleId":"servo2","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":90},{"FromModuleId":"servo2","ToModuleId":"servo3","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":0},{"FromModuleId":"servo3","ToModuleId":"servo4","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":90},{"FromModuleId":"servo4","ToModuleId":"servo5","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":0},{"FromModuleId":"servo5","ToModuleId":"servo6","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":90},{"FromModuleId":"servo6","ToModuleId":"endModule","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":0}]} diff --git a/Assets/Resources/mockDataSimple.json b/Assets/Resources/mockDataSimple.json new file mode 100644 index 0000000..ef7a286 --- /dev/null +++ b/Assets/Resources/mockDataSimple.json @@ -0,0 +1 @@ +{"Modules":[{"Id":"battery1","Type":"Battery","Degree":null},{"Id":"servo1","Type":"Servo1","Degree":45},{"Id":"servo2","Type":"Servo2","Degree":60},{"Id":"servo3","Type":"Servo1","Degree":90},{"Id":"servo4","Type":"Servo2","Degree":45},{"Id":"endModule","Type":"DC","Degree":null}],"Connections":[{"FromModuleId":"battery1","ToModuleId":"servo1","FromSocket":"MaleSocket","ToSocket":"BodySocket","Orientation":0},{"FromModuleId":"servo1","ToModuleId":"servo2","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":90},{"FromModuleId":"servo2","ToModuleId":"servo3","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":0},{"FromModuleId":"servo3","ToModuleId":"servo4","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":90},{"FromModuleId":"servo4","ToModuleId":"endModule","FromSocket":"MaleSocket","ToSocket":"FemaleSocket","Orientation":0}]} diff --git a/Assets/Resources/mockDataSimple.json.meta b/Assets/Resources/mockDataSimple.json.meta new file mode 100644 index 0000000..281af40 --- /dev/null +++ b/Assets/Resources/mockDataSimple.json.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 415eb2ad073c149198677d12765af959 +TextScriptImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scenes/SampleScene.unity b/Assets/Scenes/SampleScene.unity index 9b9104d..50efd10 100644 --- a/Assets/Scenes/SampleScene.unity +++ b/Assets/Scenes/SampleScene.unity @@ -942,6 +942,9 @@ MonoBehaviour: m_Name: m_EditorClassIdentifier: spawner: {fileID: 2021810911} + useJsonFile: 0 + skipControlLibraryCalls: 0 + jsonFileName: mockData --- !u!4 &387671720 Transform: m_ObjectHideFlags: 0 diff --git a/Assets/ServoMotorModule.cs b/Assets/ServoMotorModule.cs index 7908e39..3463ccd 100644 --- a/Assets/ServoMotorModule.cs +++ b/Assets/ServoMotorModule.cs @@ -25,10 +25,10 @@ public abstract class ServoMotorModule : ModuleBase public abstract void InitialSetAngle(float angle); - public void SetAngleAndSendControlLibrary(float angle) + public void SetAngleAndSendControlLibrary(float angle, float minChangeDegrees = 0.1f) { SetAngle(angle); - if (Mathf.Abs(currentAngle - lastSentAngle) > 0.1f) + if (Mathf.Abs(currentAngle - lastSentAngle) > minChangeDegrees) { SendToControlLibrary(servoType, currentAngle); lastSentAngle = currentAngle;