Revision 7b94db2c

View differences:

scout/libscout/src/test_behaviors/ParkScout.cpp
1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25

  
26
#include "ParkScout.h"
27
#define LEFT_EMITTER_ID 110
28
#define RIGHT_EMITTER_ID 111
29

  
30

  
31
#define LEFT 0
32
#define RIGHT 1
33

  
34
using namespace std;
35

  
36
ParkScout::ParkScout(string scoutname, Sensors* sensors) 
37
    : Behavior(scoutname, "Park Scout", sensors) {
38
    name = scoutname;
39
    scout_pos = new pos;
40
}
41

  
42
void ParkScout::run() {
43
    
44
   /* update_readings();
45
    start_side = get_side(readings);
46

  
47
    while ((start_side == LEFT && checkBOM(3, RIGHT_EMITTER_ID)) ||
48
           (start_side == RIGHT && checkBOM(6, RIGHT_EMITTER_ID))) {
49
       motors->set_sides(100, 100, MOTOR_PERCENT);
50
       bomR = bom->query();
51
       update_readings();
52
    }
53

  
54
    motors->set_sides(0, 0, MOTOR_PERCENT);
55
           
56
    
57
    while(ok) {   
58
        reverse_park();  
59
    }  */
60

  
61

  
62
}
63
/*
64
void reverse_park() {
65
    if(checkBOM(5, LEFT_EMITTER_ID) || 
66
       checkBOM(4, RIGHT_EMITTER_ID)) { //move forward
67
        motors->set_sides(10, 10, MOTOR_PERCENT);
68
    }
69
    else if(checkBOM(4, LEFT_EMITTER_ID) && 
70
            checkBOM(5, RIGHT_EMITTER_ID)) {
71
        motors->set_sides(-20, -20, MOTOR_PERCENT);
72
    }
73
    else {   
74
        if (start_side == RIGHT) { //reverseright
75
            motors->set_sides(100, 20, MOTOR_PERCENT);
76
        }
77
        else { //reverseleft
78
            motors->set_sides(20, 100, MOTOR_PERCENT);
79
        
80
        }
81
    }
82
    
83
}
84

  
85
int get_side() {
86
    
87
    return RIGHT;
88
}
89

  
90
bool checkBOM(int bNum, int ID) {    
91
    if(readings[bNum] == true) {
92
        ROS_INFO("Emitter ID: %d", senders[bNum]);
93
        return senders[bNum] == ID;
94
    }    
95
    return false;
96
    
97
}
98

  
99
void update_readings() {
100
    BomReadings bomR = bom->query();
101
    readings = bomR.readings;
102
    senders = bomR.senders;
103

  
104
}*/
scout/libscout/src/test_behaviors/ParkScout.h
1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25

  
26
#ifndef _PARK_SCOUT_H_
27
#define _PARK_SCOUT_H_
28
#include "../Behavior.h"
29
#include "../Sensors.h"
30
#include "../helper_classes/ScoutPosition.h"
31
/* Details about map:
32
 * 1 meter = 200 pixels
33
 * 1 block is a 13x13 pixel section
34
 * actual meter x meter map has 15 blocks in one row/column
35
 * # blocks per robot map row = 2*(block per map's row)-1
36
 */
37

  
38
// number of blocks in one row/column of robot's map
39
#define MAP_LENGTH 29
40

  
41

  
42
class ParkScout : public Behavior
43
{
44
    public:
45
    ParkScout(std::string scoutname, Sensors* sensors);
46
    void run();
47
    private:    
48
    std::string name;
49
    pos* scout_pos;
50
    /*int start_side;
51
    std::vector<uint32_t> readings;
52
    std::vector<uint32_t> senders;
53

  
54
    void reverse_park(); 
55
    int get_side(); 
56
    bool checkBOM(int bNum, int ID); 
57
    void update_readings(); */
58

  
59
};
60
#endif
scout/scoutsim/src/sim_frame.cpp
713 713

  
714 714
    void SimFrame::checkBOM(geometry_msgs::Pose2D scout_pos, 
715 715
                            M_Scout::iterator it)
