43 const std::string& frame_id_prefix,
double _f,
double _t,
double _scale)
75 int drange=dmax-dmin+1;
79 sensor_msgs::ImagePtr im = boost::make_shared<sensor_msgs::Image>();
81 const uint64_t freq = 1000000000ul;
84 im->header.seq =
seq++;
85 im->header.stamp.sec = time / freq;
86 im->header.stamp.nsec = time - freq * im->header.stamp.sec;
91 im->width =
static_cast<uint32_t
>(buffer->
getWidth(part));
92 im->height =
static_cast<uint32_t
>(buffer->
getHeight(part));
98 const uint8_t* ps =
static_cast<const uint8_t*
>(buffer->
getBase(part));
105 im->step = 3 * im->width *
sizeof(uint8_t);
107 im->data.resize(im->step * im->height);
108 uint8_t* pt =
reinterpret_cast<uint8_t*
>(&im->data[0]);
110 for (uint32_t k = 0; k < im->height; k++)
112 for (uint32_t i = 0; i < im->width; i++)
118 d = (ps[0] << 8) | ps[1];
122 d = (ps[1] << 8) | ps[0];
129 double v = (
scale * d - dmin) / drange;
132 double r = std::max(0.0, std::min(1.0, (1.5 - 4 * fabs(v - 0.75))));
133 double g = std::max(0.0, std::min(1.0, (1.5 - 4 * fabs(v - 0.5))));
134 double b = std::max(0.0, std::min(1.0, (1.5 - 4 * fabs(v - 0.25))));
136 *pt++ = 255 * r + 0.5;
137 *pt++ = 255 * g + 0.5;
138 *pt++ = 255 * b + 0.5;
Interface for all publishers relating to images, point clouds or other stereo-camera data...
bool used() override
Returns true if there are subscribers to the topic.
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
size_t getXPadding(std::uint32_t part) const
uint32_t getNumSubscribers() const
size_t getWidth(std::uint32_t part) const
void publish(const rcg::Buffer *buffer, uint32_t part, uint64_t pixelformat) override
Offers a buffer for publication.
image_transport::Publisher pub
void * getBase(std::uint32_t part) const
void publish(const sensor_msgs::Image &message) const
uint64_t getTimestampNS() const
DisparityColorPublisher(image_transport::ImageTransport &it, const std::string &frame_id_prefix, double f, double t, double scale)
Initialization of publisher.
size_t getHeight(std::uint32_t part) const
void setDepthRange(double _mindepth, double _maxdepth)
Set the depth range of the disparity images.