<commit_before>﻿#include <tf2/LinearMath/Quaternion.h>
#include "obstacledetect.h"


// Normal threshold, |z_norm_| > th_z_norm_ means the point is from plane
float th_z_norm_ = 0.7;

// Region growing threshold
float th_smooth_ = 8;

// Voxel grid threshold
float th_leaf_ = 0.01;
// th_deltaz_ must 2 times bigger than th_leaf_
float th_deltaz_ = 2 * th_leaf_;
float th_ratio_ = 5 * th_leaf_; // flatness ratio max value of plane

// Depth threshold
float th_max_depth_ = 1.3;

ObstacleDetect::ObstacleDetect(bool use_od, string base_frame, float base_to_ground, 
                               float table_height, float table_area) :
  use_od_(use_od),
  pub_it_(nh_),
  src_cloud_(new PointCloudMono),
  src_z_inliers_(new pcl::PointIndices),
  m_tf_(new Transform),
  base_frame_(base_frame),
  base_link_above_ground_(base_to_ground),
  table_height_(table_height),
  th_height_(0.2),
  th_area_(table_area)
{
  param_running_mode_ = "/status/running_mode";
  
  // For store max hull id and area
  global_area_temp_ = 0;
  
  sub_pointcloud_ = nh_.subscribe<sensor_msgs::PointCloud2>("/vision/depth_registered/points", 1, 
                                                            &ObstacleDetect::cloudCallback, this);
  initDepthCallback();
  
  pub_table_pose_ = nh_.advertise<geometry_msgs::PoseStamped>("/ctrl/vision/detect/table", 1);
  pub_table_points_ = nh_.advertise<sensor_msgs::PointCloud2>("/vision/table/points", 1);
  pub_exp_obj_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>("/vision/points_except_object", 1);
  pub_exp_obj_depth_ = pub_it_.advertise("/vision/depth/image_rect/except_object", 1);
}

ObstacleDetect::ObstacleDetect(bool use_od, string base_frame, 
                               float base_to_ground, float table_height, float table_area,
                               float grasp_area_x, float grasp_area_y, float tolerance) :
  use_od_(use_od),
  pub_it_(nh_),
  src_cloud_(new PointCloudMono),
  src_z_inliers_(new pcl::PointIndices), 
  m_tf_(new Transform),
  base_frame_(base_frame),
  base_link_above_ground_(base_to_ground),
  table_height_(table_height),
  th_height_(0.2),
  th_area_(table_area),
  grasp_area_x_(grasp_area_x),
  grasp_area_y_(grasp_area_y), 
  tolerance_(tolerance)
{
  param_running_mode_ = "/status/running_mode";
  
  // For store max hull id and area
  global_area_temp_ = 0;
  
  sub_pointcloud_ = nh_.subscribe<sensor_msgs::PointCloud2>("/vision/depth_registered/points", 1, 
                                                            &ObstacleDetect::cloudCallback, this);
  
  pub_table_points_ = nh_.advertise<sensor_msgs::PointCloud2>("/vision/table/points", 1);
  pub_exp_obj_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>("/vision/points_except_object", 1);
  pub_exp_obj_depth_ = pub_it_.advertise("/vision/depth/image_rect/except_object", 1);
}

void ObstacleDetect::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
{
  if (!use_od_)
    return;
  
  if (ros::param::has(param_running_mode_)) {<commit_msg>add depth image without object<commit_after>﻿#include <tf2/LinearMath/Quaternion.h>
#include "obstacledetect.h"


// Normal threshold, |z_norm_| > th_z_norm_ means the point is from plane
float th_z_norm_ = 0.7;

// Region growing threshold
float th_smooth_ = 8;

// Voxel grid threshold
float th_leaf_ = 0.01;
// th_deltaz_ must 2 times bigger than th_leaf_
float th_deltaz_ = 2 * th_leaf_;
float th_ratio_ = 5 * th_leaf_; // flatness ratio max value of plane

// Depth threshold
float th_max_depth_ = 1.3;

ObstacleDetect::ObstacleDetect(bool use_od, string base_frame, float base_to_ground, 
                               float table_height, float table_area) :
  use_od_(use_od),
  pub_it_(nh_),
  src_cloud_(new PointCloudMono),
  src_z_inliers_(new pcl::PointIndices),
  m_tf_(new Transform),
  base_frame_(base_frame),
  base_link_above_ground_(base_to_ground),
  table_height_(table_height),
  th_height_(0.2),
  th_area_(table_area)
{
  param_running_mode_ = "/status/running_mode";
  
  // For store max hull id and area
  global_area_temp_ = 0;
  
  sub_pointcloud_ = nh_.subscribe<sensor_msgs::PointCloud2>("/vision/depth_registered/points", 1, 
                                                            &ObstacleDetect::cloudCallback, this);
  initDepthCallback();
  
  pub_table_pose_ = nh_.advertise<geometry_msgs::PoseStamped>("/ctrl/vision/detect/table", 1);
  pub_table_points_ = nh_.advertise<sensor_msgs::PointCloud2>("/vision/table/points", 1);
  pub_exp_obj_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>("/vision/points_except_object", 1);
  pub_exp_obj_depth_ = pub_it_.advertise("/vision/depth/except_object", 1);
}

ObstacleDetect::ObstacleDetect(bool use_od, string base_frame, 
                               float base_to_ground, float table_height, float table_area,
                               float grasp_area_x, float grasp_area_y, float tolerance) :
  use_od_(use_od),
  pub_it_(nh_),
  src_cloud_(new PointCloudMono),
  src_z_inliers_(new pcl::PointIndices), 
  m_tf_(new Transform),
  base_frame_(base_frame),
  base_link_above_ground_(base_to_ground),
  table_height_(table_height),
  th_height_(0.2),
  th_area_(table_area),
  grasp_area_x_(grasp_area_x),
  grasp_area_y_(grasp_area_y), 
  tolerance_(tolerance)
{
  param_running_mode_ = "/status/running_mode";
  
  // For store max hull id and area
  global_area_temp_ = 0;
  
  sub_pointcloud_ = nh_.subscribe<sensor_msgs::PointCloud2>("/vision/depth_registered/points", 1, 
                                                            &ObstacleDetect::cloudCallback, this);
  initDepthCallback();
  pub_table_points_ = nh_.advertise<sensor_msgs::PointCloud2>("/vision/table/points", 1);
  pub_exp_obj_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>("/vision/points_except_object", 1);
  pub_exp_obj_depth_ = pub_it_.advertise("/vision/depth/except_object", 1);
}

void ObstacleDetect::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
{
  if (!use_od_)
    return;
  
  if (ros::param::has(param_running_mode_)) {
{'fraction_non_alphanumeric': 0.10864156576546691, 'fraction_numerical': 0.011278819041300382, 'mean_word_length': 3.9304987735077677, 'pattern_counts': {'":': 0, '<': 19, '<?xml version=': 0, '>': 21, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 8, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5809348', 'n_tokens_mistral': 2179, 'n_tokens_neox': 2078, 'n_words': 351}