00001 /****************************************************************************** 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2015, Michal Drwiega (drwiega.michal@gmail.com) 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions are met: 00009 * 1. Redistributions of source code must retain the above copyright 00010 * notice, this list of conditions and the following disclaimer. 00011 * 2. Redistributions in binary form must reproduce the above copyright 00012 * notice, this list of conditions and the following disclaimer in the 00013 * documentation and/or other materials provided with the distribution. 00014 * 3. Neither the name of the copyright holder nor the names of its 00015 * contributors may be used to endorse or promote products derived 00016 * from this software without specific prior written permission. 00017 * 00018 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00019 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00020 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 00021 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 00022 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 00023 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 00024 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 00025 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 00026 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 00027 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00028 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00029 *****************************************************************************/ 00037 #ifndef DEPTH_SENSOR_POSE_NODE 00038 #define DEPTH_SENSOR_POSE_NODE 00039 00040 #include <ros/ros.h> 00041 #include <image_transport/image_transport.h> 00042 #include <dynamic_reconfigure/server.h> 00043 #include <std_msgs/Float64.h> 00044 #include <sensor_msgs/Image.h> 00045 #include <sensor_msgs/image_encodings.h> 00046 00047 #include <opencv2/highgui/highgui.hpp> 00048 #include <opencv2/imgproc/imgproc.hpp> 00049 #include <cv_bridge/cv_bridge.h> 00050 #include <boost/thread/mutex.hpp> 00051 00052 #include <depth_sensor_pose/DepthSensorPoseConfig.h> 00053 #include <depth_sensor_pose/depth_sensor_pose.h> 00054 00055 namespace depth_sensor_pose 00056 { 00057 class DepthSensorPoseNode 00058 { 00059 public: 00060 DepthSensorPoseNode(ros::NodeHandle& n, ros::NodeHandle& pnh); 00061 00062 ~DepthSensorPoseNode(); 00068 void setNodeRate(const float rate); 00074 float getNodeRate(); 00075 private: 00085 void depthCb(const sensor_msgs::ImageConstPtr& depth_msg, 00086 const sensor_msgs::CameraInfoConstPtr& info_msg); 00095 void connectCb(); 00103 void disconnectCb(); 00112 void reconfigureCb(depth_sensor_pose::DepthSensorPoseConfig& config, uint32_t level); 00113 //------------------------------------------------------------------------------------------------- 00114 private: // Private fields 00115 00116 float node_rate_hz_; 00117 ros::NodeHandle pnh_; 00118 00119 image_transport::ImageTransport it_; 00120 image_transport::CameraSubscriber sub_; 00121 00122 ros::Publisher pub_height_; 00123 ros::Publisher pub_angle_; 00124 image_transport::Publisher pub_; 00125 00127 dynamic_reconfigure::Server<depth_sensor_pose::DepthSensorPoseConfig> srv_; 00128 00130 depth_sensor_pose::DepthSensorPose estimator_; 00131 00133 boost::mutex connection_mutex_; 00134 }; 00135 } 00136 00137 #endif