Ejemplo n.º 1
0
        /// <summary>
        /// Helper for notification handlers.  This relies on being called from
        /// an "exclusive" handler - it modifies the state directly!
        /// </summary>
        /// <param name="autoDef"></param>
        /// <param name="values"></param>
        /// <param name="timestamp"></param>
        private void autoSubscribeNotificationHelper(AutoDefinition autoDef, List <Double> values, DateTime timestamp, bool keysChanged)
        {
            int useableValueCount =
                ((autoDef.subsetStart + (autoDef.subsetCount >= 1 ? autoDef.subsetCount : Int32.MaxValue)) <= values.Count) ?
                autoDef.subsetCount :
                (values.Count - autoDef.subsetStart);

            if (useableValueCount < 0)
            {
                useableValueCount = 0;
            }
            int vectorPartCount =
                (autoDef.subsetCount >= 1) ?
                autoDef.subsetCount :
                useableValueCount;

            //Console.WriteLine("Vector: for " + autoDef.infoAsPartner.Name.Name + " got " + values.Count + " values with " + useableCount + " useable");

            _state.Timestamp = timestamp;

            // If the number of values has changed, we need to rearrange the vector
            if (modifyStateSubset(autoDef, values))
            {
                _state.Validate();

                // Reapply keys
                applyKeysFromSubsets();

                // Send replace notification, a lot changed
                LogInfo("Vector length changed");
                SendNotification(new Replace(_state));
            }
            else
            {
                if (keysChanged)
                {
                    applyKeysFromSubsets();
                }

                // Send notification
                var indices = new List <int>(autoDef.count);
                for (int i = autoDef.startIndex; i < autoDef.startIndex + autoDef.count; i++)
                {
                    indices.Add(i);
                }
                SendNotification(new SetByIndex(new SetByIndexRequestType()
                {
                    Indices   = indices,
                    Values    = _state.Values.GetRange(autoDef.startIndex, autoDef.count),
                    Timestamp = timestamp
                }));
            }
        }
Ejemplo n.º 2
0
 /// <summary>
 /// Helper for notification handlers.  This relies on being called from
 /// an "exclusive" handler - it modifies the state directly!
 /// </summary>
 /// <param name="autoDef"></param>
 /// <param name="index"></param>
 /// <param name="value"></param>
 /// <param name="timestamp"></param>
 private void autoSubscribeNotificationUpdateHelper(AutoDefinition autoDef, int index, double value, DateTime timestamp)
 {
     if ((index - autoDef.subsetStart) < autoDef.count && index >= autoDef.subsetStart)
     {
         _state.Set(index - autoDef.subsetStart + autoDef.startIndex, value, timestamp);
         //Console.WriteLine("Updating " + index + " with start " + autoDef.startIndex + " and subset start " + autoDef.subsetStart + ".  State index " + (index - autoDef.subsetStart + autoDef.startIndex));
     }
     SendNotification(new SetByIndex(new SetByIndexRequestType()
     {
         Indices = new List <int>()
         {
             autoDef.startIndex + index
         }, Values = new List <double>()
         {
             value
         }, Timestamp = timestamp
     }));
 }
