$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 work with RGB point clouds 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 #include "sensor_msgs/PointCloud2.h" 00043 #include "pcl/point_cloud.h" 00044 #include "pcl_ros/point_cloud.h" 00045 #include "pcl/point_types.h" 00046 #include "pcl/ros/conversions.h" 00047 00048 00049 namespace srs_env_model 00050 { 00051 typedef sensor_msgs::PointCloud2 PointCloud; 00052 //typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud; 00053 00054 class CloudThrottle : public nodelet::Nodelet 00055 { 00056 public: 00057 //Constructor 00058 CloudThrottle(): max_update_rate_(0) 00059 { 00060 }; 00061 00062 private: 00063 ros::Time last_update_; 00064 double max_update_rate_; 00065 virtual void onInit() 00066 { 00067 ros::NodeHandle& nh = getNodeHandle(); 00068 ros::NodeHandle& private_nh = getPrivateNodeHandle(); 00069 00070 private_nh.getParam("max_rate", max_update_rate_); 00071 00072 pub_ = nh.advertise<PointCloud>("cloud_out", 10); 00073 sub_ = nh.subscribe<PointCloud>("cloud_in", 10, &CloudThrottle::callback, this); 00074 }; 00075 00076 void callback(const PointCloud::ConstPtr& cloud) 00077 { 00078 if (max_update_rate_ > 0.0) 00079 { 00080 NODELET_DEBUG("update set to %f", max_update_rate_); 00081 if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now()) 00082 { 00083 NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec()); 00084 return; 00085 } 00086 } 00087 else 00088 NODELET_DEBUG("update_rate unset continuing"); 00089 last_update_ = ros::Time::now(); 00090 pub_.publish(cloud); 00091 } 00092 00093 ros::Publisher pub_; 00094 ros::Subscriber sub_; 00095 00096 }; 00097 00098 00099 PLUGINLIB_DECLARE_CLASS(srs_env_model, CloudThrottle, srs_env_model::CloudThrottle, nodelet::Nodelet); 00100 } 00101