diff --git a/Packages/UnitySensors/Runtime/Scripts/DataType/Sensor/ContactData.cs b/Packages/UnitySensors/Runtime/Scripts/DataType/Sensor/ContactData.cs new file mode 100644 index 00000000..3c06aae9 --- /dev/null +++ b/Packages/UnitySensors/Runtime/Scripts/DataType/Sensor/ContactData.cs @@ -0,0 +1,13 @@ +using UnityEngine; + +namespace UnitySensors.DataType.Sensor +{ + [System.Serializable] + public struct ContactData + { + public string colliderName; + public Vector3 position; + public Vector3 normal; + public Vector3 force; + } +} diff --git a/Packages/UnitySensors/Runtime/Scripts/DataType/Sensor/ContactData.cs.meta b/Packages/UnitySensors/Runtime/Scripts/DataType/Sensor/ContactData.cs.meta new file mode 100644 index 00000000..f5247915 --- /dev/null +++ b/Packages/UnitySensors/Runtime/Scripts/DataType/Sensor/ContactData.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 19d1bbf628364466acd604951487b7a0 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Packages/UnitySensors/Runtime/Scripts/Interfaces/Sensor/IContactDataInterface.cs b/Packages/UnitySensors/Runtime/Scripts/Interfaces/Sensor/IContactDataInterface.cs new file mode 100644 index 00000000..45efcf24 --- /dev/null +++ b/Packages/UnitySensors/Runtime/Scripts/Interfaces/Sensor/IContactDataInterface.cs @@ -0,0 +1,17 @@ +using System.Collections.Generic; +using UnityEngine; + +using UnitySensors.DataType.Sensor; + +namespace UnitySensors.Interface.Sensor +{ + public interface IContactDataInterface + { + public bool isContact { get; } + public Vector3 totalForce { get; } + public Vector3 totalTorque { get; } + public Vector3 localTotalForce { get; } + public Vector3 localTotalTorque { get; } + public IReadOnlyList contacts { get; } + } +} diff --git a/Packages/UnitySensors/Runtime/Scripts/Interfaces/Sensor/IContactDataInterface.cs.meta b/Packages/UnitySensors/Runtime/Scripts/Interfaces/Sensor/IContactDataInterface.cs.meta new file mode 100644 index 00000000..b08c695a --- /dev/null +++ b/Packages/UnitySensors/Runtime/Scripts/Interfaces/Sensor/IContactDataInterface.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 551179d557c14100b6453a754f07f0b9 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Packages/UnitySensors/Runtime/Scripts/Interfaces/Sensor/IWrenchInterface.cs b/Packages/UnitySensors/Runtime/Scripts/Interfaces/Sensor/IWrenchInterface.cs new file mode 100644 index 00000000..f8902595 --- /dev/null +++ b/Packages/UnitySensors/Runtime/Scripts/Interfaces/Sensor/IWrenchInterface.cs @@ -0,0 +1,13 @@ +using UnityEngine; + +namespace UnitySensors.Interface.Sensor +{ + /// + /// A force/torque pair expressed in the sensor's local frame. + /// + public interface IWrenchInterface + { + public Vector3 force { get; } + public Vector3 torque { get; } + } +} diff --git a/Packages/UnitySensors/Runtime/Scripts/Interfaces/Sensor/IWrenchInterface.cs.meta b/Packages/UnitySensors/Runtime/Scripts/Interfaces/Sensor/IWrenchInterface.cs.meta new file mode 100644 index 00000000..1b1c2729 --- /dev/null +++ b/Packages/UnitySensors/Runtime/Scripts/Interfaces/Sensor/IWrenchInterface.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: de3dd290f0a6464fb5f084d7ecc1ea2d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Packages/UnitySensors/Runtime/Scripts/Interfaces/Std/IBoolStateInterface.cs b/Packages/UnitySensors/Runtime/Scripts/Interfaces/Std/IBoolStateInterface.cs new file mode 100644 index 00000000..198355e5 --- /dev/null +++ b/Packages/UnitySensors/Runtime/Scripts/Interfaces/Std/IBoolStateInterface.cs @@ -0,0 +1,7 @@ +namespace UnitySensors.Interface.Std +{ + public interface IBoolStateInterface + { + public bool state { get; } + } +} diff --git a/Packages/UnitySensors/Runtime/Scripts/Interfaces/Std/IBoolStateInterface.cs.meta b/Packages/UnitySensors/Runtime/Scripts/Interfaces/Std/IBoolStateInterface.cs.meta new file mode 100644 index 00000000..e680dd1d --- /dev/null +++ b/Packages/UnitySensors/Runtime/Scripts/Interfaces/Std/IBoolStateInterface.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 05b3402cc5ec4adc9cdcd41da04f57d1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Packages/UnitySensors/Runtime/Scripts/Sensors/Contact.meta b/Packages/UnitySensors/Runtime/Scripts/Sensors/Contact.meta new file mode 100644 index 00000000..79557cb1 --- /dev/null +++ b/Packages/UnitySensors/Runtime/Scripts/Sensors/Contact.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: de03d6b510604e90981c88a5b984d0da +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Packages/UnitySensors/Runtime/Scripts/Sensors/Contact/ContactEventRelay.cs b/Packages/UnitySensors/Runtime/Scripts/Sensors/Contact/ContactEventRelay.cs new file mode 100644 index 00000000..2afd8951 --- /dev/null +++ b/Packages/UnitySensors/Runtime/Scripts/Sensors/Contact/ContactEventRelay.cs @@ -0,0 +1,38 @@ +using UnityEngine; + +namespace UnitySensors.Sensor.Contact +{ + /// + /// Forwards OnCollision* messages to a ContactSensor that does not sit on the + /// GameObject Unity delivers them to. Added automatically by ContactSensor; not meant + /// to be attached by hand. + /// + [AddComponentMenu("")] + public class ContactEventRelay : MonoBehaviour + { + private ContactSensor _sensor; + + /// Which sensor this relay feeds, so duplicates can be spotted. + public ContactSensor Target { get => _sensor; } + + public void Initialize(ContactSensor sensor) + { + _sensor = sensor; + } + + private void OnCollisionEnter(Collision collision) + { + if (_sensor != null) _sensor.HandleRelayedCollisionEnter(collision); + } + + private void OnCollisionStay(Collision collision) + { + if (_sensor != null) _sensor.HandleRelayedCollisionStay(collision); + } + + private void OnCollisionExit(Collision collision) + { + if (_sensor != null) _sensor.HandleRelayedCollisionExit(collision); + } + } +} diff --git a/Packages/UnitySensors/Runtime/Scripts/Sensors/Contact/ContactEventRelay.cs.meta b/Packages/UnitySensors/Runtime/Scripts/Sensors/Contact/ContactEventRelay.cs.meta new file mode 100644 index 00000000..6c3809bf --- /dev/null +++ b/Packages/UnitySensors/Runtime/Scripts/Sensors/Contact/ContactEventRelay.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e853ad165a164f55b26ebb55f8b91ca8 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Packages/UnitySensors/Runtime/Scripts/Sensors/Contact/ContactSensor.cs b/Packages/UnitySensors/Runtime/Scripts/Sensors/Contact/ContactSensor.cs new file mode 100644 index 00000000..7a2d1c6c --- /dev/null +++ b/Packages/UnitySensors/Runtime/Scripts/Sensors/Contact/ContactSensor.cs @@ -0,0 +1,213 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +using UnitySensors.Attribute; +using UnitySensors.DataType.Sensor; +using UnitySensors.Interface.Sensor; +using UnitySensors.Interface.Std; + +namespace UnitySensors.Sensor.Contact +{ + public class ContactSensor : UnitySensor, IContactDataInterface, IBoolStateInterface, IWrenchInterface + { + [SerializeField, ReadOnly] + private bool _isContact; + [SerializeField, ReadOnly] + private List _contacts = new List(); + + private Dictionary _activeContacts = new Dictionary(); + private List _removeBuffer = new List(); + // Colliders that belong to this sensor. Empty means "anything that reaches me". + private HashSet _ownColliders = new HashSet(); + private Vector3 _totalForce; + private Vector3 _totalTorque; + + public bool isContact { get => _isContact; } + public Vector3 totalForce { get => _totalForce; } + public Vector3 totalTorque { get => _totalTorque; } + public Vector3 localTotalForce { get => transform.InverseTransformDirection(_totalForce); } + public Vector3 localTotalTorque { get => transform.InverseTransformDirection(_totalTorque); } + public IReadOnlyList contacts { get => _contacts; } + + // Generic serializer sources: bumper state and the net contact wrench + // in the sensor's local frame. + bool IBoolStateInterface.state { get => _isContact; } + Vector3 IWrenchInterface.force { get => localTotalForce; } + Vector3 IWrenchInterface.torque { get => localTotalTorque; } + + protected override void Init() + { + // Unity delivers OnCollision* only to the GameObject that carries the + // Rigidbody/ArticulationBody - never to a collider-only child. Relays on the + // children therefore fire for the case where the sensor sits on the body + // itself and the colliders hang below it, but not for a sensor on a part that + // shares a body with its parent. RegisterBodyRelay() covers the latter. + AttachRelays(transform); + } + + /// + /// Listen to the collisions of a body this sensor does not own, keeping only the + /// ones that touched a collider registered through . + /// + /// + /// A URDF link attached by a fixed joint has no body of its own - its colliders + /// belong to the nearest ancestor that has one, and that ancestor is where Unity + /// sends the collision. Without this the sensor hears nothing; with this but no + /// own-collider filter it would report every contact of the whole assembly, so a + /// bumper brushing a wall would read as a hit on the plate. + /// + public void RegisterBodyRelay(GameObject bodyObject) + { + if (bodyObject == null || bodyObject == gameObject) return; + foreach (ContactEventRelay existing in bodyObject.GetComponents()) + { + if (existing.Target == this) return; + } + bodyObject.AddComponent().Initialize(this); + } + + /// + /// Declare a collider as belonging to this sensor. Once any is declared, contacts + /// on other colliders of the same body are ignored. + /// + public void RegisterOwnCollider(Collider collider) + { + if (collider != null) _ownColliders.Add(collider); + } + + /// + /// Relay OnCollision* events from the given collider's GameObject to + /// this sensor. Use this when the collider sits outside the default + /// scan, e.g. on a child GameObject that carries its own body. + /// + public void RegisterCollider(Collider target) + { + if (target == null || target.gameObject == gameObject) return; + if (target.GetComponent() == null) + { + target.gameObject.AddComponent().Initialize(this); + } + } + + private void AttachRelays(Transform target) + { + if (target != transform && + (target.GetComponent() != null || target.GetComponent() != null)) + { + return; + } + + if (target != transform && + target.GetComponent() != null && + target.GetComponent() == null) + { + target.gameObject.AddComponent().Initialize(this); + } + + for (int i = 0; i < target.childCount; i++) + { + AttachRelays(target.GetChild(i)); + } + } + + // Colliders on the same GameObject as the body report here directly; + // collider-only children report through ContactEventRelay. + private void OnCollisionEnter(Collision collision) + { + UpdateContact(collision); + } + + private void OnCollisionStay(Collision collision) + { + UpdateContact(collision); + } + + private void OnCollisionExit(Collision collision) + { + _activeContacts.Remove(collision.collider); + } + + internal void HandleRelayedCollisionEnter(Collision collision) + { + UpdateContact(collision); + } + + internal void HandleRelayedCollisionStay(Collision collision) + { + UpdateContact(collision); + } + + internal void HandleRelayedCollisionExit(Collision collision) + { + _activeContacts.Remove(collision.collider); + } + + private void UpdateContact(Collision collision) + { + if (collision.contactCount == 0) return; + + // Walk every contact point: one Collision can carry points on several of the + // body's colliders, and only some of them may be ours. + int matched = 0; + ContactPoint mine = default; + for (int i = 0; i < collision.contactCount; i++) + { + ContactPoint point = collision.GetContact(i); + if (_ownColliders.Count > 0 && !_ownColliders.Contains(point.thisCollider)) continue; + if (matched == 0) mine = point; + matched++; + } + if (matched == 0) + { + // Nothing on our side of the assembly was touched. + _activeContacts.Remove(collision.collider); + return; + } + + ContactData contact = new ContactData(); + contact.colliderName = collision.collider.name; + contact.position = mine.point; + contact.normal = mine.normal; + // impulse is reported per collider pair, so when only part of the contact + // points are ours the force is scaled by their share rather than measured. + contact.force = collision.impulse / Time.fixedDeltaTime + * ((float)matched / collision.contactCount); + _activeContacts[collision.collider] = contact; + } + + protected override IEnumerator UpdateSensor() + { + // A collider destroyed while touching never sends OnCollisionExit, + // so drop destroyed keys before aggregating. + _removeBuffer.Clear(); + foreach (Collider collider in _activeContacts.Keys) + { + if (collider == null) _removeBuffer.Add(collider); + } + foreach (Collider collider in _removeBuffer) + { + _activeContacts.Remove(collider); + } + + _contacts.Clear(); + _totalForce = Vector3.zero; + _totalTorque = Vector3.zero; + + Vector3 origin = transform.position; + foreach (ContactData contact in _activeContacts.Values) + { + _contacts.Add(contact); + _totalForce += contact.force; + _totalTorque += Vector3.Cross(contact.position - origin, contact.force); + } + _isContact = _contacts.Count > 0; + + yield return null; + } + + protected override void OnSensorDestroy() + { + } + } +} diff --git a/Packages/UnitySensors/Runtime/Scripts/Sensors/Contact/ContactSensor.cs.meta b/Packages/UnitySensors/Runtime/Scripts/Sensors/Contact/ContactSensor.cs.meta new file mode 100644 index 00000000..01d501cf --- /dev/null +++ b/Packages/UnitySensors/Runtime/Scripts/Sensors/Contact/ContactSensor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4cdc18db19914e8b8a05fc691fb8a871 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Packages/UnitySensorsROS/Runtime/Scripts/Publishers/GeometryMsgs/WrenchStampedMsgPublisher.cs b/Packages/UnitySensorsROS/Runtime/Scripts/Publishers/GeometryMsgs/WrenchStampedMsgPublisher.cs new file mode 100644 index 00000000..4b468011 --- /dev/null +++ b/Packages/UnitySensorsROS/Runtime/Scripts/Publishers/GeometryMsgs/WrenchStampedMsgPublisher.cs @@ -0,0 +1,10 @@ +using RosMessageTypes.Geometry; + +using UnitySensors.ROS.Serializer.Geometry; + +namespace UnitySensors.ROS.Publisher.Geometry +{ + public class WrenchStampedMsgPublisher : RosMsgPublisher + { + } +} diff --git a/Packages/UnitySensorsROS/Runtime/Scripts/Publishers/GeometryMsgs/WrenchStampedMsgPublisher.cs.meta b/Packages/UnitySensorsROS/Runtime/Scripts/Publishers/GeometryMsgs/WrenchStampedMsgPublisher.cs.meta new file mode 100644 index 00000000..cf29a84a --- /dev/null +++ b/Packages/UnitySensorsROS/Runtime/Scripts/Publishers/GeometryMsgs/WrenchStampedMsgPublisher.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7b6ee44b07d1418fb3e05fd0870921da +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Packages/UnitySensorsROS/Runtime/Scripts/Publishers/StdMsgs.meta b/Packages/UnitySensorsROS/Runtime/Scripts/Publishers/StdMsgs.meta new file mode 100644 index 00000000..97ddcfe5 --- /dev/null +++ b/Packages/UnitySensorsROS/Runtime/Scripts/Publishers/StdMsgs.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 3486f3df896046dbb3da1311547da094 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Packages/UnitySensorsROS/Runtime/Scripts/Publishers/StdMsgs/BoolMsgPublisher.cs b/Packages/UnitySensorsROS/Runtime/Scripts/Publishers/StdMsgs/BoolMsgPublisher.cs new file mode 100644 index 00000000..5d6612fd --- /dev/null +++ b/Packages/UnitySensorsROS/Runtime/Scripts/Publishers/StdMsgs/BoolMsgPublisher.cs @@ -0,0 +1,10 @@ +using RosMessageTypes.Std; + +using UnitySensors.ROS.Serializer.Std; + +namespace UnitySensors.ROS.Publisher.Std +{ + public class BoolMsgPublisher : RosMsgPublisher + { + } +} diff --git a/Packages/UnitySensorsROS/Runtime/Scripts/Publishers/StdMsgs/BoolMsgPublisher.cs.meta b/Packages/UnitySensorsROS/Runtime/Scripts/Publishers/StdMsgs/BoolMsgPublisher.cs.meta new file mode 100644 index 00000000..2fe4a876 --- /dev/null +++ b/Packages/UnitySensorsROS/Runtime/Scripts/Publishers/StdMsgs/BoolMsgPublisher.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b21eb530bb194d13a3fbb60776470bc4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Packages/UnitySensorsROS/Runtime/Scripts/Serializers/GeometryMsgs/WrenchStampedMsgSerializer.cs b/Packages/UnitySensorsROS/Runtime/Scripts/Serializers/GeometryMsgs/WrenchStampedMsgSerializer.cs new file mode 100644 index 00000000..00c6fa80 --- /dev/null +++ b/Packages/UnitySensorsROS/Runtime/Scripts/Serializers/GeometryMsgs/WrenchStampedMsgSerializer.cs @@ -0,0 +1,39 @@ +using UnityEngine; + +using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using RosMessageTypes.Geometry; + +using UnitySensors.Attribute; +using UnitySensors.Interface.Sensor; +using UnitySensors.ROS.Serializer.Std; + +namespace UnitySensors.ROS.Serializer.Geometry +{ + [System.Serializable] + public class WrenchStampedMsgSerializer : RosMsgSerializer + { + [SerializeField, Interface(typeof(IWrenchInterface))] + private Object _source; + [SerializeField] + private HeaderSerializer _header; + + private IWrenchInterface _sourceInterface; + + public override void Init() + { + base.Init(); + _header.Init(); + _sourceInterface = _source as IWrenchInterface; + } + + public override WrenchStampedMsg Serialize() + { + _msg.header = _header.Serialize(); + // IWrenchInterface vectors are already in the header frame + // (the sensor's local frame). + _msg.wrench.force = _sourceInterface.force.To(); + _msg.wrench.torque = _sourceInterface.torque.To(); + return _msg; + } + } +} diff --git a/Packages/UnitySensorsROS/Runtime/Scripts/Serializers/GeometryMsgs/WrenchStampedMsgSerializer.cs.meta b/Packages/UnitySensorsROS/Runtime/Scripts/Serializers/GeometryMsgs/WrenchStampedMsgSerializer.cs.meta new file mode 100644 index 00000000..8420d55a --- /dev/null +++ b/Packages/UnitySensorsROS/Runtime/Scripts/Serializers/GeometryMsgs/WrenchStampedMsgSerializer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 75a0ebfa03874f1e9b0ee9705b930f8f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Packages/UnitySensorsROS/Runtime/Scripts/Serializers/StdMsgs/BoolMsgSerializer.cs b/Packages/UnitySensorsROS/Runtime/Scripts/Serializers/StdMsgs/BoolMsgSerializer.cs new file mode 100644 index 00000000..68f13bcc --- /dev/null +++ b/Packages/UnitySensorsROS/Runtime/Scripts/Serializers/StdMsgs/BoolMsgSerializer.cs @@ -0,0 +1,29 @@ +using UnityEngine; +using RosMessageTypes.Std; + +using UnitySensors.Attribute; +using UnitySensors.Interface.Std; + +namespace UnitySensors.ROS.Serializer.Std +{ + [System.Serializable] + public class BoolMsgSerializer : RosMsgSerializer + { + [SerializeField, Interface(typeof(IBoolStateInterface))] + private Object _source; + + private IBoolStateInterface _sourceInterface; + + public override void Init() + { + base.Init(); + _sourceInterface = _source as IBoolStateInterface; + } + + public override BoolMsg Serialize() + { + _msg.data = _sourceInterface.state; + return _msg; + } + } +} diff --git a/Packages/UnitySensorsROS/Runtime/Scripts/Serializers/StdMsgs/BoolMsgSerializer.cs.meta b/Packages/UnitySensorsROS/Runtime/Scripts/Serializers/StdMsgs/BoolMsgSerializer.cs.meta new file mode 100644 index 00000000..3dd53b8e --- /dev/null +++ b/Packages/UnitySensorsROS/Runtime/Scripts/Serializers/StdMsgs/BoolMsgSerializer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ff6e550d839c40a7830b21731c11e118 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/README.md b/README.md index c3174475..d879c4f2 100644 --- a/README.md +++ b/README.md @@ -33,6 +33,7 @@ The following sensors are available inside : - Fisheye Camera ([UCM][external-UCM-link], [EUCM][external-EUCM-link], [Double Sphere][external-DS-link], [Kannala-Brandt (KB4)][external-KB4-link], [OCamCalib][external-OCamCalib-link], and Equidistant models with adjustable parameters) - IMU - GNSS +- Contact (Bumper) - (GroundTruth) - (TF)