driver.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011 Willow Garage, Inc.
5  * Radu Bogdan Rusu <rusu@willowgarage.com>
6  * Suat Gedikli <gedikli@willowgarage.com>
7  * Patrick Mihelich <mihelich@willowgarage.com>
8  *
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above
18  * copyright notice, this list of conditions and the following
19  * disclaimer in the documentation and/or other materials provided
20  * with the distribution.
21  * * Neither the name of Willow Garage, Inc. nor the names of its
22  * contributors may be used to endorse or promote products derived
23  * from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  *
38  */
39 #ifndef OPENNI_CAMERA_DRIVER_H
40 #define OPENNI_CAMERA_DRIVER_H
41 
42 // ROS communication
43 #include <ros/ros.h>
44 #include <nodelet/nodelet.h>
46 #include <boost/thread.hpp>
47 
48 // Configuration
50 #include <dynamic_reconfigure/server.h>
51 #include <xiaoqiang_freenect_camera/FreenectConfig.h>
52 
53 // freenect wrapper
54 //#include <xiaoqiang_freenect_camera/freenect_driver.hpp>
56 // diagnostics
59 
61 {
64  {
65  public:
66  virtual ~DriverNodelet ();
67  private:
68  typedef FreenectConfig Config;
69  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
73 
75  virtual void onInit ();
76  void onInitImpl ();
77  void setupDevice ();
78  void updateModeMaps ();
79  void startSynchronization ();
80  void stopSynchronization ();
81 
83  int mapMode2ConfigMode (const OutputMode& output_mode) const;
84  OutputMode mapConfigMode2OutputMode (int mode) const;
85 
86  // Callback methods
87  void rgbCb(const ImageBuffer& image, void* cookie);
88  void depthCb(const ImageBuffer& depth_image, void* cookie);
89  void irCb(const ImageBuffer&_image, void* cookie);
90  void configCb(Config &config, uint32_t level);
91 
92  void rgbConnectCb();
93  void depthConnectCb();
94  void irConnectCb();
95 
96  // Methods to get calibration parameters for the various cameras
97  sensor_msgs::CameraInfoPtr getDefaultCameraInfo(int width, int height, double f) const;
98  sensor_msgs::CameraInfoPtr getRgbCameraInfo(const ImageBuffer& image, ros::Time time) const;
99  sensor_msgs::CameraInfoPtr getIrCameraInfo(const ImageBuffer& image, ros::Time time) const;
100  sensor_msgs::CameraInfoPtr getDepthCameraInfo(const ImageBuffer& image, ros::Time time) const;
101  sensor_msgs::CameraInfoPtr getProjectorCameraInfo(const ImageBuffer& image, ros::Time time) const;
102 
103  // published topics
108 
109  // Maintain frequency diagnostics on all sensors
112  TopicDiagnosticPtr pub_rgb_freq_;
114  TopicDiagnosticPtr pub_depth_freq_;
116  TopicDiagnosticPtr pub_ir_freq_;
118  boost::thread diagnostics_thread_;
119  void updateDiagnostics();
121 
123  boost::thread tilt_thread_;
125 
132  // publish methods
133  void publishRgbImage(const ImageBuffer& image, ros::Time time) const;
134  void publishDepthImage(const ImageBuffer& depth, ros::Time time) const;
135  void publishIrImage(const ImageBuffer& ir, ros::Time time) const;
136 
139  boost::thread init_thread_;
140  boost::mutex connect_mutex_;
141 
144  Config config_;
145 
148  std::string rgb_frame_id_;
149  std::string depth_frame_id_;
153 
154  // Counters/flags for skipping frames
155  boost::mutex counter_mutex_;
162  void checkFrameCounters();
163 
164  void watchDog(const ros::TimerEvent& event);
165 
167  double time_out_;
173 
176 
177  std::map<OutputMode, int> mode2config_map_;
178  std::map<int, OutputMode> config2mode_map_;
179  unsigned int num_frame_;
180  };
181 }
182 
183 #endif
image_transport::CameraPublisher pub_ir_
Definition: driver.h:106
dynamic_reconfigure::Server< Config > ReconfigureServer
Definition: driver.h:69
freenect_device_flags subdevs_
Definition: driver.h:131
image_transport::CameraPublisher pub_rgb_
Definition: driver.h:104
void publishDepthImage(const ImageBuffer &depth, ros::Time time) const
Definition: driver.cpp:612
void configCb(Config &config, uint32_t level)
Definition: driver.cpp:780
image_transport::CameraPublisher pub_depth_
Definition: driver.h:105
void rgbCb(const ImageBuffer &image, void *cookie)
Definition: driver.cpp:508
virtual void onInit()
Nodelet initialization routine.
Definition: driver.cpp:71
void publishIrImage(const ImageBuffer &ir, ros::Time time) const
Definition: driver.cpp:657
boost::shared_ptr< TopicDiagnostic > TopicDiagnosticPtr
Definition: driver.h:72
void depthCb(const ImageBuffer &depth_image, void *cookie)
Definition: driver.cpp:530
OutputMode mapConfigMode2OutputMode(int mode) const
Definition: driver.cpp:899
sensor_msgs::CameraInfoPtr getDefaultCameraInfo(int width, int height, double f) const
Definition: driver.cpp:676
image_transport::CameraPublisher pub_depth_registered_
Definition: driver.h:105
Holds an image buffer with all the metadata required to transmit the image over ROS channels...
diagnostic_updater::FrequencyStatusParam FrequencyStatusParam
Definition: driver.h:70
std::map< OutputMode, int > mode2config_map_
Definition: driver.h:177
boost::shared_ptr< camera_info_manager::CameraInfoManager > rgb_info_manager_
Camera info manager objects.
Definition: driver.h:147
TopicDiagnosticPtr pub_depth_freq_
Definition: driver.h:114
std::map< int, OutputMode > config2mode_map_
Definition: driver.h:178
int mapMode2ConfigMode(const OutputMode &output_mode) const
Definition: driver.cpp:886
sensor_msgs::CameraInfoPtr getRgbCameraInfo(const ImageBuffer &image, ros::Time time) const
Definition: driver.cpp:711
freenect_resolution
sensor_msgs::CameraInfoPtr getIrCameraInfo(const ImageBuffer &image, ros::Time time) const
Definition: driver.cpp:732
void publishRgbImage(const ImageBuffer &image, ros::Time time) const
Definition: driver.cpp:578
boost::shared_ptr< camera_info_manager::CameraInfoManager > ir_info_manager_
Definition: driver.h:147
diagnostic_updater::HeaderlessTopicDiagnostic TopicDiagnostic
Definition: driver.h:71
bool libfreenect_debug_
enable libfreenect debugging
Definition: driver.h:175
boost::shared_ptr< ReconfigureServer > reconfigure_server_
reconfigure server
Definition: driver.h:143
unsigned int uint32_t
freenect_device_flags
double time_out_
timeout value in seconds to throw TIMEOUT exception
Definition: driver.h:167
void irCb(const ImageBuffer &_image, void *cookie)
Definition: driver.cpp:557
boost::shared_ptr< diagnostic_updater::Updater > diagnostic_updater_
Definition: driver.h:110
sensor_msgs::CameraInfoPtr getDepthCameraInfo(const ImageBuffer &image, ros::Time time) const
Definition: driver.cpp:753
void watchDog(const ros::TimerEvent &event)
Definition: driver.cpp:911
boost::shared_ptr< FreenectDevice > device_
the actual openni device
Definition: driver.h:138
sensor_msgs::CameraInfoPtr getProjectorCameraInfo(const ImageBuffer &image, ros::Time time) const
Definition: driver.cpp:769


xiaoqiang_freenect_camera
Author(s): Patrick Mihelich, Suat Gedikli, Radu Bogdan Rusu (original openni_camera driver)., Piyush Khandelwal (libfreenect port).
autogenerated on Mon Jun 10 2019 15:53:18