$search
00001 /* 00002 * Copyright (c) 2010, Willow Garage, 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 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above copyright 00015 * notice, this list of conditions and the following disclaimer in the 00016 * documentation and/or other materials provided with the distribution. 00017 * * Neither the name of the Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived from 00019 * this software without specific prior written permission. 00020 * 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00023 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00024 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00025 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00026 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00027 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00028 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00029 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00030 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00031 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 */ 00034 00035 /* 00036 * Modified to throttle all Kinect data by Michal Spanel, Robo@FIT, BUT 00037 */ 00038 00039 #include <ros/ros.h> 00040 #include <pluginlib/class_list_macros.h> 00041 #include <nodelet/nodelet.h> 00042 00043 #include <sensor_msgs/PointCloud2.h> 00044 #include <sensor_msgs/Image.h> 00045 #include <sensor_msgs/CameraInfo.h> 00046 00047 #include <message_filters/subscriber.h> 00048 #include <message_filters/synchronizer.h> 00049 //#include <message_filters/sync_policies/exact_time.h> 00050 #include <message_filters/sync_policies/approximate_time.h> 00051 00052 #include <pcl/point_cloud.h> 00053 #include <pcl_ros/point_cloud.h> 00054 #include <pcl/point_types.h> 00055 #include <pcl/ros/conversions.h> 00056 00057 00058 namespace srs_env_model_utils 00059 { 00060 00061 typedef sensor_msgs::PointCloud2 tPointCloud; 00062 typedef sensor_msgs::Image tImage; 00063 typedef sensor_msgs::CameraInfo tCameraInfo; 00064 //typedef message_filters::sync_policies::ExactTime<tPointCloud, tImage, tCameraInfo, tImage, tPointCloud> tSyncPolicy; 00065 typedef message_filters::sync_policies::ApproximateTime<tPointCloud, tImage, tCameraInfo, tImage, tPointCloud> tSyncPolicy; 00066 00067 //const int QUEUE_SIZE = 10; 00068 const int QUEUE_SIZE = 1; 00069 00070 class KinectThrottle : public nodelet::Nodelet 00071 { 00072 public: 00073 //Constructor 00074 KinectThrottle(): max_update_rate_(0), sync_(tSyncPolicy(10)) 00075 { 00076 } 00077 00078 private: 00079 ros::Time last_update_; 00080 double max_update_rate_; 00081 virtual void onInit() 00082 { 00083 ros::NodeHandle& nh = getNodeHandle(); 00084 ros::NodeHandle& private_nh = getPrivateNodeHandle(); 00085 00086 private_nh.getParam("max_rate", max_update_rate_); 00087 00088 rgb_cloud_pub_ = nh.advertise<tPointCloud>("rgb_cloud_out", QUEUE_SIZE); 00089 rgb_image_pub_ = nh.advertise<tImage>("rgb_image_out", QUEUE_SIZE); 00090 rgb_caminfo_pub_ = nh.advertise<tCameraInfo>("rgb_caminfo_out", QUEUE_SIZE); 00091 depth_image_pub_ = nh.advertise<tImage>("depth_image_out", QUEUE_SIZE); 00092 cloud_pub_ = nh.advertise<tPointCloud>("cloud_out", QUEUE_SIZE); 00093 00094 rgb_cloud_sub_.subscribe(nh, "rgb_cloud_in", QUEUE_SIZE); 00095 rgb_image_sub_.subscribe(nh, "rgb_image_in", QUEUE_SIZE); 00096 rgb_caminfo_sub_.subscribe(nh, "rgb_caminfo_in", QUEUE_SIZE); 00097 depth_image_sub_.subscribe(nh, "depth_image_in", QUEUE_SIZE); 00098 cloud_sub_.subscribe(nh, "cloud_in", QUEUE_SIZE); 00099 00100 sync_.connectInput(rgb_cloud_sub_, rgb_image_sub_, rgb_caminfo_sub_, depth_image_sub_, cloud_sub_); 00101 sync_.registerCallback(boost::bind(&srs_env_model_utils::KinectThrottle::callback, this, _1, _2, _3, _4, _5)); 00102 } 00103 00104 void callback(const tPointCloud::ConstPtr& rgb_cloud, 00105 const tImage::ConstPtr& rgb_image, 00106 const tCameraInfo::ConstPtr& rgb_caminfo, 00107 const tImage::ConstPtr& depth_image, 00108 const tPointCloud::ConstPtr& cloud 00109 ) 00110 { 00111 if (max_update_rate_ > 0.0) 00112 { 00113 NODELET_DEBUG("update set to %f", max_update_rate_); 00114 if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now()) 00115 { 00116 NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec()); 00117 return; 00118 } 00119 } 00120 else 00121 NODELET_DEBUG("update_rate unset continuing"); 00122 00123 last_update_ = ros::Time::now(); 00124 00125 rgb_cloud_pub_.publish(rgb_cloud); 00126 rgb_image_pub_.publish(rgb_image); 00127 rgb_caminfo_pub_.publish(rgb_caminfo); 00128 depth_image_pub_.publish(depth_image); 00129 cloud_pub_.publish(cloud); 00130 } 00131 00132 ros::Publisher rgb_cloud_pub_, rgb_image_pub_, rgb_caminfo_pub_, depth_image_pub_, cloud_pub_; 00133 00134 message_filters::Subscriber<tPointCloud> rgb_cloud_sub_; 00135 message_filters::Subscriber<tImage> rgb_image_sub_; 00136 message_filters::Subscriber<tCameraInfo> rgb_caminfo_sub_; 00137 message_filters::Subscriber<tImage> depth_image_sub_; 00138 message_filters::Subscriber<tPointCloud> cloud_sub_; 00139 00140 message_filters::Synchronizer<tSyncPolicy> sync_; 00141 }; 00142 00143 00144 PLUGINLIB_DECLARE_CLASS(srs_env_model_utils, KinectThrottle, srs_env_model_utils::KinectThrottle, nodelet::Nodelet); 00145 } 00146