root / scout / scoutsim / src / sonar_viz.cpp @ dd065971
History | View | Annotate | Download (1.01 KB)
1 |
#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 |
} |