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