Ejemplo n.º 3
0
        private bool modifyStateSubset(AutoDefinition autoDef, List <Double> values)
        {
            // Figure out how many values we are actually using from this
            // publisher, based on the subset specification for this
            // subscription
            int useableValueCount =
                ((autoDef.subsetStart + (autoDef.subsetCount >= 1 ? autoDef.subsetCount : Int32.MaxValue)) <= values.Count) ?
                autoDef.subsetCount :
                (values.Count - autoDef.subsetStart);

            if (useableValueCount < 0)
            {
                useableValueCount = 0;
            }
            int vectorPartCount =
                (autoDef.subsetCount >= 1) ?
                autoDef.subsetCount :
                useableValueCount;

            if (vectorPartCount != autoDef.count)
            {
                // Shift Vector elements (not calling SetAll because we'll call SetKeys later to rebuild index cache)
                var newValues = new List <double>(_state.Values.Count + vectorPartCount - autoDef.count);
                newValues.AddRange(_state.Values.GetRange(0, autoDef.startIndex));
                if (useableValueCount > 0)
                {
                    newValues.AddRange(values.GetRange(autoDef.subsetStart, useableValueCount));
                }
                for (int i = useableValueCount; i < vectorPartCount; i++)
                {
                    newValues.Add(0.0);
                }
                newValues.AddRange(_state.Values.GetRange(autoDef.startIndex + autoDef.count, _state.Values.Count - (autoDef.startIndex + autoDef.count)));
                if (newValues.Count != (_state.Values.Count + vectorPartCount - autoDef.count))
                {
                    LogError("Vector length consistency check No. 1 failed!!!");
                }
                _state.Values = newValues;

                // Update AutoDefinitions startIndex's
                int firstChangedDef = autoDefs.IndexOf(autoDef) + 1;
                autoDef.count = vectorPartCount;
                int lastIndex = autoDef.startIndex + vectorPartCount;
                for (int i = firstChangedDef; i < autoDefs.Count; i++)
                {
                    autoDefs[i].startIndex = lastIndex;
                    lastIndex += autoDefs[i].count;
                }

                // Consistency check
                if (lastIndex != _state.Values.Count)
                {
                    LogError("Vector length consistency check No. 2 failed!!!");
                }

                return(true);
            }
            else
            {
                // Just modify the affected elements
                for (int i = 0; i < useableValueCount; i++)
                {
                    _state.Values[autoDef.startIndex + i] = values[autoDef.subsetStart + i];
                }

                return(false);
            }
        }
