Revision 98ed4757

View differences:

scout/scoutsim/src/scout.cpp
102 102
        pose_pub = node.advertise<Pose>("pose", 1);
103 103
        color_pub = node.advertise<Color>("color_sensor", 1);
104 104
        sonar_pub = node.advertise< ::messages::sonar_distance>("sonar_distance", 1);
105
        bom_pub = node.advertise<BOM>("BOM", 1);
105 106
        set_pen_srv = node.advertiseService("set_pen",
106 107
                                            &Scout::setPenCallback,
107 108
                                            this);
......
429 430
        }
430 431
    }
431 432

  
433
    void Scout::pub_BOM(bool bom_0) {
434
        BOM b;
435

  
436
        b.bom_0 = true;
437
        bom_pub.publish(b);
438
    }
439

  
440

  
432 441
    /// Sends back the position of this scout so scoutsim can save
433 442
    /// the world state
434 443
    /// @todo remove dt param
scout/scoutsim/src/scout.h
62 62
#include <scoutsim/Pose.h>
63 63
#include <scoutsim/SetPen.h>
64 64
#include <scoutsim/Color.h>
65
#include <scoutsim/BOM.h>
65 66

  
66 67
#include <geometry_msgs/Pose2D.h>
67 68

  
......
121 122
                                         world_state state);
122 123
            void paint(wxDC& dc);
123 124
            void set_sonar_visual(bool on);
125
            void pub_BOM(bool bom_0);
124 126

  
125 127
        private:
126 128
            float absolute_to_mps(int absolute_speed);
......
206 208
            ros::Publisher pose_pub;
207 209
            ros::Publisher color_pub;
208 210
            ros::Publisher sonar_pub;
211
            ros::Publisher bom_pub;
209 212
            ros::ServiceServer set_pen_srv;
210 213
            ros::ServiceServer query_encoders_srv;
211 214
            ros::ServiceServer query_linesensor_srv;
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
}
scout/scoutsim/src/sim_frame.h
148 148
            bool setTeleopCallback(SetTeleop::Request&, SetTeleop::Response&);
149 149

  
150 150
            void wirelessCallback(const messages::WirelessPacket::ConstPtr& msg);
151
            void readBOM(); 
151
            void readBOM(geometry_msgs::Pose2D scout_pos, 
152
                         geometry_msgs::Pose2D emitter_pos,
153
                         std::map<std::string, ScoutPtr>::iterator it); 
152 154

  
153 155

  
154 156
            ros::Subscriber wireless_send;

Also available in: Unified diff