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 <sensor_msgs/Image.h>
00030 #include <sensor_msgs/image_encodings.h>
00031 #include <sensor_msgs/PointCloud2.h>
00032 #include <sensor_msgs/CameraInfo.h>
00033 #include <pcl_conversions/pcl_conversions.h>
00034 #include <nav_msgs/Odometry.h>
00035 #include <cv_bridge/cv_bridge.h>
00036 #include <image_transport/image_transport.h>
00037 #include <tf/tf.h>
00038 #include <tf/transform_broadcaster.h>
00039 #include <std_srvs/Empty.h>
00040 #include <rtabmap_ros/MsgConversion.h>
00041 #include <rtabmap/utilite/ULogger.h>
00042 #include <rtabmap/core/util3d.h>
00043 #include <rtabmap/core/DBReader.h>
00044
00045 bool paused = false;
00046 bool pauseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00047 {
00048 if(paused)
00049 {
00050 ROS_WARN("Already paused!");
00051 }
00052 else
00053 {
00054 paused = true;
00055 ROS_INFO("paused!");
00056 }
00057 return true;
00058 }
00059
00060 bool resumeCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00061 {
00062 if(!paused)
00063 {
00064 ROS_WARN("Already running!");
00065 }
00066 else
00067 {
00068 paused = false;
00069 ROS_INFO("resumed!");
00070 }
00071 return true;
00072 }
00073
00074 int main(int argc, char** argv)
00075 {
00076 ros::init(argc, argv, "data_player");
00077
00078
00079
00080
00081
00082
00083 ros::NodeHandle nh;
00084 ros::NodeHandle pnh("~");
00085
00086 std::string frameId = "base_link";
00087 std::string odomFrameId = "odom";
00088 std::string cameraFrameId = "camera_optical_link";
00089 double rate = 1.0f;
00090 std::string databasePath = "";
00091 bool publishTf = true;
00092 int startId = 0;
00093
00094 pnh.param("frame_id", frameId, frameId);
00095 pnh.param("odom_frame_id", odomFrameId, odomFrameId);
00096 pnh.param("camera_frame_id", cameraFrameId, cameraFrameId);
00097 pnh.param("rate", rate, rate);
00098 pnh.param("database", databasePath, databasePath);
00099 pnh.param("publish_tf", publishTf, publishTf);
00100 pnh.param("start_id", startId, startId);
00101
00102 ROS_INFO("frame_id = %s", frameId.c_str());
00103 ROS_INFO("odom_frame_id = %s", odomFrameId.c_str());
00104 ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str());
00105 ROS_INFO("database = %s", databasePath.c_str());
00106 ROS_INFO("rate = %f", rate);
00107 ROS_INFO("publish_tf = %s", publishTf?"true":"false");
00108
00109 rtabmap::DBReader reader(databasePath, rate);
00110
00111 if(databasePath.empty())
00112 {
00113 ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database).");
00114 return -1;
00115 }
00116
00117 if(!reader.init(startId))
00118 {
00119 ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str());
00120 return -1;
00121 }
00122
00123 ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback);
00124 ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback);
00125
00126 image_transport::ImageTransport it(nh);
00127 image_transport::Publisher imagePub;
00128 image_transport::Publisher rgbPub;
00129 image_transport::Publisher depthPub;
00130 image_transport::Publisher leftPub;
00131 image_transport::Publisher rightPub;
00132 ros::Publisher rgbCamInfoPub;
00133 ros::Publisher depthCamInfoPub;
00134 ros::Publisher leftCamInfoPub;
00135 ros::Publisher rightCamInfoPub;
00136 ros::Publisher odometryPub;
00137 ros::Publisher scanPub;
00138 tf::TransformBroadcaster tfBroadcaster;
00139
00140 rtabmap::SensorData data = reader.getNextData();
00141 while(ros::ok() && data.isValid())
00142 {
00143 ROS_INFO("Reading sensor data %d...", data.id());
00144
00145 ros::Time time = ros::Time::now();
00146
00147 sensor_msgs::CameraInfo camInfoA;
00148 sensor_msgs::CameraInfo camInfoB;
00149
00150 camInfoA.K.assign(0);
00151 camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1;
00152 camInfoA.R.assign(0);
00153 camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1;
00154 camInfoA.P.assign(0);
00155 camInfoA.P[10] = 1;
00156
00157 camInfoA.header.frame_id = cameraFrameId;
00158 camInfoA.header.stamp = time;
00159
00160 camInfoB = camInfoA;
00161
00162 int type = -1;
00163 if(!data.depth().empty() && (data.depth().type() == CV_32FC1 || data.depth().type() == CV_16UC1))
00164 {
00165
00166 camInfoA.D.resize(5,0);
00167
00168 camInfoA.P[0] = data.fx();
00169 camInfoA.K[0] = data.fx();
00170 camInfoA.P[5] = data.fy();
00171 camInfoA.K[4] = data.fy();
00172 camInfoA.P[2] = data.cx();
00173 camInfoA.K[2] = data.cx();
00174 camInfoA.P[6] = data.cy();
00175 camInfoA.K[5] = data.cy();
00176
00177 camInfoB = camInfoA;
00178
00179 type=0;
00180
00181 if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1);
00182 if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1);
00183 if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
00184 if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
00185 }
00186 else if(!data.rightImage().empty() && data.rightImage().type() == CV_8U)
00187 {
00188
00189 camInfoA.D.resize(8,0);
00190
00191 camInfoA.P[0] = data.fx();
00192 camInfoA.K[0] = data.fx();
00193 camInfoA.P[5] = data.fx();
00194 camInfoA.K[4] = data.fx();
00195 camInfoA.P[2] = data.cx();
00196 camInfoA.K[2] = data.cx();
00197 camInfoA.P[6] = data.cy();
00198 camInfoA.K[5] = data.cy();
00199
00200 camInfoB = camInfoA;
00201 camInfoB.P[3] = data.baseline()*-data.fx();
00202
00203 type=1;
00204
00205 if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
00206 if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
00207 if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
00208 if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
00209
00210 }
00211 else
00212 {
00213 if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
00214 }
00215
00216 camInfoA.height = data.image().rows;
00217 camInfoA.width = data.image().cols;
00218 camInfoB.height = data.depthOrRightImage().rows;
00219 camInfoB.width = data.depthOrRightImage().cols;
00220
00221 if(!data.laserScan().empty())
00222 {
00223 if(scanPub.getTopic().empty()) scanPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1);
00224 }
00225
00226
00227 if(publishTf)
00228 {
00229 ros::Time tfExpiration = time + ros::Duration(1.0/rate);
00230 if(!data.localTransform().isNull())
00231 {
00232 tf::Transform baseToCamera;
00233 rtabmap_ros::transformToTF(data.localTransform(), baseToCamera);
00234 tfBroadcaster.sendTransform( tf::StampedTransform (baseToCamera, tfExpiration, frameId, cameraFrameId));
00235 }
00236
00237 if(!data.pose().isNull())
00238 {
00239 tf::Transform odomToBase;
00240 rtabmap_ros::transformToTF(data.pose(), odomToBase);
00241 tfBroadcaster.sendTransform( tf::StampedTransform (odomToBase, tfExpiration, odomFrameId, frameId));
00242 }
00243 }
00244 if(!data.pose().isNull())
00245 {
00246 if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
00247
00248 if(odometryPub.getNumSubscribers())
00249 {
00250 nav_msgs::Odometry odom;
00251 odom.child_frame_id = frameId;
00252 odom.header.frame_id = odomFrameId;
00253 odom.header.stamp = time;
00254 rtabmap_ros::transformToPoseMsg(data.pose(), odom.pose.pose);
00255 odom.pose.covariance[0] = data.poseTransVariance();
00256 odom.pose.covariance[7] = data.poseTransVariance();
00257 odom.pose.covariance[14] = data.poseTransVariance();
00258 odom.pose.covariance[21] = data.poseRotVariance();
00259 odom.pose.covariance[28] = data.poseRotVariance();
00260 odom.pose.covariance[35] = data.poseRotVariance();
00261 odometryPub.publish(odom);
00262 }
00263 }
00264
00265 if(type >= 0)
00266 {
00267 if(rgbCamInfoPub.getNumSubscribers() && type == 0)
00268 {
00269 rgbCamInfoPub.publish(camInfoA);
00270 }
00271 if(leftCamInfoPub.getNumSubscribers() && type == 1)
00272 {
00273 leftCamInfoPub.publish(camInfoA);
00274 }
00275 if(depthCamInfoPub.getNumSubscribers() && type == 0)
00276 {
00277 depthCamInfoPub.publish(camInfoB);
00278 }
00279 if(rightCamInfoPub.getNumSubscribers() && type == 1)
00280 {
00281 rightCamInfoPub.publish(camInfoB);
00282 }
00283 }
00284
00285 if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
00286 {
00287 cv_bridge::CvImage img;
00288 if(data.image().channels() == 1)
00289 {
00290 img.encoding = sensor_msgs::image_encodings::MONO8;
00291 }
00292 else
00293 {
00294 img.encoding = sensor_msgs::image_encodings::BGR8;
00295 }
00296 img.image = data.image();
00297 sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
00298 imageRosMsg->header.frame_id = cameraFrameId;
00299 imageRosMsg->header.stamp = time;
00300
00301 if(imagePub.getNumSubscribers())
00302 {
00303 imagePub.publish(imageRosMsg);
00304 }
00305 if(rgbPub.getNumSubscribers() && type == 0)
00306 {
00307 rgbPub.publish(imageRosMsg);
00308 }
00309 if(leftPub.getNumSubscribers() && type == 1)
00310 {
00311 leftPub.publish(imageRosMsg);
00312 leftCamInfoPub.publish(camInfoA);
00313 }
00314 }
00315
00316 if(depthPub.getNumSubscribers() && !data.depth().empty() && type==0)
00317 {
00318 cv_bridge::CvImage img;
00319 if(data.depth().type() == CV_32FC1)
00320 {
00321 img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00322 }
00323 else
00324 {
00325 img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00326 }
00327 img.image = data.depth();
00328 sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
00329 imageRosMsg->header.frame_id = cameraFrameId;
00330 imageRosMsg->header.stamp = time;
00331
00332 depthPub.publish(imageRosMsg);
00333 depthCamInfoPub.publish(camInfoB);
00334 }
00335
00336 if(rightPub.getNumSubscribers() && !data.rightImage().empty() && type==1)
00337 {
00338 cv_bridge::CvImage img;
00339 img.encoding = sensor_msgs::image_encodings::MONO8;
00340 img.image = data.rightImage();
00341 sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
00342 imageRosMsg->header.frame_id = cameraFrameId;
00343 imageRosMsg->header.stamp = time;
00344
00345 rightPub.publish(imageRosMsg);
00346 rightCamInfoPub.publish(camInfoB);
00347 }
00348
00349 if(scanPub.getNumSubscribers() && !data.laserScan().empty())
00350 {
00351 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(data.laserScan());
00352 sensor_msgs::PointCloud2 msg;
00353 pcl::toROSMsg(*cloud, msg);
00354 msg.header.frame_id = frameId;
00355 msg.header.stamp = time;
00356 scanPub.publish(msg);
00357 }
00358
00359 ros::spinOnce();
00360
00361 while(ros::ok() && paused)
00362 {
00363 uSleep(100);
00364 ros::spinOnce();
00365 }
00366
00367 data = reader.getNextData();
00368 }
00369
00370
00371 return 0;
00372 }