<commit_before>#include <depth_calibration/depth_adjuster.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <pluginlib/class_list_macros.h>
#include <deque>
#include <std_msgs/Float64.h>

namespace depth_calibration
{

static double m_to_depth = 1000;

void DepthAdjuster::onInit()
{
  ros::NodeHandle& nh = getNodeHandle();

  pub_calibrated_depth_raw_ = nh.advertise<sensor_msgs::Image>("output_depth_raw", 1);
  pub_inlier_ = nh.advertise<sensor_msgs::Image>("inlier", 1);

  pub_camera_info_relay_ = nh.advertise<sensor_msgs::CameraInfo>("output_camera_info_relay", 1);
  sub_camera_info_ = nh.subscribe<sensor_msgs::CameraInfo>("input_camera_info", 1, &DepthAdjuster::relay_camera_info,
                                                           this);

  invalid_ratio_pub_ = nh.advertise<std_msgs::Float64>("invalid_ratio", 1);

  ros::NodeHandle& private_nh = getPrivateNodeHandle();
  bool enable;
  private_nh.param("enable", enable, true);

  std::string calibration_file_path;
  private_nh.param("calibration_file_path", calibration_file_path, std::string("camera_info/depth_calibration.yaml"));

  if (enable)
    load_calibration(calibration_file_path);
  else
    ROS_WARN("[Depth adjuster] Depth calibration disabled by launch file");

  private_nh.param("max_distance", max_distance_, 0.0);
  max_distance_ = max_distance_ * m_to_depth;

  private_nh.param("unknown_depth_distance", unknown_depth_distance_, 0.0);
  unknown_depth_distance_ = unknown_depth_distance_ * m_to_depth;

  private_nh.param("is_occluded_percentage", is_occluded_percentage_, 1.0);
  private_nh.param("occluded_distance", occluded_distance_, 0.0);

  private_nh.param("border_percentage_top", border_percentage_top_, 0.0);
  private_nh.param("border_percentage_bottom", border_percentage_bottom_, 0.0);
  private_nh.param("border_percentage_left", border_percentage_left_, 0.0);
  private_nh.param("border_percentage_right", border_percentage_right_, 0.0);

  sub_depth_raw_ = nh.subscribe<sensor_msgs::Image>("input_depth_raw", 1, &DepthAdjuster::apply_calibration_cb, this);
}

void DepthAdjuster::load_calibration(std::string file_path)
{
  cv::FileStorage file_storage(file_path, cv::FileStorage::READ);

  if (!file_storage.isOpened())
  {
    ROS_WARN("[Depth adjuster] Cannot load depth calibration file: No file at %s", file_path.c_str());
    return;
  }

  file_storage["depth_multiplier"] >> depth_multiplier_correction_;
  file_storage.release();

  if (depth_multiplier_correction_.empty())
    ROS_WARN("[Depth adjuster] Depth calibration couldn't be loaded: Couldn't find depth_multiplier in file");
  else<commit_msg>Add warn message if occlusion detection is disabled<commit_after>#include <depth_calibration/depth_adjuster.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <pluginlib/class_list_macros.h>
#include <deque>
#include <std_msgs/Float64.h>

namespace depth_calibration
{

static double m_to_depth = 1000;

void DepthAdjuster::onInit()
{
  ros::NodeHandle& nh = getNodeHandle();

  pub_calibrated_depth_raw_ = nh.advertise<sensor_msgs::Image>("output_depth_raw", 1);
  pub_inlier_ = nh.advertise<sensor_msgs::Image>("inlier", 1);

  pub_camera_info_relay_ = nh.advertise<sensor_msgs::CameraInfo>("output_camera_info_relay", 1);
  sub_camera_info_ = nh.subscribe<sensor_msgs::CameraInfo>("input_camera_info", 1, &DepthAdjuster::relay_camera_info,
                                                           this);

  invalid_ratio_pub_ = nh.advertise<std_msgs::Float64>("invalid_ratio", 1);

  ros::NodeHandle& private_nh = getPrivateNodeHandle();
  bool enable;
  private_nh.param("enable", enable, true);

  std::string calibration_file_path;
  private_nh.param("calibration_file_path", calibration_file_path, std::string("camera_info/depth_calibration.yaml"));

  if (enable)
    load_calibration(calibration_file_path);
  else
    ROS_WARN("[Depth adjuster] Depth calibration disabled by launch file");

  private_nh.param("max_distance", max_distance_, 0.0);
  max_distance_ = max_distance_ * m_to_depth;

  private_nh.param("unknown_depth_distance", unknown_depth_distance_, 0.0);
  unknown_depth_distance_ = unknown_depth_distance_ * m_to_depth;

  private_nh.param("is_occluded_percentage", is_occluded_percentage_, 1.0);

  if(is_occluded_percentage_ <= 0.0)
  {
    ROS_WARN("[Depth adjuster] is_occluded_percentage is <= 0.0, occlusion detection is disabled");
  }

  private_nh.param("occluded_distance", occluded_distance_, 0.0);

  private_nh.param("border_percentage_top", border_percentage_top_, 0.0);
  private_nh.param("border_percentage_bottom", border_percentage_bottom_, 0.0);
  private_nh.param("border_percentage_left", border_percentage_left_, 0.0);
  private_nh.param("border_percentage_right", border_percentage_right_, 0.0);

  sub_depth_raw_ = nh.subscribe<sensor_msgs::Image>("input_depth_raw", 1, &DepthAdjuster::apply_calibration_cb, this);
}

void DepthAdjuster::load_calibration(std::string file_path)
{
  cv::FileStorage file_storage(file_path, cv::FileStorage::READ);

  if (!file_storage.isOpened())
  {
    ROS_WARN("[Depth adjuster] Cannot load depth calibration file: No file at %s", file_path.c_str());
    return;
  }

  file_storage["depth_multiplier"] >> depth_multiplier_correction_;
  file_storage.release();

  if (depth_multiplier_correction_.empty())
    ROS_WARN("[Depth adjuster] Depth calibration couldn't be loaded: Couldn't find depth_multiplier in file");
  else
{'fraction_non_alphanumeric': 0.11918337318374103, 'fraction_numerical': 0.012139047268714365, 'mean_word_length': 6.260347129506008, 'pattern_counts': {'":': 0, '<': 27, '<?xml version=': 0, '>': 29, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5073435', 'n_tokens_mistral': 1922, 'n_tokens_neox': 1819, 'n_words': 293}