Skip to content

Instantly share code, notes, and snippets.

Created June 26, 2015 11:55
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save anonymous/bbacdbc1f334da3c0de1 to your computer and use it in GitHub Desktop.
Save anonymous/bbacdbc1f334da3c0de1 to your computer and use it in GitHub Desktop.
Doorscanner Class and Functions
double Doorscanner:: getcorridorWidth(emc::LaserData& scan)
{
pointnumR = 0;
pointnumL = 0;
//Calculating Right Limits to the wall
for(int i = 2; i >= -2; --i)
{
if(scan.ranges[500-393+i] > 0.01)
{
corridor.widthR[NUM_WTRACKS-1] = corridor.widthR[NUM_WTRACKS-1] + scan.ranges[500-393+i];
pointnumR = pointnumR + 1;
}
}
corridor.widthR[NUM_WTRACKS-1] = (1/pointnumR)*corridor.widthR[NUM_WTRACKS-1];
// Calcualte Left limits to the wall
for(int i = -2; i <= 2; ++i)
{
if(scan.ranges[500+393+i] > 0.01)
{
corridor.widthL[NUM_WTRACKS-1] = corridor.widthL[NUM_WTRACKS-1] + scan.ranges[500+393+i];
pointnumL = pointnumL + 1;
}
}
corridor.widthL[NUM_WTRACKS-1] = (1/pointnumL)*corridor.widthL[NUM_WTRACKS-1];
corridor.widthTotal[NUM_WTRACKS-1] = corridor.widthL[NUM_WTRACKS-1] + corridor.widthR[NUM_WTRACKS-1];
std::cout << "________Corridor Information_________" <<
std::cout << "Corridor Width" << corridor.widthTotal[NUM_WTRACKS-1] << std::endl;
std::cout << "Corridor right" << corridor.widthR[NUM_WTRACKS-1] << std::endl;
std::cout << "Corridor left" << corridor.widthL[NUM_WTRACKS-1] << std::endl;
}
void Doorscanner:: detectDoors(emc::IO& io,emc::LaserData& scan) //First 5 items ignore the algorithm
{
doorDetectedR= false;
doorDetectedL= false;
for(int i=1; i<NUM_WTRACKS; i++)
{
corridor.widthL[i-1] = corridor.widthL[i];
corridor.widthR[i-1] = corridor.widthR[i];
corridor.widthTotal[i-1] = corridor.widthTotal[i];
}
getcorridorWidth(scan);
cont_widthTrack++;
if(cont_widthTrack >= 5 && corridor.widthR[NUM_WTRACKS-1]<2 && corridor.widthR[1]<2 && corridor.widthL[NUM_WTRACKS-1]<2 && corridor.widthL[1]<2)
{
doorIndentR = corridor.widthR[NUM_WTRACKS-1]-corridor.widthR[1];
doorIndentL = corridor.widthL[NUM_WTRACKS-1]-corridor.widthL[1];
std::cout<<"Difference in width RIGHT"<<doorIndentR<< std::endl;
std::cout<<"Difference in width LEFT"<<doorIndentL<< std::endl;
// Difference n the right limits between bounds
if(doorIndentR > 0.25 && doorIndentR < 0.5)
{
doorFound=true;
std::cout<<"Door Detected on the RIGHT!! Difference in corridor width is "<< doorIndentR<< std::endl;
}
// Difference n the right limits between bounds
if(doorIndentL > 0.25 && doorIndentL < 0.5)
{
doorFound=true;
std::cout<<"Door Detected on the LEFT!! Difference in corridor width is "<< doorIndentL<< std::endl;
}
// Lets the robot navigate a bit more to stop inside the door area
if(doorFound == true || doorFound == true)
{
if(cont_door>15)
{
doorArea = true;
}
else
cont_door++;
}
else
cont_door = 0;
}
else
{
std::cout<<"Not enough data to detect doors"<< std::endl;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment