sensor_filter_visualizer.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <cstdlib>
#include "sensor_msgs/Image.h"
#include "sensor_msgs/CameraInfo.h"
//#include "sensor_msgs/Float32MultiArray.h"
//#include "sensor_msgs/MultiArrayDimension.h"
#include "roslib/Header.h"
#include "hrl_lib/kbhit.h"
#include "pr2_overhead_grasping/SensorPoint.h"
#include <ros/package.h>

using namespace std;

float threshs[] = {9.841934, 9.841934, 9.841934, 9.841934, 9.841934, 9.841934, 18.613708, 9.187677, 9.187677, 9.187677, 9.187677, 17.681366, 17.681366, 15.975549, 15.975549, 16.740515, 16.740515, 16.740515, 12.304399, 14.602459, 9.388476, 8.168613, 8.168613, 8.168613, 8.168613, 8.168613, 18.628269, 7.003357, 7.003357, 7.003357, 7.003357, 18.517227, 18.536757, 10.080025, 10.080025, 18.306931, 18.301411, 18.301411, 8.598726, 17.308723, 14.267930, 13.891112, 13.891112, 13.891112, 13.891112, 13.891112, 19.161690, 19.149937, 19.149937, 19.149937, 19.149937, 17.493378, 17.026871, 26.186050, 26.186050, 15.682387, 15.684723, 15.684723, 11.103996, 13.537781, 0.071963, 0.065413, 0.065413, 0.065413, 0.065413, 0.065413, 1.509912, 0.059010, 0.059010, 0.059010, 0.059010, 1.480216, 1.482008, 0.107040, 0.107040, 1.412529, 1.412529, 1.412529, 0.401335, 1.276492, 0.087133, 0.061839, 0.061839, 0.061839, 0.061839, 0.061839, 1.105458, 0.071449, 0.071449, 0.071449, 0.071449, 1.098128, 1.098128, 0.135334, 0.135334, 1.076748, 1.076748, 1.076748, 0.369969, 1.031978, 0.345084, 0.345084, 0.345084, 0.345084, 0.345084, 0.345084, 2.814038, 0.282969, 0.282969, 0.282969, 0.282969, 2.753209, 2.757254, 0.406103, 0.406103, 2.611337, 2.611286, 2.611286, 0.730765, 2.327258, 0.120160, 0.120160, 0.120160, 0.120160, 0.120160, 0.120160, 0.807479, 0.098531, 0.098531, 0.098531, 0.098531, 0.821229, 0.821229, 0.185565, 0.185565, 0.839612, 0.839612, 0.839612, 0.664644, 0.811791, 0.149075, 0.159832, 0.159832, 0.159832, 0.159832, 0.159832, 2.036354, 0.131062, 0.131062, 0.131062, 0.131062, 1.982327, 1.981384, 0.214710, 0.214710, 1.837799, 1.837799, 1.837799, 0.686287, 1.772274, 0.097939, 0.083972, 0.083972, 0.083972, 0.083972, 0.097939, 0.187098, 0.098030, 0.098030, 0.098030, 0.098030, 0.184052, 0.184052, 0.134747, 0.134747, 0.174484, 0.175785, 0.175785, 0.158375, 0.149623, 0.089890, 0.058693, 0.058693, 0.058693, 0.058693, 0.058693, 0.254822, 0.073708, 0.073708, 0.073708, 0.073708, 0.242081, 0.242081, 0.127098, 0.127098, 0.200851, 0.201683, 0.201683, 0.157684, 0.188049, 5.782846, 5.434389, 5.434389, 5.434389, 5.434389, 5.434389, 23.842003, 4.741933, 4.741933, 4.741933, 4.741933, 23.834484, 23.834484, 8.499681, 8.499681, 23.805412, 23.804180, 23.804180, 15.429229, 23.755381, 8.634579, 6.064919, 6.064919, 6.064919, 6.064919, 6.064919, 23.379162, 7.080355, 7.080355, 7.080355, 7.080355, 23.367682, 23.367682, 12.345072, 12.345072, 23.333454, 23.333454, 23.333454, 19.908512, 23.290283, 3.985148, 3.141454, 3.141454, 3.141454, 3.141454, 3.141454, 12.271400, 3.267821, 3.267821, 3.267821, 3.267821, 12.271070, 12.271070, 4.441527, 4.441527, 11.994848, 11.987959, 11.987959, 7.861961, 10.564101, 3.195015, 2.917496, 2.917496, 2.917496, 2.917496, 2.917496, 9.545199, 2.619912, 2.619912, 2.619912, 2.619912, 9.058808, 9.055329, 4.343714, 4.343714, 8.966275, 8.973654, 8.973654, 6.430317, 8.683138, 0.942281, 0.942281, 0.942281, 0.942281, 0.942281, 0.942281, 7.381152, 0.772670, 0.772670, 0.772670, 0.772670, 7.225350, 7.225350, 1.379395, 1.379395, 6.891178, 6.891178, 6.891178, 2.463103, 6.123200, 3.606266, 4.247237, 4.247237, 4.247237, 4.247237, 4.247237, 7.499028, 3.482734, 3.482734, 3.482734, 3.482734, 7.056939, 7.017876, 5.465951, 5.465951, 7.044517, 7.077686, 7.077686, 5.262341, 5.002466, 4.755481, 3.272832, 3.272832, 3.272832, 3.272832, 3.272832, 7.903679, 3.899494, 3.899494, 3.899494, 3.899494, 7.233419, 7.317038, 5.453637, 5.453637, 6.185552, 6.228094, 6.228094, 4.754424, 4.871133, 5947.000000, 4662.000000, 4662.000000, 4662.000000, 4662.000000, 4662.000000, 8947.500000, 4876.541016, 4876.541016, 4876.541016, 4876.541016, 8813.455078, 8783.640625, 6907.447266, 6907.447266, 7862.945312, 7862.945312, 7862.945312, 5359.943359, 5487.173828, 4249.000000, 3357.000000, 3357.000000, 3357.000000, 3357.000000, 3357.000000, 6464.000000, 3484.179688, 3484.179688, 3484.179688, 3484.179688, 6498.160156, 6498.160156, 5025.929688, 5025.929688, 5834.697266, 5834.697266, 5834.697266, 3958.574219, 4023.701172, 10692.000000, 11355.000000, 11355.000000, 11355.000000, 11355.000000, 11355.000000, 23827.000000, 9311.101562, 9311.101562, 9311.101562, 9311.101562, 23602.300781, 23447.125000, 16349.445312, 16349.445312, 22228.650391, 22228.650391, 22228.650391, 17050.562500, 17684.099609, 12879.000000, 12879.000000, 12879.000000, 12879.000000, 12879.000000, 12879.000000, 18920.000000, 10560.781250, 10560.781250, 10560.781250, 10560.781250, 18581.423828, 18581.423828, 12171.019531, 12171.019531, 16940.597656, 16867.341797, 16867.341797, 12417.964844, 13074.669922, 0.071963, 0.071963, 0.071963, 0.071963, 0.071963, 0.071963, 1.510098, 0.059010, 0.059010, 0.059010, 0.059010, 1.482869, 1.480517, 0.106338, 0.106338, 1.412710, 1.412710, 1.412710, 0.401229, 1.276424, 0.075289, 0.093816, 0.093816, 0.093816, 0.093816, 0.093816, 1.105750, 0.097545, 0.097545, 0.097545, 0.097545, 1.098192, 1.097746, 0.139365, 0.139365, 1.076784, 1.076784, 1.076784, 0.369902, 1.032058, 0.343640, 0.343640, 0.343640, 0.343640, 0.343640, 0.343640, 2.812361, 0.281785, 0.281785, 0.281785, 0.281785, 2.757527, 2.752943, 0.406320, 0.406320, 2.611151, 2.610852, 2.610852, 0.730820, 2.327100, 0.120304, 0.120304, 0.120304, 0.120304, 0.120304, 0.120304, 0.806584, 0.098650, 0.098650, 0.098650, 0.098650, 0.820595, 0.820595, 0.185343, 0.185343, 0.839525, 0.838781, 0.838781, 0.664852, 0.812152, 0.154683, 0.150518, 0.150518, 0.150518, 0.150518, 0.150518, 2.034525, 0.126840, 0.126840, 0.126840, 0.126840, 1.979690, 1.982573, 0.226395, 0.226395, 1.837793, 1.837793, 1.837793, 0.686427, 1.771491, 0.092195, 0.092195, 0.092195, 0.092195, 0.092195, 0.092195, 0.186571, 0.080174, 0.080174, 0.080174, 0.080174, 0.185065, 0.184631, 0.130606, 0.130606, 0.176228, 0.176228, 0.176228, 0.158704, 0.151823, 0.090499, 0.090499, 0.090499, 0.090499, 0.090499, 0.090499, 0.253007, 0.074209, 0.074209, 0.074209, 0.074209, 0.242146, 0.242146, 0.126214, 0.126214, 0.203604, 0.203604, 0.203604, 0.157174, 0.188153, 1.756603, 1.785343, 1.785343, 1.785343, 1.785343, 1.785343, 20.516809, 0.868742, 0.868742, 0.868742, 0.868742, 3.263072, 19.221233, 3.117397, 3.117397, 4.912253, 4.696733, 17.767239, 4.438983, 4.499277, 4.152389, 4.152389, 4.152389, 4.152389, 4.152389, 4.152389, 18.542078, 1.063146, 1.063146, 1.063146, 1.063146, 2.726873, 18.473833, 2.893957, 2.893957, 2.987795, 3.080416, 18.292435, 2.682391, 2.682391, 2.218746, 1.942840, 1.965832, 1.965832, 1.965832, 1.965832, 19.025406, 1.759430, 1.759430, 1.759430, 1.759430, 6.740672, 17.291563, 6.740672, 6.740672, 6.959223, 6.959223, 15.245779, 3.586167, 3.586167, 0.012953, 0.012953, 0.012953, 0.012953, 0.012953, 0.012953, 1.533978, 0.002925, 0.002925, 0.002925, 0.002925, 0.005804, 1.532419, 0.005804, 0.005804, 0.016624, 0.016669, 1.525384, 0.047011, 0.047011, 0.015684, 0.015684, 0.015684, 0.015684, 0.015684, 0.015684, 1.112541, 0.003686, 0.003686, 0.003686, 0.003686, 0.008411, 1.110513, 0.008411, 0.008411, 0.023098, 0.023014, 1.101784, 0.057210, 0.057414, 0.062115, 0.062115, 0.062115, 0.062115, 0.062115, 0.062115, 2.865444, 0.022173, 0.022173, 0.022173, 0.022173, 0.054828, 2.863022, 0.054828, 0.054828, 0.063954, 0.063954, 2.852029, 0.119350, 0.119350, 0.021629, 0.021629, 0.021629, 0.021629, 0.021629, 0.021629, 0.787584, 0.007836, 0.007836, 0.007836, 0.007836, 0.022473, 0.782707, 0.022473, 0.022473, 0.033281, 0.033042, 0.765423, 0.085361, 0.085361, 0.028770, 0.028770, 0.028770, 0.028770, 0.028770, 0.028770, 2.079861, 0.008389, 0.008389, 0.008389, 0.008389, 0.026188, 2.071896, 0.024391, 0.024391, 0.066277, 0.066643, 2.054321, 0.118263, 0.118473, 0.017629, 0.017629, 0.017629, 0.017629, 0.017629, 0.017629, 0.191318, 0.005931, 0.005931, 0.005129, 0.005931, 0.019646, 0.190685, 0.019646, 0.014829, 0.029106, 0.029106, 0.185160, 0.050893, 0.051235, 0.016182, 0.016182, 0.016182, 0.016182, 0.016182, 0.016182, 0.257774, 0.004944, 0.004944, 0.004944, 0.004944, 0.012909, 0.256348, 0.014023, 0.014023, 0.024803, 0.024803, 0.242287, 0.044289, 0.044624, 1.040913, 1.040913, 1.040913, 1.040913, 1.040913, 1.040913, 23.854004, 0.407067, 0.407067, 0.407067, 0.407067, 1.014239, 23.823963, 1.129635, 1.129635, 1.577598, 1.577598, 23.803013, 2.731716, 2.731716, 1.554224, 1.554224, 1.554224, 1.554224, 1.554224, 1.554224, 23.395153, 0.532187, 0.532187, 0.532187, 0.532187, 1.403440, 23.370998, 1.508878, 1.508878, 2.278192, 2.287339, 23.337288, 3.102850, 3.113973, 0.717327, 0.717327, 0.717327, 0.717327, 0.717327, 0.717327, 12.278605, 0.235214, 0.235214, 0.235214, 0.235214, 0.550057, 12.270261, 0.649309, 0.649309, 1.156423, 1.155463, 12.239634, 1.866701, 1.866701, 0.575103, 0.575103, 0.575103, 0.575103, 0.575103, 0.575103, 9.734388, 0.203823, 0.203823, 0.203823, 0.203823, 0.508904, 9.610560, 0.508904, 0.508904, 1.003210, 0.996941, 9.215445, 1.654864, 1.654864, 0.169611, 0.169611, 0.169611, 0.169611, 0.169611, 0.169611, 7.419858, 0.087699, 0.087699, 0.087699, 0.087699, 0.249032, 7.380109, 0.249032, 0.249032, 0.406418, 0.412875, 7.332826, 0.754986, 0.758455, 0.871719, 0.764503, 0.764503, 0.764503, 0.871719, 0.871719, 7.362641, 0.443923, 0.443923, 0.443923, 0.443923, 1.254954, 7.268472, 1.208099, 1.208099, 1.455858, 1.455858, 6.573540, 1.947198, 1.961109, 0.855987, 0.855987, 0.855987, 0.855987, 0.855987, 0.855987, 8.388874, 0.382267, 0.382267, 0.382267, 0.382267, 0.880998, 8.157377, 1.052894, 1.052894, 1.452093, 1.452093, 6.917841, 1.621151, 1.621151, 1070.460938, 1070.460938, 1070.460938, 1070.460938, 1070.460938, 1070.460938, 8972.714844, 445.289062, 445.289062, 445.289062, 445.289062, 1233.505859, 8616.937500, 1263.603516, 1263.603516, 2350.132812, 2255.396484, 7549.917969, 2299.769531, 2295.101562, 764.820312, 764.820312, 764.820312, 764.820312, 764.820312, 764.820312, 6438.740234, 295.070312, 295.070312, 295.070312, 295.070312, 756.146484, 6151.984375, 851.480469, 851.480469, 1766.763672, 1721.392578, 4754.693359, 1735.089844, 1730.914062, 2043.900391, 2043.900391, 2043.900391, 2043.900391, 2043.900391, 2043.900391, 23804.568359, 845.375000, 845.375000, 845.375000, 845.375000, 2125.181641, 23597.947266, 2309.199219, 2309.199219, 4577.183594, 4571.593750, 22237.154297, 6746.152344, 6746.152344, 2318.218750, 2318.222656, 2318.222656, 2318.222656, 2318.222656, 2318.222656, 18883.697266, 1031.121094, 1031.121094, 1031.121094, 1031.121094, 2737.132812, 18555.365234, 2737.132812, 2737.132812, 3956.339844, 3887.546875, 16921.869141, 5011.472656, 5025.707031, 0.012953, 0.012953, 0.012953, 0.012953, 0.012953, 0.012953, 1.534992, 0.002909, 0.002909, 0.002909, 0.002909, 0.005829, 1.533031, 0.005318, 0.005318, 0.016623, 0.016663, 1.525283, 0.047015, 0.047015, 0.014831, 0.014831, 0.014831, 0.014831, 0.014831, 0.014831, 1.111831, 0.005352, 0.005352, 0.005352, 0.005352, 0.011251, 1.109936, 0.012200, 0.012200, 0.022627, 0.022627, 1.101830, 0.056583, 0.056583, 0.061855, 0.061855, 0.061855, 0.061855, 0.061855, 0.061855, 2.863584, 0.022520, 0.022520, 0.022520, 0.022520, 0.057134, 2.860651, 0.057134, 0.057134, 0.062378, 0.062378, 2.851887, 0.121679, 0.121679, 0.021655, 0.021655, 0.021655, 0.021655, 0.021655, 0.021655, 0.790491, 0.005807, 0.005807, 0.005807, 0.005807, 0.015082, 0.783847, 0.015082, 0.015082, 0.033740, 0.033287, 0.765422, 0.085241, 0.085241, 0.027843, 0.027843, 0.027843, 0.027843, 0.027843, 0.027843, 2.078830, 0.007803, 0.007803, 0.007803, 0.007803, 0.022759, 2.074461, 0.024776, 0.024776, 0.065391, 0.066318, 2.053727, 0.118501, 0.118501, 0.016595, 0.016595, 0.016595, 0.016595, 0.016595, 0.016595, 0.191559, 0.006511, 0.006511, 0.006511, 0.006511, 0.016278, 0.190185, 0.016278, 0.016032, 0.027825, 0.028102, 0.185229, 0.050526, 0.050986, 0.016290, 0.016290, 0.016290, 0.016290, 0.016290, 0.016290, 0.260324, 0.004913, 0.004913, 0.004913, 0.004913, 0.013853, 0.256501, 0.013853, 0.013853, 0.024948, 0.024550, 0.242195, 0.044634, 0.044634, 1.216655, 1.216655, 1.216595, 1.216549, 1.216549, 1.216549, 1.215966, 1.215966, 1.215966, 1.215966, 1.213195, 1.213195, 1.213195, 1.213195, 1.213195, 1.202262, 1.202262, 1.202161, 1.202161, 1.202161, 1.202161, 1.029520, 1.029520, 1.029216, 1.029038, 1.029038, 1.029038, 1.027266, 1.027266, 1.027266, 1.027266, 1.020072, 1.020072, 1.020072, 1.020072, 1.020072, 0.997402, 0.997402, 0.997402, 0.997402, 0.997402, 0.997402, 2.153405, 2.153405, 2.153376, 2.153325, 2.153344, 2.153344, 2.152507, 2.152535, 2.152535, 2.152535, 2.147205, 2.147091, 2.147205, 2.147205, 2.147205, 2.125323, 2.125323, 2.125163, 2.125163, 2.125163, 2.125163, 0.856608, 0.856608, 0.856087, 0.855161, 0.855695, 0.855695, 0.850922, 0.851053, 0.851053, 0.851053, 0.831917, 0.832203, 0.832203, 0.832203, 0.832203, 0.765361, 0.765361, 0.765004, 0.765004, 0.765004, 0.765004, 1.440392, 1.440392, 1.440392, 1.440041, 1.440041, 1.439950, 1.438389, 1.438389, 1.438389, 1.438389, 1.428180, 1.427938, 1.428180, 1.428180, 1.428180, 1.383569, 1.383569, 1.383462, 1.383462, 1.383462, 1.383462, 0.188307, 0.187555, 0.188307, 0.187240, 0.187459, 0.187459, 0.183460, 0.183384, 0.183460, 0.183460, 0.160780, 0.160158, 0.160780, 0.160780, 0.160780, 0.111082, 0.111082, 0.111082, 0.111082, 0.111082, 0.111082, 0.211975, 0.211975, 0.211975, 0.211975, 0.211975, 0.211975, 0.210473, 0.210473, 0.210361, 0.210361, 0.197643, 0.197266, 0.197643, 0.197643, 0.197643, 0.155539, 0.155539, 0.155533, 0.155533, 0.155533, 0.155533};

