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
00036 #include <image_transport/image_transport.h>
00037 #include <image_transport/subscriber_filter.h>
00038
00039 #include <sensor_msgs/CameraInfo.h>
00040
00041 #include <cv_bridge/cv_bridge.h>
00042
00043 #include <rtabmap/core/util3d.h>
00044
00045 namespace rtabmap_ros
00046 {
00047
00048 class StereoThrottleNodelet : public nodelet::Nodelet
00049 {
00050 public:
00051
00052 StereoThrottleNodelet():
00053 rate_(0),
00054 approxSync_(0),
00055 exactSync_(0),
00056 decimation_(1)
00057 {
00058 }
00059
00060 virtual ~StereoThrottleNodelet()
00061 {
00062 if(approxSync_)
00063 {
00064 delete approxSync_;
00065 }
00066 if(exactSync_)
00067 {
00068 delete exactSync_;
00069 }
00070 }
00071
00072 private:
00073 ros::Time last_update_;
00074 double rate_;
00075 virtual void onInit()
00076 {
00077 ros::NodeHandle& nh = getNodeHandle();
00078 ros::NodeHandle& pnh = getPrivateNodeHandle();
00079
00080 ros::NodeHandle left_nh(nh, "left");
00081 ros::NodeHandle right_nh(nh, "right");
00082 ros::NodeHandle left_pnh(pnh, "left");
00083 ros::NodeHandle right_pnh(pnh, "right");
00084 image_transport::ImageTransport left_it(left_nh);
00085 image_transport::ImageTransport right_it(right_nh);
00086 image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
00087 image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
00088
00089 int queueSize = 5;
00090 bool approxSync = false;
00091 pnh.param("approx_sync", approxSync, approxSync);
00092 pnh.param("rate", rate_, rate_);
00093 pnh.param("queue_size", queueSize, queueSize);
00094 pnh.param("decimation", decimation_, decimation_);
00095 ROS_ASSERT(decimation_ >= 1);
00096 ROS_INFO("Rate=%f Hz", rate_);
00097 ROS_INFO("Decimation=%d", decimation_);
00098 ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
00099
00100 if(approxSync)
00101 {
00102 approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
00103 approxSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4));
00104 }
00105 else
00106 {
00107 exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
00108 exactSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4));
00109 }
00110
00111 imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft);
00112 imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight);
00113 cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
00114 cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
00115
00116 imageLeftPub_ = left_it.advertise(left_nh.resolveName("image")+"_throttle", 1);
00117 imageRightPub_ = right_it.advertise(right_nh.resolveName("image")+"_throttle", 1);
00118 infoLeftPub_ = left_nh.advertise<sensor_msgs::CameraInfo>(left_nh.resolveName("camera_info")+"_throttle", 1);
00119 infoRightPub_ = right_nh.advertise<sensor_msgs::CameraInfo>(right_nh.resolveName("camera_info")+"_throttle", 1);
00120 };
00121
00122 void callback(const sensor_msgs::ImageConstPtr& imageLeft,
00123 const sensor_msgs::ImageConstPtr& imageRight,
00124 const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
00125 const sensor_msgs::CameraInfoConstPtr& camInfoRight)
00126 {
00127 if (rate_ > 0.0)
00128 {
00129 NODELET_DEBUG("update set to %f", rate_);
00130 if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
00131 {
00132 NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
00133 return;
00134 }
00135 }
00136 else
00137 NODELET_DEBUG("rate unset continuing");
00138
00139 last_update_ = ros::Time::now();
00140
00141 if(imageLeftPub_.getNumSubscribers())
00142 {
00143 if(decimation_ > 1)
00144 {
00145 cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
00146 cv_bridge::CvImage out;
00147 out.header = imagePtr->header;
00148 out.encoding = imagePtr->encoding;
00149 out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
00150 imageLeftPub_.publish(out.toImageMsg());
00151 }
00152 else
00153 {
00154 imageLeftPub_.publish(imageLeft);
00155 }
00156 }
00157 if(imageRightPub_.getNumSubscribers())
00158 {
00159 if(decimation_ > 1)
00160 {
00161 cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageRight);
00162 cv_bridge::CvImage out;
00163 out.header = imagePtr->header;
00164 out.encoding = imagePtr->encoding;
00165 out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
00166 imageRightPub_.publish(out.toImageMsg());
00167 }
00168 else
00169 {
00170 imageRightPub_.publish(imageRight);
00171 }
00172 }
00173 if(infoLeftPub_.getNumSubscribers())
00174 {
00175 if(decimation_ > 1)
00176 {
00177 sensor_msgs::CameraInfo info = *camInfoLeft;
00178 info.height /= decimation_;
00179 info.width /= decimation_;
00180 info.roi.height /= decimation_;
00181 info.roi.width /= decimation_;
00182 info.K[2]/=float(decimation_);
00183 info.K[5]/=float(decimation_);
00184 info.K[0]/=float(decimation_);
00185 info.K[4]/=float(decimation_);
00186 info.P[2]/=float(decimation_);
00187 info.P[6]/=float(decimation_);
00188 info.P[0]/=float(decimation_);
00189 info.P[5]/=float(decimation_);
00190 info.P[3]/=float(decimation_);
00191 infoLeftPub_.publish(info);
00192 }
00193 else
00194 {
00195 infoLeftPub_.publish(camInfoLeft);
00196 }
00197 }
00198 if(infoRightPub_.getNumSubscribers())
00199 {
00200 if(decimation_ > 1)
00201 {
00202 sensor_msgs::CameraInfo info = *camInfoRight;
00203 info.height /= decimation_;
00204 info.width /= decimation_;
00205 info.roi.height /= decimation_;
00206 info.roi.width /= decimation_;
00207 info.K[2]/=float(decimation_);
00208 info.K[5]/=float(decimation_);
00209 info.K[0]/=float(decimation_);
00210 info.K[4]/=float(decimation_);
00211 info.P[2]/=float(decimation_);
00212 info.P[6]/=float(decimation_);
00213 info.P[0]/=float(decimation_);
00214 info.P[5]/=float(decimation_);
00215 info.P[3]/=float(decimation_);
00216 infoRightPub_.publish(info);
00217 }
00218 else
00219 {
00220 infoRightPub_.publish(camInfoRight);
00221 }
00222 }
00223 }
00224
00225 image_transport::Publisher imageLeftPub_;
00226 image_transport::Publisher imageRightPub_;
00227 ros::Publisher infoLeftPub_;
00228 ros::Publisher infoRightPub_;
00229
00230 image_transport::SubscriberFilter imageLeft_;
00231 image_transport::SubscriberFilter imageRight_;
00232 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
00233 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
00234
00235 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
00236 message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
00237 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
00238 message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
00239
00240 int decimation_;
00241
00242 };
00243
00244
00245 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoThrottleNodelet, nodelet::Nodelet);
00246 }