diff --git a/Assets/Scenes/main.unity b/Assets/Scenes/main.unity index 66e296e..ef030b6 100644 --- a/Assets/Scenes/main.unity +++ b/Assets/Scenes/main.unity @@ -8405,6 +8405,7 @@ GameObject: - component: {fileID: 75796156} - component: {fileID: 75796155} - component: {fileID: 75796159} + - component: {fileID: 75796160} m_Layer: 8 m_Name: left_light_link m_TagString: robot @@ -8604,6 +8605,19 @@ MonoBehaviour: publisher: {fileID: 2061160361} gizmoRadius: 0.1 gizmoColor: {r: 1, g: 0, b: 0, a: 1} +--- !u!114 &75796160 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 75796153} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 6e033e4216279fa3a955d9f623bf56fa, type: 3} + m_Name: + m_EditorClassIdentifier: + maxInensity: 1 --- !u!1001 &75834288 PrefabInstance: m_ObjectHideFlags: 0 @@ -9790,6 +9804,69 @@ Transform: m_CorrespondingSourceObject: {fileID: 7367325066403342166, guid: 55fbf0987cc5d2b419db356ec3cc4cd5, type: 3} m_PrefabInstance: {fileID: 83784987} m_PrefabAsset: {fileID: 0} +--- !u!1 &85102062 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 85102063} + - component: {fileID: 85102065} + - component: {fileID: 85102064} + m_Layer: 0 + m_Name: Servers + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &85102063 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 85102062} + serializedVersion: 2 + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_ConstrainProportionsScale: 0 + m_Children: [] + m_Father: {fileID: 1278719290} + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &85102064 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 85102062} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: b8a19e6407be92307a13fb4f8216051b, type: 3} + m_Name: + m_EditorClassIdentifier: + serviceName: /unity/robot_reset_trigger + target: {fileID: 915175391} +--- !u!114 &85102065 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 85102062} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 99ec6cbad6de2357eaaa9f1a14399578, type: 3} + m_Name: + m_EditorClassIdentifier: + serviceName: /unity/light_trigger + lightList: + - {fileID: 75796160} + - {fileID: 1643870775} --- !u!1001 &85680131 PrefabInstance: m_ObjectHideFlags: 0 @@ -86689,7 +86766,7 @@ MonoBehaviour: wheelRadius: 0.22 trackWidth: 0.47 _frequency: 1 - use_namespace: 1 + use_namespace: 0 --- !u!1001 &794370220 PrefabInstance: m_ObjectHideFlags: 0 @@ -100769,6 +100846,7 @@ GameObject: - component: {fileID: 915175389} - component: {fileID: 915175388} - component: {fileID: 915175390} + - component: {fileID: 915175391} m_Layer: 8 m_Name: base_link m_TagString: robot @@ -100895,6 +100973,24 @@ MonoBehaviour: publisher: {fileID: 2061160361} gizmoRadius: 0.1 gizmoColor: {r: 0, g: 0.092690945, b: 1, a: 1} +--- !u!114 &915175391 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 915175385} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 4b9c4dfebd00ecd8aaf8dbf69d9248e2, type: 3} + m_Name: + m_EditorClassIdentifier: + currentTerrain: {fileID: 500605640} + _terrainLayer: + serializedVersion: 2 + m_Bits: 128 + _groundOffset: 1 + startRotation: 0 --- !u!1001 &916098215 PrefabInstance: m_ObjectHideFlags: 0 @@ -140638,6 +140734,7 @@ Transform: m_Children: - {fileID: 357998453} - {fileID: 1561437904} + - {fileID: 85102063} m_Father: {fileID: 0} m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} --- !u!1001 &1279160926 @@ -180179,6 +180276,7 @@ GameObject: - component: {fileID: 1643870771} - component: {fileID: 1643870770} - component: {fileID: 1643870774} + - component: {fileID: 1643870775} m_Layer: 8 m_Name: right_light_link m_TagString: robot @@ -180378,6 +180476,19 @@ MonoBehaviour: publisher: {fileID: 2061160361} gizmoRadius: 0.1 gizmoColor: {r: 1, g: 0, b: 0, a: 1} +--- !u!114 &1643870775 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1643870768} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 6e033e4216279fa3a955d9f623bf56fa, type: 3} + m_Name: + m_EditorClassIdentifier: + maxInensity: 1 --- !u!1001 &1644172386 PrefabInstance: m_ObjectHideFlags: 0 diff --git a/Assets/Scripts/ROSRobotLightServer.cs b/Assets/Scripts/ROSRobotLightServer.cs new file mode 100644 index 0000000..b614df0 --- /dev/null +++ b/Assets/Scripts/ROSRobotLightServer.cs @@ -0,0 +1,40 @@ +using UnityEngine; +using Unity.Robotics.ROSTCPConnector; +using RosMessageTypes.Std; + +public class ROSRobotLightServer : MonoBehaviour +{ + [SerializeField] private string serviceName = "/unity/trigger_event"; + [SerializeField] public RobotLightOperation[] lightList; + + private string status = "OFF"; + + void Start() + { + // Implement the service handler inside Unity + ROSConnection.GetOrCreateInstance().ImplementService(serviceName, HandleTriggerRequest); + } + + private TriggerResponse HandleTriggerRequest(TriggerRequest request) + { + // Perform your Unity-side logic here (e.g., reset simulation state) + bool wasSuccessful = TryExecuteUnityAction(); + string responseMessage = wasSuccessful ? $"Headlights switched {status} successfully" : "Failed to run Unity light switch action"; + + // Construct and return the response + return new TriggerResponse + { + success = wasSuccessful, + message = responseMessage + }; + } + + private bool TryExecuteUnityAction() + { + foreach (var item in lightList) + item.light.intensity = item.light.intensity > .0f ? .0f : item.maxInensity; + status = lightList[0].light.intensity > .0f ? "ON" : "OFF"; + return true; + } +} + diff --git a/Assets/Scripts/ROSRobotLightServer.cs.meta b/Assets/Scripts/ROSRobotLightServer.cs.meta new file mode 100644 index 0000000..6f36d87 --- /dev/null +++ b/Assets/Scripts/ROSRobotLightServer.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 99ec6cbad6de2357eaaa9f1a14399578 \ No newline at end of file diff --git a/Assets/Scripts/ROSRobotResetPosServer.cs b/Assets/Scripts/ROSRobotResetPosServer.cs new file mode 100644 index 0000000..fabf49e --- /dev/null +++ b/Assets/Scripts/ROSRobotResetPosServer.cs @@ -0,0 +1,35 @@ +using UnityEngine; +using Unity.Robotics.ROSTCPConnector; +using RosMessageTypes.Std; + +public class ROSRobotResetPosServer : MonoBehaviour +{ + [SerializeField] private string serviceName = "/unity/trigger_event"; + [SerializeField] public ResetPosition target; + + void Start() + { + // Implement the service handler inside Unity + ROSConnection.GetOrCreateInstance().ImplementService(serviceName, HandleTriggerRequest); + } + + private TriggerResponse HandleTriggerRequest(TriggerRequest request) + { + // Perform your Unity-side logic here (e.g., reset simulation state) + bool wasSuccessful = TryExecuteUnityAction(); + string responseMessage = wasSuccessful ? "Robot's position successfully reset" : "Failed to run Unity reset action"; + + // Construct and return the response + return new TriggerResponse + { + success = wasSuccessful, + message = responseMessage + }; + } + + private bool TryExecuteUnityAction() + { + target.startRotation = true; + return true; + } +} diff --git a/Assets/Scripts/ROSRobotResetPosServer.cs.meta b/Assets/Scripts/ROSRobotResetPosServer.cs.meta new file mode 100644 index 0000000..a8aa493 --- /dev/null +++ b/Assets/Scripts/ROSRobotResetPosServer.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: b8a19e6407be92307a13fb4f8216051b \ No newline at end of file diff --git a/Assets/Scripts/ResetPosition.cs b/Assets/Scripts/ResetPosition.cs new file mode 100644 index 0000000..3b39aba --- /dev/null +++ b/Assets/Scripts/ResetPosition.cs @@ -0,0 +1,57 @@ +using UnityEngine; + +public class ResetPosition : MonoBehaviour +{ + [SerializeField] private Terrain currentTerrain; + [SerializeField] private LayerMask _terrainLayer = 1 << 0; + [SerializeField] private float _groundOffset = 1.0f; // m, shift during raycast to stick robot to the ground + + public bool startRotation = false; + private Vector3 targetEuler, targetPosition; + private Quaternion targetRotation; + private Vector3 terrainPosition, terrainNormal; + private float normalizedX, normalizedZ; + private Vector3 faceDirection; + + private ArticulationBody body; + // draw ray and spawn object little up + // Start is called once before the first execution of Update after the MonoBehaviour is created + + void Start() + { + body = GetComponent(); + } + + void FixedUpdate() { + if (startRotation) + { + FixPosition(); + } + } + + void FixPosition() + { + terrainPosition = transform.position - currentTerrain.transform.position; + normalizedX = Mathf.InverseLerp(0, currentTerrain.terrainData.size.x, terrainPosition.x); + normalizedX = Mathf.Clamp01(normalizedX); + normalizedZ = Mathf.InverseLerp(0, currentTerrain.terrainData.size.z, terrainPosition.z); + normalizedZ = Mathf.Clamp01(normalizedZ); + terrainNormal = currentTerrain.terrainData.GetInterpolatedNormal(normalizedX, normalizedZ); + + faceDirection = Vector3.Cross(terrainNormal, -transform.right); + + Quaternion predictedRot = Quaternion.LookRotation(faceDirection, terrainNormal); + targetEuler = predictedRot.eulerAngles; + targetEuler.y = transform.rotation.eulerAngles.y; + + targetRotation = Quaternion.Euler(targetEuler); + targetPosition = new Vector3( + transform.position.x, + transform.position.y + _groundOffset, + transform.position.z); + + body.TeleportRoot(targetPosition, targetRotation); + + startRotation = false; + } +} diff --git a/Assets/Scripts/ResetPosition.cs.meta b/Assets/Scripts/ResetPosition.cs.meta new file mode 100644 index 0000000..3446abf --- /dev/null +++ b/Assets/Scripts/ResetPosition.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 4b9c4dfebd00ecd8aaf8dbf69d9248e2 \ No newline at end of file diff --git a/Assets/Scripts/RobotLightOperation.cs b/Assets/Scripts/RobotLightOperation.cs new file mode 100644 index 0000000..2fc506f --- /dev/null +++ b/Assets/Scripts/RobotLightOperation.cs @@ -0,0 +1,15 @@ +using UnityEngine; + +public class RobotLightOperation : MonoBehaviour +{ + //public ROSCollissionDetectionPublisher publisher; + public float maxInensity = 1.0f; + [System.NonSerialized] + public Light light; + + void Start() + { + light = GetComponent(); + light.intensity = .0f; + } +} diff --git a/Assets/Scripts/RobotLightOperation.cs.meta b/Assets/Scripts/RobotLightOperation.cs.meta new file mode 100644 index 0000000..3781f3f --- /dev/null +++ b/Assets/Scripts/RobotLightOperation.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 6e033e4216279fa3a955d9f623bf56fa \ No newline at end of file