Laser Sick Range doesn't work
Can anybody help me? I'm not able to use the laser .
[
Partner("Laser",Contract = sicklrf.
Contract.Identifier,CreationPolicy =
PartnerCreationPolicy.UseExistingOrCreate)]sicklrf.
SickLRFOperations _laserPort =new sicklrf.SickLRFOperations();sicklrf.
SickLRFOperations _laserNotify =new sicklrf.SickLRFOperations(); Then, I subscribe to it like that:
void
SubscribeToLaser(){
_laserPort.Subscribe(_laserNotify);
Activate(
Arbiter.Receive<sicklrf.Replace>(true, _laserNotify, LaserNotificationHandler));}
And, finally, the Laser Handler is:
void LaserNotificationHandler(sicklrf.Replace updatelaser){
sicklrf.
State laserData = updatelaser.Body;// comprueba si hay un obstaculo justo delanteStartMove(0);
int distancia=FindNearestObstacleInCorridor(laserData,CorridorWidthMapping,45);if (distancia < ObstaculoDistancia){
EnableMotor(
false);}
}
// Obtiene la distancia del obstaculo más cercano al robot en un ángulo dadoprivateint FindNearestObstacleInCorridor(sicklrf.State laserData,int width,int fov){
int index;int best = 8192;int count = laserData.DistanceMeasurements.Length;double rangeLow = -laserData.AngularRange / 2.0;double rangeHigh = laserData.AngularRange / 2.0;double span = laserData.AngularRange;for (index = 0; index < count; index++){
double angle = rangeLow + (span * index) / count;if (Math.Abs(angle) < fov){
angle = angle *
Math.PI / 180;int range = laserData.DistanceMeasurements[index];int x = (int)(range *Math.Sin(angle));int y = (int)(range *Math.Cos(angle));if (Math.Abs(x) < width){
if (range < best){
best = range;
}
}
}
}
return best;}
void EnableMotor(bool enable){
drive.
EnableDriveRequest request =new drive.EnableDriveRequest();request.Enable = enable;
_drivePort.EnableDrive(request);
}
}
Please, I don't know what's wrong with this code. The simulation begins, but the robot move straigh until crashes with an object. Thanks
Hi, you nseed to send a message to set the Motor velocity for both wheels, to zero. That will stop the robot.
I will also move this thread to the community forum, its more appropriate there
Hi George,
I made some changes to my laser sick code. The problem isn't only the Motor velocity, because the laser doesn't work yet.
Code Snippet
[
Partner("Laser", Contract = sicklrf.
Contract.Identifier, CreationPolicy =
PartnerCreationPolicy.UseExistingOrCreate)] sicklrf.
SickLRFOperations _laserPort = new sicklrf.SickLRFOperations(); sicklrf.
SickLRFOperations _laserNotify = new sicklrf.SickLRFOperations(); Then I use the arbites to receive the notifications:
Code Snippet
#region
request handler setup Activate(
Arbiter.Interleave( new TeardownReceiverGroup( ), new ExclusiveReceiverGroup( Arbiter.Receive<LaserRangeFinderUpdate>(true, _mainPort, LaserRangeFinderUpdateHandler), Arbiter.Receive<BumperUpdate>(true, _mainPort, BumperUpdateHandler) ),
new ConcurrentReceiverGroup( ) )
);
#endregion
#region
notification handler setup Activate(
Arbiter.Interleave( new TeardownReceiverGroup(), new ExclusiveReceiverGroup(), new ConcurrentReceiverGroup( Arbiter.Receive<CONTACTSENSOR.< FONT>Update>(true, _contactNotificationPort, BumperUpdateNotification) )
)
);
Activate(
Arbiter.ReceiveWithIterator<SICKLRF.< FONT>Replace>(false, _laserNotify, LaserReplaceNotification) );
#endregion
Subscribe to the laser:
Code Snippet
_laserPort.Subscribe(_laserNotify);
And finally, the notification and the request handlers:
Code Snippet
IEnumerator
<ITask> LaserReplaceNotification(sicklrf.Replace replace) {
// When this handler is called a couple of notifications may // have piled up. We only want the most recent one. sicklrf.
State laserData = replace.Body; LaserRangeFinderUpdate request = new LaserRangeFinderUpdate(laserData); _mainPort.Post(request);
yield return Arbiter.Choice( request.ResponsePort,
delegate(DefaultUpdateResponseType response) { }, delegate(W3C.Soap.Fault fault) { } );
}
void LaserRangeFinderUpdateHandler(LaserRangeFinderUpdate update) {
sicklrf.
State laserData = update.Body; // comprueba si hay un obstaculo justo delante int distancia=FindNearestObstacleInCorridor(laserData,CorridorWidthMapping,45); if (distancia < ObstaculoDistancia) {
EnableMotor(
false); }
}
The function FindNearestObstacleInCorridor is a function like the one in explorer tutorial sample.
I think that never begins the LaserReplaceNotification handler. i don't know what I have to do to scanning.Thanks
I dont see anything wrong with your code. However i suspect your actual alser is not really talking to our sick laer service or the service is not started.
1) What hardware platform are you using? (what robot)
2) Have you traced through the code of our sick laser service to make sure it starts? Do you know the serial port COM number, is it configured properly?
3) If you are using a Pioneer-like robot, did you try using our Arcos sample services and the Explorere tutorial? If that works, then at least we can narrow out any hardware issues
Hi George,
I'm not using any hardware. The only thing I need is a simulation. I'm using the MobileRobots.P3DX.Simulation.manifest to start, but the problem is that the bumpers are all OK, but the laser doesn't work. What could be the reason for this?
I want the robot to stop in the simulation when an obstacle is near to it.
How can I trace through my code to make sure it starts?
Thanks a lot.
