void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
        {
            _entity = (simengine.CameraEntity)ins.Body;
            _entity.ServiceContract = Contract.Identifier;

            CreateDefaultState();
        }
        void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
        {
            _entity = (simengine.LaserRangeFinderEntity)ins.Body;
            _entity.ServiceContract = Contract.Identifier;

            CreateDefaultState();

            physics.RaycastProperties raycastProperties = new physics.RaycastProperties();
            raycastProperties.StartAngle     = -_state.AngularRange / 2.0f;
            raycastProperties.EndAngle       = _state.AngularRange / 2.0f;
            raycastProperties.AngleIncrement = (float)_state.AngularResolution;
            raycastProperties.Range          = LASER_RANGE;
            raycastProperties.OriginPose     = new Pose();

            _entity.RaycastProperties = raycastProperties;
            try
            {
                _entity.Register(_raycastResults);
            }
            catch (Exception ex)
            {
                LogError(ex);
            }

            // attach handler to raycast results port
            Activate(Arbiter.Receive(true, _raycastResults, RaycastResultsHandler));
        }
Ejemplo n.º 3
0
        void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
        {
            // _entity = (simengine.BumperArrayEntity)ins.Body;
            _entity = (PioneerBumperEntity)ins.Body;
            _entity.ServiceContract = Contract.Identifier;

            // reinitialize the state
            CreateDefaultState();

            pxContactSensor.ContactSensor cs = null;

            // The simulation bumper service uses a simple heuristic to assign contact sensors, to physics shapes:
            // Half the sensors go infront, the other half in the rear. Assume front sensors are first in the list
            // In the for loop below we create a lookup table that matches simulation shapes with sensors. When
            // the physics engine notifies us with a contact, it provides the shape that came into contact. We need to
            // translate that to sensor. In the future we might add an object Context field to shapes to make this easier
            for (int i = 0; i < _entity.Shapes.Length; i++)
            {
                cs      = new pxContactSensor.ContactSensor();
                cs.Name = _entity.Shapes[i].State.Name;
                cs.HardwareIdentifier = i;
                _state.Sensors.Add(cs);
                _bumperShapeToSensorTable.Add((physics.BoxShape)_entity.Shapes[i], cs);
            }

            // subscribe to bumper simulation entity for contact notifications
            _entity.Subscribe(_contactNotificationPort);
            // Activate a handler on the notification port, it will run when contacts occur in simulation
            Activate(Arbiter.Receive(false, _contactNotificationPort, PhysicsContactNotificationHandler));
        }
        void OnInsertEntity(sime.InsertSimulationEntity insert)
        {
            _entity = insert.Body as FourWheelDriveEntity;
            if (_entity != null)
            {
                _entity.ServiceContract = Contract.Identifier;

                if (_entity.ChassisShape != null)
                {
                    _state.DistanceBetweenWheels = _entity.DistanceBetweenWheels;
                    _state.WheelBase             = _entity.WheelBase;
                }
            }
        }
