depth_layer.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2015-2016, Fetch Robotics Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Fetch Robotics Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL FETCH ROBOTICS INC. BE LIABLE FOR ANY
00021  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
00026  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027  */
00028 
00029 // Author: Anuj Pasricha, Michael Ferguson
00030 
00031 #ifndef FETCH_DEPTH_LAYER_DEPTH_LAYER_H
00032 #define FETCH_DEPTH_LAYER_DEPTH_LAYER_H
00033 
00034 #include <boost/thread/mutex.hpp>
00035 #include <boost/shared_ptr.hpp>
00036 #include <costmap_2d/voxel_layer.h>
00037 #include <cv_bridge/cv_bridge.h>
00038 #include <image_transport/image_transport.h>
00039 #include <sensor_msgs/image_encodings.h>
00040 #include <tf/message_filter.h>
00041 #include <message_filters/subscriber.h>
00042 
00043 #if CV_MAJOR_VERSION == 3
00044   #include <opencv2/rgbd.hpp>
00045   using cv::rgbd::DepthCleaner;
00046   using cv::rgbd::RgbdNormals;
00047   using cv::rgbd::RgbdPlane;
00048   using cv::rgbd::depthTo3d;
00049 #else
00050   #include <opencv2/rgbd/rgbd.hpp>
00051   using cv::DepthCleaner;
00052   using cv::RgbdNormals;
00053   using cv::RgbdPlane;
00054   using cv::depthTo3d;
00055 #endif
00056 
00057 namespace costmap_2d
00058 {
00059 
00064 class FetchDepthLayer : public VoxelLayer
00065 {
00066 public:
00070   FetchDepthLayer();
00071 
00075   virtual ~FetchDepthLayer();
00076 
00080   virtual void onInitialize();
00081 
00082 private:
00083   void cameraInfoCallback(
00084     const sensor_msgs::CameraInfo::ConstPtr& msg);
00085   void depthImageCallback(
00086     const sensor_msgs::Image::ConstPtr& msg);
00087 
00088   boost::shared_ptr<costmap_2d::ObservationBuffer> marking_buf_;
00089   boost::shared_ptr<costmap_2d::ObservationBuffer> clearing_buf_;
00090 
00091   // should we publish the marking/clearing observations
00092   bool publish_observations_;
00093 
00094   // distance away from ground plane at which
00095   // something is considered an obstacle
00096   double observations_threshold_;
00097 
00098   // should we dynamically find the ground plane?
00099   bool find_ground_plane_;
00100 
00101   // if finding ground plane, limit the tilt
00102   // with respect to base_link frame
00103   double ground_threshold_;
00104 
00105   // should NANs be treated as +inf and used for clearing
00106   bool clear_nans_;
00107 
00108   // skipping of potentially noisy rays near the edge of the image
00109   int skip_rays_bottom_;
00110   int skip_rays_top_;
00111   int skip_rays_left_;
00112   int skip_rays_right_;
00113 
00114   // should skipped edge rays be used for clearing?
00115   bool clear_with_skipped_rays_;
00116 
00117   // retrieves depth image from head_camera
00118   // used to fit ground plane to
00119   boost::shared_ptr< message_filters::Subscriber<sensor_msgs::Image> > depth_image_sub_;
00120   boost::shared_ptr< tf::MessageFilter<sensor_msgs::Image> > depth_image_filter_;
00121 
00122   // retrieves camera matrix for head_camera
00123   // used in calculating ground plane
00124   ros::Subscriber camera_info_sub_;
00125 
00126   // publishes clearing observations
00127   ros::Publisher clearing_pub_;
00128 
00129   // publishes marking observations
00130   ros::Publisher marking_pub_;
00131 
00132   // camera intrinsics
00133   boost::mutex mutex_K_;
00134   cv::Mat K_;
00135 
00136   // clean the depth image
00137   cv::Ptr<DepthCleaner> depth_cleaner_;
00138 
00139   // depth image estimation
00140   cv::Ptr<RgbdNormals> normals_estimator_;
00141   cv::Ptr<RgbdPlane> plane_estimator_;
00142 };
00143 
00144 }  // namespace costmap_2d
00145 
00146 #endif  // FETCH_DEPTH_LAYER_DEPTH_LAYER_H


fetch_depth_layer
Author(s):
autogenerated on Sat Aug 5 2017 04:00:24