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