commit 7b94db2c14faecf7d0e26707fda7b90a3217aedf
Author: Hui Jun Tay <viki@c3po.(none)>
Date:   Mon Feb 3 07:52:26 2014 -0600

    Fixed bug with BOM sensors reading from emitters (edge detection)
    Added basic starter code for ParkScout behavior

diff --git a/scout/scoutsim/src/sim_frame.cpp b/scout/scoutsim/src/sim_frame.cpp
index 6c479dd..7817721 100644
--- a/scout/scoutsim/src/sim_frame.cpp
+++ b/scout/scoutsim/src/sim_frame.cpp
@@ -713,90 +713,98 @@ namespace scoutsim
 
     void SimFrame::checkBOM(geometry_msgs::Pose2D scout_pos, 
                             M_Scout::iterator it)
-     {
+    {
 
-        M_Emitter::iterator m_it = emitters.begin();
-        M_Emitter::iterator m_end = emitters.end();
+        geometry_msgs::Pose2D bom_pos;       
+        double x_offset;
+        double y_offset;    
+        double offset_angle;
+        double diag_dis = pow((pow((SCOUT_W/2.0),2) + 
+                          pow((SCOUT_H/2.0),2)),(0.5));
+        int emitter_id;
 
-            //iterate over Emitters:
-       for (; m_it != m_end; ++m_it)
-        {
-            geometry_msgs::Pose2D emitter_pos;
-            emitter_pos = m_it->second->get_pos();
-            if (is_inrange(scout_pos, emitter_pos, em_aperture, em_distance)) { 
-            //if scout is within emitter range
-            //check if emitter is within each BOM range
-            double x_offset;
-            double y_offset;    
-            double offset_angle;
-            double diag_dis = pow((pow((SCOUT_W/2.0),2) + 
+        for (int i = 0; i < 10; i++) {
+            emitter_id = -1;
+                 
+            diag_dis = pow((pow((SCOUT_W/2.0),2) + 
                               pow((SCOUT_H/2.0),2)),(0.5));
 
-                for (int i = 0; i<10; i++) {
-                geometry_msgs::Pose2D bom_pos;
-                
-                if (i == 2) { //middle left
-                    offset_angle = PI/2.0;
-                    bom_pos.theta = scout_pos.theta + PI/2.0;
-                }
-                else if (i == 7) { //middle right
-                    offset_angle = 3*PI/2.0;
-                    bom_pos.theta = scout_pos.theta + 3.0*PI/2.0;
-                }
-                else if (i < 2) { //top left
-                    offset_angle =  atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
-                    bom_pos.theta = scout_pos.theta + PI/2.0 * i;
-                }
-                else if (i >= 2 && i <5) { //bottom left
-                    offset_angle =  PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
-                    bom_pos.theta = scout_pos.theta + PI/2.0 + PI/2.0 * (i/4);
-                }
-                else if (i >= 5 && i < 8) { //bottom right
-                    offset_angle =  PI + atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
-                    bom_pos.theta = scout_pos.theta + PI + PI/2.0 * (i/6);
-                }
-                else { //top right
-                    offset_angle =  2*PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
-                    bom_pos.theta = scout_pos.theta + 3*PI/2.0 + PI/2.0 * (i/9);
-                }
-                offset_angle += scout_pos.theta;
-                
-                if (i == 2 || i == 7) {
-                    x_offset = (SCOUT_W/2.0)*cos(offset_angle);
-                    y_offset = (SCOUT_W/2.0)*sin(offset_angle);
-                }
-                else {
-                    x_offset = diag_dis*cos(offset_angle);
-                    y_offset = diag_dis*sin(offset_angle);
-                }
+            if (i == 2) { //middle left
+                offset_angle = PI/2.0;
+                bom_pos.theta = scout_pos.theta + PI/2.0;
+            }
+            else if (i == 7) { //middle right
+                offset_angle = 3*PI/2.0;
+                bom_pos.theta = scout_pos.theta + 3.0*PI/2.0;
+            }
+            else if (i < 2) { //top left
+                offset_angle =  atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
+                bom_pos.theta = scout_pos.theta + PI/2.0 * i;
+            }
+            else if (i >= 2 && i <5) { //bottom left
+                offset_angle =  PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
+                bom_pos.theta = scout_pos.theta + PI/2.0 + PI/2.0 * (i/4);
+            }
+            else if (i >= 5 && i < 8) { //bottom right
+                offset_angle =  PI + atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
+                bom_pos.theta = scout_pos.theta + PI + PI/2.0 * (i/6);
+            }
+            else { //top right
+                offset_angle =  2*PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
+                bom_pos.theta = scout_pos.theta + 3*PI/2.0 + PI/2.0 * (i/9);
+            }
+            offset_angle += scout_pos.theta;
+            
+            if (i == 2 || i == 7) {
+                x_offset = (SCOUT_W/2.0)*cos(offset_angle);
+                y_offset = (SCOUT_W/2.0)*sin(offset_angle);
+            }
+            else {
+                x_offset = diag_dis*cos(offset_angle);
+                y_offset = diag_dis*sin(offset_angle);
+            }
 
-                bom_pos.x = scout_pos.x + x_offset;
-                bom_pos.y = scout_pos.y - y_offset;  
+            bom_pos.x = scout_pos.x + x_offset;
+            bom_pos.y = scout_pos.y - y_offset;  
 
-                if (bom_pos.theta > 2*PI) {
-                    bom_pos.theta -= 2*PI;
-                }  
-                else if (bom_pos.theta < 0) {
-                    bom_pos.theta += 2*PI;
-                }
+            if (bom_pos.theta > 2*PI) {
+                bom_pos.theta -= 2*PI;
+            }  
+            else if (bom_pos.theta < 0) {
+                bom_pos.theta += 2*PI;
+            }
 
-                if(is_inrange(emitter_pos, bom_pos, BOM_APERTURE, BOM_DISTANCE)) {
-                    string em_id_str = m_it->first.substr(m_it->first.find("_")+1);
-                    int emitter_id = atoi(em_id_str.c_str());
-                    it->second->update_BOM(i, 1, emitter_id);
-                }
-                else {
-                    it->second->update_BOM(i, 0, 0);
-                }
-                }
-            } else {
-                // nothing in the emitter range, set everything to 0
-                for (int i = 0; i<10; i++) {
-                    it->second->update_BOM(i, 0, 0);
-                }
+            emitter_id = getActiveEmitter(bom_pos);
+
+            if (emitter_id >= 0) {                
+                it->second->update_BOM(i, 1, emitter_id);
             }
+            else {
+                it->second->update_BOM(i, 0, -1);
+            }
+
         }
 
+
+    }
+
+    int SimFrame::getActiveEmitter(geometry_msgs::Pose2D bom_pos) {
+
+        M_Emitter::iterator m_it = emitters.begin();
+        M_Emitter::iterator m_end = emitters.end();
+        int emitter_id = -1;
+        for (; m_it != m_end; ++m_it) {
+            geometry_msgs::Pose2D emitter_pos;
+            emitter_pos = m_it->second->get_pos();
+
+            if(is_inrange(bom_pos, emitter_pos, em_aperture, em_distance) &&
+               is_inrange(emitter_pos, bom_pos, BOM_APERTURE, BOM_DISTANCE)) {
+                string em_id_str = m_it->first.substr(m_it->first.find("_")+1);
+                emitter_id = atoi(em_id_str.c_str());
+                return emitter_id;
+            }
+        }
+        return -1;
     }
 
 
