Revision 98ed4757 scout/scoutsim/src/sim_frame.cpp

View differences:

scout/scoutsim/src/sim_frame.cpp
137 137
            &SimFrame::wirelessCallback, this);
138 138

  
139 139
        //Emitter default values
140
        em_aperture = PI / 6.0;
141
        em_distance = 2;
140
        em_aperture = PI / 3.0;
141
        em_distance = 1;
142

  
142 143

  
143 144
        // Teleop
144 145
        teleop_type = TELEOP_OFF;
......
221 222
                                scoutsim::Kill::Response&)
222 223
    {
223 224
        M_Scout::iterator it = scouts.find(req.name);
225
        M_Emitter::iterator m_it = emitters.find(req.name);
224 226
        if (it == scouts.end())
225 227
        {
226
            ROS_WARN("Tried to kill scout [%s], which does not exist",
227
                     req.name.c_str());
228
            return false;
228
            if (m_it == emitters.end()) {
229
                ROS_WARN("Tried to kill scout/emitter [%s], which does not exist",
230
                req.name.c_str());
231
                return false;
232
            }
233
            emitters.erase(m_it);
234
            return true;
229 235
        }
230 236

  
231 237
        scouts.erase(it);
......
667 673
        state.canvas_width = width_in_meters;
668 674
        state.canvas_height = height_in_meters;
669 675

  
670
        for (; it != end; ++it)
676
        for (; m_it != m_end; ++m_it)
671 677
        {
672 678

  
673
            it->second->update(SIM_TIME_REFRESH_RATE,
674
                               path_dc, sonar_dc,
679
            m_it->second->update(SIM_TIME_REFRESH_RATE,
680
                               path_dc,
675 681
                               path_image, lines_image, walls_image,
676 682
                               path_dc.GetBackground().GetColour(),
677
                               sonar_dc.GetBackground().GetColour(),
678 683
                               state);
679 684
        }
680 685

  
681 686

  
682
        for (; m_it != m_end; ++m_it)
687
        for (; it != end; ++it)
683 688
        {
684

  
685
            m_it->second->update(SIM_TIME_REFRESH_RATE,
686
                               path_dc,
689
            //ROS_INFO(it->second);
690
            geometry_msgs::Pose2D scout_pos;
691
            scout_pos = it->second->update(SIM_TIME_REFRESH_RATE,
692
                               path_dc, sonar_dc,
687 693
                               path_image, lines_image, walls_image,
688 694
                               path_dc.GetBackground().GetColour(),
695
                               sonar_dc.GetBackground().GetColour(),
689 696
                               state);
697

  
698
            //iterate over Emitters:
699
           for (m_it = emitters.begin(); m_it != m_end; ++m_it)
700
            {
701
                geometry_msgs::Pose2D emitter_pos;
702
                emitter_pos = m_it->second->get_pos();
703
                readBOM(scout_pos, emitter_pos,it);
704
            }
705
            
690 706
        }
691 707

  
708

  
692 709
        for (unsigned int i = 0; i < ghost_scouts.size(); ++i)
693 710
        {
694 711
            ghost_scouts.at(i)->update(SIM_TIME_REFRESH_RATE, path_dc, sonar_dc,
......
725 742
    }
726 743

  
727 744

  
728
    void SimFrame::readBOM()
745
    void SimFrame::readBOM(geometry_msgs::Pose2D scout_pos,
746
                           geometry_msgs::Pose2D emitter_pos,
747
                           M_Scout::iterator it)
729 748
    {
749
        //ROS_INFO("SCOUT: %f %f %f ", scout_pos.x, scout_pos.x, scout_pos.theta);
750
        //ROS_INFO("EMITTER: %f %f %f ", emitter_pos.x, emitter_pos.x, emitter_pos.theta);
751
        double se_distance = pow((pow((scout_pos.x - emitter_pos.x),2) + 
752
                              pow((scout_pos.y - emitter_pos.y),2)),(0.5));
753
        double se_angle = atan((scout_pos.x - emitter_pos.x)/(scout_pos.y - emitter_pos.y));
754
        if (scout_pos.y < emitter_pos.y)
755
        {
756
            se_angle = se_angle + PI/2;
757
        }        
758
        else 
759
        {
760
            se_angle = se_angle + PI*3/2;
761
        }
762
        double emitter_orient = emitter_pos.theta;
763
        
764
        //ROS_INFO("distance: %f, angle: %f, orient: %f", se_distance, se_angle, emitter_orient);
765

  
766
        if ((se_distance <= em_distance)) { //distance check
730 767

  
768
            double upper_limit = (emitter_orient + em_aperture/2);
769
            double lower_limit = (emitter_orient - em_aperture/2);
770

  
771
            //ROS_INFO("upper: %f, lower: %f", upper_limit, lower_limit+2*PI);
772

  
773
            if (upper_limit > 2*PI) {
774
               if (((se_angle < 2*PI) && (se_angle > (lower_limit))) ||
775
                   ((se_angle < upper_limit-2*PI) && (se_angle >= 0))) {
776
                it->second->pub_BOM(true);
777
                }
778
                
779
            }
780
            else if (lower_limit < 0) {
781
                if (((se_angle < 2*PI) && (se_angle > (lower_limit+2*PI))) ||
782
                   ((se_angle < upper_limit) && (se_angle >= 0))) {
783
                it->second->pub_BOM(true);
784
                }
785

  
786

  
787
            }
788
            else {
789
                if ((se_angle < upper_limit) && //angle check 
790
                    (se_angle > lower_limit)) {
791
                it->second->pub_BOM(true);      
792
                }   
793
            }
794
        }
795
            
731 796
    }
732 797

  
733 798
}

Also available in: Unified diff