hz_measure_nodelet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2014, JSK Lab
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
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/o2r other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <pluginlib/class_list_macros.h>
00036 #include "jsk_topic_tools/hz_measure_nodelet.h"
00037 
00038 #include "std_msgs/Float32.h"
00039 
00040 namespace jsk_topic_tools
00041 {
00042   void HzMeasure::onInit()
00043   {
00044     pnh_ = getPrivateNodeHandle();
00045     if (!pnh_.getParam("message_num", average_message_num_)) {
00046       average_message_num_ = 10; // defaults to 10
00047     }
00048     hz_pub_ = pnh_.advertise<std_msgs::Float32>("output", 1);
00049     sub_ = pnh_.subscribe<topic_tools::ShapeShifter>("input", 1,
00050                                                      &HzMeasure::inputCallback, this);
00051   }
00052 
00053   void HzMeasure::inputCallback(const boost::shared_ptr<topic_tools::ShapeShifter const>& msg)
00054   {
00055     ros::Time now = ros::Time::now();
00056     buffer_.push(now);
00057     if (buffer_.size() > average_message_num_) {
00058       ros::Time oldest = buffer_.front();
00059       double whole_time = (now - oldest).toSec();
00060       double average_time = whole_time / (buffer_.size() - 1);
00061       std_msgs::Float32 output;
00062       output.data = 1.0 / average_time;
00063       hz_pub_.publish(output);
00064       buffer_.pop();
00065     }
00066     else {
00067       NODELET_DEBUG("there is no enough messages yet");
00068     }
00069   }
00070   
00071 }
00072 
00073 typedef jsk_topic_tools::HzMeasure HzMeasure;
00074 PLUGINLIB_EXPORT_CLASS(HzMeasure, nodelet::Nodelet)


jsk_topic_tools
Author(s): Kei Okada , Yusuke Furuta
autogenerated on Sun Jan 25 2015 12:38:34