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/libscout/src/test_behaviors/ParkScout.cpp b/scout/libscout/src/test_behaviors/ParkScout.cpp
new file mode 100644
index 0000000..9ce878c
--- /dev/null
+++ b/scout/libscout/src/test_behaviors/ParkScout.cpp
@@ -0,0 +1,104 @@
+/**
+ * Copyright (c) 2011 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "ParkScout.h"
+#define LEFT_EMITTER_ID 110
+#define RIGHT_EMITTER_ID 111
+
+
+#define LEFT 0
+#define RIGHT 1
+
+using namespace std;
+
+ParkScout::ParkScout(string scoutname, Sensors* sensors) 
+    : Behavior(scoutname, "Park Scout", sensors) {
+    name = scoutname;
+    scout_pos = new pos;
+}
+
+void ParkScout::run() {
+    
+   /* update_readings();
+    start_side = get_side(readings);
+
+    while ((start_side == LEFT && checkBOM(3, RIGHT_EMITTER_ID)) ||
+           (start_side == RIGHT && checkBOM(6, RIGHT_EMITTER_ID))) {
+       motors->set_sides(100, 100, MOTOR_PERCENT);
+       bomR = bom->query();
+       update_readings();
+    }
+
+    motors->set_sides(0, 0, MOTOR_PERCENT);
+           
+    
+    while(ok) {   
+        reverse_park();  
+    }  */
+
+
+}
+/*
+void reverse_park() {
+    if(checkBOM(5, LEFT_EMITTER_ID) || 
+       checkBOM(4, RIGHT_EMITTER_ID)) { //move forward
+        motors->set_sides(10, 10, MOTOR_PERCENT);
+    }
+    else if(checkBOM(4, LEFT_EMITTER_ID) && 
+            checkBOM(5, RIGHT_EMITTER_ID)) {
+        motors->set_sides(-20, -20, MOTOR_PERCENT);
+    }
+    else {   
+        if (start_side == RIGHT) { //reverseright
+            motors->set_sides(100, 20, MOTOR_PERCENT);
+        }
+        else { //reverseleft
+            motors->set_sides(20, 100, MOTOR_PERCENT);
+        
+        }
+    }
+    
+}
+
+int get_side() {
+    
+    return RIGHT;
+}
+
+bool checkBOM(int bNum, int ID) {    
+    if(readings[bNum] == true) {
+        ROS_INFO("Emitter ID: %d", senders[bNum]);
+        return senders[bNum] == ID;
+    }    
+    return false;
+    
+}
+
+void update_readings() {
+    BomReadings bomR = bom->query();
+    readings = bomR.readings;
+    senders = bomR.senders;
+
+}*/
diff --git a/scout/libscout/src/test_behaviors/ParkScout.h b/scout/libscout/src/test_behaviors/ParkScout.h
new file mode 100644
index 0000000..cd37612
--- /dev/null
+++ b/scout/libscout/src/test_behaviors/ParkScout.h
@@ -0,0 +1,60 @@
+/**
+ * Copyright (c) 2011 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef _PARK_SCOUT_H_
+#define _PARK_SCOUT_H_
+#include "../Behavior.h"
+#include "../Sensors.h"
+#include "../helper_classes/ScoutPosition.h"
+/* Details about map:
+ * 1 meter = 200 pixels
+ * 1 block is a 13x13 pixel section
+ * actual meter x meter map has 15 blocks in one row/column
+ * # blocks per robot map row = 2*(block per map's row)-1
+ */
+
+// number of blocks in one row/column of robot's map
+#define MAP_LENGTH 29
+
+
+class ParkScout : public Behavior
+{
+    public:
+    ParkScout(std::string scoutname, Sensors* sensors);
+    void run();
+    private:    
+    std::string name;
+    pos* scout_pos;
+    /*int start_side;
+    std::vector<uint32_t> readings;
+    std::vector<uint32_t> senders;
+
+    void reverse_park(); 
+    int get_side(); 
+    bool checkBOM(int bNum, int ID); 
+    void update_readings(); */
+
+};
+#endif
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;
     }
 
 
diff --git a/scout/scoutsim/src/sim_frame.h b/scout/scoutsim/src/sim_frame.h
index 96d85f3..e5742f1 100644
--- a/scout/scoutsim/src/sim_frame.h
+++ b/scout/scoutsim/src/sim_frame.h
@@ -157,9 +157,8 @@ namespace scoutsim
                          double aperture, double distance);
             void checkBOM(geometry_msgs::Pose2D scout_pos,
                           M_Scout::iterator it); 
-                         // std::map<std::string, EmitterPtr>::iterator m_it,
-                         // std::map<std::string, EmitterPtr>::iterator m_end);
-
+            int getActiveEmitter(geometry_msgs::Pose2D bom_pos);
+            
 
             ros::Subscriber wireless_send;
             ros::Publisher wireless_receive;
