add collision detection

2026-06-23 12:30:45 +03:00
parent 670e77f091
commit c0c2a4d197
6 changed files with 12730 additions and 89 deletions
@@ -0,0 +1,59 @@
+using UnityEngine;
+using System.Collections.Generic;
+
+public class CollisionDetector : MonoBehaviour
+{
+    public ROSCollissionDetectionPublisher publisher;
+
+    [Header("Gizmo Settings")]
+    public float gizmoRadius = 0.02f;
+    public Color gizmoColor = Color.red;
+    private Vector3 collision_point;
+    private Transform collision_obj;
+
+    private bool selfCollisionStatus = false;
+
+    private List<Vector3> collisionPoints = new List<Vector3>();
+
+    private void OnCollisionEnter(Collision collision)
+    {
+        // Filter by the tag of the object you hit
+        if (collision.gameObject.CompareTag("obstacles"))
+        {
+            collision_point = collision.contacts[^1].point; //last elem
+            //Debug.Log($"Collision {transform.name} with {collision.gameObject.name} {transform.position} occurred at point: {collision_point}");
+            selfCollisionStatus = transform.name.Contains("wheel") ? collision_point.y >= transform.position.y : true;
+            if (selfCollisionStatus)
+            {
+                collisionPoints.Add(collision_point);
+                //Debug.Log($"{transform.name} collide with {collision.gameObject.name} occurred at point: {collision_point}");
+            }
+            publisher.result.data = publisher.result.data ? publisher.result.data : selfCollisionStatus;
+            //publisher.ros.Publish(publisher.topicName, publisher.result);
+        }
+    }
+
+    void OnCollisionStay(Collision collision)
+    {
+        if (collision.gameObject.CompareTag("obstacles"))
+        {
+            collision_point = collision.contacts[^1].point; //last elem
+            //Debug.Log($"Collision {transform.name} with {collision.gameObject.name} {transform.position} occurred at point: {collision_point}");
+            selfCollisionStatus = transform.name.Contains("wheel") ? collision_point.y >= transform.position.y : true;
+            publisher.result.data = publisher.result.data ? publisher.result.data : selfCollisionStatus;
+            //publisher.ros.Publish(publisher.topicName, publisher.result);
+        }
+    }
+
+    void OnCollisionExit(Collision collisionInfo)
+    {
+        publisher.result.data = false;
+    }
+
+    private void OnDrawGizmos()
+    {
+        Gizmos.color = gizmoColor;
+        foreach (Vector3 point in collisionPoints)
+            Gizmos.DrawSphere(point, gizmoRadius);
+    }
+}