protected List <UnityEngine.Transform> linkList; // link list of the robot
protected virtual void Awake()
{
this.InitializeVariables();
foreach (UnityEngine.Transform link in this.linkList)
{
if (link.name == this.baseFootprintName)
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = this.odomName;
localTransformStamped.child_frame_id = link.name;
UnityEngine.Transform baseFootprintRigidbody = SIGVerseUtils.FindTransformFromChild(this.transform.root, this.baseFootprintRigidbodyName);
TfInfo localTfInfo = new TfInfo(baseFootprintRigidbody, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
else
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = link.parent.name;
localTransformStamped.child_frame_id = link.name;
TfInfo localTfInfo = new TfInfo(link, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
}
this.synchronizer = this.GetComponent <RobotPubSynchronizer>();
this.publishSequenceNumber = this.synchronizer.GetAssignedSequenceNumber();
this.isUsingThread = this.synchronizer.useThread;
}
protected virtual UnityEngine.Vector3 GetRotObservationNoise()
{
float vz = Mathf.Clamp(SIGVerseUtils.GetRandomNumberFollowingNormalDistribution(0.01f), -0.03f, +0.03f);
return(new UnityEngine.Vector3(0.0f, 0.0f, vz));
}
private void ExecCollisionProcess(Collision collision)
{
float collisionVelocity = this.colliderVelocities[Array.IndexOf(this.colliders, collision.contacts[0].thisCollider)];
SIGVerseLogger.Info("CollisionDetector Collision Detection! Time=" + Time.time + ", Collision Velocity=" + collisionVelocity +
", Part=" + collision.contacts[0].thisCollider.name + ", Collided object=" + SIGVerseUtils.GetHierarchyPath(collision.collider.transform));
// Effect
GameObject effect = MonoBehaviour.Instantiate(this.collisionEffect);
Vector3 contactPoint = SIGVerseUtils.CalcContactAveragePoint(collision);
effect.transform.position = contactPoint;
effect.transform.localScale = new Vector3(0.5f, 0.5f, 0.5f);
Destroy(effect, 1.0f);
// Sound
this.collisionAudioSource.PlayOneShot(this.collisionClip);
// Send the collision notification
foreach (GameObject destination in this.collisionNotificationDestinations)
{
ExecuteEvents.Execute <IRobotCollisionHandler>
(
target: destination,
eventData: null,
functor: (reciever, eventData) => reciever.OnRobotCollisionEnter(collision, collisionVelocity, 0.5f)
);
}
this.collidedTime = Time.time;
}
protected bool IsValidTriggerEnter(Collider other)
{
if (other.isTrigger)
{
return(false);
}
if (other.attachedRigidbody == null)
{
return(false);
}
if (this.rigidbodyMap.ContainsKey(other))
{
SIGVerseLogger.Warn("This Collider has already been added. (" + this.GetType().FullName + ") name=" + SIGVerseUtils.GetHierarchyPath(other.transform));
return(false);
}
return(true);
}
protected bool IsValidTriggerExit(Collider other)
{
if (other.isTrigger)
{
return(false);
}
if (other.attachedRigidbody == null)
{
return(false);
}
if (!this.rigidbodyMap.ContainsKey(other))
{
SIGVerseLogger.Warn("This Collider does not exist in the Dictionary. (" + this.GetType().FullName + ") name=" + SIGVerseUtils.GetHierarchyPath(other.transform));
return(false);
}
return(true);
}
protected virtual float GetRotNoise(float val)
{
float randomNumber = SIGVerseUtils.GetRandomNumberFollowingNormalDistribution(0.3f); // sigma=0.3
return(val * Mathf.Clamp(randomNumber, -0.9f, +0.9f)); // 3 * sigma
}
