add collision detection

Assets/Scripts/CollisionDetector.cs

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

Assets/Scripts/CollisionDetector.cs.meta

@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: 2855ede88eab60feb8b47d1c566dcfa4

Assets/Scripts/ROSCollissionDetectionPublisher.cs

@@ -0,0 +1,38 @@
+using UnityEngine;
+using Unity.Robotics.Core;
+using Unity.Robotics.ROSTCPConnector;
+using RosMessageTypes.Std;
+
+public class ROSCollissionDetectionPublisher : MonoBehaviour
+{
+    public string topicName = "/collision_detection";
+    public bool useNamespace = true;
+    public BoolMsg result;
+
+
+    protected string _namespace;
+
+    ROSConnection ros;
+
+    private void Awake()
+    {
+        if (useNamespace)
+        {
+            _namespace = "/" + gameObject.name;
+            topicName = topicName[0] == '/' ? _namespace + topicName : _namespace + "/" + topicName;
+        }
+    }
+    // Start is called once before the first execution of Update after the MonoBehaviour is created
+    void Start()
+    {
+        ros = ROSConnection.GetOrCreateInstance();
+        ros.RegisterPublisher<BoolMsg>(topicName);
+        result = new BoolMsg(false);
+    }
+
+    // Update is called once per frame
+    void Update()
+    {
+        ros.Publish(topicName, result);
+    }
+}

Assets/Scripts/ROSCollissionDetectionPublisher.cs.meta

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

ProjectSettings/TagManager.asset

@@ -5,6 +5,9 @@ TagManager:
   serializedVersion: 3
   tags:
   - robot
+  - UtilityObj
+  - terrain
+  - obstacles
   layers:
   - Default
   - TransparentFX