Revision 11bb78c2 scout/scoutsim/src/sim_frame.cpp

View differences:

scout/scoutsim/src/sim_frame.cpp
136 136
        wireless_send = nh.subscribe("/wireless/send", 1000,
137 137
            &SimFrame::wirelessCallback, this);
138 138

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

  
143

  
144 139
        // Teleop
145 140
        teleop_type = TELEOP_OFF;
146 141
        teleop_l_speed = 0;
147 142
        teleop_r_speed = 0;
148 143
        teleop_scoutname = "scout1";
149 144

  
145
        //Emitter default values
146
        em_aperture = PI / 3.0;
147
        em_distance = 1.0;
148

  
149

  
150 150
        teleop_pub = nh.advertise< ::messages::set_motors>("/scout1/set_motors", 1000);
151 151

  
152 152
        ROS_INFO("Starting scoutsim with node name %s",
......
718 718
        M_Emitter::iterator m_it = emitters.begin();
719 719
        M_Emitter::iterator m_end = emitters.end();
720 720

  
721
        //Emitter default values
722
        double bom_aperture = PI / 3.0;
723
        double bom_distance = 1.0;
724
    
721 725
            //iterate over Emitters:
722 726
       for (; m_it != m_end; ++m_it)
723 727
        {
724 728
            geometry_msgs::Pose2D emitter_pos;
725 729
            emitter_pos = m_it->second->get_pos();
726
            if (is_inrange(scout_pos, emitter_pos)) { //if scout is within emitter range
730
            if (is_inrange(scout_pos, emitter_pos, em_aperture, em_distance)) { 
731
            //if scout is within emitter range
727 732
            //check if emitter is within each BOM range
728
                
733
            double x_offset;
734
            double y_offset;    
735
            double offset_angle;
736
            double diag_dis = pow((pow((SCOUT_W/2.0),2) + 
737
                              pow((SCOUT_H/2.0),2)),(0.5));   
738
                    
729 739
                for (int i = 0; i<10; i++) {
730 740
                geometry_msgs::Pose2D bom_pos;
731
                bom_pos.x = scout_pos.x;
732
                bom_pos.y = scout_pos.y;  
733
                bom_pos.theta = (scout_pos.theta+(2*PI/10.0)*i);
741
                
742
                if (i == 2) { //middle left
743
                    offset_angle = PI/2.0;
744
                    bom_pos.theta = scout_pos.theta + PI/2.0;
745
                }
746
                else if (i == 7) { //middle right
747
                    offset_angle = 3*PI/2.0;
748
                    bom_pos.theta = scout_pos.theta + 3.0*PI/2.0;
749
                }
750
                else if (i < 2) { //top left
751
                    offset_angle =  atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
752
                    bom_pos.theta = scout_pos.theta + PI/2.0 * i;
753
                }
754
                else if (i >= 2 && i <5) { //bottom left
755
                    offset_angle =  PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
756
                    bom_pos.theta = scout_pos.theta + PI/2.0 + PI/2.0 * (i/4);
757
                }
758
                else if (i >= 5 && i < 8) { //bottom right
759
                    offset_angle =  PI + atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
760
                    bom_pos.theta = scout_pos.theta + PI + PI/2.0 * (i/6);
761
                }
762
                else { //top right
763
                    offset_angle =  2*PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
764
                    bom_pos.theta = scout_pos.theta + 3*PI/2.0 + PI/2.0 * (i/9);
765
                }
766
                offset_angle += scout_pos.theta;
767
                
768
                if (i == 2 || i == 7) {
769
                    x_offset = (SCOUT_W/2.0)*cos(offset_angle);
770
                    y_offset = (SCOUT_W/2.0)*sin(offset_angle);
771
                }
772
                else {
773
                    x_offset = diag_dis*cos(offset_angle);
774
                    y_offset = diag_dis*sin(offset_angle);
775
                }
776

  
777
                bom_pos.x = scout_pos.x + x_offset;
778
                bom_pos.y = scout_pos.y - y_offset;  
734 779

  
735 780
                if (bom_pos.theta > 2*PI) {
736 781
                    bom_pos.theta -= 2*PI;
......
739 784
                    bom_pos.theta += 2*PI;
740 785
                }
741 786

  
742
                if(is_inrange(emitter_pos, bom_pos)) {
743
                        //ROS_INFO("BOM%d %f: true", i, bom_pos.theta);
744

  
787
                if(is_inrange(emitter_pos, bom_pos, bom_aperture, bom_distance)) {
788
                        
745 789
                        it->second->update_BOM(i, true);
790
                        
746 791
                    }
747 792
                else {
748
                        //ROS_INFO("BOM%d %f: false", i, bom_pos.theta);
749
                
793
                        
750 794
                        it->second->update_BOM(i, false);
795
                        
751 796
                }
752 797
                }
753 798
                
......
783 828

  
784 829

  
785 830
    bool SimFrame::is_inrange(geometry_msgs::Pose2D scout_pos,
786
                           geometry_msgs::Pose2D emitter_pos)
831
                           geometry_msgs::Pose2D emitter_pos,
832
                           double aperture, double distance)
787 833
                           
788 834
    {
789 835
        //ROS_INFO("SCOUT: %f %f %f ", scout_pos.x, scout_pos.x, scout_pos.theta);
......
803 849
        
804 850
        //ROS_INFO("distance: %f, angle: %f, orient: %f", se_distance, se_angle, emitter_orient);
805 851

  
806
        if ((se_distance <= em_distance)) { //distance check
852
        if ((se_distance <= distance)) { //distance check
807 853

  
808
            double upper_limit = (emitter_orient + em_aperture/2);
809
            double lower_limit = (emitter_orient - em_aperture/2);
854
            double upper_limit = (emitter_orient + aperture/2);
855
            double lower_limit = (emitter_orient - aperture/2);
810 856

  
811 857
            //ROS_INFO("upper: %f, lower: %f", upper_limit, lower_limit+2*PI);
812 858

  

Also available in: Unified diff