Statistics
| Branch: | Revision:

root / scout / scoutsim / src / sonar_viz.cpp @ e5ac3afb

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
}