<commit_before>#include <depth_image_proc/depth_conversions.h>
#include <plane_calibration/depth_visualizer.hpp>
#include "plane_calibration/image_msg_eigen_converter.hpp"

namespace plane_calibration
{

DepthVisualizer::DepthVisualizer(ros::NodeHandle node_handle) :
    node_handle_(node_handle)
{
}

void DepthVisualizer::setCameraModel(const image_geometry::PinholeCameraModel& camera_model)
{
  std::lock_guard<std::mutex> lock(camera_model_mutex_);
  camera_model_ = camera_model;
}

void DepthVisualizer::publishImage(const std::string& topic, const Eigen::MatrixXf& image_matrix, std::string frame_id)
{
  sensor_msgs::Image image_msg;
  image_msg.header.frame_id = frame_id;
  ImageMsgEigenConverter::convert(image_matrix, image_msg);
  publishImage(topic, image_msg);
}

void DepthVisualizer::publishImage(const std::string& topic, const sensor_msgs::Image& image_msg)
{
  addPublisherIfNotExist<sensor_msgs::Image>(topic);
  publishers_[topic].publish(image_msg);
}

void DepthVisualizer::publishCloud(const std::string& topic, const Eigen::MatrixXf& image_matrix, std::string frame_id)
{
  sensor_msgs::Image image_msg;
  image_msg.header.frame_id = frame_id;
  ImageMsgEigenConverter::convert(image_matrix, image_msg);
  publishCloud(topic, image_msg);
}

void DepthVisualizer::publishCloud(const std::string& topic, const sensor_msgs::Image& image_msg)
{
  addPublisherIfNotExist<sensor_msgs::PointCloud2>(topic);
  sensor_msgs::PointCloud2Ptr point_cloud_msg_ptr = imageMsgToPointCloud(image_msg);
  publishers_[topic].publish(point_cloud_msg_ptr);
}

sensor_msgs::PointCloud2Ptr DepthVisualizer::imageMsgToPointCloud(const sensor_msgs::Image& image_msg)
{
  std::lock_guard<std::mutex> lock(camera_model_mutex_);
  if (!camera_model_.initialized())
  {
    return boost::make_shared<sensor_msgs::PointCloud2>();
  }

  return floatImageMsgToPointCloud(image_msg, camera_model_);
}

sensor_msgs::PointCloud2Ptr DepthVisualizer::floatImageMsgToPointCloud(<commit_msg>DepthVisualizer publish only if subscribed<commit_after>#include <depth_image_proc/depth_conversions.h>
#include <plane_calibration/depth_visualizer.hpp>
#include "plane_calibration/image_msg_eigen_converter.hpp"

namespace plane_calibration
{

DepthVisualizer::DepthVisualizer(ros::NodeHandle node_handle) :
    node_handle_(node_handle)
{
}

void DepthVisualizer::setCameraModel(const image_geometry::PinholeCameraModel& camera_model)
{
  std::lock_guard<std::mutex> lock(camera_model_mutex_);
  camera_model_ = camera_model;
}

void DepthVisualizer::publishImage(const std::string& topic, const Eigen::MatrixXf& image_matrix, std::string frame_id)
{
  sensor_msgs::Image image_msg;
  image_msg.header.frame_id = frame_id;
  ImageMsgEigenConverter::convert(image_matrix, image_msg);
  publishImage(topic, image_msg);
}

void DepthVisualizer::publishImage(const std::string& topic, const sensor_msgs::Image& image_msg)
{
  addPublisherIfNotExist<sensor_msgs::Image>(topic);

  if (publishers_[topic].getNumSubscribers() > 0)
  {
    publishers_[topic].publish(image_msg);
  }
}

void DepthVisualizer::publishCloud(const std::string& topic, const Eigen::MatrixXf& image_matrix, std::string frame_id)
{
  sensor_msgs::Image image_msg;
  image_msg.header.frame_id = frame_id;
  ImageMsgEigenConverter::convert(image_matrix, image_msg);
  publishCloud(topic, image_msg);
}

void DepthVisualizer::publishCloud(const std::string& topic, const sensor_msgs::Image& image_msg)
{
  addPublisherIfNotExist<sensor_msgs::PointCloud2>(topic);
  sensor_msgs::PointCloud2Ptr point_cloud_msg_ptr = imageMsgToPointCloud(image_msg);

  if (publishers_[topic].getNumSubscribers() > 0)
  {
    publishers_[topic].publish(point_cloud_msg_ptr);
  }
}

sensor_msgs::PointCloud2Ptr DepthVisualizer::imageMsgToPointCloud(const sensor_msgs::Image& image_msg)
{
  std::lock_guard<std::mutex> lock(camera_model_mutex_);
  if (!camera_model_.initialized())
  {
    return boost::make_shared<sensor_msgs::PointCloud2>();
  }

  return floatImageMsgToPointCloud(image_msg, camera_model_);
}

sensor_msgs::PointCloud2Ptr DepthVisualizer::floatImageMsgToPointCloud(
{'fraction_non_alphanumeric': 0.11184050571359105, 'fraction_numerical': 0.0029175784099197666, 'mean_word_length': 9.31077694235589, 'pattern_counts': {'":': 0, '<': 17, '<?xml version=': 0, '>': 19, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4790095', 'n_tokens_mistral': 1388, 'n_tokens_neox': 1368, 'n_words': 187}