root / scout / scoutsim / src / sonar_viz.cpp @ f09d002e
History | View | Annotate | Download (1.01 KB)
1 | a2e6bd4c | Alex | #include <iostream> |
---|---|---|---|
2 | #include <iomanip> |
||
3 | |||
4 | #include <ros/ros.h> |
||
5 | #include <sonar/sonar_distance.h> |
||
6 | |||
7 | #define QUEUE_SIZE 10 |
||
8 | |||
9 | using namespace std; |
||
10 | |||
11 | vector<unsigned int> distances; |
||
12 | |||
13 | void sonar_distance_callback(const sonar::sonar_distance::ConstPtr& msg) |
||
14 | { |
||
15 | distances[msg->pos] = msg->distance0; |
||
16 | distances[msg->pos + 24] = msg->distance1;
|
||
17 | |||
18 | cout << endl << endl << endl << "[ ";
|
||
19 | for (int i = 0; i < 48; i++) |
||
20 | { |
||
21 | cout << setw(3) << distances[i] << " "; |
||
22 | } |
||
23 | cout << "]" << endl;
|
||
24 | } |
||
25 | |||
26 | int main(int argc, char **argv) |
||
27 | { |
||
28 | if (argc < 2) |
||
29 | { |
||
30 | cout << "Usage: " << argv[0] << " <scout_name>" << endl; |
||
31 | exit(1);
|
||
32 | } |
||
33 | |||
34 | ros::init(argc, argv, "sonar_viz");
|
||
35 | |||
36 | distances = vector<unsigned int>(48, 0); |
||
37 | ros::NodeHandle n; |
||
38 | |||
39 | string scoutname = argv[1]; |
||
40 | ros::Subscriber sonar_distance_sub = n.subscribe(scoutname + "/sonar_distance",
|
||
41 | QUEUE_SIZE, |
||
42 | sonar_distance_callback); |
||
43 | |||
44 | ros::spin(); |
||
45 | } |