Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <pluginlib/class_list_macros.h>
00036 #include "jsk_topic_tools/lightweight_throttle_nodelet.h"
00037
00038 namespace jsk_topic_tools
00039 {
00040 void LightweightThrottle::onInit()
00041 {
00042 pnh_ = this->getPrivateNodeHandle();
00043 latest_stamp_ = ros::Time::now();
00044 advertised_ = false;
00045 subscribing_ = false;
00046 pnh_.param("update_rate", update_rate_, 1.0);
00047 sub_.reset(new ros::Subscriber(
00048 pnh_.subscribe<topic_tools::ShapeShifter>("input", 1,
00049 &LightweightThrottle::inCallback,
00050 this,
00051 th_)));
00052 }
00053
00054 void LightweightThrottle::connectionCallback(
00055 const ros::SingleSubscriberPublisher& pub)
00056 {
00057 if (pub_.getNumSubscribers() > 0) {
00058 if (!subscribing_) {
00059 sub_.reset(new ros::Subscriber(
00060 pnh_.subscribe<topic_tools::ShapeShifter>(
00061 "input", 1,
00062 &LightweightThrottle::inCallback,
00063 this,
00064 th_)));
00065 subscribing_ = false;
00066 }
00067 }
00068 else {
00069 if (subscribing_) {
00070 sub_->shutdown();
00071 subscribing_ = true;
00072 }
00073 }
00074 }
00075
00076 void LightweightThrottle::inCallback(
00077 const boost::shared_ptr<topic_tools::ShapeShifter const>& msg)
00078 {
00079
00080 if (!advertised_) {
00081 ros::SubscriberStatusCallback connect_cb
00082 = boost::bind(&LightweightThrottle::connectionCallback, this, _1);
00083 ros::AdvertiseOptions opts("output", 1,
00084 msg->getMD5Sum(),
00085 msg->getDataType(),
00086 msg->getMessageDefinition(),
00087 connect_cb,
00088 connect_cb);
00089 pub_ = pnh_.advertise(opts);
00090 advertised_ = true;
00091 sub_->shutdown();
00092 }
00093 ros::Time now = ros::Time::now();
00094 if ((now - latest_stamp_).toSec() > 1 / update_rate_) {
00095
00096
00097 if (pub_.getNumSubscribers()) {
00098 pub_.publish(msg);
00099 }
00100 latest_stamp_ = now;
00101 }
00102 }
00103 }
00104
00105 typedef jsk_topic_tools::LightweightThrottle LightweightThrottle;
00106 PLUGINLIB_EXPORT_CLASS(LightweightThrottle, nodelet::Nodelet)