Ejemplo n.º 5
0
 void InsertEntityNotificationHandlerFirstTime(simengine.InsertSimulationEntity ins)
 {
     InsertEntityNotificationHandler(ins);
     base.Start();
     MainPortInterleave.CombineWith(
         new Interleave(
             new TeardownReceiverGroup(),
             new ExclusiveReceiverGroup(
                 Arbiter.Receive <simengine.InsertSimulationEntity>(true, _notificationTarget, InsertEntityNotificationHandler),
                 Arbiter.Receive <simengine.DeleteSimulationEntity>(true, _notificationTarget, DeleteEntityNotificationHandler)
                 ),
             new ConcurrentReceiverGroup()
             )
         );
 }
        void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
        {
            _entity = (simengine.DifferentialDriveEntity)ins.Body;
            _entity.ServiceContract = Contract.Identifier;

            // create default state based on the physics entity
            if (_entity.ChassisShape != null)
            {
                _state.DistanceBetweenWheels = _entity.ChassisShape.BoxState.Dimensions.X;
            }

            _state.LeftWheel.MotorState.PowerScalingFactor  = _entity.MotorTorqueScaling;
            _state.RightWheel.MotorState.PowerScalingFactor = _entity.MotorTorqueScaling;
            //SpawnIterator(TestDriveDistanceAndRotateDegrees);
        }
        void OnFoundEntity(sime.InsertSimulationEntity insert)
        {
            OnInsertEntity(insert);

            MainPortInterleave.CombineWith(
                new Interleave(
                    new TeardownReceiverGroup(),
                    new ExclusiveReceiverGroup(
                        Arbiter.Receive <sime.InsertSimulationEntity>(true, _simNotify, OnInsertEntity),
                        Arbiter.Receive <sime.DeleteSimulationEntity>(true, _simNotify, OnDeleteEntity)
                        ),
                    new ConcurrentReceiverGroup()
                    )
                );
        }
        void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
        {
            _entity = (simengine.KukaLBR3Entity)ins.Body;
            _entity.ServiceContract = Contract.Identifier;

            _state = new arm.ArticulatedArmState();

            // use the entity state as our state
            _state.Joints = _entity.Joints;

            // create dictionary for quick lookup of joints from name
            _jointLookup = new Dictionary <string, Joint>();
            foreach (Joint j in _state.Joints)
            {
                _jointLookup.Add(j.State.Name, j);
            }
        }
Ejemplo n.º 9
0
        void InsertEntityNotificationHandlerFirstTime(simengine.InsertSimulationEntity ins)
        {
            InsertEntityNotificationHandler(ins);

            base.Start();

            // Listen on the main port for requests and call the appropriate handler.
            MainPortInterleave.CombineWith(
                new Interleave(
                    new TeardownReceiverGroup(),
                    new ExclusiveReceiverGroup(
                        Arbiter.Receive <simengine.InsertSimulationEntity>(true, _notificationTarget, InsertEntityNotificationHandler),
                        Arbiter.Receive <simengine.DeleteSimulationEntity>(true, _notificationTarget, DeleteEntityNotificationHandler),
                        Arbiter.Receive <FromWinformMsg>(true, _fromWinformPort, OnWinformMessageHandler)
                        ),
                    new ConcurrentReceiverGroup()
                    )
                );
        }
Ejemplo n.º 10
0
        void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
        {
            _entity = (simengine.VisualEntity)ins.Body;
            _entity.ServiceContract = Contract.Identifier;

            // traverse the entities associated with this entity and build a list of joints
            _state.Joints.Clear();
            Dictionary <string, simengine.VisualEntity> entities = new Dictionary <string, simengine.VisualEntity>();

            FindJointsAndEntities(_entity, entities);

            // add a slider for each joint to the UI
            WinFormsServicePort.FormInvoke(
                delegate()
            {
                _simulatedBipedMoverUI.AddEntityName(_entity.State.Name, _state.Joints == null || _state.Joints.Count == 0);
                _simulatedBipedMoverUI.AddSliders(_state.Joints);
            }
                );
        }
Ejemplo n.º 11
0
        IEnumerator <ITask> OnInsertEntity(sime.InsertSimulationEntity insert)
        {
            _entity = insert.Body;

            var query = new sime.VisualEntity();

            query.State.Name = _state.CameraName;

            yield return(Arbiter.Choice(
                             _simPort.Query(query),
                             success => _camera = success.Entity as Entities.PursuitCamera.PursuitCameraEntity,
                             failure => LogError("Unable to find camera", failure)
                             ));

            if (_camera == null)
            {
                _entity = null;
                yield break;
            }

            SetCameraProperties();
            _camera.Target = _entity;
        }