716
     {
716
    {
717 717

  
718
        M_Emitter::iterator m_it = emitters.begin();
719
        M_Emitter::iterator m_end = emitters.end();
718
        geometry_msgs::Pose2D bom_pos;       
719
        double x_offset;
720
        double y_offset;    
721
        double offset_angle;
722
        double diag_dis = pow((pow((SCOUT_W/2.0),2) + 
723
                          pow((SCOUT_H/2.0),2)),(0.5));
724
        int emitter_id;
720 725

  
721
            //iterate over Emitters:
722
       for (; m_it != m_end; ++m_it)
723
        {
724
            geometry_msgs::Pose2D emitter_pos;
725
            emitter_pos = m_it->second->get_pos();
726
            if (is_inrange(scout_pos, emitter_pos, em_aperture, em_distance)) { 
727
            //if scout is within emitter range
728
            //check if emitter is within each BOM range
729
            double x_offset;
730
            double y_offset;    
731
            double offset_angle;
732
            double diag_dis = pow((pow((SCOUT_W/2.0),2) + 
726
        for (int i = 0; i < 10; i++) {
727
            emitter_id = -1;
728
                 
729
            diag_dis = pow((pow((SCOUT_W/2.0),2) + 
733 730
                              pow((SCOUT_H/2.0),2)),(0.5));
734 731

  
735
                for (int i = 0; i<10; i++) {
736
                geometry_msgs::Pose2D bom_pos;
737
                
738
                if (i == 2) { //middle left
739
                    offset_angle = PI/2.0;
740
                    bom_pos.theta = scout_pos.theta + PI/2.0;
741
                }
742
                else if (i == 7) { //middle right
743
                    offset_angle = 3*PI/2.0;
744
                    bom_pos.theta = scout_pos.theta + 3.0*PI/2.0;
745
                }
746
                else if (i < 2) { //top left
747
                    offset_angle =  atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
748
                    bom_pos.theta = scout_pos.theta + PI/2.0 * i;
749
                }
750
                else if (i >= 2 && i <5) { //bottom left
751
                    offset_angle =  PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
752
                    bom_pos.theta = scout_pos.theta + PI/2.0 + PI/2.0 * (i/4);
753
                }
754
                else if (i >= 5 && i < 8) { //bottom right
755
                    offset_angle =  PI + atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
756
                    bom_pos.theta = scout_pos.theta + PI + PI/2.0 * (i/6);
757
                }
758
                else { //top right
759
                    offset_angle =  2*PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
760
                    bom_pos.theta = scout_pos.theta + 3*PI/2.0 + PI/2.0 * (i/9);
761
                }
762
                offset_angle += scout_pos.theta;
763
                
764
                if (i == 2 || i == 7) {
765
                    x_offset = (SCOUT_W/2.0)*cos(offset_angle);
766
                    y_offset = (SCOUT_W/2.0)*sin(offset_angle);
767
                }
768
                else {
769
                    x_offset = diag_dis*cos(offset_angle);
770
                    y_offset = diag_dis*sin(offset_angle);
771
                }
732
            if (i == 2) { //middle left
733
                offset_angle = PI/2.0;
734
                bom_pos.theta = scout_pos.theta + PI/2.0;
735
            }
736
            else if (i == 7) { //middle right
737
                offset_angle = 3*PI/2.0;
738
                bom_pos.theta = scout_pos.theta + 3.0*PI/2.0;
739
            }
740
            else if (i < 2) { //top left
741
                offset_angle =  atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
742
                bom_pos.theta = scout_pos.theta + PI/2.0 * i;
743
            }
744
            else if (i >= 2 && i <5) { //bottom left
745
                offset_angle =  PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
746
                bom_pos.theta = scout_pos.theta + PI/2.0 + PI/2.0 * (i/4);
747
            }
748
            else if (i >= 5 && i < 8) { //bottom right
749
                offset_angle =  PI + atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
750
                bom_pos.theta = scout_pos.theta + PI + PI/2.0 * (i/6);
751
            }
752
            else { //top right
753
                offset_angle =  2*PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
754
                bom_pos.theta = scout_pos.theta + 3*PI/2.0 + PI/2.0 * (i/9);
755
            }
756
            offset_angle += scout_pos.theta;
757
            
758
            if (i == 2 || i == 7) {
759
                x_offset = (SCOUT_W/2.0)*cos(offset_angle);
760
                y_offset = (SCOUT_W/2.0)*sin(offset_angle);
761
            }
762
            else {
763
                x_offset = diag_dis*cos(offset_angle);
764
                y_offset = diag_dis*sin(offset_angle);
765
            }
772 766

  
773
                bom_pos.x = scout_pos.x + x_offset;
774
                bom_pos.y = scout_pos.y - y_offset;  
767
            bom_pos.x = scout_pos.x + x_offset;
768
            bom_pos.y = scout_pos.y - y_offset;  
775 769

  
776
                if (bom_pos.theta > 2*PI) {
777
                    bom_pos.theta -= 2*PI;
778
                }  
779
                else if (bom_pos.theta < 0) {
780
                    bom_pos.theta += 2*PI;
781
                }
770
            if (bom_pos.theta > 2*PI) {
771
                bom_pos.theta -= 2*PI;
772
            }  
773
            else if (bom_pos.theta < 0) {
774
                bom_pos.theta += 2*PI;
775
            }
782 776

  
783
                if(is_inrange(emitter_pos, bom_pos, BOM_APERTURE, BOM_DISTANCE)) {
784
                    string em_id_str = m_it->first.substr(m_it->first.find("_")+1);
785
                    int emitter_id = atoi(em_id_str.c_str());
786
                    it->second->update_BOM(i, 1, emitter_id);
787
                }
788
                else {
789
                    it->second->update_BOM(i, 0, 0);
790
                }
791
                }
792
            } else {
793
                // nothing in the emitter range, set everything to 0
794
                for (int i = 0; i<10; i++) {
795
                    it->second->update_BOM(i, 0, 0);
796
                }
777
            emitter_id = getActiveEmitter(bom_pos);
778

  
779
            if (emitter_id >= 0) {                
780
                it->second->update_BOM(i, 1, emitter_id);
797 781
            }
782
            else {
783
                it->second->update_BOM(i, 0, -1);
784
            }
785

  
798 786
        }
799 787

  
788

  
789
    }
790

  
791
    int SimFrame::getActiveEmitter(geometry_msgs::Pose2D bom_pos) {
792

  
793
        M_Emitter::iterator m_it = emitters.begin();
794
        M_Emitter::iterator m_end = emitters.end();
795
        int emitter_id = -1;
796
        for (; m_it != m_end; ++m_it) {
797
            geometry_msgs::Pose2D emitter_pos;
798
            emitter_pos = m_it->second->get_pos();
799

  
800
            if(is_inrange(bom_pos, emitter_pos, em_aperture, em_distance) &&
801
               is_inrange(emitter_pos, bom_pos, BOM_APERTURE, BOM_DISTANCE)) {
802
                string em_id_str = m_it->first.substr(m_it->first.find("_")+1);
803
                emitter_id = atoi(em_id_str.c_str());
804
                return emitter_id;
805
            }
806
        }
807
        return -1;
800 808
    }
801 809

  
802 810

  
scout/scoutsim/src/sim_frame.h
157 157
                         double aperture, double distance);
158 158
            void checkBOM(geometry_msgs::Pose2D scout_pos,
159 159
                          M_Scout::iterator it); 
160
                         // std::map<std::string, EmitterPtr>::iterator m_it,
161
                         // std::map<std::string, EmitterPtr>::iterator m_end);
162

  
160
            int getActiveEmitter(geometry_msgs::Pose2D bom_pos);
161
            
163 162

  
164 163
            ros::Subscriber wireless_send;
165 164
            ros::Publisher wireless_receive;

Also available in: Unified diff