pointcloud_to_laserscan: pointcloud_to_laserscan: cloud

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  *     * Redistributions of source code must retain the above copyright                                                  
00009  *       notice, this list of conditions and the following disclaimer.                                                   
00010  *     * Redistributions in binary form must reproduce the above copyright                                               
00011  *       notice, this list of conditions and the following disclaimer in the                                             
00012  *       documentation and/or other materials provided with the distribution.                                            
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its                                                
00014  *       contributors may be used to endorse or promote products derived from                                            
00015  *       this software without specific prior written permission.                                                        
00016  *                                                                                                                       
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"                                           
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE                                             
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE                                            
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE                                              
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR                                                   
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF                                                  
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS                                              
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN                                               
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)                                               
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE                                            
00027  * POSSIBILITY OF SUCH DAMAGE.                                                                                           
00028  */
00029 
00030 #include "ros/ros.h"
00031 #include "pluginlib/class_list_macros.h"
00032 #include "nodelet/nodelet.h"
00033 #include "sensor_msgs/LaserScan.h"
00034 #include "pcl/point_cloud.h"
00035 #include "pcl_ros/point_cloud.h"
00036 #include "pcl/point_types.h"
00037 #include "pcl/ros/conversions.h"
00038 
00039 
00040 
00041 
00042 namespace pointcloud_to_laserscan
00043 {
00044 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00045 
00046 class CloudThrottle : public nodelet::Nodelet
00047 {
00048 public:
00049   //Constructor
00050   CloudThrottle(): max_update_rate_(0)
00051   {
00052   };
00053 
00054 private:
00055   ros::Time last_update_;
00056   double max_update_rate_;
00057   virtual void onInit()
00058   {
00059     ros::NodeHandle& nh = getNodeHandle();
00060     ros::NodeHandle& private_nh = getPrivateNodeHandle();
00061     
00062     private_nh.getParam("max_rate", max_update_rate_);
00063 
00064     pub_ = nh.advertise<PointCloud>("cloud_out", 10);
00065     sub_ = nh.subscribe<PointCloud>("cloud_in", 10, &CloudThrottle::callback, this);
00066   };
00067 
00068   void callback(const PointCloud::ConstPtr& cloud)
00069   {
00070     if (max_update_rate_ > 0.0)
00071     {
00072       NODELET_DEBUG("update set to %f", max_update_rate_);
00073       if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
00074       {
00075         NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
00076         return;
00077       }
00078     }
00079     else
00080       NODELET_DEBUG("update_rate unset continuing");
00081     last_update_ = ros::Time::now();
00082     pub_.publish(cloud);
00083   }
00084 
00085 
00086   ros::Publisher pub_;
00087   ros::Subscriber sub_;
00088   
00089 };
00090 
00091 
00092 PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, CloudThrottle, pointcloud_to_laserscan::CloudThrottle, nodelet::Nodelet);
00093 }
cloud_throttle.cpp
