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 <sensor_msgs/Image.h>
00033 #include <sensor_msgs/CompressedImage.h>
00034 #include <sensor_msgs/image_encodings.h>
00035 #include <sensor_msgs/CameraInfo.h>
00036
00037 #include <image_transport/image_transport.h>
00038 #include <image_transport/subscriber_filter.h>
00039
00040 #include <message_filters/sync_policies/approximate_time.h>
00041 #include <message_filters/sync_policies/exact_time.h>
00042 #include <message_filters/subscriber.h>
00043
00044 #include <cv_bridge/cv_bridge.h>
00045 #include <opencv2/highgui/highgui.hpp>
00046
00047 #include <boost/thread.hpp>
00048
00049 #include "rtabmap_ros/RGBDImage.h"
00050
00051 #include "rtabmap/core/Compression.h"
00052 #include "rtabmap/utilite/UConversion.h"
00053
00054 namespace rtabmap_ros
00055 {
00056
00057 class RGBDSync : public nodelet::Nodelet
00058 {
00059 public:
00060 RGBDSync() :
00061 depthScale_(1.0),
00062 warningThread_(0),
00063 callbackCalled_(false),
00064 approxSyncDepth_(0),
00065 exactSyncDepth_(0)
00066 {}
00067
00068 virtual ~RGBDSync()
00069 {
00070 if(approxSyncDepth_)
00071 delete approxSyncDepth_;
00072 if(exactSyncDepth_)
00073 delete exactSyncDepth_;
00074
00075 if(warningThread_)
00076 {
00077 callbackCalled_=true;
00078 warningThread_->join();
00079 delete warningThread_;
00080 }
00081 }
00082
00083 private:
00084 virtual void onInit()
00085 {
00086 ros::NodeHandle & nh = getNodeHandle();
00087 ros::NodeHandle & pnh = getPrivateNodeHandle();
00088
00089 int queueSize = 10;
00090 bool approxSync = true;
00091 pnh.param("approx_sync", approxSync, approxSync);
00092 pnh.param("queue_size", queueSize, queueSize);
00093 pnh.param("depth_scale", depthScale_, depthScale_);
00094
00095 NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
00096 NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
00097 NODELET_INFO("%s: depth_scale = %f", getName().c_str(), depthScale_);
00098
00099 rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image", 1);
00100 rgbdImageCompressedPub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image/compressed", 1);
00101
00102 if(approxSync)
00103 {
00104 approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
00105 approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
00106 }
00107 else
00108 {
00109 exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
00110 exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
00111 }
00112
00113 ros::NodeHandle rgb_nh(nh, "rgb");
00114 ros::NodeHandle depth_nh(nh, "depth");
00115 ros::NodeHandle rgb_pnh(pnh, "rgb");
00116 ros::NodeHandle depth_pnh(pnh, "depth");
00117 image_transport::ImageTransport rgb_it(rgb_nh);
00118 image_transport::ImageTransport depth_it(depth_nh);
00119 image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
00120 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
00121
00122 imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
00123 imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
00124 cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
00125
00126 std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
00127 getName().c_str(),
00128 approxSync?"approx":"exact",
00129 imageSub_.getTopic().c_str(),
00130 imageDepthSub_.getTopic().c_str(),
00131 cameraInfoSub_.getTopic().c_str());
00132
00133 warningThread_ = new boost::thread(boost::bind(&RGBDSync::warningLoop, this, subscribedTopicsMsg, approxSync));
00134 NODELET_INFO("%s", subscribedTopicsMsg.c_str());
00135 }
00136
00137 void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
00138 {
00139 ros::Duration r(5.0);
00140 while(!callbackCalled_)
00141 {
00142 r.sleep();
00143 if(!callbackCalled_)
00144 {
00145 ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
00146 "published (\"$ rostopic hz my_topic\") and the timestamps in their "
00147 "header are set. %s%s",
00148 getName().c_str(),
00149 approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
00150 "topics should have all the exact timestamp for the callback to be called.",
00151 subscribedTopicsMsg.c_str());
00152 }
00153 }
00154 }
00155
00156 void callback(
00157 const sensor_msgs::ImageConstPtr& image,
00158 const sensor_msgs::ImageConstPtr& depth,
00159 const sensor_msgs::CameraInfoConstPtr& cameraInfo)
00160 {
00161 callbackCalled_ = true;
00162 if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
00163 {
00164 rtabmap_ros::RGBDImage msg;
00165 msg.header.frame_id = cameraInfo->header.frame_id;
00166 msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
00167 msg.rgbCameraInfo = *cameraInfo;
00168 msg.depthCameraInfo = *cameraInfo;
00169
00170 if(rgbdImageCompressedPub_.getNumSubscribers())
00171 {
00172 rtabmap_ros::RGBDImage msgCompressed = msg;
00173
00174 cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
00175 imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG);
00176
00177 cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
00178 msgCompressed.depthCompressed.header = imageDepthPtr->header;
00179 if(depthScale_ != 1.0)
00180 {
00181 msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image*depthScale_, ".png");
00182 }
00183 else
00184 {
00185 msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
00186 }
00187 msgCompressed.depthCompressed.format = "png";
00188
00189 rgbdImageCompressedPub_.publish(msgCompressed);
00190 }
00191
00192 if(rgbdImagePub_.getNumSubscribers())
00193 {
00194 msg.rgb = *image;
00195 if(depthScale_ != 1.0)
00196 {
00197 cv_bridge::CvImagePtr imageDepthPtr = cv_bridge::toCvCopy(depth);
00198 imageDepthPtr->image*=depthScale_;
00199 msg.depth = *imageDepthPtr->toImageMsg();
00200 }
00201 else
00202 {
00203 msg.depth = *depth;
00204 }
00205 rgbdImagePub_.publish(msg);
00206 }
00207 }
00208 }
00209
00210 private:
00211 double depthScale_;
00212 boost::thread * warningThread_;
00213 bool callbackCalled_;
00214
00215 ros::Publisher rgbdImagePub_;
00216 ros::Publisher rgbdImageCompressedPub_;
00217
00218 image_transport::SubscriberFilter imageSub_;
00219 image_transport::SubscriberFilter imageDepthSub_;
00220 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
00221
00222 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
00223 message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
00224
00225 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
00226 message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
00227 };
00228
00229 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDSync, nodelet::Nodelet);
00230 }
00231