Ejemplo n.º 4
0
        /// <summary>
        /// Tries to subscribe to an auto partner single contract, or throws an exception
        /// if we don't support the contract.  Also starts a 1-second poll that continues
        /// until publishers send non-zero-length arrays.
        /// </summary>
        /// <param name="def"></param>
        /// <param name="serviceInfo"></param>
        private void subscribeAutoSingle(AutoDefinition def, ServiceInfoType serviceInfo)
        {
            // Check for each contract we know about and subscribe.

            ////////////////// Analog Sensor
            if (serviceInfo.Contract.Equals(analog.Contract.Identifier))
            {
                var partnerPort = ServiceForwarder <analog.AnalogSensorOperations>(new Uri(serviceInfo.Service));
                var notifyPort  = new Port <analog.Replace>();
                Activate(Arbiter.Choice(partnerPort.Subscribe(notifyPort, typeof(analog.Replace)),
                                        delegate(SubscribeResponseType success)
                {
                    MainPortInterleave.CombineWith(new Interleave(
                                                       new ExclusiveReceiverGroup(
                                                           Arbiter.Receive(true, notifyPort,
                                                                           delegate(analog.Replace replace)
                                                                           { autoSubscribeNotificationHelper(def, new List <double>(1)
                        {
                            replace.Body.RawMeasurement
                        }, replace.Body.TimeStamp, false); }
                                                                           )),
                                                       new ConcurrentReceiverGroup()));
                },
                                        delegate(Fault failure)
                {
                    LogError("Fault while subscribing to auto partner", failure);
                }));
            }

            ////////////////// Analog Sensor Array
            else if (serviceInfo.Contract.Equals(analogArray.Contract.Identifier))
            {
                var  partnerPort     = ServiceForwarder <analogArray.AnalogSensorOperations>(new Uri(serviceInfo.Service));
                var  notifyPort      = new Port <analogArray.Replace>();
                var  pollPort        = base.TimeoutPort(1000);
                bool gotNonzeroState = false;
                Activate(Arbiter.Receive(true, pollPort,
                                         delegate(DateTime time)
                {
                    if (gotNonzeroState == false)
                    {
                        Activate(Arbiter.Choice(partnerPort.Get(),
                                                delegate(analogArray.AnalogSensorArrayState state) { notifyPort.Post(new analogArray.Replace(state)); },
                                                delegate(Fault failure) { LogError("Vector got fault while trying to get polled state", failure); }));
                        TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(1000.0), pollPort);
                    }
                }));
                Activate(Arbiter.Choice(partnerPort.Subscribe(notifyPort, typeof(analogArray.Replace)),
                                        delegate(SubscribeResponseType success)
                {
                    MainPortInterleave.CombineWith(new Interleave(
                                                       new ExclusiveReceiverGroup(
                                                           Arbiter.Receive(true, notifyPort,
                                                                           delegate(analogArray.Replace replace)
                    {
                        if (replace.Body.Sensors.Count > 0)
                        {
                            gotNonzeroState = true;
                        }
                        autoSubscribeNotificationHelper(def,
                                                        new List <double>(from s in replace.Body.Sensors select s.RawMeasurement),
                                                        (replace.Body.Sensors.Count > 0 ? replace.Body.Sensors[0].TimeStamp : DateTime.Now), false);
                    })),
                                                       new ConcurrentReceiverGroup()));
                },
                                        delegate(Fault failure)
                {
                    LogError("Fault while subscribing to auto partner", failure);
                }));
            }

            ////////////////// Contact Sensor
            else if (serviceInfo.Contract.Equals(contact.Contract.Identifier))
            {
                var partnerPort = ServiceForwarder <contact.ContactSensorArrayOperations>(new Uri(serviceInfo.Service));
                var notifyPort  = new PortSet <contact.Replace, contact.Update>();
                // Post an initial Replace notification to update the HW ID map (no, this is now done automatically)
                //notifyPort.Post(new contact.Replace(RSUtils.ReceiveSync(TaskQueue, partnerPort.Get(), Params.defaultRecieveTimeout)));
                var  pollPort        = base.TimeoutPort(1000);
                bool gotNonzeroState = false;
                Activate(Arbiter.Receive(true, pollPort,
                                         delegate(DateTime time)
                {
                    if (gotNonzeroState == false)
                    {
                        Activate(Arbiter.Choice(partnerPort.Get(),
                                                delegate(contact.ContactSensorArrayState state) { notifyPort.Post(new contact.Replace(state)); },
                                                delegate(Fault failure) { LogError("Vector got fault while trying to get polled state", failure); }));
                        TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(1000.0), pollPort);
                    }
                }));
                Activate(Arbiter.Choice(partnerPort.Subscribe(notifyPort, typeof(contact.Replace), typeof(contact.Update)),
                                        delegate(SubscribeResponseType success)
                {
                    var contactHWIDMap = new Dictionary <int, int>();
                    MainPortInterleave.CombineWith(new Interleave(
                                                       new ExclusiveReceiverGroup(
                                                           Arbiter.Receive <contact.Replace>(true, notifyPort,
                                                                                             delegate(contact.Replace replace)
                    {
                        if (replace.Body.Sensors.Count > 0)
                        {
                            gotNonzeroState = true;
                        }
                        if (def.hwKeys.Count < replace.Body.Sensors.Count)
                        {
                            def.hwKeys.Capacity = replace.Body.Sensors.Count;
                            while (def.hwKeys.Count < replace.Body.Sensors.Count)
                            {
                                def.hwKeys.Add("");
                            }
                        }
                        // Refresh the HW ID map
                        contactHWIDMap.Clear();
                        for (int i = 0; i < replace.Body.Sensors.Count; i++)
                        {
                            contactHWIDMap.Add(replace.Body.Sensors[i].HardwareIdentifier, i);
                            //if (def.hwKeys[i] == null || def.hwKeys[i].Length <= 0)
                            def.hwKeys[i] = replace.Body.Sensors[i].Name;
                        }
                        autoSubscribeNotificationHelper(def,
                                                        new List <double>(from s in replace.Body.Sensors select(s.Pressed ? 1.0 : 0.0)),
                                                        (replace.Body.Sensors.Count > 0 ? replace.Body.Sensors[0].TimeStamp : DateTime.Now), true);
                    }),
                                                           Arbiter.Receive <contact.Update>(true, notifyPort,
                                                                                            delegate(contact.Update update)
                    {
                        gotNonzeroState = true;
                        try
                        {
                            int i = contactHWIDMap[update.Body.HardwareIdentifier];
                            autoSubscribeNotificationUpdateHelper(def, i, (update.Body.Pressed ? 1.0 : 0.0), update.Body.TimeStamp);
                        }
                        catch (KeyNotFoundException)
                        {
                            LogError("Vector got update for contact sensor hardware identifier " + update.Body.HardwareIdentifier + " and doesn't know about this hardware ID.  Trying to fetch entire state.");
                            Activate(Arbiter.Choice(partnerPort.Get(),
                                                    delegate(contact.ContactSensorArrayState state) { notifyPort.Post(new contact.Replace(state)); },
                                                    delegate(Fault failure) { LogError("Vector got fault while trying to get contact state", failure); }));
                        }
                    })),
                                                       new ConcurrentReceiverGroup()));
                },
                                        delegate(Fault failure)
                {
                    LogError("Fault while subscribing to auto partner", failure);
                }));
            }
            else
            {
                throw new UnrecognizedContractException();
            }
        }
Ejemplo n.º 5
0
        ///// <summary>
        ///// </summary>
        //private void subscribeAutos()
        //{
        //    // Get the list of partners whose names start with "auto:"
        //    //var request = new partnerList.GetOperation()
        //    //{
        //    //    Body = new GetRequestType(),
        //    //    ResponsePort = new DsspResponsePort<PartnerListType>()
        //    //};
        //    //base.PartnerListManagerPort.Post(request);
        //    //Activate(Arbiter.Choice<PartnerListType, Fault>(
        //    //    request.ResponsePort,
        //    //    delegate(PartnerListType partners)
        //    //    {
        //    //        string autoPrefix = "Auto_";
        //    //        List<PartnerType> autoPartners = new List<PartnerType>(
        //    //            from partner in partners.PartnerList
        //    //            where partner.Name.Name.StartsWith(autoPrefix)
        //    //            select partner);

        //    //        foreach (var p in autoPartners)
        //    //            Console.WriteLine(p.Name);

        //    //        // This method will take care of subscribing and updating state
        //    //        //if (autoPartners.Count > 0)
        //    //        //    subscribeAutos2(autoPartners, autoPrefix.Length);
        //    //    },
        //    //    delegate(Fault failure)
        //    //    {
        //    //        LogError("Fault while getting partner list to subscribe to autos", failure);
        //    //    }));

        //    //List<PartnerType> partnerList = new List<PartnerType>();
        //    //var responsePort = new DsspResponsePort<DefaultUpsertResponseType>();
        //    //Activate(Arbiter.MultipleItemReceive(responsePort, _state.AutoSubsets.Count,
        //    //    delegate(ICollection<DefaultUpsertResponseType> successes, ICollection<Fault> failures)
        //    //    {
        //    //        foreach (Fault failure in failures)
        //    //            LogError("Inserting partner failed", failure);
        //    subscribeAutos2();
        //    //}));
        //}

        /// <summary>
        /// Subscribes to any services specified in the partner list to
        /// automatically update the state.
        /// Once we have a list of auto partners, this method creates AutoDefiniton
        /// objects for each auto partner.  It then waits for each partner to be
        /// found, and calls subscribeAutoSingle to subscribe to each one.
        /// </summary>
        /// <param name="partList"></param>
        /// <param name="removeFromName"></param>
        private void subscribeAutos()
        {
            //foreach (var subset in _state.AutoSubsets)
            //{
            //    PartnerType partner = new PartnerType() { Name = new XmlQualifiedName(subset.PartnerName, Contract.Identifier) };
            //    partnerList.Add(partner);
            //    //base.PartnerListManagerPort.Post(new partnerList.Upsert() { Body = partner, ResponsePort = responsePort });
            //}
            // Create AutoDefinition objects
            autoDefs = new List <AutoDefinition>(_state.AutoSubsets.Count);
            //int lastIndex = 0;
            //List<string> allKeys = new List<string>();
            List <double> allValues = new List <double>();

            foreach (var subset in _state.AutoSubsets)
            {
                // Create AutoDefinition object and add to list
                //string[] keys = part.Name.Name.Substring(removeFromName).Split('_');
                //AutoSubset subset = _state.AutoSubsets.Find((s => part.Name.Name.Equals(s.PartnerName, StringComparison.OrdinalIgnoreCase)));
                //if (subset != default(AutoSubset))
                //{
                //var newKeys = new List<string>();
                //for (int i = 0; i < subset.startIndex; i++)
                //    newKeys.Add("");
                //newKeys.AddRange(subset.keys);
                //}
                AutoDefinition autoDef = new AutoDefinition()
                {
                    partnerName = subset.PartnerName,
                    startIndex  = 0,
                    count       = 0,
                    subsetStart = (subset == default(AutoSubset) ? 0 : subset.StartIndex),
                    subsetCount = (subset == default(AutoSubset) ? 0 : subset.Count),
                    keys        = (subset == default(AutoSubset) ? new List <string>() : subset.Keys)
                };
                autoDefs.Add(autoDef);
            }

            foreach (var autoDef in autoDefs)
            {
                if (autoDef.subsetCount >= 1)
                {
                    // Also build a default vector
                    var values = new List <double>(autoDef.subsetCount);
                    for (int i = 0; i < autoDef.subsetCount; i++)
                    {
                        values.Add(0.0);
                    }
                    modifyStateSubset(autoDef, values);
                }
            }


            // Set values, and take keys provided in subset defs by user
            //_state.SetAll(allValues, DateTime.Now);
            //applyKeysFromSubsets();

            //var upsertRespPort = new DsspResponsePort<DefaultUpsertResponseType>();
            //PartnerListManagerPort.Post(new partnerList.Upsert() { Body = new PartnerType() { Name = new XmlQualifiedName("iRobotGenericDrive", Microsoft.Dss.ServiceModel.Dssp.Contract.Identifier) }, ResponsePort = upsertRespPort });
            //Activate(Arbiter.Choice(upsertRespPort,
            //    delegate(DefaultUpsertResponseType suc)
            //    {
            //        //var queryRespPort = new DsspResponsePort<Microsoft.Dss.Schemas.PartnerListManager.QueryResponseType>();
            //        //PartnerListManagerPort.Post(new partnerList.Query() { Body = new partnerListS.QueryRequestType() { Name = new XmlQualifiedName("iRobotGenericDrive", Contract.Identifier) }, ResponsePort = queryRespPort });
            //        //Activate(Arbiter.Choice(queryRespPort,
            //        //    delegate(partnerListS.QueryResponseType qsuc)
            //        //    {
            //        //        Console.WriteLine(qsuc.Partner.Name + " : " + qsuc.Partner.Contract + " : " + qsuc.Partner.Service);
            //        //    },
            //        //    delegate(Fault failure)
            //        //    {
            //        //        Console.WriteLine("Fault querying");
            //        //    }));
            //        var partsRespPort = base.IssuePartnerSubscribe(new XmlQualifiedName("iRobotGenericDrive", Microsoft.Dss.ServiceModel.Dssp.Contract.Identifier));
            //        Activate(Arbiter.Choice(partsRespPort,
            //            delegate(PartnerListType partListResponse)
            //            {
            //                foreach (var partner in partListResponse.PartnerList)
            //                {
            //                    Console.WriteLine(partner.Name + " : " + partner.Contract + " : " + partner.Service);
            //                }
            //            },
            //            delegate(Fault failure)
            //            {
            //                Console.WriteLine("Fault querying");
            //            }));
            //    },
            //    delegate(Fault fail)
            //    {
            //        Console.WriteLine("Fault upserting");
            //    }));


            // Create the new vector state reflecting the keys
            //List<double> allValues = new List<double>(from k in allKeys select 0.0);
            //VectorState newState = new VectorState(allValues, allKeys, DateTime.Now);
            //var responsePort = new DsspResponsePort<DefaultReplaceResponseType>();
            //OperationsPort.Post(new Replace(newState, responsePort));
            //Activate(Arbiter.Choice(responsePort,
            //    delegate(DefaultReplaceResponseType r) { },
            //    delegate(Fault f) { LogError("Fault while replacing initial vector state", f); }));

            // Try to subscribe to compatible contracts with the AutoDefinition objects
            foreach (var autoDef in autoDefs)
            {
                AutoDefinition myAutoDef = autoDef;
                //Console.WriteLine("Waiting 10 seconds before subscribing to partner list...");
                //Thread.Sleep(10000);
                var partRespPort = base.IssuePartnerSubscribe(new XmlQualifiedName(myAutoDef.partnerName, Contract.Identifier));
                //Console.WriteLine("Looking for " + myAutoDef.infoAsPartner.Name);
                Activate(Arbiter.Choice(partRespPort,
                                        delegate(PartnerListType partListResponse)
                {
                    //Console.WriteLine("Found " + partListResponse.PartnerList.Count + " for " + myAutoDef.infoAsPartner.Name);
                    if (partListResponse.PartnerList.Count > 1)
                    {
                        LogWarning("More than one partner found for " + myAutoDef.partnerName);
                    }
                    else if (partListResponse.PartnerList.Count < 1)
                    {
                        LogWarning("No partners found for " + myAutoDef.partnerName);
                    }
                    foreach (var partner in partListResponse.PartnerList)
                    {
                        if (myAutoDef.infoAsConnected == null)
                        {
                            //Console.WriteLine("Searching for a contract for " + partner.Service);
                            Activate(Arbiter.Choice(RSUtils.FindCompatibleContract(TaskQueue, new Uri(partner.Service), new List <string>()
                            {
                                analog.Contract.Identifier, analogArray.Contract.Identifier, contact.Contract.Identifier
                            }),
                                                    delegate(ServiceInfoType serviceInfoResponse)
                            {
                                try
                                {
                                    subscribeAutoSingle(myAutoDef, serviceInfoResponse);
                                    myAutoDef.infoAsConnected = serviceInfoResponse;
                                    LogInfo("Vector service " + base.ServiceInfo.Service + " subscribed auto update to " + serviceInfoResponse.Service + " with contract " + serviceInfoResponse.Contract);
                                }
                                catch (Exception e)
                                {
                                    LogError("Exception while subscribing to auto partner", e);
                                }
                            },
                                                    delegate(Fault failure)
                            {
                                Exception e = RSUtils.ExceptionOfFault(failure);
                                if (e is NoContractFoundException)
                                {
                                    LogError("Could not subscribe to auto partner " + myAutoDef.partnerName + ".  Could not find a supported contract.");
                                }
                                else if (e is Exception)
                                {
                                    LogError("Fault while searching for compatible contract", e);
                                }
                            }));
                        }
                    }
                },
                                        delegate(Fault failure)
                {
                    Console.WriteLine("Fault for " + myAutoDef.partnerName);
                    LogError("Fault from subscription to partner list service", failure);
                }));
                //Thread.Sleep(2000);
            }
        }