Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include "disparity_color_publisher.h"
00035
00036 #include <rc_genicam_api/pixel_formats.h>
00037
00038 #include <sensor_msgs/image_encodings.h>
00039
00040 namespace rc
00041 {
00042
00043 DisparityColorPublisher::DisparityColorPublisher(image_transport::ImageTransport &it,
00044 std::string frame_id_prefix,
00045 double _scale)
00046 : GenICam2RosPublisher(frame_id_prefix)
00047 {
00048 scale=_scale;
00049 disprange=0;
00050
00051 pub=it.advertise("disparity_color", 1);
00052 }
00053
00054 void DisparityColorPublisher::setDisprange(int _disprange)
00055 {
00056 disprange=_disprange;
00057 }
00058
00059 bool DisparityColorPublisher::used()
00060 {
00061 return pub.getNumSubscribers() > 0;
00062 }
00063
00064 void DisparityColorPublisher::publish(const rcg::Buffer *buffer, uint64_t pixelformat)
00065 {
00066 if (pub.getNumSubscribers() > 0 && pixelformat == Coord3D_C16)
00067 {
00068
00069
00070 sensor_msgs::ImagePtr im=boost::make_shared<sensor_msgs::Image>();
00071
00072 const uint64_t freq=1000000000ul;
00073 uint64_t time=buffer->getTimestampNS();
00074
00075 im->header.seq=seq++;
00076 im->header.stamp.sec=time/freq;
00077 im->header.stamp.nsec=time-freq*im->header.stamp.sec;
00078 im->header.frame_id=frame_id;
00079
00080
00081
00082 im->width=static_cast<uint32_t>(buffer->getWidth());
00083 im->height=static_cast<uint32_t>(buffer->getHeight());
00084 im->is_bigendian=rcg::isHostBigEndian();
00085
00086
00087
00088 size_t px=buffer->getXPadding();
00089 const uint8_t *ps=static_cast<const uint8_t *>(buffer->getBase())+buffer->getImageOffset();
00090
00091
00092
00093 im->encoding=sensor_msgs::image_encodings::RGB8;
00094 im->step=3*im->width*sizeof(uint8_t);
00095
00096 im->data.resize(im->step*im->height);
00097 uint8_t *pt=reinterpret_cast<uint8_t *>(&im->data[0]);
00098
00099 bool bigendian=buffer->isBigEndian();
00100
00101 for (uint32_t k=0; k<im->height; k++)
00102 {
00103 for (uint32_t i=0; i<im->width; i++)
00104 {
00105 uint16_t d;
00106
00107 if (bigendian)
00108 {
00109 d=(ps[0]<<8)|ps[1];
00110 }
00111 else
00112 {
00113 d=(ps[1]<<8)|ps[0];
00114 }
00115
00116 ps+=2;
00117
00118 if (d != 0)
00119 {
00120 double v=scale*d/disprange;
00121 v=v/1.15+0.1;
00122
00123 double r=std::max(0.0, std::min(1.0, (1.5 - 4*fabs(v-0.75))));
00124 double g=std::max(0.0, std::min(1.0, (1.5 - 4*fabs(v-0.5))));
00125 double b=std::max(0.0, std::min(1.0, (1.5 - 4*fabs(v-0.25))));
00126
00127 *pt++=255*r+0.5;
00128 *pt++=255*g+0.5;
00129 *pt++=255*b+0.5;
00130 }
00131 else
00132 {
00133 *pt++=0;
00134 *pt++=0;
00135 *pt++=0;
00136 }
00137 }
00138
00139 ps+=px;
00140 }
00141
00142
00143
00144 pub.publish(im);
00145 }
00146 }
00147
00148 }