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 StereoSync : public nodelet::Nodelet
00058 {
00059 public:
00060 StereoSync() :
00061 warningThread_(0),
00062 callbackCalled_(false),
00063 approxSync_(0),
00064 exactSync_(0)
00065 {}
00066
00067 virtual ~StereoSync()
00068 {
00069 if(approxSync_)
00070 delete approxSync_;
00071 if(exactSync_)
00072 delete exactSync_;
00073
00074 if(warningThread_)
00075 {
00076 callbackCalled_=true;
00077 warningThread_->join();
00078 delete warningThread_;
00079 }
00080 }
00081
00082 private:
00083 virtual void onInit()
00084 {
00085 ros::NodeHandle & nh = getNodeHandle();
00086 ros::NodeHandle & pnh = getPrivateNodeHandle();
00087
00088 int queueSize = 10;
00089 bool approxSync = false;
00090 pnh.param("approx_sync", approxSync, approxSync);
00091 pnh.param("queue_size", queueSize, queueSize);
00092
00093 NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
00094 NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
00095
00096 rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image", 1);
00097 rgbdImageCompressedPub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image/compressed", 1);
00098
00099 if(approxSync)
00100 {
00101 approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
00102 approxSync_->registerCallback(boost::bind(&StereoSync::callback, this, _1, _2, _3, _4));
00103 }
00104 else
00105 {
00106 exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
00107 exactSync_->registerCallback(boost::bind(&StereoSync::callback, this, _1, _2, _3, _4));
00108 }
00109
00110 ros::NodeHandle left_nh(nh, "left");
00111 ros::NodeHandle right_nh(nh, "right");
00112 ros::NodeHandle left_pnh(pnh, "left");
00113 ros::NodeHandle right_pnh(pnh, "right");
00114 image_transport::ImageTransport rgb_it(left_nh);
00115 image_transport::ImageTransport depth_it(right_nh);
00116 image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), left_pnh);
00117 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), right_pnh);
00118
00119 imageLeftSub_.subscribe(rgb_it, left_nh.resolveName("image_rect"), 1, hintsRgb);
00120 imageRightSub_.subscribe(depth_it, right_nh.resolveName("image_rect"), 1, hintsDepth);
00121 cameraInfoLeftSub_.subscribe(left_nh, "camera_info", 1);
00122 cameraInfoRightSub_.subscribe(right_nh, "camera_info", 1);
00123
00124 std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
00125 getName().c_str(),
00126 approxSync?"approx":"exact",
00127 imageLeftSub_.getTopic().c_str(),
00128 imageRightSub_.getTopic().c_str(),
00129 cameraInfoLeftSub_.getTopic().c_str(),
00130 cameraInfoRightSub_.getTopic().c_str());
00131
00132 warningThread_ = new boost::thread(boost::bind(&StereoSync::warningLoop, this, subscribedTopicsMsg, approxSync));
00133 NODELET_INFO("%s", subscribedTopicsMsg.c_str());
00134 }
00135
00136 void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
00137 {
00138 ros::Duration r(5.0);
00139 while(!callbackCalled_)
00140 {
00141 r.sleep();
00142 if(!callbackCalled_)
00143 {
00144 ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
00145 "published (\"$ rostopic hz my_topic\") and the timestamps in their "
00146 "header are set. %s%s",
00147 getName().c_str(),
00148 approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
00149 "topics should have all the exact timestamp for the callback to be called.",
00150 subscribedTopicsMsg.c_str());
00151 }
00152 }
00153 }
00154
00155 void callback(
00156 const sensor_msgs::ImageConstPtr& imageLeft,
00157 const sensor_msgs::ImageConstPtr& imageRight,
00158 const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
00159 const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
00160 {
00161 callbackCalled_ = true;
00162 if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
00163 {
00164 rtabmap_ros::RGBDImage msg;
00165 msg.header.frame_id = cameraInfoLeft->header.frame_id;
00166 msg.header.stamp = imageLeft->header.stamp>imageRight->header.stamp?imageLeft->header.stamp:imageRight->header.stamp;
00167 msg.rgbCameraInfo = *cameraInfoLeft;
00168 msg.depthCameraInfo = *cameraInfoRight;
00169
00170 if(rgbdImageCompressedPub_.getNumSubscribers())
00171 {
00172 rtabmap_ros::RGBDImage msgCompressed = msg;
00173
00174 cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
00175 imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG);
00176
00177 cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageRight);
00178 imageDepthPtr->toCompressedImageMsg(msgCompressed.depthCompressed, cv_bridge::JPG);
00179
00180 rgbdImageCompressedPub_.publish(msgCompressed);
00181 }
00182
00183 if(rgbdImagePub_.getNumSubscribers())
00184 {
00185 msg.rgb = *imageLeft;
00186 msg.depth = *imageRight;
00187 rgbdImagePub_.publish(msg);
00188 }
00189 }
00190 }
00191
00192 private:
00193 boost::thread * warningThread_;
00194 bool callbackCalled_;
00195
00196 ros::Publisher rgbdImagePub_;
00197 ros::Publisher rgbdImageCompressedPub_;
00198
00199 image_transport::SubscriberFilter imageLeftSub_;
00200 image_transport::SubscriberFilter imageRightSub_;
00201 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeftSub_;
00202 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRightSub_;
00203
00204 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
00205 message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
00206
00207 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
00208 message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
00209 };
00210
00211 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoSync, nodelet::Nodelet);
00212 }
00213