Strange error while listening to LRF
Hi,
I'm new to CCR and DSS so I need a little advice on how to best carry out this task:
So, I'm trying to listen to 2 ports, the timerPort (like in ServiceTutorials) and another port called laserResultPort which carries RaycastResults. I have my LRF registered to the laserResultPort and everytime a raycastResult message is posted on this port, I want to update my service state to the latest value. The timerPort is used to update my other state members at a different rate.
private
Port<DateTime> _timerPort =newPort<DateTime>();privatePort<RaycastResult> _laserResultPort =newPort<RaycastResult>();protected
overridevoid Start(){
base.Start();//orient sim camera view point
SetupCamera();
//Add objects (entities)PopulateWorld();
_timerPort.Post(
DateTime.Now);Activate(
Arbiter.Interleave(newTeardownReceiverGroup(),newExclusiveReceiverGroup(),newConcurrentReceiverGroup(
Arbiter.Receive(true, _timerPort, TimerHandler),Arbiter.Receive(true, _laserResultPort, LaserResultHandler))));
}
void
LaserResultHandler(RaycastResult latestResult){
bool haveResults = _laserResultPort.Test(out newLaserResult);if (haveResults)
{
newint[newLaserResult.SampleCount + 1];int initValue = (int)(LASER_RANGE * 1000f);for (int i = 0; i < (newLaserResult.SampleCount + 1); i++)_state.DistanceMeasurements =
{
_state.DistanceMeasurements[i] = initValue;
}
foreach (RaycastImpactPoint ptin newLaserResult.ImpactPoints){
int)(pt.Position.W * 1000f);_state.DistanceMeasurements[pt.ReadingIndex] = (
}
_state.AngularRange = (
int)Math.Abs(robot.RobotsLaser.RaycastProperties.EndAngle- robot.RobotsLaser.RaycastProperties.StartAngle);
_state.AngularResolution = robot.RobotsLaser.RaycastProperties.AngleIncrement;
LogInfo(
"state.Measurement 0 :" + _state.DistanceMeasurements[0]);}
}
void
AddRobot(){
.
.
.
Contract.CreateService(lrf.
ConstructorPort,
Microsoft.Robotics.Simulation.
Partners.CreateEntityPartner("http://localhost/" + robot.RobotsLaser.State.Name));SimulationEngine.GlobalInstancePort.Insert(robot.RobotsLaser);robot.RoversLaser.Register(_laserResultPort);
SimulationEngine.GlobalInstancePort.Insert(robot);
}
I dont think I'm doing it right, because I'm not getting any data from the _laserPort using this method.
Before, I used to make the timerPort call its Handler to Pop stuff out of the LaserResultPort periodically. But this causes memory leak, since not all messages in the laserPort were handled.However, the point is, I can verify that the RaycastResults DO make it to the _laserResultPort. Now that I'm using a new method, I'm just not getting any message on the laserResultPort.
So to recap, my question is how to we listen to 2 ports for 2 different things, and then call there corresponding handlers.
Thanks, guys
