48 static const std::string
LOGNAME =
"depth_self_filter_nodelet";
58 input_depth_transport_ = std::make_shared<image_transport::ImageTransport>(nh);
59 filtered_depth_transport_ = std::make_shared<image_transport::ImageTransport>(nh);
60 filtered_label_transport_ = std::make_shared<image_transport::ImageTransport>(nh);
61 model_depth_transport_ = std::make_shared<image_transport::ImageTransport>(nh);
62 model_label_transport_ = std::make_shared<image_transport::ImageTransport>(nh);
65 private_nh.
param(
"queue_size", queue_size_, 1);
66 private_nh.
param(
"near_clipping_plane_distance", near_clipping_plane_distance_, 0.4);
67 private_nh.
param(
"far_clipping_plane_distance", far_clipping_plane_distance_, 5.0);
69 private_nh.
param(
"shadow_threshold", shadow_threshold_, 0.3);
70 private_nh.
param(
"padding_scale", padding_scale_, 1.0);
71 private_nh.
param(
"padding_offset", padding_offset_, 0.005);
72 double tf_update_rate = 30.;
73 private_nh.
param(
"tf_update_rate", tf_update_rate, 30.0);
74 transform_provider_.setUpdateRate(tf_update_rate);
79 std::lock_guard<std::mutex> lock(connect_mutex_);
80 pub_filtered_depth_image_ =
81 filtered_depth_transport_->advertiseCamera(
"/filtered/depth", queue_size_, itssc, itssc, rssc, rssc);
82 pub_filtered_label_image_ =
83 filtered_label_transport_->advertiseCamera(
"/filtered/labels", queue_size_, itssc, itssc, rssc, rssc);
84 pub_model_depth_image_ =
85 model_depth_transport_->advertiseCamera(
"/model/depth", queue_size_, itssc, itssc, rssc, rssc);
86 pub_model_label_image_ =
87 model_depth_transport_->advertiseCamera(
"/model/label", queue_size_, itssc, itssc, rssc, rssc);
89 filtered_depth_ptr_ = std::make_shared<cv_bridge::CvImage>();
90 filtered_label_ptr_ = std::make_shared<cv_bridge::CvImage>();
91 model_depth_ptr_ = std::make_shared<cv_bridge::CvImage>();
92 model_label_ptr_ = std::make_shared<cv_bridge::CvImage>();
94 mesh_filter_ = std::make_shared<MeshFilter<StereoCameraModel>>(
96 return tfp.getTransform(mesh, tf);
99 mesh_filter_->parameters().setDepthRange(near_clipping_plane_distance_, far_clipping_plane_distance_);
100 mesh_filter_->setShadowThreshold(shadow_threshold_);
101 mesh_filter_->setPaddingOffset(padding_offset_);
102 mesh_filter_->setPaddingScale(padding_scale_);
104 addMeshes(*mesh_filter_);
105 transform_provider_.start();
109 const sensor_msgs::CameraInfoConstPtr& info_msg)
111 transform_provider_.setFrame(depth_msg->header.frame_id);
114 params.
setCameraParameters(info_msg->K[0], info_msg->K[4], info_msg->K[2], info_msg->K[5]);
115 params.
setImageSize(depth_msg->width, depth_msg->height);
121 "The input depth image uses a 16UC1 encoding. Consider converting the publisher to "
122 "generate the canonical 32FC1 encoding instead according to ROS REP-118.");
123 mesh_filter_->filter(depth_msg->data.data(), GL_UNSIGNED_SHORT);
126 mesh_filter_->filter(
reinterpret_cast<const float*
>(depth_msg->data.data()), GL_FLOAT);
133 if (pub_filtered_depth_image_.getNumSubscribers() > 0)
136 filtered_depth_ptr_->header = depth_msg->header;
138 if (
static_cast<uint32_t
>(filtered_depth_ptr_->image.cols) != depth_msg->width ||
139 static_cast<uint32_t
>(filtered_depth_ptr_->image.rows) != depth_msg->height)
140 filtered_depth_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_32FC1);
141 mesh_filter_->getFilteredDepth((
float*)filtered_depth_ptr_->image.data);
142 pub_filtered_depth_image_.publish(filtered_depth_ptr_->toImageMsg(), info_msg);
146 if (pub_model_depth_image_.getNumSubscribers() > 0)
149 model_depth_ptr_->header = depth_msg->header;
151 if (
static_cast<uint32_t
>(model_depth_ptr_->image.cols) != depth_msg->width ||
152 static_cast<uint32_t
>(model_depth_ptr_->image.rows) != depth_msg->height)
153 model_depth_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_32FC1);
154 mesh_filter_->getModelDepth((
float*)model_depth_ptr_->image.data);
155 pub_model_depth_image_.publish(model_depth_ptr_->toImageMsg(), info_msg);
158 if (pub_filtered_label_image_.getNumSubscribers() > 0)
161 filtered_label_ptr_->header = depth_msg->header;
163 if (
static_cast<uint32_t
>(filtered_label_ptr_->image.cols) != depth_msg->width ||
164 static_cast<uint32_t
>(filtered_label_ptr_->image.rows) != depth_msg->height)
165 filtered_label_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_8UC4);
166 mesh_filter_->getFilteredLabels((
unsigned int*)filtered_label_ptr_->image.data);
167 pub_filtered_label_image_.publish(filtered_label_ptr_->toImageMsg(), info_msg);
170 if (pub_model_label_image_.getNumSubscribers() > 0)
173 model_label_ptr_->header = depth_msg->header;
174 if (
static_cast<uint32_t
>(model_label_ptr_->image.cols) != depth_msg->width ||
175 static_cast<uint32_t
>(model_label_ptr_->image.rows) != depth_msg->height)
176 model_label_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_8UC4);
177 mesh_filter_->getModelLabels((
unsigned int*)model_label_ptr_->image.data);
178 pub_model_label_image_.publish(model_label_ptr_->toImageMsg(), info_msg);
186 const auto& links = robot_model->getLinkModelsWithCollisionGeometry();
187 for (
auto link : links)
189 const auto&
shapes = link->getShapes();
190 for (
const auto& shape :
shapes)
196 transform_provider_.addHandle(mesh_handle, link->getName());
205 std::lock_guard<std::mutex> lock(connect_mutex_);
206 if (pub_filtered_depth_image_.getNumSubscribers() == 0 && pub_filtered_label_image_.getNumSubscribers() == 0 &&
207 pub_model_depth_image_.getNumSubscribers() == 0 && pub_model_label_image_.getNumSubscribers() == 0)
209 sub_depth_image_.shutdown();
211 else if (!sub_depth_image_)
220 const sensor_msgs::CameraInfoConstPtr& info_msg)
222 filter(depth_msg, info_msg);