Ejemplo n.º 1
0
        IEnumerator <ITask> OnConnectArticulatedArmHandler(OnConnectArticulatedArm onConnect)
        {
            arm.ArticulatedArmState armState = null;

            if (onConnect.DriveControl != _driveControl)
            {
                yield break;
            }

            _articulatedArmPort = ServiceForwarder <arm.ArticulatedArmOperations>(onConnect.Service);
            yield return(Arbiter.Choice(
                             _articulatedArmPort.Get(new GetRequestType()),
                             delegate(arm.ArticulatedArmState state) { armState = state; },
                             delegate(Fault f) { LogError(f); }
                             ));

            if (armState == null)
            {
                yield break;
            }

            WinFormsServicePort.FormInvoke(delegate()
            {
                _driveControl.ReplaceArticulatedArmJointList(armState);
            });

            yield break;
        }
Ejemplo n.º 2
0
        IEnumerator<ITask> OnConnectArticulatedArmHandler(OnConnectArticulatedArm onConnect)
        {
            arm.ArticulatedArmState armState = null;

            if (onConnect.DriveControl != _driveControl)
                yield break;

            _articulatedArmPort = ServiceForwarder<arm.ArticulatedArmOperations>(onConnect.Service);
            yield return Arbiter.Choice(
                _articulatedArmPort.Get(new GetRequestType()),
                delegate(arm.ArticulatedArmState state) { armState = state; },
                delegate(Fault f) { LogError(f); }
            );

            if (armState == null)
                yield break;

            WinFormsServicePort.FormInvoke(delegate()
            {
                _driveControl.ReplaceArticulatedArmJointList(armState);
            });

            yield break;
        }
Ejemplo n.º 3
0
        IEnumerator <ITask> AddArticulatedArm()
        {
            Vector3 position = new Vector3(0, 0, 0);

            // Create an instance of our custom arm entity.
            // Source code for this entity can be found under
            // Samples\Simulation\Entities\Entities.cs
            KukaLBR3Entity entity = new KukaLBR3Entity(position);

            // Name the entity
            entity.State.Name = "LBR3Arm";

            // Insert entity in simulation.
            SimulationEngine.GlobalInstancePort.Insert(entity);

            // create simulated arm service
            DsspResponsePort <CreateResponse> resultPort = CreateService(
                Microsoft.Robotics.Services.Simulation.LBR3Arm.Proxy.Contract.Identifier,
                Microsoft.Robotics.Simulation.Partners.CreateEntityPartner("http://localhost/" + entity.State.Name));

            // asynchronously handle service creation result.
            yield return(Arbiter.Choice(resultPort,
                                        delegate(CreateResponse rsp)
            {
                _armServicePort = ServiceForwarder <arm.ArticulatedArmOperations>(rsp.Service);
            },
                                        delegate(Fault fault)
            {
                LogError(fault);
            }));

            if (_armServicePort == null)
            {
                yield break;
            }

            // we re-issue the get until we get a response with a fully initialized state
            do
            {
                yield return(Arbiter.Receive(false, TimeoutPort(1000), delegate(DateTime signal) { }));

                yield return(Arbiter.Choice(
                                 _armServicePort.Get(new GetRequestType()),
                                 delegate(arm.ArticulatedArmState state)
                {
                    _cachedArmState = state;
                },
                                 delegate(Fault f)
                {
                    LogError(f);
                }));

                // exit on error
                if (_cachedArmState == null)
                {
                    yield break;
                }
            } while (_cachedArmState.Joints == null);

            // Start a timer that will move the arm in a loop
            // Comment out the line below if you want to control
            // the arm through SimpleDashboard
            // Spawn<DateTime>(DateTime.Now, MoveArm);
        }