Colony » Colony Scout » Colony Scout OS

#include <iostream>

#include <iomanip>

#include <ros/ros.h>

#include <sonar/sonar_distance.h>

#define QUEUE_SIZE 10

using namespace std;

vector<unsigned int> distances;

void sonar_distance_callback(const sonar::sonar_distance::ConstPtr& msg)

{

    distances[msg->pos] = msg->distance0;

    distances[msg->pos + 24] = msg->distance1;

    cout << endl << endl << endl << "[ ";

    for (int i = 0; i < 48; i++)

    {

        cout << setw(3) << distances[i] << " ";

    }

    cout << "]" << endl;

}

int main(int argc, char **argv)

{

    if (argc < 2)

    {

        cout << "Usage: " << argv[0] << " <scout_name>" << endl;

        exit(1);

    }

    ros::init(argc, argv, "sonar_viz");

    distances = vector<unsigned int>(48, 0);

    ros::NodeHandle n;

    string scoutname = argv[1];

    ros::Subscriber sonar_distance_sub = n.subscribe(scoutname + "/sonar_distance",

                                                     QUEUE_SIZE,

                                                     sonar_distance_callback);

    ros::spin();

}