add reset robot pos and onoff lights by server

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