image_test.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[] = {19.620001, 14.216836, 19.620001, 0.955078, 0.499350, 2.024202, 1.484848, 2.394039, 1.659796, 2.603328, 11.975079, 11.689432, 6.150004, 4.935371, 2.544402, 1.806370, 0.733231, 22566.000000, 24486.000000, 52523.000000, 36431.000000, 0.371339, 0.125200, 0.259454, 0.197322, 0.394980, 0.406547, 0.375090, 15.021563, 14.125633, 14.125633, 13.971776, 13.820721, 13.985712, 13.488202, 13.525270, 13.525270, 13.525270, 12.402515, 12.402515, 12.367475, 12.389447, 12.402515, 11.045438, 11.050995, 11.026236, 11.050995, 11.043262, 11.050995, 9.826062, 9.826062, 9.825631, 9.826062, 9.826062, 9.826062, 9.822459, 12.090058, 11.142015, 11.662404, 11.399366, 11.399366, 11.275280, 10.195260, 10.128319, 10.195260, 9.989114, 7.370826, 7.370826, 7.276694, 7.338591, 7.370826, 5.309089, 5.309089, 5.287902, 5.309089, 5.309089, 5.309089, 2.520398, 2.520349, 2.520626, 2.520626, 2.520398, 2.520349, 2.520626, 19.620001, 13.943995, 14.613068, 13.280015, 13.280015, 11.937847, 9.705602, 9.705602, 9.481308, 9.415582, 5.462885, 5.536917, 5.536917, 5.536917, 5.462885, 2.923963, 2.919566, 2.923963, 2.921001, 2.923963, 2.919566, 2.156815, 2.156815, 2.156815, 2.156815, 2.156322, 2.156359, 2.156815, 0.955078, 0.954242, 0.955078, 0.954217, 0.954217, 0.953271, 0.951272, 0.950557, 0.951272, 0.951206, 0.939995, 0.939995, 0.939822, 0.939399, 0.939995, 0.899113, 0.899175, 0.899175, 0.899175, 0.899113, 0.899175, 0.800304, 0.800304, 0.800304, 0.800304, 0.800304, 0.800304, 0.800252, 0.499350, 0.499213, 0.499350, 0.498969, 0.498955, 0.498969, 0.495579, 0.496294, 0.496294, 0.496294, 0.484388, 0.484388, 0.484187, 0.484028, 0.484388, 0.465437, 0.465437, 0.465269, 0.465437, 0.465437, 0.465437, 0.439365, 0.439346, 0.439386, 0.439386, 0.439365, 0.439346, 0.439386, 2.024202, 2.021922, 2.024202, 2.022469, 2.022469, 2.021293, 2.016821, 2.016115, 2.016821, 2.016133, 1.998645, 1.998645, 1.998549, 1.997585, 1.998645, 1.943817, 1.943817, 1.942327, 1.943817, 1.943817, 1.943817, 1.848391, 1.848494, 1.848520, 1.848520, 1.848391, 1.848494, 1.848520, 1.484848, 1.484848, 1.484848, 1.484624, 1.484624, 1.484459, 1.483591, 1.483673, 1.483764, 1.483764, 1.479240, 1.479336, 1.479336, 1.479240, 1.479336, 1.461381, 1.461381, 1.461322, 1.461152, 1.461381, 1.461381, 1.416743, 1.416694, 1.416765, 1.416765, 1.416743, 1.416694, 1.416765, 2.394039, 2.381688, 2.394039, 2.386015, 2.387199, 2.387199, 2.365810, 2.365810, 2.360743, 2.365148, 2.296832, 2.298057, 2.298057, 2.298057, 2.296832, 2.149084, 2.148390, 2.149084, 2.148736, 2.149084, 2.148390, 2.066907, 2.066907, 2.066890, 2.066899, 2.066907, 2.066907, 2.066890, 1.659796, 1.659796, 1.659796, 1.658434, 1.657272, 1.658358, 1.654461, 1.654315, 1.654461, 1.654461, 1.635611, 1.635611, 1.635383, 1.633777, 1.635383, 1.563225, 1.563225, 1.562725, 1.563225, 1.563225, 1.563225, 1.495842, 1.495994, 1.495994, 1.495994, 1.495842, 1.495994, 1.495994, 2.603328, 2.603327, 2.603327, 2.601092, 2.601092, 2.599325, 2.593207, 2.592283, 2.593207, 2.591944, 2.565994, 2.564951, 2.565994, 2.565478, 2.565994, 2.538996, 2.538952, 2.538996, 2.538925, 2.538996, 2.538952, 2.526875, 2.526856, 2.526875, 2.526866, 2.526875, 2.526856, 2.526875, 11.975079, 11.975079, 11.975079, 11.955174, 11.936070, 11.955174, 11.910201, 11.904527, 11.910201, 11.896680, 11.871384, 11.868929, 11.873945, 11.873945, 11.871384, 11.725009, 11.725009, 11.721021, 11.725009, 11.725009, 11.725009, 8.343627, 8.347384, 8.347384, 8.347384, 8.343627, 8.347384, 8.347384, 11.689432, 11.689432, 11.689432, 11.647749, 11.653043, 11.653043, 11.566812, 11.566812, 11.564187, 11.566812, 10.648105, 10.648105, 10.642935, 10.648105, 10.648105, 7.663048, 7.663048, 7.660179, 7.663048, 7.663048, 7.663048, 5.933642, 5.934472, 5.935408, 5.935408, 5.933642, 5.934472, 5.935408, 6.150004, 6.147051, 6.147051, 6.122823, 6.122823, 6.066919, 5.968835, 5.974460, 5.974460, 5.974460, 5.599409, 5.605433, 5.605433, 5.605433, 5.599409, 4.743260, 4.742177, 4.743260, 4.740095, 4.743260, 4.742177, 4.154572, 4.155571, 4.155571, 4.155571, 4.154572, 4.155571, 4.155571, 4.950092, 4.795031, 4.795031, 4.750491, 4.750491, 4.738428, 4.466695, 4.469632, 4.477669, 4.477669, 4.348033, 4.344782, 4.349673, 4.349673, 4.348033, 3.814406, 3.814406, 3.806543, 3.814406, 3.814406, 3.814406, 2.194580, 2.194580, 2.194580, 2.194580, 2.194580, 2.191056, 2.194580, 2.544402, 2.439656, 2.544402, 2.450856, 2.450856, 2.365022, 2.170750, 2.168525, 2.176392, 2.176392, 1.781001, 1.781001, 1.775226, 1.766881, 1.781001, 1.021964, 1.019216, 1.024631, 1.024631, 1.021964, 1.019216, 0.450903, 0.450828, 0.450903, 0.450802, 0.450903, 0.450828, 0.450903, 1.806370, 1.789181, 1.789181, 1.783043, 1.792881, 1.792881, 1.749881, 1.749881, 1.741135, 1.748409, 1.616700, 1.623775, 1.623775, 1.623775, 1.616700, 1.279494, 1.279494, 1.279388, 1.275616, 1.279494, 1.279494, 0.895815, 0.895815, 0.895802, 0.895807, 0.895815, 0.895815, 0.895802, 0.733231, 0.727404, 0.722451, 0.718948, 0.726715, 0.726715, 0.707949, 0.707949, 0.704209, 0.707949, 0.668861, 0.668861, 0.667774, 0.668861, 0.668861, 0.567334, 0.567334, 0.567148, 0.567334, 0.567334, 0.567334, 0.402403, 0.402247, 0.402403, 0.402141, 0.402403, 0.402247, 0.402403, 22566.000000, 22566.000000, 22566.000000, 22347.423828, 22347.423828, 22248.894531, 21678.787109, 21584.806641, 21678.787109, 21610.828125, 20382.167969, 20382.167969, 20347.314453, 20365.240234, 20382.167969, 19082.203125, 19086.730469, 19086.730469, 19086.730469, 19082.203125, 19086.730469, 18205.369141, 18205.369141, 18204.195312, 18205.369141, 18205.369141, 18205.369141, 18204.195312, 24486.000000, 24486.000000, 24486.000000, 24377.732422, 24249.791016, 24377.732422, 24092.654297, 24092.654297, 24078.468750, 24092.654297, 23495.970703, 23510.779297, 23510.779297, 23510.779297, 23495.970703, 22457.431641, 22450.171875, 22457.431641, 22454.832031, 22457.431641, 22450.171875, 21735.087891, 21735.052734, 21735.449219, 21735.449219, 21735.087891, 21735.052734, 21735.449219, 52523.000000, 52023.859375, 52523.000000, 51733.582031, 51733.582031, 51501.414062, 49104.554688, 48690.960938, 49104.554688, 49038.730469, 45296.761719, 45401.800781, 45401.800781, 45401.800781, 45296.761719, 39828.871094, 39789.574219, 39828.871094, 39818.117188, 39828.871094, 39789.574219, 35325.714844, 35325.714844, 35322.554688, 35325.714844, 35325.714844, 35325.714844, 35322.554688, 36431.000000, 36302.000000, 36431.000000, 36213.492188, 36312.359375, 36312.359375, 35759.949219, 35815.566406, 35815.566406, 35815.566406, 34345.980469, 34307.277344, 34345.980469, 34309.449219, 34345.980469, 32146.890625, 32147.041016, 32147.041016, 32147.041016, 32146.890625, 32147.041016, 30600.539062, 30600.140625, 30601.650391, 30601.650391, 30600.539062, 30600.140625, 30601.650391, 0.371339, 0.371339, 0.371190, 0.370983, 0.370924, 0.370983, 0.367970, 0.368254, 0.368598, 0.368598, 0.357244, 0.356711, 0.357244, 0.356795, 0.357244, 0.315817, 0.315817, 0.315403, 0.315817, 0.315817, 0.315817, 0.216844, 0.216844, 0.216844, 0.216844, 0.216844, 0.216844, 0.216592, 0.125200, 0.125200, 0.125200, 0.124678, 0.123970, 0.124678, 0.120998, 0.120998, 0.120601, 0.120998, 0.110078, 0.109617, 0.110078, 0.109990, 0.110078, 0.092406, 0.092406, 0.092294, 0.092317, 0.092406, 0.092406, 0.070980, 0.070980, 0.070980, 0.070952, 0.070980, 0.070980, 0.070980, 0.259454, 0.259454, 0.259454, 0.258088, 0.258088, 0.257049, 0.253105, 0.252759, 0.253105, 0.252071, 0.242697, 0.242677, 0.242697, 0.242438, 0.242697, 0.231031, 0.231031, 0.231021, 0.230910, 0.231031, 0.231031, 0.206694, 0.206717, 0.206717, 0.206717, 0.206694, 0.206717, 0.206717, 0.197322, 0.197322, 0.197322, 0.197139, 0.197139, 0.197033, 0.196621, 0.196621, 0.196501, 0.196621, 0.192887, 0.193196, 0.193201, 0.193201, 0.192887, 0.168579, 0.168350, 0.168579, 0.168385, 0.168579, 0.168350, 0.097723, 0.097723, 0.097723, 0.097723, 0.097650, 0.097723, 0.097723, 0.394980, 0.386931, 0.382881, 0.378843, 0.387075, 0.387075, 0.362403, 0.362403, 0.357166, 0.362403, 0.292980, 0.294883, 0.294883, 0.294883, 0.292980, 0.164820, 0.163733, 0.164869, 0.164869, 0.164820, 0.163733, 0.064039, 0.063919, 0.064039, 0.064036, 0.063919, 0.063911, 0.064039, 0.406547, 0.403626, 0.403007, 0.401787, 0.404012, 0.404012, 0.395571, 0.395571, 0.393784, 0.395571, 0.365816, 0.367236, 0.367603, 0.367603, 0.365816, 0.287490, 0.287490, 0.287193, 0.286854, 0.287490, 0.287490, 0.199227, 0.199227, 0.199221, 0.199227, 0.199227, 0.199227, 0.199221, 0.375090, 0.372099, 0.373751, 0.373158, 0.373391, 0.373391, 0.367559, 0.367532, 0.367559, 0.365931, 0.346520, 0.346520, 0.345900, 0.346520, 0.346520, 0.290441, 0.290641, 0.290641, 0.290641, 0.290441, 0.290641, 0.203801, 0.203705, 0.203801, 0.203684, 0.203801, 0.203705, 0.203801, 5.422324, 5.958809, 5.958809, 5.958809, 5.422324, 5.958809, 9.534094, 4.530610, 4.530610, 4.049690, 4.530610, 9.382374, 8.751566, 7.255185, 6.878926, 8.289936, 8.289936, 8.289936, 7.814451, 6.216639, 10.423125, 11.496094, 11.496094, 10.423125, 10.423125, 11.496094, 16.734097, 8.385251, 10.410096, 10.410096, 10.410096, 17.171848, 17.683342, 10.319109, 10.081901, 14.303150, 14.156590, 14.303150, 14.368715, 9.263550, 19.016457, 24.079525, 24.079525, 19.016457, 19.016457, 24.079525, 18.396816, 12.807415, 12.807415, 9.845831, 9.872845, 17.683472, 17.683472, 16.380215, 15.794281, 11.337152, 11.874765, 11.874765, 7.500029, 5.857010, 0.053309, 0.054221, 0.054221, 0.052148, 0.053309, 0.054221, 0.396874, 0.051755, 0.051755, 0.050615, 0.051755, 0.394158, 0.394158, 0.089897, 0.089897, 0.389873, 0.389902, 0.389902, 0.294433, 0.383613, 0.050249, 0.050249, 0.050249, 0.035699, 0.050249, 0.050249, 0.165686, 0.045011, 0.045011, 0.035699, 0.045011, 0.159461, 0.160394, 0.062949, 0.062949, 0.156331, 0.157232, 0.156538, 0.143439, 0.124324, 0.153941, 0.153941, 0.153941, 0.153941, 0.139829, 0.153941, 0.317788, 0.126231, 0.126231, 0.126231, 0.126231, 0.320420, 0.332929, 0.146182, 0.136901, 0.374483, 0.371761, 0.374483, 0.336945, 0.324268, 0.088310, 0.079913, 0.088310, 0.088310, 0.088310, 0.079913, 0.340211, 0.072414, 0.072414, 0.072414, 0.072414, 0.339460, 0.339460, 0.118806, 0.119266, 0.330480, 0.330480, 0.330480, 0.220533, 0.279462, 0.175797, 0.146585, 0.162897, 0.175797, 0.175797, 0.146585, 0.399016, 0.133576, 0.144154, 0.144154, 0.144154, 0.403507, 0.401823, 0.263172, 0.255155, 0.382213, 0.382860, 0.382860, 0.445511, 0.318684, 0.207625, 0.215456, 0.215456, 0.207625, 0.207625, 0.215456, 0.384977, 0.176674, 0.176674, 0.176674, 0.170252, 0.386110, 0.385190, 0.270117, 0.270117, 0.377594, 0.378521, 0.378521, 0.398593, 0.349828, 0.121477, 0.152629, 0.101115, 0.152629, 0.121477, 0.152629, 0.438591, 0.125156, 0.099611, 0.125156, 0.125156, 0.439101, 0.438525, 0.180894, 0.169816, 0.432970, 0.433716, 0.432857, 0.360699, 0.412799, 5.259209, 8.313435, 5.259209, 8.313435, 5.259209, 8.313435, 18.718138, 6.817017, 4.965021, 7.108006, 7.108006, 19.244370, 19.376129, 9.636043, 9.335676, 18.621452, 18.677986, 18.597456, 20.677540, 15.794928, 6.012668, 5.324999, 6.012668, 6.012668, 6.012668, 5.324999, 17.548256, 5.118605, 5.118605, 5.084940, 5.084940, 16.831469, 16.831469, 8.081167, 8.211760, 18.694199, 18.501362, 18.585556, 17.486572, 13.518389, 5.012938, 5.012938, 5.012938, 3.986132, 5.012938, 2.586211, 8.348333, 4.110609, 4.110609, 3.268628, 4.110609, 9.322501, 9.365068, 4.455745, 4.728048, 10.243491, 10.168900, 10.243491, 9.367641, 8.474174, 2.345555, 2.345555, 2.345555, 2.276857, 2.345555, 2.345555, 7.808703, 1.923355, 1.923355, 1.867023, 1.923355, 7.731540, 7.714741, 3.187221, 3.240684, 7.535435, 7.528878, 7.535435, 5.157943, 6.257938, 1.044544, 1.701949, 1.008021, 1.701949, 1.044544, 1.701949, 2.664001, 1.479454, 0.967554, 1.479454, 1.414006, 2.669666, 2.639044, 1.744660, 1.643854, 2.404531, 2.408429, 2.369894, 2.666842, 1.783265, 0.649063, 1.016488, 1.016488, 0.895092, 0.649063, 1.016488, 1.711488, 0.833520, 0.833520, 0.833520, 0.733976, 1.715188, 1.705350, 1.124753, 1.086531, 1.670716, 1.672188, 1.672188, 1.705357, 1.545373, 0.213657, 0.184522, 0.213657, 0.213657, 0.213657, 0.184522, 0.858512, 0.175199, 0.175199, 0.175199, 0.175199, 0.851752, 0.860374, 0.290216, 0.276730, 0.837301, 0.837301, 0.837301, 0.660649, 0.798823, 2191.000000, 3750.000000, 2191.000000, 3750.000000, 2191.000000, 3750.000000, 5484.000000, 3075.000000, 2070.300781, 3075.000000, 3075.000000, 5149.244141, 5265.923828, 3705.419922, 3975.396484, 4477.285156, 4546.705078, 4455.177734, 3140.166016, 3189.283203, 2435.000000, 2040.000000, 2040.000000, 2435.000000, 2435.000000, 2040.000000, 4239.500000, 1672.800781, 1996.701172, 1996.701172, 1996.701172, 4076.572266, 4100.945312, 2259.826172, 2315.035156, 3813.732422, 3813.732422, 3813.732422, 3138.095703, 3231.974609, 12232.000000, 11692.000000, 11004.000000, 12232.000000, 12232.000000, 11692.000000, 23062.500000, 9637.082031, 10439.000000, 11240.914062, 11240.914062, 22249.832031, 22347.923828, 17509.519531, 17727.988281, 19650.189453, 19712.029297, 19396.478516, 15798.361328, 16048.873047, 4685.000000, 4685.000000, 4685.000000, 3435.000000, 4685.000000, 4685.000000, 7685.998047, 3841.699219, 3841.699219, 3513.859375, 3841.699219, 7691.361328, 7585.656250, 5748.400391, 5771.066406, 7238.837891, 7207.914062, 7238.837891, 5772.830078, 5765.115234, 0.054718, 0.053558, 0.054718, 0.054718, 0.054718, 0.053558, 0.394951, 0.047996, 0.044869, 0.047996, 0.044869, 0.392708, 0.392708, 0.080905, 0.084417, 0.390100, 0.390100, 0.390100, 0.291563, 0.383790, 0.035191, 0.066745, 0.044666, 0.066745, 0.035191, 0.066745, 0.164894, 0.054731, 0.044729, 0.055660, 0.055660, 0.157587, 0.159835, 0.065092, 0.068211, 0.152043, 0.152315, 0.151065, 0.142962, 0.127352, 0.147366, 0.147366, 0.147366, 0.125558, 0.101184, 0.147366, 0.319238, 0.120840, 0.120840, 0.109567, 0.120840, 0.331038, 0.340953, 0.134719, 0.134719, 0.380200, 0.379221, 0.380200, 0.337561, 0.324402, 0.079190, 0.098734, 0.061962, 0.098734, 0.079190, 0.098734, 0.339632, 0.080962, 0.064935, 0.082056, 0.082056, 0.339772, 0.339772, 0.121650, 0.115452, 0.332037, 0.332037, 0.332037, 0.219503, 0.284456, 0.171777, 0.177012, 0.169029, 0.177012, 0.171777, 0.177012, 0.405720, 0.145150, 0.144069, 0.155491, 0.155491, 0.406285, 0.397793, 0.258848, 0.258848, 0.382470, 0.382470, 0.382470, 0.451778, 0.321051, 0.212237, 0.222157, 0.222157, 0.212237, 0.212237, 0.222157, 0.386440, 0.182168, 0.182168, 0.182168, 0.174034, 0.386826, 0.384601, 0.267653, 0.267653, 0.378385, 0.378385, 0.378385, 0.405966, 0.350412, 0.121216, 0.152194, 0.113645, 0.152194, 0.121216, 0.152194, 0.438145, 0.124799, 0.099397, 0.124799, 0.124799, 0.439204, 0.439438, 0.188205, 0.172884, 0.431847, 0.433413, 0.433413, 0.360614, 0.412140, 1.986525, 1.717516, 1.772697, 1.986525, 1.986525, 1.717516, 8.103998, 0.553608, 0.615938, 0.615938, 0.615938, 1.644562, 7.326385, 1.673642, 1.673642, 1.950831, 1.941697, 6.363025, 2.399946, 2.367270, 4.179980, 4.179980, 4.179980, 4.042027, 4.179980, 4.179980, 14.979723, 1.188988, 1.233188, 1.358437, 1.358437, 3.267897, 13.979407, 3.601361, 2.920404, 3.536852, 3.699157, 12.069758, 5.100442, 5.100442, 9.456688, 8.913496, 8.913496, 9.456688, 9.456688, 8.913496, 18.004951, 1.778331, 2.463165, 2.463165, 2.463165, 4.648219, 15.485514, 4.085513, 4.648219, 4.242717, 4.291277, 12.198571, 4.579796, 4.456489, 0.009760, 0.009760, 0.009760, 0.009760, 0.009760, 0.009760, 0.396413, 0.003443, 0.003443, 0.003149, 0.003443, 0.007466, 0.393454, 0.007466, 0.006276, 0.011865, 0.012283, 0.381992, 0.040820, 0.040820, 0.009045, 0.009045, 0.009045, 0.009045, 0.009045, 0.009045, 0.166968, 0.003549, 0.002598, 0.003549, 0.002514, 0.009616, 0.164363, 0.008566, 0.009616, 0.013984, 0.013617, 0.152563, 0.030134, 0.030134, 0.027709, 0.027709, 0.027709, 0.027709, 0.027709, 0.027709, 0.319942, 0.018102, 0.018102, 0.017835, 0.018102, 0.047223, 0.317322, 0.047481, 0.047223, 0.033123, 0.033123, 0.313748, 0.068358, 0.068983, 0.015896, 0.015896, 0.015896, 0.015896, 0.015896, 0.015896, 0.339316, 0.006788, 0.006788, 0.006070, 0.006788, 0.017014, 0.338452, 0.017741, 0.017014, 0.036403, 0.034852, 0.333844, 0.064346, 0.063491, 0.031644, 0.031644, 0.031644, 0.031644, 0.031644, 0.031644, 0.409099, 0.013979, 0.016629, 0.016629, 0.016629, 0.040852, 0.402836, 0.048980, 0.048980, 0.076504, 0.076504, 0.382448, 0.148973, 0.146328, 0.038782, 0.038782, 0.038782, 0.038782, 0.038782, 0.038782, 0.386057, 0.015015, 0.015015, 0.015487, 0.015487, 0.042118, 0.386110, 0.042109, 0.042118, 0.070117, 0.070879, 0.377594, 0.083842, 0.082977, 0.027473, 0.027473, 0.027473, 0.027473, 0.027473, 0.027473, 0.439245, 0.008898, 0.009541, 0.009541, 0.009541, 0.023453, 0.433957, 0.026199, 0.023743, 0.039666, 0.039910, 0.412435, 0.067304, 0.067180, 1.496418, 1.496418, 1.496418, 1.496418, 1.496418, 1.496418, 18.236946, 0.514117, 0.514117, 0.503551, 0.514117, 0.957149, 17.890255, 1.302381, 1.302381, 1.990590, 1.966882, 17.377838, 4.199351, 4.199351, 1.050033, 1.050033, 1.050033, 1.050033, 1.050033, 1.050033, 17.665710, 0.595312, 0.595312, 0.595312, 0.581012, 1.531449, 17.356789, 1.531449, 1.531449, 2.517420, 2.509978, 15.981113, 3.754938, 3.754938, 0.902329, 0.902329, 0.902329, 0.717504, 0.902329, 0.902329, 8.301954, 0.480881, 0.480881, 0.480881, 0.432448, 1.270921, 8.133418, 1.270921, 1.270921, 1.046696, 1.067491, 7.425756, 2.102208, 2.102208, 0.422200, 0.422200, 0.422200, 0.422200, 0.422200, 0.422200, 7.681666, 0.135065, 0.128688, 0.135065, 0.128688, 0.421070, 7.591995, 0.387684, 0.421070, 0.943172, 0.958793, 7.454325, 1.651458, 1.655489, 0.287944, 0.287943, 0.287943, 0.287944, 0.287944, 0.287943, 2.680753, 0.116516, 0.186738, 0.186738, 0.186738, 0.413269, 2.635342, 0.514476, 0.514476, 0.547895, 0.585443, 2.415219, 0.862027, 0.865693, 0.182968, 0.182968, 0.182968, 0.182968, 0.182968, 0.182968, 1.711488, 0.074075, 0.074075, 0.078762, 0.078762, 0.209641, 1.715188, 0.183747, 0.209641, 0.294877, 0.294877, 1.670716, 0.344929, 0.343324, 0.038458, 0.038458, 0.038458, 0.038458, 0.038458, 0.038458, 0.848111, 0.013487, 0.013487, 0.013832, 0.013832, 0.034536, 0.830267, 0.032261, 0.034536, 0.058414, 0.060661, 0.792400, 0.118698, 0.118698, 675.000000, 675.000000, 675.000000, 675.000000, 675.000000, 675.000000, 5453.962891, 317.105469, 317.105469, 188.214844, 317.105469, 676.267578, 5146.431641, 864.964844, 736.595703, 1296.619141, 1296.619141, 4510.906250, 1295.437500, 1295.437500, 438.298828, 438.298828, 438.298828, 438.298828, 438.298828, 438.298828, 4259.820312, 191.394531, 286.949219, 286.949219, 286.949219, 567.433594, 4140.082031, 728.294922, 728.294922, 581.875000, 584.539062, 3833.974609, 1053.347656, 1043.757812, 2201.757812, 2201.757812, 2201.757812, 2201.757812, 2201.757812, 2201.757812, 23164.001953, 1021.585938, 1021.585938, 634.355469, 1021.585938, 2852.375000, 22136.050781, 3196.347656, 2694.851562, 5717.363281, 5738.667969, 19661.779297, 5902.230469, 5902.230469, 843.300781, 843.300781, 843.300781, 843.300781, 843.300781, 843.300781, 7714.363281, 375.935547, 330.312500, 375.935547, 349.097656, 975.759766, 7609.789062, 852.552734, 975.759766, 1562.621094, 1506.117188, 6915.824219, 2160.236328, 2199.089844, 0.009849, 0.009849, 0.009849, 0.009849, 0.009849, 0.009849, 0.395630, 0.003408, 0.003408, 0.003124, 0.003408, 0.007026, 0.393304, 0.008127, 0.008127, 0.012502, 0.013051, 0.381704, 0.041427, 0.041427, 0.012014, 0.012014, 0.012014, 0.012014, 0.012014, 0.012014, 0.167442, 0.004615, 0.004615, 0.004447, 0.004615, 0.008827, 0.163905, 0.011293, 0.011293, 0.012505, 0.012505, 0.151967, 0.029243, 0.029243, 0.026526, 0.026526, 0.026526, 0.022600, 0.026526, 0.026526, 0.319151, 0.019275, 0.019275, 0.008226, 0.019275, 0.038546, 0.317005, 0.050440, 0.021920, 0.033252, 0.038793, 0.313646, 0.070672, 0.071040, 0.017772, 0.017772, 0.017772, 0.017772, 0.017772, 0.017772, 0.338969, 0.006786, 0.006786, 0.006659, 0.006786, 0.012669, 0.337948, 0.016936, 0.016936, 0.034470, 0.036533, 0.333515, 0.061074, 0.062083, 0.031862, 0.031862, 0.031862, 0.031862, 0.031862, 0.031862, 0.410850, 0.016403, 0.016403, 0.014149, 0.016403, 0.036685, 0.405476, 0.043395, 0.043395, 0.078242, 0.078242, 0.382358, 0.149236, 0.147614, 0.039988, 0.039988, 0.039988, 0.039988, 0.039988, 0.039988, 0.385821, 0.015218, 0.015218, 0.015877, 0.015877, 0.042866, 0.386826, 0.043307, 0.042866, 0.071044, 0.071443, 0.378385, 0.084480, 0.084480, 0.027395, 0.027395, 0.027395, 0.027395, 0.027395, 0.027395, 0.438836, 0.008740, 0.009544, 0.009544, 0.009544, 0.023678, 0.433132, 0.026092, 0.024432, 0.040119, 0.039944, 0.410572, 0.067512, 0.067356};

