depthcloud_encoder.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  *  Copyright (c) 2014, Willow Garage, Inc.
00004  *  All rights reserved.
00005  *
00006  *  Redistribution and use in source and binary forms, with or without
00007  *  modification, are permitted provided that the following conditions
00008  *  are met:
00009  *
00010  *   * Redistributions of source code must retain the above copyright
00011  *     notice, this list of conditions and the following disclaimer.
00012  *   * Redistributions in binary form must reproduce the above
00013  *     copyright notice, this list of conditions and the following
00014  *     disclaimer in the documentation and/or other materials provided
00015  *     with the distribution.
00016  *   * Neither the name of the Willow Garage nor the names of its
00017  *     contributors may be used to endorse or promote products derived
00018  *     from this software without specific prior written permission.
00019  *
00020  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  *  POSSIBILITY OF SUCH DAMAGE.
00032 
00033  *  Author: Julius Kammerl (jkammerl@willowgarage.com)
00034  *
00035  */
00036 
00037 #ifndef DEPTHCLOUD_ENCODER_H
00038 #define DEPTHCLOUD_ENCODER_H
00039 
00040 #include <iostream>
00041 #include <string>
00042 #include <boost/thread.hpp>
00043 
00044 #include <image_transport/image_transport.h>
00045 #include <image_transport/subscriber_filter.h>
00046 
00047 #include <sensor_msgs/image_encodings.h>
00048 
00049 #include <message_filters/subscriber.h>
00050 #include <message_filters/synchronizer.h>
00051 #include <message_filters/sync_policies/approximate_time.h>
00052 
00053 #include <tf/transform_listener.h>
00054 #include <geometry_msgs/TransformStamped.h>
00055 
00056 #include <sensor_msgs/PointCloud2.h>
00057 #include <pcl/point_types.h>
00058 
00059 #include "ros/ros.h"
00060 
00061 namespace depthcloud
00062 {
00063 
00064 class DepthCloudEncoder
00065 {
00066 public:
00067   DepthCloudEncoder(ros::NodeHandle& nh, ros::NodeHandle& pnh);
00068   virtual ~DepthCloudEncoder();
00069 
00070 protected:
00071 
00072   void connectCb();
00073 
00074   void subscribe(std::string& depth_topic, std::string& color_topic);
00075   void subscribeCloud(std::string& cloud_topic);
00076   void unsubscribe();
00077 
00078   void cloudCB(const sensor_msgs::PointCloud2& cloud_msg);
00079 
00080   void depthCB(const sensor_msgs::ImageConstPtr& depth_msg);
00081 
00082   void depthColorCB(const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::ImageConstPtr& color_msg);
00083 
00084   void depthInterpolation(sensor_msgs::ImageConstPtr depth_msg,
00085                           sensor_msgs::ImagePtr depth_int_msg,
00086                           sensor_msgs::ImagePtr mask_msg);
00087 
00088   void cloudToDepth(const sensor_msgs::PointCloud2& cloud_msg, sensor_msgs::ImagePtr depth_msg, sensor_msgs::ImagePtr color_msg);
00089   void process(const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::ImageConstPtr& color_msg, const std::size_t crop_size);
00090 
00091 
00092   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> SyncPolicyDepthColor;
00093   typedef message_filters::Synchronizer<SyncPolicyDepthColor> SynchronizerDepthColor;
00094 
00095   ros::NodeHandle& nh_;
00096   ros::NodeHandle& pnh_;
00097 
00098   // ROS stuff
00099   boost::shared_ptr<image_transport::SubscriberFilter > depth_sub_;
00100   boost::shared_ptr<image_transport::SubscriberFilter > color_sub_;
00101   ros::Subscriber cloud_sub_;
00102 
00103   boost::shared_ptr<SynchronizerDepthColor> sync_depth_color_;
00104 
00105   boost::mutex connect_mutex_;
00106 
00107   image_transport::ImageTransport pub_it_;
00108   image_transport::Publisher pub_;
00109 
00110   std::size_t crop_size_;
00111 
00112   std::string depthmap_topic_;
00113   std::string rgb_image_topic_;
00114   std::string cloud_topic_;
00115   std::string camera_frame_id_;
00116   std::string depth_source_;
00117 
00118   tf::TransformListener tf_listener_;
00119 
00120   double f_;
00121 
00122   bool connectivityExceptionFlag, lookupExceptionFlag;
00123 };
00124 
00125 }
00126 
00127 #endif
00128 


depthcloud_encoder
Author(s): Julius Kammer
autogenerated on Thu Feb 11 2016 22:52:02