Ejemplo n.º 12
0
        void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
        {
            _entity = (simengine.DepthCameraEntity)ins.Body;
            _entity.ServiceContract = Contract.Identifier;

            try
            {
                _entity.Register(_raycastResults);
            }
            catch (Exception ex)
            {
                LogError(ex);
            }

            if (_rayCastQueue == null)
            {
                _rayCastQueue = new DispatcherQueue(_entity.EntityState.Name + "depthNotifications",
                                                    TaskQueue.Dispatcher,
                                                    TaskExecutionPolicy.ConstrainQueueDepthDiscardTasks,
                                                    1);
                // attach handler to raycast results port
                _rayCastQueue.Enqueue(Arbiter.ReceiveWithIterator(false, _raycastResults, RaycastResultsHandler));
            }
        }
        void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
        {
            _entity = (simengine.CameraEntity)ins.Body;
            _entity.ServiceContract = Contract.Identifier;
            _state.CameraDeviceName = _entity.State.Name;
            _state.Pose             = _entity.State.Pose;
            _state.ViewAngle        = _entity.ViewAngle;
            _state.ImageSize        = new Vector2(_entity.ViewSizeX, _entity.ViewSizeY);

            // send an initial update to self so we get image data filled in
            PostUpdateFrameIfNeeded();

            if (_entity.IsRealTimeCamera)
            {
                if (_entity.UpdateInterval != 0)
                {
                    Activate(Arbiter.Receive(false, TimeoutPort(_entity.UpdateInterval), CheckForUpdate));
                }
                else
                {
                    Activate(Arbiter.Receive(false, TimeoutPort(1000), CheckForUpdate));
                }
            }
        }
Ejemplo n.º 14
0
        /// <summary>
        /// This handler receives an announcement from the simulation engine that
        /// contains a pointer to the entity associated with this service.
        /// </summary>
        /// <param name="ins"></param>
        void InsertEntityNotificationHandlerFirstTime(simengine.InsertSimulationEntity ins)
        {
            _entity = (simengine.IRobotCreate)ins.Body;
            _entity.ServiceContract = Contract.Identifier;
            _contactPort            = new Port <simcommon.EntityContactNotification>();
            // add the contact handler
            MainPortInterleave.CombineWith(new Interleave(
                                               new TeardownReceiverGroup(
                                                   ),
                                               new ExclusiveReceiverGroup
                                               (
                                               ),
                                               new ConcurrentReceiverGroup
                                               (
                                                   Arbiter.Receive <simcommon.EntityContactNotification>(true, _contactPort, ContactHandler)
                                               )
                                               ));

            _entity.Subscribe(_contactPort);

            PartnerType irEntityPartner = Dss.ServiceModel.DsspServiceBase.DsspServiceBase.FindPartner(
                new System.Xml.XmlQualifiedName("IREntity", Contract.Identifier), ServiceInfo.PartnerList);


            // only add an IR Entity if one was requested
            if (irEntityPartner != null)
            {
                // add an IR sensor, if necessary
                _irEntity = null;
                foreach (simengine.VisualEntity child in _entity.Children)
                {
                    if (child.GetType() == typeof(IREntity))
                    {
                        _irEntity = (IREntity)child;
                    }
                }

                if (_irEntity == null)
                {
                    // didn't find on already there so add one

                    // irentity is on the right front quadrant of the Create facing to the right
                    _irEntity = new IREntity(_entity.State.Name + "_IR", new Pose(
                                                 new Vector3(0.115f, 0.055f, -0.115f),
                                                 simengine.TypeConversion.FromXNA(xna.Quaternion.CreateFromAxisAngle(new xna.Vector3(0, 1, 0), (float)(Math.PI / 2)))));
                    _entity.InsertEntity(_irEntity);
                    // refresh the whole entity
                    System.Reflection.MethodInfo mInfo = typeof(simengine.SimulationEngine).GetMethod("RefreshEntity",
                                                                                                      System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic | System.Reflection.BindingFlags.Public);
                    if (mInfo != null)
                    {
                        mInfo.Invoke(simengine.SimulationEngine.GlobalInstance, new object[] { _entity });
                    }
                }
            }

            // start up the polling timer
            int timerInterval = (_state.PollingInterval <= 0) ? 200 : _state.PollingInterval;

            if ((timerInterval > 0 && timerInterval < 200))
            {
                timerInterval = 200;
            }

            StartTimer(timerInterval);
            // create default state based on the physics entity
            //_state.DistanceBetweenWheels = _entity.ChassisShape.BoxState.Dimensions.X;
            //_state.LeftWheel.MotorState.PowerScalingFactor = _entity.MotorTorqueScaling;
            //_state.RightWheel.MotorState.PowerScalingFactor = _entity.MotorTorqueScaling;
        }