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