Revision 93cebb99
added BomControl class and to the scout sensor
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