add reset robot pos and onoff lights by server

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

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<TriggerRequest, TriggerResponse>(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; 
+    }
+}
+

Assets/Scripts/ROSRobotLightServer.cs.meta

@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: 99ec6cbad6de2357eaaa9f1a14399578

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<TriggerRequest, TriggerResponse>(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; 
+    }
+}

Assets/Scripts/ROSRobotResetPosServer.cs.meta

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

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<ArticulationBody>();
+    }
+
+    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;
+    }
+}

Assets/Scripts/ResetPosition.cs.meta

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

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>();
+        light.intensity = .0f;
+    }
+}

Assets/Scripts/RobotLightOperation.cs.meta

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