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 #include "ros/ros.h"
00029 #include "pluginlib/class_list_macros.h"
00030 #include "nodelet/nodelet.h"
00031
00032 #include <message_filters/subscriber.h>
00033 #include <message_filters/time_synchronizer.h>
00034 #include <message_filters/sync_policies/approximate_time.h>
00035 #include <message_filters/sync_policies/exact_time.h>
00036
00037 #include <image_transport/image_transport.h>
00038 #include <image_transport/subscriber_filter.h>
00039
00040 #include <sensor_msgs/CameraInfo.h>
00041
00042 #include <cv_bridge/cv_bridge.h>
00043
00044 #include <rtabmap/core/util3d.h>
00045
00046 namespace rtabmap_ros
00047 {
00048
00049 class DataThrottleNodelet : public nodelet::Nodelet
00050 {
00051 public:
00052
00053 DataThrottleNodelet():
00054 rate_(0),
00055 approxSync_(0),
00056 exactSync_(0),
00057 decimation_(1)
00058 {
00059 }
00060
00061 virtual ~DataThrottleNodelet()
00062 {
00063 if(approxSync_)
00064 {
00065 delete approxSync_;
00066 }
00067 if(exactSync_)
00068 {
00069 delete exactSync_;
00070 }
00071 }
00072
00073 private:
00074 ros::Time last_update_;
00075 double rate_;
00076 virtual void onInit()
00077 {
00078 ros::NodeHandle& nh = getNodeHandle();
00079 ros::NodeHandle& private_nh = getPrivateNodeHandle();
00080
00081 ros::NodeHandle rgb_nh(nh, "rgb");
00082 ros::NodeHandle depth_nh(nh, "depth");
00083 ros::NodeHandle rgb_pnh(private_nh, "rgb");
00084 ros::NodeHandle depth_pnh(private_nh, "depth");
00085 image_transport::ImageTransport rgb_it(rgb_nh);
00086 image_transport::ImageTransport depth_it(depth_nh);
00087 image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
00088 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
00089
00090 int queueSize = 10;
00091 bool approxSync = true;
00092 if(private_nh.getParam("max_rate", rate_))
00093 {
00094 ROS_WARN("\"max_rate\" is now known as \"rate\".");
00095 }
00096 private_nh.param("rate", rate_, rate_);
00097 private_nh.param("queue_size", queueSize, queueSize);
00098 private_nh.param("approx_sync", approxSync, approxSync);
00099 private_nh.param("decimation", decimation_, decimation_);
00100 ROS_ASSERT(decimation_ >= 1);
00101 ROS_INFO("Rate=%f Hz", rate_);
00102 ROS_INFO("Decimation=%d", decimation_);
00103 ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
00104
00105 if(approxSync)
00106 {
00107 approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
00108 approxSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3));
00109 }
00110 else
00111 {
00112 exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
00113 exactSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3));
00114 }
00115
00116 image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb);
00117 image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
00118 info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
00119
00120 imagePub_ = rgb_it.advertise("image_out", 1);
00121 imageDepthPub_ = depth_it.advertise("image_out", 1);
00122 infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 1);
00123 };
00124
00125 void callback(const sensor_msgs::ImageConstPtr& image,
00126 const sensor_msgs::ImageConstPtr& imageDepth,
00127 const sensor_msgs::CameraInfoConstPtr& camInfo)
00128 {
00129 if (rate_ > 0.0)
00130 {
00131 NODELET_DEBUG("update set to %f", rate_);
00132 if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
00133 {
00134 NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
00135 return;
00136 }
00137 }
00138 else
00139 NODELET_DEBUG("rate unset continuing");
00140
00141 last_update_ = ros::Time::now();
00142
00143 if(imagePub_.getNumSubscribers())
00144 {
00145 if(decimation_ > 1)
00146 {
00147 cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
00148 cv_bridge::CvImage out;
00149 out.header = imagePtr->header;
00150 out.encoding = imagePtr->encoding;
00151 out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
00152 imagePub_.publish(out.toImageMsg());
00153 }
00154 else
00155 {
00156 imagePub_.publish(image);
00157 }
00158 }
00159 if(imageDepthPub_.getNumSubscribers())
00160 {
00161 if(decimation_ > 1)
00162 {
00163 cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageDepth);
00164 cv_bridge::CvImage out;
00165 out.header = imagePtr->header;
00166 out.encoding = imagePtr->encoding;
00167 out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
00168 imageDepthPub_.publish(out.toImageMsg());
00169 }
00170 else
00171 {
00172 imageDepthPub_.publish(imageDepth);
00173 }
00174 }
00175 if(infoPub_.getNumSubscribers())
00176 {
00177 if(decimation_ > 1)
00178 {
00179 sensor_msgs::CameraInfo info = *camInfo;
00180 info.height /= decimation_;
00181 info.width /= decimation_;
00182 info.roi.height /= decimation_;
00183 info.roi.width /= decimation_;
00184 info.K[2]/=float(decimation_);
00185 info.K[5]/=float(decimation_);
00186 info.K[0]/=float(decimation_);
00187 info.K[4]/=float(decimation_);
00188 info.P[2]/=float(decimation_);
00189 info.P[6]/=float(decimation_);
00190 info.P[0]/=float(decimation_);
00191 info.P[5]/=float(decimation_);
00192 infoPub_.publish(info);
00193 }
00194 else
00195 {
00196 infoPub_.publish(camInfo);
00197 }
00198 }
00199 }
00200
00201 image_transport::Publisher imagePub_;
00202 image_transport::Publisher imageDepthPub_;
00203 ros::Publisher infoPub_;
00204
00205 image_transport::SubscriberFilter image_sub_;
00206 image_transport::SubscriberFilter image_depth_sub_;
00207 message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
00208
00209 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
00210 message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
00211 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
00212 message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
00213
00214 int decimation_;
00215
00216 };
00217
00218
00219 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::DataThrottleNodelet, nodelet::Nodelet);
00220 }