Project

General

Profile

Revision 93cebb99

ID93cebb998b81dc98f54dac5728560ecc511c7224
Parent e99a72bf
Child 7327a8ed

Added by Yuyang Guo over 10 years ago

added BomControl class and to the scout sensor

View differences:

scout/libscout/src/Behavior.cpp
60 60
    name = my_name;
61 61
    keep_running = true;
62 62
    sensors->init(&motors, &buttons, &sonar, &encoders, &linesensor,
63
                        &wl_sender, &wl_receiver, &paintboard, &metal_detector);
63
                  &wl_sender, &wl_receiver, &paintboard, &metal_detector,
64
                  &bom);
64 65
    wl_receiver->register_callback(std::bind(&Behavior::default_callback,
65 66
        this, std::placeholders::_1));
66 67

  
scout/libscout/src/Behavior.h
94 94
        WirelessReceiver * wl_receiver;
95 95
        PaintBoardControl * paintboard;
96 96
        MetalDetectorControl * metal_detector;
97
        BomControl * bom;
97 98

  
98 99
        // Default callback for wireless receiver.
99 100
        void default_callback(std::vector<uint8_t> data);
scout/libscout/src/BomControl.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
/**
27
 * @file BomControl.h
28
 * @brief Contains line follwing sensor declarations and functions
29
 *
30
 * @author Colony Project, CMU Robotics Club
31
 * @author Yuyang (Misty) Guo
32
 */
33

  
34
 #include "BomControl.h"
35

  
36
 using namespace std;
37

  
38
/**
39
 * @brief Initialize the BOM module of libscout.
40
 */
41
BomControl::BomControl(const ros::NodeHandle& libscout_node, string scoutname)
42
    : node(libscout_node)
43
{
44
    query_client =
45
        node.serviceClient< ::messages::query_boms>(scoutname+"/query_boms");
46
}
47

  
48
vector<uint32_t> BomControl::query()
49
{
50
    ::messages::query_boms srv;
51

  
52
    if (!query_client.call(srv))
53
    {
54
        ROS_ERROR("BOM_Control query failed.");
55
    }
56

  
57
    if (srv.response.readings.size() != 10)
58
    {
59
        ROS_WARN("BOM_Control reading vector has %d readings, 10 expected.",
60
                 int(srv.response.readings.size()));
61
    }
62

  
63
    return srv.response.readings;
64
}
65

  
scout/libscout/src/BomControl.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
/**
27
 * @file BomControl.h
28
 * @brief Contains line follwing sensor declarations and functions
29
 *
30
 * @author Colony Project, CMU Robotics Club
31
 * @author Yuyang (Misty) Guo
32
 */
33

  
34
#ifndef _BOM_CONTROL_H_
35
#define _BOM_CONTROL_H_
36

  
37
#include <ros/ros.h>
38
#include <messages/query_boms.h>
39

  
40
class BomControl
41
{
42
    public:
43
        /** Set up the encoders class and prepare to communicate over ROS */
44
        BomControl(const ros::NodeHandle& libscout_node,
45
                          std::string scoutname);
46

  
47
        /** Use ROS to get the current readings. */
48
        std::vector<uint32_t> query();;
49

  
50
        /**  */
51
        //std::double getSourceEstimation();
52

  
53
    private:
54
        /* ROS publisher and client declaration */
55
        ros::ServiceClient query_client;
56
        ros::NodeHandle node;
57
};
58

  
59
#endif /* _BOM_CONTORL_H_ */
60

  
scout/libscout/src/Sensors.cpp
50 50
    wl_sender = new WirelessSender(node);
51 51
    paintboard = new PaintBoardControl(node, scoutname);
52 52
    metal_detector = new MetalDetectorControl(node, scoutname);
53
    bom = new BomControl(node, scoutname);
53 54
}
54 55

  
55 56
// b_stuff stand for behavior control class
......
59 60
                  WirelessSender ** b_wl_sender,
60 61
                  WirelessReceiver ** b_wl_receiver,
61 62
                  PaintBoardControl ** b_paintboard,
62
                  MetalDetectorControl ** b_metal_detector)
63
                  MetalDetectorControl ** b_metal_detector,
64
                  BomControl ** b_bom)
63 65
{
64 66
    //(MotorControl *)(* b_motors) = motors;
65 67
    *b_motors = motors;
......
71 73
    *b_wl_receiver = new WirelessReceiver(node);
72 74
    *b_paintboard = paintboard;
73 75
    *b_metal_detector = metal_detector;
76
    *b_bom = bom;
74 77
}
scout/libscout/src/Sensors.h
43 43
#include "MotorControl.h"
44 44
#include "HeadlightControl.h"
45 45
#include "ButtonControl.h"
46
#include "BomControl.h"
46 47
#include "SonarControl.h"
47 48
//#include "CliffsensorControl.h"
48 49
#include "EncodersControl.h"
......
63 64
                  WirelessSender ** b_wl_sender,
64 65
                  WirelessReceiver ** b_wl_receiver,
65 66
                  PaintBoardControl ** b_paintboard,
66
                  MetalDetectorControl ** b_metal_detector);
67
                  MetalDetectorControl ** b_metal_detector,
68
                  BomControl ** b_bom
69
                  );
67 70
        std::string name;
68 71
    private:
69 72
        ros::NodeHandle node;
......
77 80
        WirelessReceiver * wl_receiver;
78 81
        PaintBoardControl * paintboard;
79 82
        MetalDetectorControl * metal_detector;
