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);
        }
Beispiel #6
0
        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
        }