Revision 8ae3771a
change type of bom reading to include sender
scout/libscout/src/BomControl.cpp | ||
---|---|---|
45 | 45 |
node.serviceClient< ::messages::query_boms>(scoutname+"/query_BOM"); |
46 | 46 |
} |
47 | 47 |
|
48 |
vector<uint32_t> BomControl::query()
|
|
48 |
BomReadings BomControl::query()
|
|
49 | 49 |
{ |
50 | 50 |
::messages::query_boms srv; |
51 | 51 |
|
... | ... | |
60 | 60 |
int(srv.response.readings.size())); |
61 | 61 |
} |
62 | 62 |
|
63 |
return srv.response.readings; |
|
63 |
BomReadings result; |
|
64 |
result.readings = srv.response.readings; |
|
65 |
result.senders = srv.response.senders; |
|
66 |
return result; |
|
64 | 67 |
} |
65 | 68 |
|
scout/libscout/src/BomControl.h | ||
---|---|---|
37 | 37 |
#include <ros/ros.h> |
38 | 38 |
#include <messages/query_boms.h> |
39 | 39 |
|
40 |
typedef struct BomReadings { |
|
41 |
std::vector<uint32_t> readings; |
|
42 |
std::vector<uint32_t> senders; |
|
43 |
} BomReadings; |
|
44 |
|
|
40 | 45 |
class BomControl |
41 | 46 |
{ |
42 | 47 |
public: |
... | ... | |
45 | 50 |
std::string scoutname); |
46 | 51 |
|
47 | 52 |
/** Use ROS to get the current readings. */ |
48 |
std::vector<uint32_t> query();;
|
|
53 |
BomReadings query();
|
|
49 | 54 |
|
50 | 55 |
/** */ |
51 | 56 |
//std::double getSourceEstimation(); |
scout/libscout/src/test_behaviors/ApproachEmitter.cpp | ||
---|---|---|
39 | 39 |
} |
40 | 40 |
|
41 | 41 |
void ApproachEmitter::run() { |
42 |
vector<uint32_t> r;
|
|
42 |
BomReadings bomR;
|
|
43 | 43 |
int r_weighted, l_weighted; |
44 | 44 |
int speed = 20; |
45 |
vector<uint32_t> r; |
|
46 |
vector<uint32_t> s; |
|
45 | 47 |
while (ok()) { |
46 |
r = bom->query(); |
|
48 |
bomR = bom->query(); |
|
49 |
r = bomR.readings; |
|
50 |
s = bomR.senders; |
|
47 | 51 |
if (accumulate(r.begin(), r.end(), 0)) { |
48 |
l_weighted = r[0] *BOM_W1 + r[1]*BOM_W2 + r[2]*BOM_W3 + r[3]*BOM_W4 + r[4]*BOM_W5;
|
|
49 |
r_weighted = r[9] *BOM_W1 + r[8]*BOM_W2 + r[7]*BOM_W3 + r[6]*BOM_W4 - r[5]*BOM_W5;
|
|
50 |
motors->set_sides(min(r_weighted+speed, 100),
|
|
51 |
min(l_weighted+speed, 100), MOTOR_PERCENT);
|
|
52 |
l_weighted = r[0] *BOM_W1 + r[1]*BOM_W2 + r[2]*BOM_W3 + r[3]*BOM_W4 + r[4]*BOM_W5; |
|
53 |
r_weighted = r[9] *BOM_W1 + r[8]*BOM_W2 + r[7]*BOM_W3 + r[6]*BOM_W4 - r[5]*BOM_W5; |
|
54 |
motors->set_sides(min(r_weighted+speed, 100), |
|
55 |
min(l_weighted+speed, 102), MOTOR_PERCENT);
|
|
52 | 56 |
} |
53 | 57 |
} |
54 | 58 |
} |
Also available in: Unified diff