int data_layout[] = {0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 2 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0};
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, n_pts = 1932;
      ros::NodeHandle nh;
      ros::Subscriber sub;
      ros::Publisher pub, pub_info;
      int32_t buffer[b_len][n_pts];
      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);

      //float maxs[n_pts];
      //for(int i=0;i<n_pts;i++)
      //   maxs[i] = 0.0;
      while(ros::ok()) {// && !kb_getc()) {
        
        ros::spinOnce();
        r.sleep();
        if(msg_in)
          break;
          //continue;
        //for(int i=0;i<n_pts;i++)
        //  if(fabs(cur_msg->data[i]) > maxs[i])
        //    maxs[i] = fabs(cur_msg->data[i]);

        ROS_INFO("Waiting for features message");
      }
      //for(int i=0;i<n_pts;i++)
      //  printf("%f, ",maxs[i]);
      //return;
      for(int i=0;i<b_len;i++) {
        for(int j=0;j<n_pts;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(int j=0;j<n_pts;j++) {
          buffer[seq % b_len][j] = convToColor(j, cur_msg->data[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 = n_pts - 812 + 55;
        cinfo.height = img.height; cinfo.width = img.width;
        img.encoding = "rgb8";
        img.is_bigendian = false;
        img.step = n_pts*3;
        
        std::vector<uint8_t> data;
        for(int i=0;i<b_len;i++)
          for(int j=812;j<n_pts;j++) {
            if(data_layout[j] != 0) {
              uint8_t r = 0x00, g = 0x00, b = 0xFF;
              if(data_layout[j] == 1) {
                g = 0xFF;
              } else if(data_layout[j] == 3) {
                r = 0xFF;
                g = 0xFF;
              } else {
                r = 0xFF;
              }
              data.push_back(r);
              data.push_back(g);
              data.push_back(b);
            }
            uint8_t 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);
          }

        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, "image_test");
  Main m(argv[1][0]);

  return 0;
}