37 #include <rc_genicam_api/config.h> 44 std::function<
void()>& sub_changed)
71 sensor_msgs::ImagePtr im = boost::make_shared<sensor_msgs::Image>();
76 im->header.stamp.sec = time / 1000000000ul;
77 im->header.stamp.nsec = time % 1000000000ul;
82 im->width =
static_cast<uint32_t
>(buffer->
getWidth(part));
83 im->height =
static_cast<uint32_t
>(buffer->
getHeight(part));
89 const uint8_t* ps =
static_cast<const uint8_t*
>(buffer->
getBase(part));
101 mindepth = std::max(mindepth, 2.5*t);
104 maxdepth = std::max(mindepth, maxdepth);
106 int dmin=
static_cast<int>(std::floor(f*t/maxdepth));
107 int dmax=
static_cast<int>(std::ceil(f*t/mindepth));
108 int drange=dmax-dmin+1;
113 im->step = 3 * im->width *
sizeof(uint8_t);
115 im->data.resize(im->step * im->height);
116 uint8_t* pt =
reinterpret_cast<uint8_t*
>(&im->data[0]);
118 for (uint32_t k = 0; k < im->height; k++)
120 for (uint32_t i = 0; i < im->width; i++)
126 d = (ps[0] << 8) | ps[1];
130 d = (ps[1] << 8) | ps[0];
137 double v = (scale * d - dmin) / drange;
140 double r = std::max(0.0, std::min(1.0, (1.5 - 4 * fabs(v - 0.75))));
141 double g = std::max(0.0, std::min(1.0, (1.5 - 4 * fabs(v - 0.5))));
142 double b = std::max(0.0, std::min(1.0, (1.5 - 4 * fabs(v - 0.25))));
144 *pt++ = 255 * r + 0.5;
145 *pt++ = 255 * g + 0.5;
146 *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.
static const int ComponentDisparity
std::function< void()> sub_callback
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
bool setEnum(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const char *name, const char *value, bool exception=false)
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
DisparityColorPublisher(image_transport::ImageTransport &it, const std::string &frame_id, std::function< void()> &sub_changed)
double getFloat(const std::shared_ptr< GenApi::CNodeMapRef > &nodemap, const char *name, double *vmin=0, double *vmax=0, bool exception=false, bool igncache=false)
uint64_t getTimestampNS() const
void subChangedIt(const image_transport::SingleSubscriberPublisher &pub)
Called by publisher if subscription changed.
std::shared_ptr< GenApi::CNodeMapRef > nodemap
void requiresComponents(int &components, bool &color) override
Adds components and if color images are required to the given values.
size_t getHeight(std::uint32_t part) const