class Main {
  public:
    pr2_overhead_grasping::SensorPoint::ConstPtr cur_msg;
    pr2_overhead_grasping::SensorPoint::ConstPtr init_msg;
    bool msg_in;

    void callback(pr2_overhead_grasping::SensorPoint::ConstPtr msg) {
      if(!msg_in) init_msg = msg;
      msg_in = true;
      cur_msg = msg;
    }

    int32_t convToColor(int i, float v) {
      int64_t ret;
      //ret = fabs(v - init_msg->data[i]) / threshs[i] * 1000.0;
      //ret = 80.0 * log(fabs(v) / threshs[i] * 2.0) + 256.0; // * 1000.0;
      ret = fabs(v) / threshs[i] * 2000.0;
      //ROS_INFO("%d", ret);
      if(ret > 255l) ret = 255l;
      if(ret < 0l) ret = 0l;
      if(v < 0.0)
        return -ret;
      return ret;
    }


    Main(char arm) {
      msg_in = false;
      int b_len = 800;
      ros::NodeHandle nh;
      ros::Subscriber sub;
      ros::Publisher pub, pub_info;
      int32_t buffer[b_len][2000];
      if(arm == 'r')
        sub = nh.subscribe("/r_arm_features", 1,
                           &Main::callback, this);
      else
        sub = nh.subscribe("/l_arm_features", 1,
                           &Main::callback, this);
      ros::Rate r(100);

      /*double maxs[1267];
      for(int i=0;i<1267;i++)
         maxs[i] = 0.0;
      while(ros::ok()) {// && !kb_getc()) {
        
        ros::spinOnce();
        r.sleep();
        if(!msg_in)
          //break;
          continue;
        for(uint32_t i=0;i<cur_msg->features.size();i++)
          if(fabs(cur_msg->features[i]) > maxs[i])
            maxs[i] = fabs(cur_msg->features[i]);

        ROS_INFO("Waiting for features message");
      }
      for(uint32_t i=0;i<cur_msg->features.size();i++)
        printf("%f, ",maxs[i]);
      return;
      */
      while(ros::ok()) {
        ros::spinOnce();
        r.sleep();
        if(msg_in)
          break;
        ROS_INFO("Waiting for features message");
      }
      vector<int> split_inds(cur_msg->features.size(), 0);
      for(int i=0;i<cur_msg->features.size();i++) {
            if(find(cur_msg->split_inds3.begin(), cur_msg->split_inds3.end(), i)
                                     != cur_msg->split_inds3.end()) {
                split_inds[i] = 3;
            } else if(find(cur_msg->split_inds2.begin(), cur_msg->split_inds2.end(), i)
                                     != cur_msg->split_inds2.end()) {
                split_inds[i] = 2;
            } else if(find(cur_msg->split_inds1.begin(), cur_msg->split_inds1.end(), i)
                                     != cur_msg->split_inds1.end()) {
                split_inds[i] = 1;
            }
        }
      for(int i=0;i<b_len;i++) {
        for(uint32_t j=0;j<cur_msg->features.size();j++) {
          buffer[i][j] = 0; //convToColor(j, cur_msg->data[j]);
        }
      }
      pub = nh.advertise<sensor_msgs::Image>("arm_features_image", 1);
      pub_info = nh.advertise<sensor_msgs::CameraInfo>("camera_info", 1);
      int seq = 0;
      
      while(ros::ok()) {
        ros::spinOnce();
        for(uint32_t j=0;j<cur_msg->features.size();j++) {
          buffer[seq % b_len][j] = convToColor(j, cur_msg->features[j]);
        }

        roslib::Header h;
        h.seq = seq;
        h.stamp = ros::Time::now();
        h.frame_id = "base_link";
        sensor_msgs::CameraInfo cinfo;
        cinfo.header = h;
        sensor_msgs::Image img;
        img.header = h;
        img.height = b_len; img.width = cur_msg->features.size() + cur_msg->split_inds1.size() +
                                                cur_msg->split_inds2.size() +
                                                cur_msg->split_inds3.size() - 8;
        cinfo.height = img.height; cinfo.width = img.width;
        img.encoding = "rgb8";
        img.is_bigendian = false;
        img.step = img.width*3;
        
        std::vector<uint8_t> data;
        int n_markers = 0;
        for(int i=0;i<b_len - n_markers;i++) {
          if((i + seq) % 100 == 0) {
            for(uint32_t j=0;j<img.width;j++) {
              // add white second marker
              uint8_t r = 0xFF, g = 0xFF, b = 0xFF;
              data.push_back(r);
              data.push_back(g);
              data.push_back(b);
            }
            n_markers++;
            if(i == b_len - n_markers)
              break;
          }
          int ind = 0;
          for(uint32_t j=0;j<cur_msg->features.size();j++) {
            uint8_t r, g, b;
            if(split_inds[j] == 3) {
              r = 0xFF, g = 0x00, b = 0xFF;
              data.push_back(r); data.push_back(g); data.push_back(b);
              ind++;
            } else if(split_inds[j] == 2) {
              r = 0x00, g = 0xFF, b = 0xFF;
              data.push_back(r); data.push_back(g); data.push_back(b);
              ind++;
            } else if(split_inds[j] == 1) {
              r = 0xFF, g = 0xFF, b = 0xFF;
              data.push_back(r); data.push_back(g); data.push_back(b);
              ind++;
            }
            r = 0x00, g = 0x00, b = 0x00;
            if(buffer[(seq + i) % b_len][j] < 0) 
              g = (uint8_t) abs(buffer[(seq + i) % b_len][j]);
            else
              r = (uint8_t) buffer[(seq + i) % b_len][j];
            //r = (uint8_t) rand() % 256;
            //ROS_INFO("r %d, b %d", r, b);
            data.push_back(r);
            data.push_back(g);
            data.push_back(b);
              ind++;
          }
        //ROS_INFO("%d, %d", ind, img.width);
        ind = 0;
        }

        img.data = data;
        pub.publish(img);
        pub_info.publish(cinfo);

        r.sleep();
        seq++;
      }
    }


};

int main(int argc, char **argv)
{
  if(argc < 2 || (argv[1][0] != 'r' && argv[1][0] != 'l')) {
    ROS_FATAL("Must provide r or l as the parameter designating the arm.");
    return -1;
  }

  ros::init(argc, argv, "sensor_visualizer");
  Main m(argv[1][0]);

  return 0;
}