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_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 DisparityPublisher::DisparityPublisher(ros::NodeHandle &nh,
00044 std::string frame_id_prefix, double _f,
00045 double _t, double _scale)
00046 : GenICam2RosPublisher(frame_id_prefix)
00047 {
00048 seq=0;
00049 f=_f;
00050 t=_t;
00051 scale=_scale;
00052 disprange=0;
00053
00054 pub=nh.advertise<stereo_msgs::DisparityImage>("disparity", 1);
00055 }
00056
00057 void DisparityPublisher::setDisprange(int _disprange)
00058 {
00059 disprange=_disprange;
00060 }
00061
00062 bool DisparityPublisher::used()
00063 {
00064 return pub.getNumSubscribers() > 0;
00065 }
00066
00067 void DisparityPublisher::publish(const rcg::Buffer *buffer, uint64_t pixelformat)
00068 {
00069 if (pub.getNumSubscribers() > 0 && pixelformat == Coord3D_C16)
00070 {
00071
00072
00073 stereo_msgs::DisparityImagePtr p=boost::make_shared<stereo_msgs::DisparityImage>();
00074
00075 const uint64_t freq=1000000000ul;
00076 uint64_t time=buffer->getTimestampNS();
00077
00078 p->header.seq=seq++;
00079 p->header.stamp.sec=time/freq;
00080 p->header.stamp.nsec=time-freq*p->header.stamp.sec;
00081 p->header.frame_id=frame_id;
00082
00083
00084
00085 p->image.header=p->header;
00086 p->image.width=static_cast<uint32_t>(buffer->getWidth());
00087 p->image.height=static_cast<uint32_t>(buffer->getHeight());
00088 p->image.encoding=sensor_msgs::image_encodings::TYPE_32FC1;
00089 p->image.is_bigendian=rcg::isHostBigEndian();
00090 p->image.step=p->image.width*sizeof(float);
00091
00092 size_t px=buffer->getXPadding();
00093 const uint8_t *ps=static_cast<const uint8_t *>(buffer->getBase())+buffer->getImageOffset();
00094
00095
00096
00097 p->image.data.resize(p->image.step*p->image.height);
00098
00099 float *pt=reinterpret_cast<float *>(&p->image.data[0]);
00100 float dmax=0;
00101
00102 bool bigendian=buffer->isBigEndian();
00103
00104 for (uint32_t k=0; k<p->image.height; k++)
00105 {
00106 if (bigendian)
00107 {
00108 for (uint32_t i=0; i<p->image.width; i++)
00109 {
00110 uint16_t d=(ps[0]<<8)|ps[1];
00111
00112 *pt=-1.0f;
00113
00114 if (d != 0)
00115 {
00116 *pt=scale*d;
00117 dmax=std::max(dmax, *pt);
00118 }
00119
00120 ps+=2;
00121 pt++;
00122 }
00123 }
00124 else
00125 {
00126 for (uint32_t i=0; i<p->image.width; i++)
00127 {
00128 uint16_t d=(ps[1]<<8)|ps[0];
00129
00130 *pt=-1.0f;
00131
00132 if (d != 0)
00133 {
00134 *pt=scale*d;
00135 dmax=std::max(dmax, *pt);
00136 }
00137
00138 ps+=2;
00139 pt++;
00140 }
00141 }
00142
00143 ps+=px;
00144 }
00145
00146 p->f=f*p->image.width;
00147 p->T=t;
00148 p->valid_window.x_offset=0;
00149 p->valid_window.y_offset=0;
00150 p->valid_window.width=p->image.width;
00151 p->valid_window.height=p->image.height;
00152 p->min_disparity=0;
00153 p->max_disparity=std::max(dmax, static_cast<float>(disprange));
00154 p->delta_d=scale;
00155
00156
00157
00158 pub.publish(p);
00159 }
00160 }
00161
00162 }