depth_self_filter_nodelet.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Suat Gedikli */
36 
40 #include <ros/ros.h>
43 #include <moveit/robot_model_loader/robot_model_loader.h>
45 #include <eigen3/Eigen/Eigen>
46 #include <cv_bridge/cv_bridge.h>
47 
49 using namespace std;
50 using namespace boost;
51 
53 {
54 }
55 
57 {
58  ros::NodeHandle& nh = getNodeHandle();
59  ros::NodeHandle& private_nh = getPrivateNodeHandle();
60  input_depth_transport_.reset(new image_transport::ImageTransport(nh));
61  filtered_depth_transport_.reset(new image_transport::ImageTransport(nh));
62  filtered_label_transport_.reset(new image_transport::ImageTransport(nh));
63  model_depth_transport_.reset(new image_transport::ImageTransport(nh));
64  model_label_transport_.reset(new image_transport::ImageTransport(nh));
65 
66  // Read parameters
67  private_nh.param("queue_size", queue_size_, 1);
68  private_nh.param("near_clipping_plane_distance", near_clipping_plane_distance_, 0.4);
69  private_nh.param("far_clipping_plane_distance", far_clipping_plane_distance_, 5.0);
70  ;
71  private_nh.param("shadow_threshold", shadow_threshold_, 0.3);
72  private_nh.param("padding_scale", padding_scale_, 1.0);
73  private_nh.param("padding_offset", padding_offset_, 0.005);
74  double tf_update_rate = 30;
75  private_nh.param("tf_update_rate", tf_update_rate, 30.0);
76  transform_provider_.setUpdateInterval(long(1000000.0 / tf_update_rate));
77 
78  image_transport::SubscriberStatusCallback itssc = bind(&DepthSelfFiltering::connectCb, this);
79  ros::SubscriberStatusCallback rssc = bind(&DepthSelfFiltering::connectCb, this);
80 
81  lock_guard<mutex> lock(connect_mutex_);
82  pub_filtered_depth_image_ =
83  filtered_depth_transport_->advertiseCamera("/filtered/depth", queue_size_, itssc, itssc, rssc, rssc);
84  pub_filtered_label_image_ =
85  filtered_label_transport_->advertiseCamera("/filtered/labels", queue_size_, itssc, itssc, rssc, rssc);
86  pub_model_depth_image_ =
87  model_depth_transport_->advertiseCamera("/model/depth", queue_size_, itssc, itssc, rssc, rssc);
88  pub_model_label_image_ =
89  model_depth_transport_->advertiseCamera("/model/label", queue_size_, itssc, itssc, rssc, rssc);
90 
91  filtered_depth_ptr_.reset(new cv_bridge::CvImage);
92  filtered_label_ptr_.reset(new cv_bridge::CvImage);
93  model_depth_ptr_.reset(new cv_bridge::CvImage);
94  model_label_ptr_.reset(new cv_bridge::CvImage);
95 
96  mesh_filter_.reset(
97  new MeshFilter<StereoCameraModel>(bind(&TransformProvider::getTransform, &transform_provider_, _1, _2),
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_);
103  // add meshesfla
104  addMeshes(*mesh_filter_);
105  transform_provider_.start();
106 }
107 
108 void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& depth_msg,
109  const sensor_msgs::CameraInfoConstPtr& info_msg)
110 {
111  transform_provider_.setFrame(depth_msg->header.frame_id);
112  // do filtering here
113  mesh_filter::StereoCameraModel::Parameters& params = mesh_filter_->parameters();
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);
116 
117  const float* src = (const float*)&depth_msg->data[0];
118  //*
119  static unsigned dataSize = 0;
120  static unsigned short* data = 0;
121  if (dataSize < depth_msg->width * depth_msg->height)
122  data = new unsigned short[depth_msg->width * depth_msg->height];
123  for (unsigned idx = 0; idx < depth_msg->width * depth_msg->height; ++idx)
124  data[idx] = (unsigned short)(src[idx] * 1000.0);
125 
126  mesh_filter_->filter(data, GL_UNSIGNED_SHORT);
127  // delete[] data;
128  /*/
129  mesh_filter_->filter ((void*) &depth_msg->data[0], GL_FLOAT);
130  //*/
131  if (pub_filtered_depth_image_.getNumSubscribers() > 0)
132  {
133  filtered_depth_ptr_->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
134  filtered_depth_ptr_->header = depth_msg->header;
135 
136  if (filtered_depth_ptr_->image.cols != depth_msg->width || filtered_depth_ptr_->image.rows != depth_msg->height)
137  filtered_depth_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_32FC1);
138  mesh_filter_->getFilteredDepth((float*)filtered_depth_ptr_->image.data);
139  pub_filtered_depth_image_.publish(filtered_depth_ptr_->toImageMsg(), info_msg);
140  }
141 
142  // this is from rendering of the model
143  if (pub_model_depth_image_.getNumSubscribers() > 0)
144  {
145  model_depth_ptr_->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
146  model_depth_ptr_->header = depth_msg->header;
147 
148  if (model_depth_ptr_->image.cols != depth_msg->width || model_depth_ptr_->image.rows != depth_msg->height)
149  model_depth_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_32FC1);
150  mesh_filter_->getModelDepth((float*)model_depth_ptr_->image.data);
151  pub_model_depth_image_.publish(model_depth_ptr_->toImageMsg(), info_msg);
152  }
153 
154  if (pub_filtered_label_image_.getNumSubscribers() > 0)
155  {
156  filtered_label_ptr_->encoding = sensor_msgs::image_encodings::RGBA8;
157  filtered_label_ptr_->header = depth_msg->header;
158 
159  if (filtered_label_ptr_->image.cols != depth_msg->width || filtered_label_ptr_->image.rows != depth_msg->height)
160  filtered_label_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_8UC4);
161  mesh_filter_->getFilteredLabels((unsigned int*)filtered_label_ptr_->image.data);
162  pub_filtered_label_image_.publish(filtered_label_ptr_->toImageMsg(), info_msg);
163  }
164 
165  if (pub_model_label_image_.getNumSubscribers() > 0)
166  {
167  model_label_ptr_->encoding = sensor_msgs::image_encodings::RGBA8;
168  model_label_ptr_->header = depth_msg->header;
169  if (model_label_ptr_->image.cols != depth_msg->width || model_label_ptr_->image.rows != depth_msg->height)
170  model_label_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_8UC4);
171  mesh_filter_->getModelLabels((unsigned int*)model_label_ptr_->image.data);
172  pub_model_label_image_.publish(model_label_ptr_->toImageMsg(), info_msg);
173  }
174 }
175 
177 {
178  robot_model_loader::RobotModelLoader robotModelLoader("robot_description");
179  robot_model::RobotModelConstPtr robotModel = robotModelLoader.getModel();
180  const vector<robot_model::LinkModel*>& links = robotModel->getLinkModelsWithCollisionGeometry();
181  for (size_t i = 0; i < links.size(); ++i)
182  {
183  shapes::ShapeConstPtr shape = links[i]->getShape();
184  if (shape->type == shapes::MESH)
185  {
186  const shapes::Mesh& m = static_cast<const shapes::Mesh&>(*shape);
187  MeshHandle mesh_handle = mesh_filter.addMesh(m);
188  transform_provider_.addHandle(mesh_handle, links[i]->getName());
189  }
190  }
191 }
192 
193 // Handles (un)subscribing when clients (un)subscribe
195 {
196  lock_guard<mutex> lock(connect_mutex_);
197  if (pub_filtered_depth_image_.getNumSubscribers() == 0 && pub_filtered_label_image_.getNumSubscribers() == 0 &&
198  pub_model_depth_image_.getNumSubscribers() == 0 && pub_model_label_image_.getNumSubscribers() == 0)
199  {
200  sub_depth_image_.shutdown();
201  }
202  else if (!sub_depth_image_)
203  {
204  image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
205  sub_depth_image_ =
206  input_depth_transport_->subscribeCamera("depth", queue_size_, &DepthSelfFiltering::depthCb, this, hints);
207  }
208 }
209 
210 void mesh_filter::DepthSelfFiltering::depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
211  const sensor_msgs::CameraInfoConstPtr& info_msg)
212 {
213  filter(depth_msg, info_msg);
214 }
215 
Nodelet for filtering meshes from depth images. e.g. meshes of the robot or any attached object where...
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
std::string getName(void *handle)
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
MeshHandle addMesh(const shapes::Mesh &mesh)
adds a mesh to the filter object.
void filter(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
main filtering routine
void depthCb(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
Callback for subscribed depth images.
static const StereoCameraModel::Parameters & RegisteredPSDKParams
predefined sensor model for OpenNI compatible devices (e.g., PrimeSense, Kinect, Asus Xtion) ...
bool getTransform(mesh_filter::MeshHandle handle, Eigen::Affine3d &transform) const
returns the current transformation of a mesh given by its handle
Parameters for Stereo-like devices.
bool param(const std::string &param_name, T &param_val, const T &default_val) const
const std::string TYPE_32FC1
void setCameraParameters(float fx, float fy, float cx, float cy)
sets the camera parameters of the pinhole camera where the disparities were obtained. Usually the left camera
virtual void onInit()
Nodelet init callback.
void setImageSize(unsigned width, unsigned height)
sets the image size
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void connectCb()
Callback for connection/deconnection of listener.
void addMeshes(mesh_filter::MeshFilter< mesh_filter::StereoCameraModel > &mesh_filter)
adding the meshes to a given mesh filter object.
unsigned int MeshHandle
std::shared_ptr< const Shape > ShapeConstPtr


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Tue Jun 12 2018 02:48:04