43 const std::string& frame_id_prefix,
double _scale)
68 sensor_msgs::ImagePtr im = boost::make_shared<sensor_msgs::Image>();
70 const uint64_t freq = 1000000000ul;
73 im->header.seq =
seq++;
74 im->header.stamp.sec = time / freq;
75 im->header.stamp.nsec = time - freq * im->header.stamp.sec;
80 im->width =
static_cast<uint32_t
>(buffer->
getWidth(part));
81 im->height =
static_cast<uint32_t
>(buffer->
getHeight(part));
87 const uint8_t* ps =
static_cast<const uint8_t*
>(buffer->
getBase(part));
97 const uint8_t* p = ps;
101 for (uint32_t k = 0; k < im->height; k++)
103 for (uint32_t i = 0; i < im->width; i++)
105 dmax = std::max(dmax, (static_cast<int>(p[0]) << 8) | p[1]);
112 for (uint32_t k = 0; k < im->height; k++)
114 for (uint32_t i = 0; i < im->width; i++)
116 dmax = std::max(dmax, (static_cast<int>(p[1]) << 8) | p[0]);
122 drange = std::max(
disprange, static_cast<int>(std::ceil(dmax *
scale)));
128 im->step = 3 * im->width *
sizeof(uint8_t);
130 im->data.resize(im->step * im->height);
131 uint8_t* pt =
reinterpret_cast<uint8_t*
>(&im->data[0]);
133 for (uint32_t k = 0; k < im->height; k++)
135 for (uint32_t i = 0; i < im->width; i++)
141 d = (ps[0] << 8) | ps[1];
145 d = (ps[1] << 8) | ps[0];
152 double v =
scale * d / drange;
155 double r = std::max(0.0, std::min(1.0, (1.5 - 4 * fabs(v - 0.75))));
156 double g = std::max(0.0, std::min(1.0, (1.5 - 4 * fabs(v - 0.5))));
157 double b = std::max(0.0, std::min(1.0, (1.5 - 4 * fabs(v - 0.25))));
159 *pt++ = 255 * r + 0.5;
160 *pt++ = 255 * g + 0.5;
161 *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
DisparityColorPublisher(image_transport::ImageTransport &it, const std::string &frame_id_prefix, double scale)
Initialization of publisher.
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
size_t getHeight(std::uint32_t part) const
void setDisprange(int disprange)
Set the disparity range for scaling of images.