38 #include <opencv2/highgui/highgui.hpp>
39 #include <boost/make_shared.hpp>
55 void CompressedDepthPublisher::advertiseImpl(
ros::NodeHandle &nh,
const std::string &base_topic, uint32_t queue_size,
61 Base::advertiseImpl(nh, base_topic, queue_size, user_connect_cb, user_disconnect_cb, tracked_object, latch);
64 reconfigure_server_ = boost::make_shared<ReconfigureServer>(this->nh());
65 ReconfigureServer::CallbackType
f = boost::bind(&CompressedDepthPublisher::configCb,
this, boost::placeholders::_1, boost::placeholders::_2);
66 reconfigure_server_->setCallback(
f);
69 void CompressedDepthPublisher::configCb(
Config& config, uint32_t level)
74 void CompressedDepthPublisher::publish(
const sensor_msgs::Image& message,
const PublishFn& publish_fn)
const
76 if (!config_.enable) {
80 sensor_msgs::CompressedImage::Ptr compressed_image =
85 publish_fn(*compressed_image);