Project

General

Profile

Revision 7b94db2c

ID7b94db2c14faecf7d0e26707fda7b90a3217aedf
Parent b71f5bca
Child fb9dad3d

Added by Hui Jun Tay about 10 years ago

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

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