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/LaserScan.h>
00033 #include <sensor_msgs/CameraInfo.h>
00034 #include <pcl_conversions/pcl_conversions.h>
00035 #include <nav_msgs/Odometry.h>
00036 #include <cv_bridge/cv_bridge.h>
00037 #include <image_transport/image_transport.h>
00038 #include <tf2_ros/transform_broadcaster.h>
00039 #include <std_srvs/Empty.h>
00040 #include <rtabmap_ros/MsgConversion.h>
00041 #include <rtabmap_ros/SetGoal.h>
00042 #include <rtabmap/utilite/ULogger.h>
00043 #include <rtabmap/utilite/UStl.h>
00044 #include <rtabmap/utilite/UThread.h>
00045 #include <rtabmap/utilite/UDirectory.h>
00046 #include <rtabmap/utilite/UConversion.h>
00047 #include <rtabmap/core/util3d.h>
00048 #include <rtabmap/core/DBReader.h>
00049 #include <rtabmap/core/OdometryEvent.h>
00050 #include <cmath>
00051
00052 bool paused = false;
00053 bool pauseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00054 {
00055 if(paused)
00056 {
00057 ROS_WARN("Already paused!");
00058 }
00059 else
00060 {
00061 paused = true;
00062 ROS_INFO("paused!");
00063 }
00064 return true;
00065 }
00066
00067 bool resumeCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00068 {
00069 if(!paused)
00070 {
00071 ROS_WARN("Already running!");
00072 }
00073 else
00074 {
00075 paused = false;
00076 ROS_INFO("resumed!");
00077 }
00078 return true;
00079 }
00080
00081 int main(int argc, char** argv)
00082 {
00083 ros::init(argc, argv, "data_player");
00084
00085
00086
00087
00088
00089
00090 ros::NodeHandle nh;
00091 ros::NodeHandle pnh("~");
00092
00093 std::string frameId = "base_link";
00094 std::string odomFrameId = "odom";
00095 std::string cameraFrameId = "camera_optical_link";
00096 std::string scanFrameId = "base_laser_link";
00097 double rate = -1.0f;
00098 std::string databasePath = "";
00099 bool publishTf = true;
00100 int startId = 0;
00101
00102 pnh.param("frame_id", frameId, frameId);
00103 pnh.param("odom_frame_id", odomFrameId, odomFrameId);
00104 pnh.param("camera_frame_id", cameraFrameId, cameraFrameId);
00105 pnh.param("scan_frame_id", scanFrameId, scanFrameId);
00106 pnh.param("rate", rate, rate);
00107 pnh.param("database", databasePath, databasePath);
00108 pnh.param("publish_tf", publishTf, publishTf);
00109 pnh.param("start_id", startId, startId);
00110
00111
00112 double scanHeight, scanAngleMin, scanAngleMax, scanAngleIncrement, scanTime, scanRangeMin, scanRangeMax;
00113 pnh.param<double>("scan_height", scanHeight, 0.3);
00114 pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI / 2.0);
00115 pnh.param<double>("scan_angle_max", scanAngleMax, M_PI / 2.0);
00116 pnh.param<double>("scan_angle_increment", scanAngleIncrement, M_PI / 360.0);
00117 pnh.param<double>("scan_time", scanTime, 1.0 / 10.0);
00118 pnh.param<double>("scan_range_min", scanRangeMin, 0.02);
00119 pnh.param<double>("scan_range_max", scanRangeMax, 6.0);
00120
00121 ROS_INFO("frame_id = %s", frameId.c_str());
00122 ROS_INFO("odom_frame_id = %s", odomFrameId.c_str());
00123 ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str());
00124 ROS_INFO("scan_frame_id = %s", scanFrameId.c_str());
00125 ROS_INFO("rate = %f", rate);
00126 ROS_INFO("publish_tf = %s", publishTf?"true":"false");
00127 ROS_INFO("start_id = %d", startId);
00128
00129 if(databasePath.empty())
00130 {
00131 ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database).");
00132 return -1;
00133 }
00134 databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
00135 if(databasePath.size() && databasePath.at(0) != '/')
00136 {
00137 databasePath = UDirectory::currentDir(true) + databasePath;
00138 }
00139 ROS_INFO("database = %s", databasePath.c_str());
00140
00141 rtabmap::DBReader reader(databasePath, rate, false, false, false, startId);
00142 if(!reader.init())
00143 {
00144 ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str());
00145 return -1;
00146 }
00147
00148 ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback);
00149 ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback);
00150
00151 image_transport::ImageTransport it(nh);
00152 image_transport::Publisher imagePub;
00153 image_transport::Publisher rgbPub;
00154 image_transport::Publisher depthPub;
00155 image_transport::Publisher leftPub;
00156 image_transport::Publisher rightPub;
00157 ros::Publisher rgbCamInfoPub;
00158 ros::Publisher depthCamInfoPub;
00159 ros::Publisher leftCamInfoPub;
00160 ros::Publisher rightCamInfoPub;
00161 ros::Publisher odometryPub;
00162 ros::Publisher scanPub;
00163 tf2_ros::TransformBroadcaster tfBroadcaster;
00164
00165 UTimer timer;
00166 rtabmap::CameraInfo info;
00167 rtabmap::SensorData data = reader.takeImage(&info);
00168 rtabmap::OdometryEvent odom(data, info.odomPose, info.odomCovariance);
00169 double acquisitionTime = timer.ticks();
00170 while(ros::ok() && odom.data().id())
00171 {
00172 ROS_INFO("Reading sensor data %d...", odom.data().id());
00173
00174 ros::Time time = ros::Time::now();
00175
00176 sensor_msgs::CameraInfo camInfoA;
00177 sensor_msgs::CameraInfo camInfoB;
00178
00179 camInfoA.K.assign(0);
00180 camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1;
00181 camInfoA.R.assign(0);
00182 camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1;
00183 camInfoA.P.assign(0);
00184 camInfoA.P[10] = 1;
00185
00186 camInfoA.header.frame_id = cameraFrameId;
00187 camInfoA.header.stamp = time;
00188
00189 camInfoB = camInfoA;
00190
00191 int type = -1;
00192 if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1))
00193 {
00194 if(odom.data().cameraModels().size() > 1)
00195 {
00196 ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
00197 }
00198 else
00199 {
00200
00201 if(odom.data().cameraModels().size())
00202 {
00203 camInfoA.D.resize(5,0);
00204
00205 camInfoA.P[0] = odom.data().cameraModels()[0].fx();
00206 camInfoA.K[0] = odom.data().cameraModels()[0].fx();
00207 camInfoA.P[5] = odom.data().cameraModels()[0].fy();
00208 camInfoA.K[4] = odom.data().cameraModels()[0].fy();
00209 camInfoA.P[2] = odom.data().cameraModels()[0].cx();
00210 camInfoA.K[2] = odom.data().cameraModels()[0].cx();
00211 camInfoA.P[6] = odom.data().cameraModels()[0].cy();
00212 camInfoA.K[5] = odom.data().cameraModels()[0].cy();
00213
00214 camInfoB = camInfoA;
00215 }
00216
00217 type=0;
00218
00219 if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1);
00220 if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1);
00221 if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
00222 if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
00223 }
00224 }
00225 else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
00226 {
00227
00228 if(odom.data().stereoCameraModel().isValidForProjection())
00229 {
00230 camInfoA.D.resize(8,0);
00231
00232 camInfoA.P[0] = odom.data().stereoCameraModel().left().fx();
00233 camInfoA.K[0] = odom.data().stereoCameraModel().left().fx();
00234 camInfoA.P[5] = odom.data().stereoCameraModel().left().fy();
00235 camInfoA.K[4] = odom.data().stereoCameraModel().left().fy();
00236 camInfoA.P[2] = odom.data().stereoCameraModel().left().cx();
00237 camInfoA.K[2] = odom.data().stereoCameraModel().left().cx();
00238 camInfoA.P[6] = odom.data().stereoCameraModel().left().cy();
00239 camInfoA.K[5] = odom.data().stereoCameraModel().left().cy();
00240
00241 camInfoB = camInfoA;
00242 camInfoB.P[3] = odom.data().stereoCameraModel().right().Tx();
00243 }
00244
00245 type=1;
00246
00247 if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
00248 if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
00249 if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
00250 if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
00251
00252 }
00253 else
00254 {
00255 if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
00256 }
00257
00258 camInfoA.height = odom.data().imageRaw().rows;
00259 camInfoA.width = odom.data().imageRaw().cols;
00260 camInfoB.height = odom.data().depthOrRightRaw().rows;
00261 camInfoB.width = odom.data().depthOrRightRaw().cols;
00262
00263 if(!odom.data().laserScanRaw().empty())
00264 {
00265 if(scanPub.getTopic().empty()) scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
00266 }
00267
00268
00269 if(publishTf)
00270 {
00271 rtabmap::Transform localTransform;
00272 if(odom.data().cameraModels().size() == 1)
00273 {
00274 localTransform = odom.data().cameraModels()[0].localTransform();
00275 }
00276 else if(odom.data().stereoCameraModel().isValidForProjection())
00277 {
00278 localTransform = odom.data().stereoCameraModel().left().localTransform();
00279 }
00280 if(!localTransform.isNull())
00281 {
00282 geometry_msgs::TransformStamped baseToCamera;
00283 baseToCamera.child_frame_id = cameraFrameId;
00284 baseToCamera.header.frame_id = frameId;
00285 baseToCamera.header.stamp = time;
00286 rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform);
00287 tfBroadcaster.sendTransform(baseToCamera);
00288 }
00289
00290 if(!odom.pose().isNull())
00291 {
00292 geometry_msgs::TransformStamped odomToBase;
00293 odomToBase.child_frame_id = frameId;
00294 odomToBase.header.frame_id = odomFrameId;
00295 odomToBase.header.stamp = time;
00296 rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform);
00297 tfBroadcaster.sendTransform(odomToBase);
00298 }
00299
00300 if(!scanPub.getTopic().empty())
00301 {
00302 geometry_msgs::TransformStamped baseToLaserScan;
00303 baseToLaserScan.child_frame_id = scanFrameId;
00304 baseToLaserScan.header.frame_id = frameId;
00305 baseToLaserScan.header.stamp = time;
00306 rtabmap_ros::transformToGeometryMsg(rtabmap::Transform(0,0,scanHeight,0,0,0), baseToLaserScan.transform);
00307 tfBroadcaster.sendTransform(baseToLaserScan);
00308 }
00309 }
00310 if(!odom.pose().isNull())
00311 {
00312 if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
00313
00314 if(odometryPub.getNumSubscribers())
00315 {
00316 nav_msgs::Odometry odomMsg;
00317 odomMsg.child_frame_id = frameId;
00318 odomMsg.header.frame_id = odomFrameId;
00319 odomMsg.header.stamp = time;
00320 rtabmap_ros::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
00321 UASSERT(odomMsg.pose.covariance.size() == 36 &&
00322 odom.covariance().total() == 36 &&
00323 odom.covariance().type() == CV_64FC1);
00324 memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
00325 odometryPub.publish(odomMsg);
00326 }
00327 }
00328
00329 if(type >= 0)
00330 {
00331 if(rgbCamInfoPub.getNumSubscribers() && type == 0)
00332 {
00333 rgbCamInfoPub.publish(camInfoA);
00334 }
00335 if(leftCamInfoPub.getNumSubscribers() && type == 1)
00336 {
00337 leftCamInfoPub.publish(camInfoA);
00338 }
00339 if(depthCamInfoPub.getNumSubscribers() && type == 0)
00340 {
00341 depthCamInfoPub.publish(camInfoB);
00342 }
00343 if(rightCamInfoPub.getNumSubscribers() && type == 1)
00344 {
00345 rightCamInfoPub.publish(camInfoB);
00346 }
00347 }
00348
00349 if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
00350 {
00351 cv_bridge::CvImage img;
00352 if(odom.data().imageRaw().channels() == 1)
00353 {
00354 img.encoding = sensor_msgs::image_encodings::MONO8;
00355 }
00356 else
00357 {
00358 img.encoding = sensor_msgs::image_encodings::BGR8;
00359 }
00360 img.image = odom.data().imageRaw();
00361 sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
00362 imageRosMsg->header.frame_id = cameraFrameId;
00363 imageRosMsg->header.stamp = time;
00364
00365 if(imagePub.getNumSubscribers())
00366 {
00367 imagePub.publish(imageRosMsg);
00368 }
00369 if(rgbPub.getNumSubscribers() && type == 0)
00370 {
00371 rgbPub.publish(imageRosMsg);
00372 }
00373 if(leftPub.getNumSubscribers() && type == 1)
00374 {
00375 leftPub.publish(imageRosMsg);
00376 leftCamInfoPub.publish(camInfoA);
00377 }
00378 }
00379
00380 if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0)
00381 {
00382 cv_bridge::CvImage img;
00383 if(odom.data().depthRaw().type() == CV_32FC1)
00384 {
00385 img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00386 }
00387 else
00388 {
00389 img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00390 }
00391 img.image = odom.data().depthRaw();
00392 sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
00393 imageRosMsg->header.frame_id = cameraFrameId;
00394 imageRosMsg->header.stamp = time;
00395
00396 depthPub.publish(imageRosMsg);
00397 depthCamInfoPub.publish(camInfoB);
00398 }
00399
00400 if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1)
00401 {
00402 cv_bridge::CvImage img;
00403 img.encoding = sensor_msgs::image_encodings::MONO8;
00404 img.image = odom.data().rightRaw();
00405 sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
00406 imageRosMsg->header.frame_id = cameraFrameId;
00407 imageRosMsg->header.stamp = time;
00408
00409 rightPub.publish(imageRosMsg);
00410 rightCamInfoPub.publish(camInfoB);
00411 }
00412
00413 if(scanPub.getNumSubscribers() && !odom.data().laserScanRaw().empty())
00414 {
00415
00416 sensor_msgs::LaserScan msg;
00417 msg.header.frame_id = scanFrameId;
00418 msg.header.stamp = time;
00419
00420 msg.angle_min = scanAngleMin;
00421 msg.angle_max = scanAngleMax;
00422 msg.angle_increment = scanAngleIncrement;
00423 msg.time_increment = 0.0;
00424 msg.scan_time = scanTime;
00425 msg.range_min = scanRangeMin;
00426 msg.range_max = scanRangeMax;
00427
00428 uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
00429 msg.ranges.assign(rangesSize, 0.0);
00430
00431 const cv::Mat & scan = odom.data().laserScanRaw();
00432 UASSERT(scan.type() == CV_32FC2 || scan.type() == CV_32FC3);
00433 UASSERT(scan.rows == 1);
00434 for (int i=0; i<scan.cols; ++i)
00435 {
00436 cv::Vec2f pos = scan.at<cv::Vec2f>(i);
00437 double range = hypot(pos[0], pos[1]);
00438 if (range >= scanRangeMin && range <=scanRangeMax)
00439 {
00440 double angle = atan2(pos[1], pos[0]);
00441 if (angle >= msg.angle_min && angle <= msg.angle_max)
00442 {
00443 int index = (angle - msg.angle_min) / msg.angle_increment;
00444 if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
00445 {
00446 msg.ranges[index] = range;
00447 }
00448 }
00449 }
00450 }
00451
00452 scanPub.publish(msg);
00453 }
00454
00455 if(odom.data().userDataRaw().type() == CV_8SC1 &&
00456 odom.data().userDataRaw().cols >= 7 &&
00457 odom.data().userDataRaw().rows == 1 &&
00458 memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
00459 {
00460
00461 std::string goalStr = (const char *)odom.data().userDataRaw().data;
00462 if(!goalStr.empty())
00463 {
00464 std::list<std::string> strs = uSplit(goalStr, ':');
00465 if(strs.size() == 2)
00466 {
00467 int goalId = atoi(strs.rbegin()->c_str());
00468
00469 if(goalId > 0)
00470 {
00471 ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId);
00472 rtabmap_ros::SetGoal setGoalSrv;
00473 setGoalSrv.request.node_id = goalId;
00474 setGoalSrv.request.node_label = "";
00475 if(!ros::service::call("set_goal", setGoalSrv))
00476 {
00477 ROS_ERROR("Can't call \"set_goal\" service");
00478 }
00479 }
00480 }
00481 }
00482 }
00483
00484 ros::spinOnce();
00485
00486 while(ros::ok() && paused)
00487 {
00488 uSleep(100);
00489 ros::spinOnce();
00490 }
00491
00492 timer.restart();
00493 info = rtabmap::CameraInfo();
00494 data = reader.takeImage(&info);
00495 odom = rtabmap::OdometryEvent(data, info.odomPose, info.odomCovariance);
00496 acquisitionTime = timer.ticks();
00497 }
00498
00499
00500 return 0;
00501 }