FramerateNode.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012-2016, Kevin Hallenbeck
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/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Kevin Hallenbeck 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 <ueye/FramerateNode.h>
00036 
00037 namespace ueye
00038 {
00039 
00040 FramerateNode::FramerateNode(ros::NodeHandle node, ros::NodeHandle priv_nh) :
00041     first_(true), rate_(0.0), stamp_old_(0)
00042 {
00043   // Grab the topic name from the ROS parameter
00044   std::string topic = std::string("image_raw");
00045   priv_nh.getParam("topic", topic);
00046 
00047   // Set up Subscribers
00048   sub_ = node.subscribe(topic, 2, &FramerateNode::imageRecv, this, ros::TransportHints().tcpNoDelay(true));
00049 }
00050 
00051 FramerateNode::~FramerateNode()
00052 {
00053 }
00054 
00055 void FramerateNode::imageRecv(const sensor_msgs::Image::ConstPtr& msg)
00056 {
00057   ros::Time stamp = ros::Time::now();
00058   if (first_) {
00059     first_ = false;
00060   } else {
00061     double temp_rate = (double)1e9 / (double)(stamp - stamp_old_).toNSec();
00062     if (rate_ == 0.0) {
00063       rate_ = temp_rate;
00064     } else {
00065       rate_ += (temp_rate - rate_) * 0.2;
00066     }
00067   }
00068   stamp_old_ = stamp;
00069 
00070   ROS_INFO("%d %dx%d at %0.2fHz", msg->header.seq, msg->width, msg->height, rate_);
00071 }
00072 
00073 } // namespace ueye


ueye
Author(s): Kevin Hallenbeck
autogenerated on Thu Sep 7 2017 09:31:27