00001
00060 #include "ros/ros.h"
00061 #include "pluginlib/class_list_macros.h"
00062 #include "nodelet/nodelet.h"
00063
00064 #include "sensor_msgs/PointCloud2.h"
00065 #include <sensor_msgs/Image.h>
00066 #include <sensor_msgs/CameraInfo.h>
00067
00068 #include <message_filters/subscriber.h>
00069 #include <message_filters/synchronizer.h>
00070 #include <message_filters/sync_policies/approximate_time.h>
00071
00072
00073 namespace cob_camera_sensors
00074 {
00075 typedef sensor_msgs::PointCloud2 tPointCloud;
00076 typedef sensor_msgs::Image tImage;
00077 typedef sensor_msgs::CameraInfo tCameraInfo;
00078 typedef message_filters::sync_policies::ApproximateTime<tPointCloud, tImage, tCameraInfo, tImage, tPointCloud> tSyncPolicy;
00079
00080 class Cam3DThrottle : public nodelet::Nodelet
00081 {
00082 public:
00083
00084 Cam3DThrottle() :
00085 max_update_rate_(0),
00086 sub_counter_(0),
00087 sync_(tSyncPolicy(5))
00088 {
00089 };
00090
00091 private:
00092 virtual void onInit()
00093 {
00094 nh_ = getNodeHandle();
00095 ros::NodeHandle& private_nh = getPrivateNodeHandle();
00096
00097 private_nh.getParam("max_rate", max_update_rate_);
00098
00099 rgb_cloud_pub_ = nh_.advertise<tPointCloud>("rgb_cloud_out", 1, boost::bind(&Cam3DThrottle::connectCB, this, _1), boost::bind(&Cam3DThrottle::disconnectCB, this, _1));
00100 rgb_image_pub_ = nh_.advertise<tImage>("rgb_image_out", 1, boost::bind(&Cam3DThrottle::connectCB, this, _1), boost::bind(&Cam3DThrottle::disconnectCB, this, _1));
00101 rgb_caminfo_pub_ = nh_.advertise<tCameraInfo>("rgb_caminfo_out", 1, boost::bind(&Cam3DThrottle::connectCB, this, _1), boost::bind(&Cam3DThrottle::disconnectCB, this, _1));
00102 depth_image_pub_ = nh_.advertise<tImage>("depth_image_out", 1, boost::bind(&Cam3DThrottle::connectCB, this, _1), boost::bind(&Cam3DThrottle::disconnectCB, this, _1));
00103 cloud_pub_ = nh_.advertise<tPointCloud>("cloud_out", 1, boost::bind(&Cam3DThrottle::connectCB, this, _1), boost::bind(&Cam3DThrottle::disconnectCB, this, _1));
00104
00105 sync_.connectInput(rgb_cloud_sub_, rgb_image_sub_, rgb_caminfo_sub_, depth_image_sub_, cloud_sub_);
00106 sync_.registerCallback(boost::bind(&Cam3DThrottle::callback, this, _1, _2, _3, _4, _5));
00107 };
00108
00109 void callback(const tPointCloud::ConstPtr& rgb_cloud,
00110 const tImage::ConstPtr& rgb_image,
00111 const tCameraInfo::ConstPtr& rgb_caminfo,
00112 const tImage::ConstPtr& depth_image,
00113 const tPointCloud::ConstPtr& cloud
00114 )
00115 {
00116 if (max_update_rate_ > 0.0)
00117 {
00118 NODELET_DEBUG("update set to %f", max_update_rate_);
00119 if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
00120 {
00121 NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
00122 return;
00123 }
00124 }
00125 else
00126 NODELET_DEBUG("update_rate unset continuing");
00127 last_update_ = ros::Time::now();
00128 rgb_cloud_pub_.publish(rgb_cloud);
00129 rgb_image_pub_.publish(rgb_image);
00130 rgb_caminfo_pub_.publish(rgb_caminfo);
00131 depth_image_pub_.publish(depth_image);
00132 cloud_pub_.publish(cloud);
00133 }
00134
00135 void connectCB(const ros::SingleSubscriberPublisher& pub)
00136 {
00137 sub_counter_++;
00138 if(sub_counter_ == 1)
00139 {
00140 ROS_DEBUG("connecting");
00141 rgb_cloud_sub_.subscribe(nh_, "rgb_cloud_in", 1);
00142 rgb_image_sub_.subscribe(nh_, "rgb_image_in", 1);
00143 rgb_caminfo_sub_.subscribe(nh_, "rgb_caminfo_in", 1);
00144 depth_image_sub_.subscribe(nh_, "depth_image_in", 1);
00145 cloud_sub_.subscribe(nh_, "cloud_in", 1);
00146 }
00147 }
00148
00149 void disconnectCB(const ros::SingleSubscriberPublisher& pub)
00150 {
00151 sub_counter_--;
00152 if(sub_counter_ == 0)
00153 {
00154 ROS_DEBUG("disconnecting");
00155 rgb_cloud_sub_.unsubscribe();
00156 rgb_image_sub_.unsubscribe();
00157 rgb_caminfo_sub_.unsubscribe();
00158 depth_image_sub_.unsubscribe();
00159 cloud_sub_.unsubscribe();
00160 }
00161 }
00162
00163 ros::Time last_update_;
00164 double max_update_rate_;
00165 unsigned int sub_counter_;
00166
00167 ros::NodeHandle nh_;
00168 ros::Subscriber sub_;
00169 ros::Subscriber sub2_;
00170 ros::Publisher rgb_cloud_pub_, rgb_image_pub_, rgb_caminfo_pub_, depth_image_pub_, cloud_pub_;
00171
00172 message_filters::Subscriber<tPointCloud> rgb_cloud_sub_;
00173 message_filters::Subscriber<tImage> rgb_image_sub_;
00174 message_filters::Subscriber<tCameraInfo> rgb_caminfo_sub_;
00175 message_filters::Subscriber<tImage> depth_image_sub_;
00176 message_filters::Subscriber<tPointCloud> cloud_sub_;
00177 message_filters::Synchronizer<tSyncPolicy> sync_;
00178
00179 };
00180
00181
00182 PLUGINLIB_DECLARE_CLASS(cob_camera_sensors, Cam3DThrottle, cob_camera_sensors::Cam3DThrottle, nodelet::Nodelet);
00183 }
00184