83
        BomControl * bom;
80 84
};
81 85

  
82 86

  
scout/messages/srv/query_boms.srv
1 1
---
2 2
# All units are simply in encoder ticks since encoders were turned
3 3
uint32[] readings
4
int8[] senders
4
uint32[] senders
scout/scoutsim/src/scout.cpp
121 121
                                  &Scout::query_linesensor_callback,
122 122
                                  this);
123 123

  
124
        query_BOM_srv =
125
            node.advertiseService("query_BOM",
126
                                  &Scout::query_BOM_callback,
127
                                  this);
128

  
129

  
124 130
        for (unsigned int i = 0; i < NUM_LINESENSORS; i++)
125 131
        {
126 132
            linesensor_readings.push_back(0);
127 133
        }
128 134

  
135
        for (unsigned int i = 0; i < NUM_BOMS; i++)
136
        {
137
            BOM_readings.push_back(0);
138
            BOM_senders.push_back(0);
139
        }
140

  
129 141
        // Initialize sonar
130 142
        sonar_position = 0;
131 143
        sonar_stop_l = 0;
......
276 288
        return true;
277 289
    }
278 290

  
291
    bool Scout::query_BOM_callback(::messages::query_boms::Request&,
292
                                   ::messages::query_boms::Response& res)
293
    {
294
        res.readings = BOM_readings;
295
        res.senders = BOM_senders;
296

  
297
        return true;
298
    }
299

  
279 300
    // Scale to linesensor value
280 301
    unsigned int Scout::rgb_to_grey(unsigned char r,
281 302
                                    unsigned char g,
......
429 450
        }
430 451
    }
431 452

  
432
    void Scout::update_BOM(int bom_index, bool bom_value) {
453
    void Scout::update_BOM(int bom_index,
454
                           unsigned int bom_value,
455
                           unsigned int sender) {
433 456
        ROS_INFO("BOM%d: %d", bom_index, bom_value);
457
        BOM_readings[bom_index] = bom_value;
458
        BOM_senders[bom_index] = sender;
434 459
    }
435 460

  
436

  
437 461
    /// Sends back the position of this scout so scoutsim can save
438 462
    /// the world state
439 463
    /// @todo remove dt param
scout/scoutsim/src/scout.h
55 55
#include <messages/set_motors.h>
56 56
#include <messages/query_encoders.h>
57 57
#include <messages/query_linesensor.h>
58
#include <messages/query_boms.h>
58 59
#include <messages/sonar_distance.h>
59 60
#include <messages/sonar_toggle.h>
60 61
#include <messages/sonar_set_scan.h>
......
73 74
#define PI 3.14159265
74 75

  
75 76
#define NUM_LINESENSORS 8
77
#define NUM_BOMS 10
76 78

  
77 79
// Distance, pixels, from center of robot to the linesensors.
78 80
#define LNSNSR_D 20
......
121 123
                                         world_state state);
122 124
            void paint(wxDC& dc);
123 125
            void set_sonar_visual(bool on);
124
            void update_BOM(int bom_index,bool bom_value);
126
            void update_BOM(int bom_index, unsigned int bom_value,
127
                            unsigned int sender);
125 128

  
126 129
        private:
127 130
            float absolute_to_mps(int absolute_speed);
......
132 135
                                     messages::query_encoders::Response&);
133 136
            bool query_linesensor_callback(messages::query_linesensor::Request&,
134 137
                                     messages::query_linesensor::Response&);
138
            bool query_BOM_callback(messages::query_boms::Request&,
139
                                     messages::query_boms::Response&);
135 140
            bool handle_sonar_toggle(messages::sonar_toggle::Request  &req,
136 141
                                     messages::sonar_toggle::Response &res);
137 142
            bool handle_sonar_set_scan(messages::sonar_set_scan::Request  &req,
......
147 152
                              double robot_theta, wxMemoryDC& sonar_dc);
148 153
            void update_linesensor(const wxImage& lines_image, int x, int y,
149 154
                                   double theta);
155

  
150 156
            int old_front_dx;
151 157
            int old_front_dy;
152 158
            int old_back_dx;
......
196 202

  
197 203
            // A vector of the 8 linesensor readings
198 204
            std::vector<unsigned int> linesensor_readings;
205
            std::vector<unsigned int> BOM_readings;
206
            std::vector<unsigned int> BOM_senders;
199 207

  
200 208
            // Each scout has a unique id number, which is also displayed on its image.
201 209
            int scout_id;
......
211 219
            ros::ServiceServer set_pen_srv;
212 220
            ros::ServiceServer query_encoders_srv;
213 221
            ros::ServiceServer query_linesensor_srv;
222
            ros::ServiceServer query_BOM_srv;
214 223
            ros::ServiceServer toggle_sonar_srv;
215 224
            ros::ServiceServer set_sonar_srv;
216 225

  
scout/scoutsim/src/sim_frame.cpp
786 786

  
787 787
                if(is_inrange(emitter_pos, bom_pos, bom_aperture, bom_distance)) {
788 788
                        
789
                        it->second->update_BOM(i, true);
789
                        it->second->update_BOM(i, 1, 0);
790
                            // TODO: change to last arg to actual sender ID
790 791
                        
791 792
                    }
792 793
                else {
793 794
                        
794
                        it->second->update_BOM(i, false);
795
                        it->second->update_BOM(i, 1, 0);
796
                            // TODO: change to last arg to actual sender ID
795 797
                        
796 798
                }
797 799
                }

Also available in: Unified diff