Revision 7b94db2c scout/scoutsim/src/sim_frame.cpp

View differences:

scout/scoutsim/src/sim_frame.cpp
713 713

  
714 714
    void SimFrame::checkBOM(geometry_msgs::Pose2D scout_pos, 
715 715
                            M_Scout::iterator it)
716
     {
716
    {
717 717

  
718
        M_Emitter::iterator m_it = emitters.begin();
719
        M_Emitter::iterator m_end = emitters.end();
718
        geometry_msgs::Pose2D bom_pos;       
719
        double x_offset;
720
        double y_offset;    
721
        double offset_angle;
722
        double diag_dis = pow((pow((SCOUT_W/2.0),2) + 
723
                          pow((SCOUT_H/2.0),2)),(0.5));
724
        int emitter_id;
720 725

  
721
            //iterate over Emitters:
722
       for (; m_it != m_end; ++m_it)
723
        {
724
            geometry_msgs::Pose2D emitter_pos;
725
            emitter_pos = m_it->second->get_pos();
726
            if (is_inrange(scout_pos, emitter_pos, em_aperture, em_distance)) { 
727
            //if scout is within emitter range
728
            //check if emitter is within each BOM range
729
            double x_offset;
730
            double y_offset;    
731
            double offset_angle;
732
            double diag_dis = pow((pow((SCOUT_W/2.0),2) + 
726
        for (int i = 0; i < 10; i++) {
727
            emitter_id = -1;
728
                 
729
            diag_dis = pow((pow((SCOUT_W/2.0),2) + 
733 730
                              pow((SCOUT_H/2.0),2)),(0.5));
734 731

  
735
                for (int i = 0; i<10; i++) {
736
                geometry_msgs::Pose2D bom_pos;
737
                
738
                if (i == 2) { //middle left
739
                    offset_angle = PI/2.0;
740
                    bom_pos.theta = scout_pos.theta + PI/2.0;
741
                }
742
                else if (i == 7) { //middle right
743
                    offset_angle = 3*PI/2.0;
744
                    bom_pos.theta = scout_pos.theta + 3.0*PI/2.0;
745
                }
746
                else if (i < 2) { //top left
747
                    offset_angle =  atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
748
                    bom_pos.theta = scout_pos.theta + PI/2.0 * i;
749
                }
750
                else if (i >= 2 && i <5) { //bottom left
751
                    offset_angle =  PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
752
                    bom_pos.theta = scout_pos.theta + PI/2.0 + PI/2.0 * (i/4);
753
                }
754
                else if (i >= 5 && i < 8) { //bottom right
755
                    offset_angle =  PI + atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
756
                    bom_pos.theta = scout_pos.theta + PI + PI/2.0 * (i/6);
757
                }
758
                else { //top right
759
                    offset_angle =  2*PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
760
                    bom_pos.theta = scout_pos.theta + 3*PI/2.0 + PI/2.0 * (i/9);
761
                }
762
                offset_angle += scout_pos.theta;
763
                
764
                if (i == 2 || i == 7) {
765
                    x_offset = (SCOUT_W/2.0)*cos(offset_angle);
766
                    y_offset = (SCOUT_W/2.0)*sin(offset_angle);
767
                }
768
                else {
769
                    x_offset = diag_dis*cos(offset_angle);
770
                    y_offset = diag_dis*sin(offset_angle);
771
                }
732
            if (i == 2) { //middle left
733
                offset_angle = PI/2.0;
734
                bom_pos.theta = scout_pos.theta + PI/2.0;
735
            }
736
            else if (i == 7) { //middle right
737
                offset_angle = 3*PI/2.0;
738
                bom_pos.theta = scout_pos.theta + 3.0*PI/2.0;
739
            }
740
            else if (i < 2) { //top left
741
                offset_angle =  atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
742
                bom_pos.theta = scout_pos.theta + PI/2.0 * i;
743
            }
744
            else if (i >= 2 && i <5) { //bottom left
745
                offset_angle =  PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
746
                bom_pos.theta = scout_pos.theta + PI/2.0 + PI/2.0 * (i/4);
747
            }
748
            else if (i >= 5 && i < 8) { //bottom right
749
                offset_angle =  PI + atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
750
                bom_pos.theta = scout_pos.theta + PI + PI/2.0 * (i/6);
751
            }
752
            else { //top right
753
                offset_angle =  2*PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
754
                bom_pos.theta = scout_pos.theta + 3*PI/2.0 + PI/2.0 * (i/9);
755
            }
756
            offset_angle += scout_pos.theta;
757
            
758
            if (i == 2 || i == 7) {
759
                x_offset = (SCOUT_W/2.0)*cos(offset_angle);
760
                y_offset = (SCOUT_W/2.0)*sin(offset_angle);
761
            }
762
            else {
763
                x_offset = diag_dis*cos(offset_angle);
764
                y_offset = diag_dis*sin(offset_angle);
765
            }
772 766

  
773
                bom_pos.x = scout_pos.x + x_offset;
774
                bom_pos.y = scout_pos.y - y_offset;  
767
            bom_pos.x = scout_pos.x + x_offset;
768
            bom_pos.y = scout_pos.y - y_offset;  
775 769

  
776
                if (bom_pos.theta > 2*PI) {
777
                    bom_pos.theta -= 2*PI;
778
                }  
779
                else if (bom_pos.theta < 0) {
780
                    bom_pos.theta += 2*PI;
781
                }
770
            if (bom_pos.theta > 2*PI) {
771
                bom_pos.theta -= 2*PI;
772
            }  
773
            else if (bom_pos.theta < 0) {
774
                bom_pos.theta += 2*PI;
775
            }
782 776

  
783
                if(is_inrange(emitter_pos, bom_pos, BOM_APERTURE, BOM_DISTANCE)) {
784
                    string em_id_str = m_it->first.substr(m_it->first.find("_")+1);
785
                    int emitter_id = atoi(em_id_str.c_str());
786
                    it->second->update_BOM(i, 1, emitter_id);
787
                }
788
                else {
789
                    it->second->update_BOM(i, 0, 0);
790
                }
791
                }
792
            } else {
793
                // nothing in the emitter range, set everything to 0
794
                for (int i = 0; i<10; i++) {
795
                    it->second->update_BOM(i, 0, 0);
796
                }
777
            emitter_id = getActiveEmitter(bom_pos);
778

  
779
            if (emitter_id >= 0) {                
780
                it->second->update_BOM(i, 1, emitter_id);
797 781
            }
782
            else {
783
                it->second->update_BOM(i, 0, -1);
784
            }
785

  
798 786
        }
799 787

  
788

  
789
    }
790

  
791
    int SimFrame::getActiveEmitter(geometry_msgs::Pose2D bom_pos) {
792

  
793
        M_Emitter::iterator m_it = emitters.begin();
794
        M_Emitter::iterator m_end = emitters.end();
795
        int emitter_id = -1;
796
        for (; m_it != m_end; ++m_it) {
797
            geometry_msgs::Pose2D emitter_pos;
798
            emitter_pos = m_it->second->get_pos();
799

  
800
            if(is_inrange(bom_pos, emitter_pos, em_aperture, em_distance) &&
801
               is_inrange(emitter_pos, bom_pos, BOM_APERTURE, BOM_DISTANCE)) {
802
                string em_id_str = m_it->first.substr(m_it->first.find("_")+1);
803
                emitter_id = atoi(em_id_str.c_str());
804
                return emitter_id;
805
            }
806
        }
807
        return -1;
800 808
    }
801 809

  
802 810

  

Also available in: Unified diff