locked
Multiple Sonars RRS feed

  • Question

  • Hello!

    I have problem with getting measurement from multiple sonar sensors in simulation. When i am getting data i got only from one of them. (Always the same - first measurement).

    simulation init:

                SimulatedSonarEntity FrontSonar = new SimulatedSonarEntity(new Pose(new Vector3(0, position.Y + 0.23f, 0), Quaternion.FromAxisAngle(0, 1, 0, 0)));
                FrontSonar.State.Name = "Sonar Left";
                FrontSonar.Parent = robotBaseEntity;
                robotBaseEntity.InsertEntity(FrontSonar);
    
                sonar.Contract.CreateService(ConstructorPort, Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
                        "http://localhost/" + FrontSonar.State.Name));
    
                SimulatedSonarEntity LeftSonar = new SimulatedSonarEntity(new Pose(new Vector3(-0.16f, position.Y + 0.23f, 0), Quaternion.FromAxisAngle(0, 1, 0, 1.5f)));
                LeftSonar.State.Name = "Sonar Front";
                LeftSonar.Parent = robotBaseEntity;
                robotBaseEntity.InsertEntity(LeftSonar);
                sonar.Contract.CreateService(ConstructorPort, Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
             "http://localhost/" + LeftSonar.State.Name));
    
                SimulatedSonarEntity RightSonar = new SimulatedSonarEntity(new Pose(new Vector3(-0.009f, position.Y + 0.177f, -0.173f), Quaternion.FromAxisAngle(0, 1, 0, -1.5f)));
                RightSonar.State.Name = "Sonar Right";
                RightSonar.Parent = robotBaseEntity;
                robotBaseEntity.InsertEntity(RightSonar);
                sonar.Contract.CreateService(ConstructorPort, Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
             "http://localhost/" + RightSonar.State.Name));


    And in the control project:

            [Partner("Sonar Front", Contract = sonar.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
            sonar.SonarOperations _FrontSonarPort = new sonar.SonarOperations();
    
            [Partner("Sonar Left", Contract = sonar.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
            sonar.SonarOperations _LeftSonarPort = new sonar.SonarOperations();
      
            [Partner("Sonar Right", Contract = sonar.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
            sonar.SonarOperations _RightSonarPort = new sonar.SonarOperations(); 
    
    
    .
    .
    .
    
    
           IEnumerator<ITask> UpdateSensorData(DateTime dateTime)
            {
                var resultPort = new CompletionPort();
                PostOnTaskCompletion(resultPort, UpdateCompass);
                PostOnTaskCompletion(resultPort, UpdateWebCamImage);
                PostOnTaskCompletion(resultPort, UpdateFrontSonar);
                PostOnTaskCompletion(resultPort, UpdateLeftSonar);      
                PostOnTaskCompletion(resultPort, UpdateInterval);
    
                Activate(Arbiter.MultipleItemReceive(false, resultPort, 5, allComplete =>
                {
                    Activate(Arbiter.ReceiveWithIterator(false, _dateTimePort, UpdateSensorData));
                    TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(100), _dateTimePort);
                }));
    
                yield break;
            }
    
    
           
             IEnumerator<ITask> UpdateFrontSonar() 
             {
    
                 var sensorOrFault = _FrontSonarPort.Get();
                   sensorOrFault = _FrontSonarPort.Get();
                 yield return sensorOrFault.Choice();
    
                 if (!HasError(sensorOrFault))
                 {
                     sonar.SonarState sensorState = (sonar.SonarState)sensorOrFault;
                     WinFormsServicePort.Post(new FormInvoke(() =>
                     {
                         Okno.label4.Text= (sensorState.DistanceMeasurement).ToString();
                     }));
                 }
                 yield break;
             }
             IEnumerator<ITask> UpdateLeftSonar()
             {
    
                 var sensorOrFault = _LeftSonarPort.Get();
                 yield return sensorOrFault.Choice();
    
                 if (!HasError(sensorOrFault))
                 {
                     sonar.SonarState sensorState = (sonar.SonarState)sensorOrFault;
                     WinFormsServicePort.Post(new FormInvoke(() =>
                     {
                         Okno.label5.Text = (sensorState.DistanceMeasurement).ToString();
                     }));
                 }
                 yield break;
             }

    Thank you for help!


    Tuesday, October 23, 2012 3:34 PM

Answers