Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #ifndef OPENNI_NODELET_OPENNI_H_
00040 #define OPENNI_NODELET_OPENNI_H_
00041
00042 #include <nodelet/nodelet.h>
00043 #include "openni_camera/openni_driver.h"
00044 #include <boost/shared_ptr.hpp>
00045 #include <dynamic_reconfigure/server.h>
00046 #include <openni_camera_deprecated/OpenNIConfig.h>
00047 #include <image_transport/image_transport.h>
00048 #include <string>
00049 #include <message_filters/synchronizer.h>
00050 #include <message_filters/sync_policies/approximate_time.h>
00051 #include <Eigen/Core>
00052 #include <pcl/PointIndices.h>
00053
00054 namespace openni_camera
00055 {
00056 using namespace openni_camera_deprecated;
00057
00059 class OpenNINodelet : public nodelet::Nodelet
00060 {
00061 public:
00062 virtual ~OpenNINodelet ();
00063 private:
00064 typedef OpenNIConfig Config;
00065 typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00066 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> SyncPolicy;
00067 typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
00068
00070 virtual void onInit ();
00071 void setupDevice (ros::NodeHandle& param_nh);
00072 void updateModeMaps ();
00073 void startSynchronization ();
00074 void stopSynchronization ();
00075 void setupDeviceModes (int image_mode, int depth_mode);
00076 bool isImageModeSupported (int image_mode) const;
00077 bool isDepthModeSupported (int depth_mode) const;
00078
00079 void configCallback (Config &config, uint32_t level);
00080 int mapXnMode2ConfigMode (const XnMapOutputMode& output_mode) const;
00081 XnMapOutputMode mapConfigMode2XnMode (int mode) const;
00082
00083
00084 void imageCallback (boost::shared_ptr<openni_wrapper::Image> image, void* cookie);
00085 void depthCallback (boost::shared_ptr<openni_wrapper::DepthImage> depth_image, void* cookie);
00086 void subscriberChangedEvent ();
00087
00088
00089 inline bool isImageStreamRequired() const;
00090 inline bool isDepthStreamRequired() const;
00091 sensor_msgs::CameraInfoPtr fillCameraInfo (ros::Time time, bool is_rgb);
00092
00093
00094 ros::Publisher pub_rgb_info_, pub_depth_info_;
00095 image_transport::Publisher pub_rgb_image_, pub_gray_image_, pub_depth_image_;
00096
00098 image_transport::Publisher pub_image_raw_, pub_depth_raw_;
00099
00100 ros::Publisher pub_disparity_;
00101 ros::Publisher pub_point_cloud_;
00102 ros::Publisher pub_point_cloud_rgb_;
00103
00105 ros::Subscriber sub_mask_indices_;
00106
00107
00108 boost::shared_ptr<Synchronizer> depth_rgb_sync_;
00109
00110
00111 void publishRgbImage (const openni_wrapper::Image& image, ros::Time time) const;
00112 void publishRgbImageRaw (const openni_wrapper::Image& image, ros::Time time) const;
00113 void publishGrayImage (const openni_wrapper::Image& image, ros::Time time) const;
00114 void publishDepthImage (const openni_wrapper::DepthImage& depth, ros::Time time) const;
00115 void publishDepthImageRaw (const openni_wrapper::DepthImage& depth, ros::Time time) const;
00116 void publishDisparity (const openni_wrapper::DepthImage& depth, ros::Time time) const;
00117 void publishXYZPointCloud (const openni_wrapper::DepthImage& depth, ros::Time time) const;
00118 void publishXYZRGBPointCloud (const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::ImageConstPtr& rgb_msg) const;
00119
00121 void maskIndicesCb(const pcl::PointIndicesConstPtr& indices);
00122
00124 boost::shared_ptr<openni_wrapper::OpenNIDevice> device_;
00125
00127 boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00128 Config config_;
00129 boost::recursive_mutex reconfigure_mutex_;
00130
00131
00132
00133 std::string rgb_frame_id_;
00134 std::string depth_frame_id_;
00135 unsigned image_width_;
00136 unsigned image_height_;
00137 unsigned depth_width_;
00138 unsigned depth_height_;
00139
00141 bool use_indices_;
00142
00144 std::vector<int32_t> mask_indices_;
00145
00146 struct modeComp
00147 {
00148 bool operator () (const XnMapOutputMode& mode1, const XnMapOutputMode& mode2) const
00149 {
00150 if (mode1.nXRes < mode2.nXRes)
00151 return true;
00152 else if (mode1.nXRes > mode2.nXRes)
00153 return false;
00154 else if (mode1.nYRes < mode2.nYRes)
00155 return true;
00156 else if (mode1.nYRes > mode2.nYRes)
00157 return false;
00158 else if (mode1.nFPS < mode2.nFPS)
00159 return true;
00160 else
00161 return false;
00162 }
00163 };
00164 std::map<XnMapOutputMode, int, modeComp> xn2config_map_;
00165 std::map<int, XnMapOutputMode> config2xn_map_;
00166 public:
00167 EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00168 };
00169
00170 bool OpenNINodelet::isImageStreamRequired() const
00171 {
00172 return (pub_rgb_image_.getNumSubscribers() > 0 ||
00173 pub_image_raw_.getNumSubscribers() > 0 ||
00174 pub_gray_image_.getNumSubscribers() > 0 ||
00175 pub_point_cloud_rgb_.getNumSubscribers() > 0 );
00176 }
00177
00178 bool OpenNINodelet::isDepthStreamRequired() const
00179 {
00180 return (pub_depth_image_.getNumSubscribers() > 0 ||
00181 pub_depth_raw_.getNumSubscribers() > 0 ||
00182 pub_disparity_.getNumSubscribers() > 0 ||
00183 pub_point_cloud_.getNumSubscribers() > 0 ||
00184 pub_point_cloud_rgb_.getNumSubscribers() > 0 );
00185 }
00186
00187 }
00188
00189 #endif //#ifndef OPENNI_NODELET_OPENNI_H_