commit 98ed4757b766f53b0a452ca49f0df34ce58c1d75
Author: Hui Jun Tay <viki@c3po.(none)>
Date:   Sat Oct 19 23:00:48 2013 -0500

    Basic BOM works (only 1 BOM in 'centre' of scout)
    Added BOM topic to scouts
    Added 'kill' for emitters

diff --git a/scout/scoutsim/src/sim_frame.cpp b/scout/scoutsim/src/sim_frame.cpp
index 34d29c5..768d44f 100644
--- a/scout/scoutsim/src/sim_frame.cpp
+++ b/scout/scoutsim/src/sim_frame.cpp
@@ -137,8 +137,9 @@ namespace scoutsim
             &SimFrame::wirelessCallback, this);
 
         //Emitter default values
-        em_aperture = PI / 6.0;
-        em_distance = 2;
+        em_aperture = PI / 3.0;
+        em_distance = 1;
+
 
         // Teleop
         teleop_type = TELEOP_OFF;
@@ -221,11 +222,16 @@ namespace scoutsim
                                 scoutsim::Kill::Response&)
     {
         M_Scout::iterator it = scouts.find(req.name);
+        M_Emitter::iterator m_it = emitters.find(req.name);
         if (it == scouts.end())
         {
-            ROS_WARN("Tried to kill scout [%s], which does not exist",
-                     req.name.c_str());
-            return false;
+            if (m_it == emitters.end()) {
+                ROS_WARN("Tried to kill scout/emitter [%s], which does not exist",
+                req.name.c_str());
+                return false;
+            }
+            emitters.erase(m_it);
+            return true;
         }
 
         scouts.erase(it);
@@ -667,28 +673,39 @@ namespace scoutsim
         state.canvas_width = width_in_meters;
         state.canvas_height = height_in_meters;
 
-        for (; it != end; ++it)
+        for (; m_it != m_end; ++m_it)
         {
 
-            it->second->update(SIM_TIME_REFRESH_RATE,
-                               path_dc, sonar_dc,
+            m_it->second->update(SIM_TIME_REFRESH_RATE,
+                               path_dc,
                                path_image, lines_image, walls_image,
                                path_dc.GetBackground().GetColour(),
-                               sonar_dc.GetBackground().GetColour(),
                                state);
         }
 
 
-        for (; m_it != m_end; ++m_it)
+        for (; it != end; ++it)
         {
-
-            m_it->second->update(SIM_TIME_REFRESH_RATE,
-                               path_dc,
+            //ROS_INFO(it->second);
+            geometry_msgs::Pose2D scout_pos;
+            scout_pos = it->second->update(SIM_TIME_REFRESH_RATE,
+                               path_dc, sonar_dc,
                                path_image, lines_image, walls_image,
                                path_dc.GetBackground().GetColour(),
+                               sonar_dc.GetBackground().GetColour(),
                                state);
+
+            //iterate over Emitters:
+           for (m_it = emitters.begin(); m_it != m_end; ++m_it)
+            {
+                geometry_msgs::Pose2D emitter_pos;
+                emitter_pos = m_it->second->get_pos();
+                readBOM(scout_pos, emitter_pos,it);
+            }
+            
         }
 
+
         for (unsigned int i = 0; i < ghost_scouts.size(); ++i)
         {
             ghost_scouts.at(i)->update(SIM_TIME_REFRESH_RATE, path_dc, sonar_dc,
@@ -725,9 +742,57 @@ namespace scoutsim
     }
 
 
-    void SimFrame::readBOM()
+    void SimFrame::readBOM(geometry_msgs::Pose2D scout_pos,
+                           geometry_msgs::Pose2D emitter_pos,
+                           M_Scout::iterator it)
     {
+        //ROS_INFO("SCOUT: %f %f %f ", scout_pos.x, scout_pos.x, scout_pos.theta);
+        //ROS_INFO("EMITTER: %f %f %f ", emitter_pos.x, emitter_pos.x, emitter_pos.theta);
+        double se_distance = pow((pow((scout_pos.x - emitter_pos.x),2) + 
+                              pow((scout_pos.y - emitter_pos.y),2)),(0.5));
+        double se_angle = atan((scout_pos.x - emitter_pos.x)/(scout_pos.y - emitter_pos.y));
+        if (scout_pos.y < emitter_pos.y)
+        {
+            se_angle = se_angle + PI/2;
+        }        
+        else 
+        {
+            se_angle = se_angle + PI*3/2;
+        }
+        double emitter_orient = emitter_pos.theta;
+        
+        //ROS_INFO("distance: %f, angle: %f, orient: %f", se_distance, se_angle, emitter_orient);
+
+        if ((se_distance <= em_distance)) { //distance check
 
+            double upper_limit = (emitter_orient + em_aperture/2);
+            double lower_limit = (emitter_orient - em_aperture/2);
+
+            //ROS_INFO("upper: %f, lower: %f", upper_limit, lower_limit+2*PI);
+
+            if (upper_limit > 2*PI) {
+               if (((se_angle < 2*PI) && (se_angle > (lower_limit))) ||
+                   ((se_angle < upper_limit-2*PI) && (se_angle >= 0))) {
+                it->second->pub_BOM(true);
+                }
+                
+            }
+            else if (lower_limit < 0) {
+                if (((se_angle < 2*PI) && (se_angle > (lower_limit+2*PI))) ||
+                   ((se_angle < upper_limit) && (se_angle >= 0))) {
+                it->second->pub_BOM(true);
+                }
+
+
+            }
+            else {
+                if ((se_angle < upper_limit) && //angle check 
+                    (se_angle > lower_limit)) {
+                it->second->pub_BOM(true);      
+                }   
+            }
+        }
+            
     }
 
 }
