gistfile1.cpp

void DoorScanner::detectDoors(emc::LaserData& scan,emc::IO &io)
{
    cont_doorTracking = 0;
    doorFound=false;
    proj_vBinVA = 0;

    for(int i=NUMBEAMS;i<scan.ranges.size()-NUMBEAMS*P3_FACTOR;i++)
    {
        if(scan.ranges[i]>0.01 && scan.ranges[i-NUMBEAMS*P3_FACTOR]>0.01 && scan.ranges[i+NUMBEAMS]>0.01)
        {
            if(scan.ranges[i]<MAX_VISION_DOORS && scan.ranges[i-NUMBEAMS]<MAX_VISION_DOORS && scan.ranges[i+NUMBEAMS*P3_FACTOR]<MAX_VISION_DOORS)
            {
                if(std::abs(scan.ranges[i]-scan.ranges[i+NUMBEAMS] <0.1))
                {
                    // Calculate the 3 points whcih will define the start and the end of the vectors
                    p1.x = scan.ranges[i+NUMBEAMS*P3_FACTOR]*cos((static_cast<double>(i+NUMBEAMS*P3_FACTOR)*270/scan.ranges.size()-45)*PI/180);
                    p1.y = scan.ranges[i+NUMBEAMS*P3_FACTOR]*sin((static_cast<double>(i+NUMBEAMS*P3_FACTOR)*270/scan.ranges.size()-45)*PI/180);

                    p2.x = scan.ranges[i]*cos((static_cast<double>(i)*270/scan.ranges.size()-45)*PI/180);
                    p2.y = scan.ranges[i]*sin((static_cast<double>(i)*270/scan.ranges.size()-45)*PI/180);

                    p3.x = scan.ranges[i-NUMBEAMS]*cos((static_cast<double>(i-NUMBEAMS)*270/scan.ranges.size()-45)*PI/180);
                    p3.y = scan.ranges[i-NUMBEAMS]*sin((static_cast<double>(i-NUMBEAMS)*270/scan.ranges.size()-45)*PI/180);

                    // Vectors are calculated
                    vA.dirX = p2.x-p1.x;
                    vA.dirY = p2.y-p1.y;

                    vB.dirX = p3.x - p2.x;
                    vB.dirY = p3.y - p2.y;

                    vAPer.dirX = -vA.dirY;
                    vAPer.dirY = vA.dirX;
                    
                    // Vector projections are calculated
                    proj_vBinVA = (vAPer.dirX*vB.dirX + vAPer.dirY*vB.dirY)/sqrt(pow(vAPer.dirX,2)+pow(vAPer.dirY,2));
                    
                    // Vector projection within the limits are stored
                    if(std::abs(proj_vBinVA)>0.265 && std::abs(proj_vBinVA)<0.5) // Projections are stored
                    {
                        if(cont_doorTracking<NUM_DOORTRACKING)
                        {
                            doorTracking[cont_doorTracking]=proj_vBinVA;
                            doorLocation[cont_doorTracking]= i+30; //Middle beam between p2 and p3 is stored
                        }
                        else
                        {
                            std::cout<< "DoorTrackingvector full:"<<std::endl;
                        }
                    }
                }

            }
         }
     }
    // Check doorTracking vectors
    for(int k=0; k<NUM_DOORTRACKING-1; k++) 
     {
         // Check sequences going from negative to positive and positive to negative
         if(doorTracking[k]<-0.01 && doorTracking[k+AUX]>0.01 || doorTracking[k]>0.01 && doorTracking[k+AUX]<-0.01)
         {
              // To calculate the door target we restore the position of the saved cornes in p5 and p6 
              p6.x = -scan.ranges[doorLocation[k+AUX]]*cos((static_cast<double>(doorLocation[k+AUX])*270/scan.ranges.size()-45)*PI/180);
              p6.y = scan.ranges[doorLocation[k+AUX]]*sin((static_cast<double>(doorLocation[k+AUX])*270/scan.ranges.size()-45)*PI/180);

              p5.x = -scan.ranges[doorLocation[k]]*cos((static_cast<double>(doorLocation[k])*270/scan.ranges.size()-45)*PI/180);
              p5.y = scan.ranges[doorLocation[k]]*sin((static_cast<double>(doorLocation[k])*270/scan.ranges.size()-45)*PI/180);

              target.x = 0.5*(p5.x+p6.x);
              target.y = 0.5*(p5.y+p6.y);
              door.width = sqrt(pow(p5.x-p6.x,2)+pow(p5.y-p6.y,2));
              
              // Checking the door width
              if((std::abs(door.width)>0.4 && std::abs(door.width<1.7))
              {
                if(target.x == 0)
                {
                     door.angle = PI/2;
                }
                else
                {
                    door.angle = std::abs(atan(target.y/target.x));
                    if(target.x<0 && target.y>0 ) //second quadrant
                        door.angle =PI-door.angle ;
                    if(target.x<0 && target.y<0 ) //third quadrant
                        door.angle =PI+door.angle ;
                    if(target.x>0 && target.y<0 ) //fourth quadrant
                        door.angle =-door.angle ;
                }
                door.radius = sqrt(pow(target.x,2)+pow(target.y,2));
                door.indentation = 0.5* (std::abs(doorTracking[k])+std::abs(doorTracking[k+1]));
                std::cout<< "Door CHECKED and TARGET set" << std::endl;
                std::cout<< " *****DOOR INFORMATION***** "<< std::endl;
                std::cout<< "door radius:"<< door.radius << std::endl;
                
                if(door.radius<TRIGGER_DOOR_RADIUS ) //&& target.y > 0.001
                {
                    doorFound=true;                   
                }
               }

         }
    }
}