Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix updating information of Bumper sensor #324

Merged
merged 1 commit into from
Nov 21, 2024
Merged

Fix updating information of Bumper sensor #324

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 23 additions & 7 deletions Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -288,13 +288,26 @@ private void SetMotorPID(

if (GetPluginParameters().IsValidNode($"{parameterPrefix}/limit"))
{
var limitIntegral = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/integral");
var limitOutput = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/output");
if (!GetPluginParameters().IsValidNode($"{parameterPrefix}/limit/integral/min") &&
!GetPluginParameters().IsValidNode($"{parameterPrefix}/limit/integral/max"))
{
var limitIntegral = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/integral");
integralMin = -Math.Abs(limitIntegral);
integralMax = Math.Abs(limitIntegral);
}

integralMin = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/integral/min", -Mathf.Abs(limitIntegral));
integralMax = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/integral/max", Mathf.Abs(limitIntegral));
outputMin = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/output/min", -Mathf.Abs(limitOutput));
outputMax = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/output/max", Mathf.Abs(limitOutput));
if (!GetPluginParameters().IsValidNode($"{parameterPrefix}/limit/integral/min") &&
!GetPluginParameters().IsValidNode($"{parameterPrefix}/limit/integral/max"))
{
var limitOutput = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/output");
outputMin = -Math.Abs(limitOutput);
outputMax = Math.Abs(limitOutput);
}

integralMin = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/integral/min", integralMin);
integralMax = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/integral/max", integralMax);
outputMin = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/output/min", outputMin);
outputMax = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/output/max", outputMax);
}

SetPID(P, I, D, integralMin, integralMax, outputMin, outputMax);
Expand Down Expand Up @@ -415,8 +428,11 @@ protected override void HandleCustomRequestMessage(in string requestType, in Any
SetEmptyResponse(ref response);
break;

case "request_bumper_topic_name":
break;

default:
break;
}
}
}
}
121 changes: 39 additions & 82 deletions Assets/Scripts/Devices/Contact.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
* SPDX-License-Identifier: MIT
*/

using System.Collections.Generic;
using System;
using UnityEngine;
using messages = cloisim.msgs;
Expand All @@ -13,15 +12,14 @@ namespace SensorDevices
{
public class ContactTrigger : MonoBehaviour
{
public Action<Collision> collisionEnter = null;
public Action<Collision> collisionStay = null;
public Action<Collision> collisionExit = null;

private void OnCollisionEnter(Collision other)
private void OnCollisionStay(Collision other)
{
if (collisionEnter != null)
if (collisionStay != null)
{
collisionEnter.Invoke(other);
collisionStay.Invoke(other);
}
}

Expand All @@ -32,14 +30,6 @@ private void OnCollisionExit(Collision other)
collisionExit.Invoke(other);
}
}

private void OnCollisionStay(Collision other)
{
if (collisionStay != null)
{
collisionStay.Invoke(other);
}
}
}

public class Contact : Device
Expand All @@ -57,6 +47,7 @@ protected override void OnAwake()

protected override void OnStart()
{
// Debug.Log("Contact target collision: " + targetCollision);
}

protected override void InitializeMessages()
Expand All @@ -68,39 +59,43 @@ protected override void InitializeMessages()
protected override void GenerateMessage()
{
contacts.Time.SetCurrentTime();
// if (contacts.contact.Count > 0)
// {
// Debug.Log(contacts.contact[0].Depths.Length + " : " + contacts.contact[0].Normals.Count);
// }
PushDeviceMessage<messages.Contacts>(contacts);
}

public string GetColliderParentName(in Collider collider)
public string GetColliderName(in Collider collider)
{
return collider.transform.parent.name;
var collisionHelper = collider.transform.gameObject.GetComponentInParent<SDF.Helper.Collision>();
if (collisionHelper == null)
{
return collider.transform.parent.name + "::" + collider.name;
}
else
{
var linkHelper = collisionHelper.GetComponentInParent<SDF.Helper.Link>();
return linkHelper.Model.name + "::" + linkHelper.name + "::" + collisionHelper.name;
}
}

public string GetColliderName(in Collider collider)
public void CollisionEnter(Collision other)
{
var childName = collider.name;
var parentName = GetColliderParentName(collider);
return parentName + "::" + childName;
// Debug.Log("CollisionEnter: " + other.contacts.Length);
}

public void CollisionEnter(Collision other)
public void CollisionStay(Collision other)
{
// Debug.Log("CollisionStay: " + other.contacts.Length);
for (var i = 0; i < other.contacts.Length; i++)
{
var collisionContact = other.contacts[i];
var collision1 = GetColliderParentName(collisionContact.thisCollider);
var collision1 = GetColliderName(collisionContact.thisCollider);
var collision2 = GetColliderName(collisionContact.otherCollider);
// Debug.Log("CollisionEnter: " + collision1 + " <-> " + collision2);
// Debug.Log($"CollisionStay: {collision1} <-> {collision2}");

if (string.Equals(collision1, targetCollision))
if (collision1.EndsWith("::" + targetCollision))
{
// find existing collision set
var foundContact = contacts.contact.Find(x => x.Collision1.Contains(collision1) && x.Collision2.Contains(collision2));
if (foundContact == null)
var existingContact = contacts.contact.Find(x => x.Collision1.Contains(collision1) && x.Collision2.Contains(collision2));
if (existingContact == null)
{
var newContact = new messages.Contact();
// newContact.Wrenchs // TODO: Need to be implemented;
Expand All @@ -109,70 +104,32 @@ public void CollisionEnter(Collision other)
newContact.Collision1 = collision1;
newContact.Collision2 = collision2;

newContact.Depths = new double[0];
newContact.Depths = new double[] { collisionContact.separation };

contacts.contact.Add(newContact);
}
}
}
}
var normal = new messages.Vector3d();
normal.Set(collisionContact.normal);
newContact.Normals.Add(normal);

public void CollisionStay(Collision other)
{
for (var i = 0; i < other.contacts.Length; i++)
{
var collisionContact = other.contacts[i];
var collision1 = GetColliderParentName(collisionContact.thisCollider);
if (!string.Equals(collision1, targetCollision))
{
continue;
}
var position = new messages.Vector3d();
position.Set(collisionContact.point);
newContact.Positions.Add(position);

var collision2 = GetColliderName(collisionContact.otherCollider);
// Debug.Log("CollsiionStay: " + collision1 + " " + collision2);
newContact.Time.SetCurrentTime();
// Debug.Log("CollisionStay: " + collision1 + " <-> " + collision2);

// find existing collision set
var existingContact = contacts.contact.Find(x => x.Collision1.Contains(collision1) && x.Collision2.Contains(collision2));
if (existingContact != null)
{
// Debug.Log("Existing!!");
var depths = existingContact.Depths;
Array.Resize(ref depths, depths.Length + 1);
depths[depths.Length - 1] = collisionContact.separation;
existingContact.Depths = depths;

var normal = new messages.Vector3d();
normal.Set(collisionContact.normal);
existingContact.Normals.Add(normal);

var position = new messages.Vector3d();
position.Set(collisionContact.point);
existingContact.Positions.Add(position);

existingContact.Time.SetCurrentTime();
// Debug.Log("CollisionStay: " + collision1 + " <-> " + collision2);
contacts.contact.Add(newContact);
}
}

// Debug.DrawLine(collisionContact.point, collisionContact.normal, Color.white);
}

// Debug.Log("CollisionStay: " + contacts.contact.Count);
}

public void CollisionExit(Collision other)
{
// Debug.Log("CollisionExit: " + other.contactCount + " ," + other.collider.name);
var collision2 = GetColliderName(other.collider);
var foundContacts = contacts.contact.FindAll(x => x.Collision2.Contains(collision2));

foreach (var foundContact in foundContacts)
{
// Debug.Log("CollisionExit: Remove " + foundContact.Collision1 + " <-> " + foundContact.Collision2);
contacts.contact.Remove(foundContact);
}

// if (contacts.contact.Count == 0)
// {
// Debug.Log("CollisionExit: no contacts");
// }
// Debug.Log($"CollisionExit: {other.contacts.Length}");
contacts.contact.Clear();
}

public bool IsContacted()
Expand Down
37 changes: 9 additions & 28 deletions Assets/Scripts/Devices/MicomSensor.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,7 @@ public partial class MicomSensor : Device
private List<SensorDevices.Sonar> ussSensors = new List<SensorDevices.Sonar>();
private List<SensorDevices.Sonar> irSensors = new List<SensorDevices.Sonar>();
// private List<SensorDevices.Magnet> magnetSensors = null;
private List<SensorDevices.Contact> bumperSensors = new List<SensorDevices.Contact>();

// private List<ArticulationBody> bumperSensors = new List<ArticulationBody>();
private List<SensorDevices.Contact> _bumperSensors = new List<SensorDevices.Contact>();


protected override void OnAwake()
Expand Down Expand Up @@ -103,48 +101,31 @@ public void SetMagnet(in List<string> magnetList)
{
// TODO: to be implemented
}
Debug.Log("Magnet is to be implemented");
}

public void SetBumper(in List<string> bumperList)
{
// Debug.Log(targetContactName);
var contactsInChild = GetComponentsInChildren<SensorDevices.Contact>();

var bumperCount = 0;
foreach (var bumper in bumperList)
{
foreach (var contact in contactsInChild)
{
if (contact.name.Equals(bumper))
{
bumperSensors.Add(contact);
_bumperSensors.Add(contact);
bumperCount++;
Debug.Log("Found " + contact.name);
break;
}
}
}

var bumperCount = bumperSensors.Count;
micomSensorData.bumper.Bumpeds = new bool[bumperCount];
}

#if false
public void SetBumperSensor(in List<string> bumperJointNameList)
{
if (bumperContact != null)
{
var linkList = GetComponentsInChildren<SDF.Helper.Link>();
foreach (var link in linkList)
{
foreach (var bumperJointName in bumperJointNameList)
{
// TODO: to be implemented
}
}

var bumperCount = bumperSensors.Count;
micomSensorData.bumper.Bumpeds = new bool[bumperCount];
}
}
#endif

public void SetBattery(in SensorDevices.Battery targetBattery)
{
micomSensorData.Battery = new messages.Battery();
Expand Down Expand Up @@ -219,9 +200,9 @@ private void UpdateBumper()
return;
}

for (var index = 0; index < bumperSensors.Count; index++)
for (var index = 0; index < _bumperSensors.Count; index++)
{
var bumperSensor = bumperSensors[index];
var bumperSensor = _bumperSensors[index];
micomSensorData.bumper.Bumpeds[index] = bumperSensor.IsContacted();
// Debug.Log(micomSensorData.bumper.Bumpeds[index]);
}
Expand Down
3 changes: 1 addition & 2 deletions Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -295,9 +295,8 @@ public static Device AddContact(this UE.GameObject targetObject, in SDF.Contact
contact.topic = element.topic;

var contactTrigger = targetObject.AddComponent<SensorDevices.ContactTrigger>();
contactTrigger.collisionEnter = contact.CollisionEnter;
contactTrigger.collisionExit = contact.CollisionExit;
contactTrigger.collisionStay = contact.CollisionStay;
contactTrigger.collisionExit = contact.CollisionExit;

return contact;
}
Expand Down