Statistics
| Branch: | Revision:

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
}