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 "GuiWrapper.h"
00029 #include <QApplication>
00030 #include <QDir>
00031
00032 #include <cv_bridge/cv_bridge.h>
00033 #include <std_srvs/Empty.h>
00034 #include <std_msgs/Empty.h>
00035 #include <sensor_msgs/image_encodings.h>
00036
00037 #include <rtabmap/utilite/UEventsManager.h>
00038 #include <rtabmap/utilite/UConversion.h>
00039 #include <rtabmap/utilite/UDirectory.h>
00040
00041 #include <opencv2/highgui/highgui.hpp>
00042
00043 #include <rtabmap/gui/MainWindow.h>
00044 #include <rtabmap/core/RtabmapEvent.h>
00045 #include <rtabmap/core/Parameters.h>
00046 #include <rtabmap/core/ParamEvent.h>
00047 #include <rtabmap/core/OdometryEvent.h>
00048 #include <rtabmap/core/util2d.h>
00049 #include <rtabmap/core/util3d.h>
00050 #include <rtabmap/core/util3d_transforms.h>
00051 #include <rtabmap/utilite/UTimer.h>
00052
00053 #include "rtabmap_ros/MsgConversion.h"
00054 #include "rtabmap_ros/GetMap.h"
00055 #include "rtabmap_ros/SetGoal.h"
00056 #include "rtabmap_ros/SetLabel.h"
00057
00058 #include "PreferencesDialogROS.h"
00059
00060 #include <pcl_ros/transforms.h>
00061 #include <pcl_conversions/pcl_conversions.h>
00062 #include <laser_geometry/laser_geometry.h>
00063
00064 float max3( const float& a, const float& b, const float& c)
00065 {
00066 float m=a>b?a:b;
00067 return m>c?m:c;
00068 }
00069
00070 GuiWrapper::GuiWrapper(int & argc, char** argv) :
00071 mainWindow_(0),
00072 frameId_("base_link"),
00073 waitForTransform_(true),
00074 waitForTransformDuration_(0.2),
00075 cameraNodeName_(""),
00076 lastOdomInfoUpdateTime_(0),
00077 depthScanSync_(0),
00078 depthSync_(0),
00079 depthOdomInfoSync_(0),
00080 stereoSync_(0),
00081 stereoScanSync_(0),
00082 stereoOdomInfoSync_(0),
00083 depth2Sync_(0),
00084 depthOdomInfo2Sync_(0)
00085 {
00086 ros::NodeHandle nh;
00087
00088 QString configFile = QDir::homePath()+"/.ros/rtabmapGUI.ini";
00089 for(int i=1; i<argc; ++i)
00090 {
00091 if(strcmp(argv[i], "-d") == 0)
00092 {
00093 ++i;
00094 if(i < argc)
00095 {
00096 configFile = argv[i];
00097 }
00098 break;
00099 }
00100 }
00101
00102 configFile.replace('~', QDir::homePath());
00103
00104 ROS_INFO("rtabmapviz: Using configuration from \"%s\"", configFile.toStdString().c_str());
00105 uSleep(500);
00106 mainWindow_ = new MainWindow(new PreferencesDialogROS(configFile));
00107 mainWindow_->setWindowTitle(mainWindow_->windowTitle()+" [ROS]");
00108 mainWindow_->show();
00109 bool paused = false;
00110 nh.param("is_rtabmap_paused", paused, paused);
00111 mainWindow_->setMonitoringState(paused);
00112
00113 ros::NodeHandle pnh("~");
00114
00115
00116 bool subscribeLaserScan2d = false;
00117 bool subscribeLaserScan3d = false;
00118 bool subscribeDepth = false;
00119 bool subscribeOdomInfo = false;
00120 bool subscribeStereo = false;
00121 int queueSize = 10;
00122 int depthCameras = 1;
00123 std::string tfPrefix;
00124 std::string initCachePath;
00125 pnh.param("frame_id", frameId_, frameId_);
00126 pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
00127 pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
00128 if(pnh.getParam("subscribe_laserScan", subscribeLaserScan2d) && subscribeLaserScan2d)
00129 {
00130 ROS_WARN("rtabmapviz: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed.");
00131 }
00132 pnh.param("subscribe_scan", subscribeLaserScan2d, subscribeLaserScan2d);
00133 pnh.param("subscribe_scan_cloud", subscribeLaserScan3d, subscribeLaserScan3d);
00134 pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
00135 pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
00136 pnh.param("depth_cameras", depthCameras, depthCameras);
00137 pnh.param("queue_size", queueSize, queueSize);
00138 pnh.param("tf_prefix", tfPrefix, tfPrefix);
00139 pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
00140 pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
00141 pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_);
00142 pnh.param("init_cache_path", initCachePath, initCachePath);
00143 if(initCachePath.size())
00144 {
00145 initCachePath = uReplaceChar(initCachePath, '~', UDirectory::homeDir());
00146 if(initCachePath.at(0) != '/')
00147 {
00148 initCachePath = UDirectory::currentDir(true) + initCachePath;
00149 }
00150 ROS_INFO("rtabmapviz: Initializing cache with local database \"%s\"", initCachePath.c_str());
00151 uSleep(2000);
00152 rtabmap_ros::GetMap getMapSrv;
00153 getMapSrv.request.global = false;
00154 getMapSrv.request.optimized = true;
00155 getMapSrv.request.graphOnly = true;
00156 if(!ros::service::call("get_map", getMapSrv))
00157 {
00158 ROS_WARN("Can't call \"get_map\" service. The cache will still be loaded "
00159 "but the clouds won't be created until next time rtabmapviz "
00160 "receives the optimized graph.");
00161 }
00162 else
00163 {
00164
00165 processRequestedMap(getMapSrv.response.data);
00166 }
00167 QMetaObject::invokeMethod(mainWindow_, "updateCacheFromDatabase", Q_ARG(QString, QString(initCachePath.c_str())));
00168 }
00169
00170 if(!tfPrefix.empty())
00171 {
00172 if(!frameId_.empty())
00173 {
00174 frameId_ = tfPrefix + "/" + frameId_;
00175 }
00176 if(!odomFrameId_.empty())
00177 {
00178 odomFrameId_ = tfPrefix + "/" + odomFrameId_;
00179 }
00180 }
00181
00182 this->setupCallbacks(
00183 subscribeDepth,
00184 subscribeLaserScan2d,
00185 subscribeLaserScan3d,
00186 subscribeOdomInfo,
00187 subscribeStereo,
00188 queueSize,
00189 depthCameras);
00190
00191 UEventsManager::addHandler(this);
00192 UEventsManager::addHandler(mainWindow_);
00193
00194 infoTopic_.subscribe(nh, "info", 1);
00195 mapDataTopic_.subscribe(nh, "mapData", 1);
00196 infoMapSync_ = new message_filters::Synchronizer<MyInfoMapSyncPolicy>(
00197 MyInfoMapSyncPolicy(queueSize),
00198 infoTopic_,
00199 mapDataTopic_);
00200 infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, _1, _2));
00201
00202 goalTopic_.subscribe(nh, "goal_node", 1);
00203 pathTopic_.subscribe(nh, "global_path", 1);
00204 goalPathSync_ = new message_filters::Synchronizer<MyGoalPathSyncPolicy>(
00205 MyGoalPathSyncPolicy(queueSize),
00206 goalTopic_,
00207 pathTopic_);
00208 goalPathSync_->registerCallback(boost::bind(&GuiWrapper::goalPathCallback, this, _1, _2));
00209 goalReachedTopic_ = nh.subscribe("goal_reached", 1, &GuiWrapper::goalReachedCallback, this);
00210 }
00211
00212 GuiWrapper::~GuiWrapper()
00213 {
00214 UDEBUG("");
00215 if(depthSync_)
00216 delete depthSync_;
00217 if(depth2Sync_)
00218 delete depth2Sync_;
00219 if(depthScanSync_)
00220 delete depthScanSync_;
00221 if(depthOdomInfoSync_)
00222 delete depthOdomInfoSync_;
00223 if(depthOdomInfo2Sync_)
00224 delete depthOdomInfo2Sync_;
00225 if(stereoSync_)
00226 delete stereoSync_;
00227 if(stereoScanSync_)
00228 delete stereoScanSync_;
00229 if(stereoOdomInfoSync_)
00230 delete stereoOdomInfoSync_;
00231
00232 for(unsigned int i=0; i<imageSubs_.size(); ++i)
00233 {
00234 delete imageSubs_[i];
00235 }
00236 imageSubs_.clear();
00237 for(unsigned int i=0; i<imageDepthSubs_.size(); ++i)
00238 {
00239 delete imageDepthSubs_[i];
00240 }
00241 imageDepthSubs_.clear();
00242 for(unsigned int i=0; i<cameraInfoSubs_.size(); ++i)
00243 {
00244 delete cameraInfoSubs_[i];
00245 }
00246 cameraInfoSubs_.clear();
00247
00248 delete infoMapSync_;
00249 delete mainWindow_;
00250 }
00251
00252 void GuiWrapper::infoMapCallback(
00253 const rtabmap_ros::InfoConstPtr & infoMsg,
00254 const rtabmap_ros::MapDataConstPtr & mapMsg)
00255 {
00256
00257
00258
00259 rtabmap::Statistics stat;
00260
00261
00262 rtabmap_ros::infoFromROS(*infoMsg, stat);
00263
00264
00265 rtabmap::Transform mapToOdom;
00266 std::map<int, rtabmap::Transform> poses;
00267 std::map<int, Signature> signatures;
00268 std::multimap<int, Link> links;
00269
00270 rtabmap_ros::mapDataFromROS(*mapMsg, poses, links, signatures, mapToOdom);
00271
00272 stat.setMapCorrection(mapToOdom);
00273 stat.setPoses(poses);
00274 stat.setSignatures(signatures);
00275 stat.setConstraints(links);
00276
00277 this->post(new RtabmapEvent(stat));
00278 }
00279
00280 void GuiWrapper::goalPathCallback(
00281 const rtabmap_ros::GoalConstPtr & goalMsg,
00282 const nav_msgs::PathConstPtr & pathMsg)
00283 {
00284
00285 std::vector<std::pair<int, Transform> > poses(pathMsg->poses.size());
00286 for(unsigned int i=0; i<pathMsg->poses.size(); ++i)
00287 {
00288 poses[i].first = -int(i)-1;
00289 poses[i].second = rtabmap_ros::transformFromPoseMsg(pathMsg->poses[i].pose);
00290 }
00291 this->post(new RtabmapGlobalPathEvent(goalMsg->node_id, goalMsg->node_label, poses, 0.0));
00292 }
00293
00294 void GuiWrapper::goalReachedCallback(
00295 const std_msgs::BoolConstPtr & value)
00296 {
00297 this->post(new RtabmapGoalStatusEvent(value->data?1:-1));
00298 }
00299
00300 void GuiWrapper::processRequestedMap(const rtabmap_ros::MapData & map)
00301 {
00302 std::map<int, Signature> signatures;
00303 std::map<int, Transform> poses;
00304 std::multimap<int, rtabmap::Link> constraints;
00305 Transform mapToOdom;
00306
00307 rtabmap_ros::mapDataFromROS(map, poses, constraints, signatures, mapToOdom);
00308
00309 RtabmapEvent3DMap e(signatures,
00310 poses,
00311 constraints);
00312 QMetaObject::invokeMethod(mainWindow_, "processRtabmapEvent3DMap", Q_ARG(rtabmap::RtabmapEvent3DMap, e));
00313 }
00314
00315 void GuiWrapper::handleEvent(UEvent * anEvent)
00316 {
00317 if(anEvent->getClassName().compare("ParamEvent") == 0)
00318 {
00319 const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters();
00320 rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters();
00321 bool modified = false;
00322 ros::NodeHandle nh;
00323 for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
00324 {
00325
00326 if(defaultParameters.find((*i).first) != defaultParameters.end())
00327 {
00328 nh.setParam((*i).first, (*i).second);
00329 modified = true;
00330 }
00331 else if((*i).first.find('/') != (*i).first.npos)
00332 {
00333 ROS_WARN("Parameter %s is not used by the rtabmap node.", (*i).first.c_str());
00334 }
00335 }
00336 if(modified)
00337 {
00338 ROS_INFO("Parameters updated");
00339 std_srvs::Empty srv;
00340 if(!ros::service::call("update_parameters", srv))
00341 {
00342 ROS_ERROR("Can't call \"update_parameters\" service");
00343 }
00344 }
00345 }
00346 else if(anEvent->getClassName().compare("RtabmapEventCmd") == 0)
00347 {
00348 std_srvs::Empty emptySrv;
00349 rtabmap::RtabmapEventCmd * cmdEvent = (rtabmap::RtabmapEventCmd *)anEvent;
00350 rtabmap::RtabmapEventCmd::Cmd cmd = cmdEvent->getCmd();
00351 if(cmd == rtabmap::RtabmapEventCmd::kCmdResetMemory)
00352 {
00353 if(!ros::service::call("reset", emptySrv))
00354 {
00355 ROS_ERROR("Can't call \"reset\" service");
00356 }
00357 }
00358 else if(cmd == rtabmap::RtabmapEventCmd::kCmdPause)
00359 {
00360
00361 if(!cameraNodeName_.empty())
00362 {
00363 std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause true", cameraNodeName_.c_str());
00364 system(str.c_str());
00365 }
00366
00367
00368 ros::service::call("pause_odom", emptySrv);
00369
00370
00371 if(!ros::service::call("pause", emptySrv))
00372 {
00373 ROS_ERROR("Can't call \"pause\" service");
00374 }
00375 }
00376 else if(cmd == rtabmap::RtabmapEventCmd::kCmdResume)
00377 {
00378
00379 if(!ros::service::call("resume", emptySrv))
00380 {
00381 ROS_ERROR("Can't call \"resume\" service");
00382 }
00383
00384
00385 ros::service::call("resume_odom", emptySrv);
00386
00387
00388 if(!cameraNodeName_.empty())
00389 {
00390 std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause false", cameraNodeName_.c_str());
00391 system(str.c_str());
00392 }
00393 }
00394 else if(cmd == rtabmap::RtabmapEventCmd::kCmdTriggerNewMap)
00395 {
00396 if(!ros::service::call("trigger_new_map", emptySrv))
00397 {
00398 ROS_ERROR("Can't call \"trigger_new_map\" service");
00399 }
00400 }
00401 else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMap)
00402 {
00403 UASSERT(cmdEvent->value1().isBool());
00404 UASSERT(cmdEvent->value2().isBool());
00405 UASSERT(cmdEvent->value3().isBool());
00406
00407 rtabmap_ros::GetMap getMapSrv;
00408 getMapSrv.request.global = cmdEvent->value1().toBool();
00409 getMapSrv.request.optimized = cmdEvent->value2().toBool();
00410 getMapSrv.request.graphOnly = cmdEvent->value3().toBool();
00411 if(!ros::service::call("get_map", getMapSrv))
00412 {
00413 ROS_WARN("Can't call \"get_map\" service");
00414 this->post(new RtabmapEvent3DMap(1));
00415 }
00416 else
00417 {
00418 processRequestedMap(getMapSrv.response.data);
00419 }
00420 }
00421 else if(cmd == rtabmap::RtabmapEventCmd::kCmdGoal)
00422 {
00423 UASSERT(cmdEvent->value1().isStr() || cmdEvent->value1().isInt() || cmdEvent->value1().isUInt());
00424 rtabmap_ros::SetGoal setGoalSrv;
00425 setGoalSrv.request.node_id = !cmdEvent->value1().isStr()?cmdEvent->value1().toInt():0;
00426 setGoalSrv.request.node_label = cmdEvent->value1().isStr()?cmdEvent->value1().toStr():"";
00427 if(!ros::service::call("set_goal", setGoalSrv))
00428 {
00429 ROS_ERROR("Can't call \"set_goal\" service");
00430 }
00431 else
00432 {
00433 UASSERT(setGoalSrv.response.path_ids.size() == setGoalSrv.response.path_poses.size());
00434 std::vector<std::pair<int, Transform> > poses(setGoalSrv.response.path_poses.size());
00435 for(unsigned int i=0; i<setGoalSrv.response.path_poses.size(); ++i)
00436 {
00437 poses[i].first = setGoalSrv.response.path_ids[i];
00438 poses[i].second = rtabmap_ros::transformFromPoseMsg(setGoalSrv.response.path_poses[i]);
00439 }
00440 this->post(new RtabmapGlobalPathEvent(setGoalSrv.request.node_id, setGoalSrv.request.node_label, poses, setGoalSrv.response.planning_time));
00441 }
00442 }
00443 else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal)
00444 {
00445 if(!ros::service::call("cancel_goal", emptySrv))
00446 {
00447 ROS_ERROR("Can't call \"cancel_goal\" service");
00448 }
00449 }
00450 else if(cmd == rtabmap::RtabmapEventCmd::kCmdLabel)
00451 {
00452 UASSERT(cmdEvent->value1().isStr());
00453 UASSERT(cmdEvent->value2().isUndef() || cmdEvent->value2().isInt() || cmdEvent->value2().isUInt());
00454 rtabmap_ros::SetLabel setLabelSrv;
00455 setLabelSrv.request.node_label = cmdEvent->value1().toStr();
00456 setLabelSrv.request.node_id = cmdEvent->value2().isUndef()?0:cmdEvent->value2().toInt();
00457 if(!ros::service::call("set_label", setLabelSrv))
00458 {
00459 ROS_ERROR("Can't call \"set_label\" service");
00460 }
00461 }
00462 else
00463 {
00464 ROS_WARN("Not handled command (%d)...", cmd);
00465 }
00466 }
00467 else if(anEvent->getClassName().compare("OdometryResetEvent") == 0)
00468 {
00469 std_srvs::Empty srv;
00470 if(!ros::service::call("reset_odom", srv))
00471 {
00472 ROS_ERROR("Can't call \"reset_odom\" service, (will only work with rtabmap/visual_odometry node.)");
00473 }
00474 }
00475 }
00476
00477 Transform GuiWrapper::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
00478 {
00479
00480 Transform transform;
00481 try
00482 {
00483 if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_ > 0.0)
00484 {
00485
00486 if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))
00487 {
00488 ROS_WARN("rtabmapviz: Could not get transform from %s to %s after %f seconds (for stamp=%f)!",
00489 fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_, stamp.toSec());
00490 return transform;
00491 }
00492 }
00493
00494 tf::StampedTransform tmp;
00495 tfListener_.lookupTransform(fromFrameId, toFrameId, stamp, tmp);
00496 transform = rtabmap_ros::transformFromTF(tmp);
00497 }
00498 catch(tf::TransformException & ex)
00499 {
00500 ROS_WARN("%s",ex.what());
00501 }
00502 return transform;
00503 }
00504
00505 void GuiWrapper::commonDepthCallback(
00506 const nav_msgs::OdometryConstPtr & odomMsg,
00507 const sensor_msgs::ImageConstPtr& imageMsg,
00508 const sensor_msgs::ImageConstPtr& depthMsg,
00509 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00510 const sensor_msgs::LaserScanConstPtr& scan2dMsg,
00511 const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
00512 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00513 {
00514 std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
00515 std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
00516 std::vector<sensor_msgs::CameraInfoConstPtr> cameraInfoMsgs;
00517 imageMsgs.push_back(imageMsg);
00518 depthMsgs.push_back(depthMsg);
00519 cameraInfoMsgs.push_back(cameraInfoMsg);
00520 commonDepthCallback(odomMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scan2dMsg, scan3dMsg, odomInfoMsg);
00521 }
00522
00523 void GuiWrapper::commonDepthCallback(
00524 const nav_msgs::OdometryConstPtr & odomMsg,
00525 const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
00526 const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
00527 const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
00528 const sensor_msgs::LaserScanConstPtr& scan2dMsg,
00529 const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
00530 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00531 {
00532 UASSERT(imageMsgs.size()>0 &&
00533 imageMsgs.size() == depthMsgs.size() &&
00534 imageMsgs.size() == cameraInfoMsgs.size());
00535
00536 std_msgs::Header odomHeader;
00537 if(odomMsg.get())
00538 {
00539 odomHeader = odomMsg->header;
00540 }
00541 else
00542 {
00543 if(scan2dMsg.get())
00544 {
00545 odomHeader = scan2dMsg->header;
00546 }
00547 else if(scan3dMsg.get())
00548 {
00549 odomHeader = scan3dMsg->header;
00550 }
00551 else if(cameraInfoMsgs.size() && cameraInfoMsgs[0].get())
00552 {
00553 odomHeader = cameraInfoMsgs[0]->header;
00554 }
00555 else if(depthMsgs.size() && depthMsgs[0].get())
00556 {
00557 odomHeader = depthMsgs[0]->header;
00558 }
00559 else if(imageMsgs.size() && imageMsgs[0].get())
00560 {
00561 odomHeader = imageMsgs[0]->header;
00562 }
00563 odomHeader.frame_id = odomFrameId_;
00564 }
00565
00566 Transform odomT = getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp);
00567 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
00568 if(odomMsg.get())
00569 {
00570 UASSERT(odomMsg->pose.covariance.size() == 36);
00571 if(!(odomMsg->pose.covariance[0] == 0 &&
00572 odomMsg->pose.covariance[7] == 0 &&
00573 odomMsg->pose.covariance[14] == 0 &&
00574 odomMsg->pose.covariance[21] == 0 &&
00575 odomMsg->pose.covariance[28] == 0 &&
00576 odomMsg->pose.covariance[35] == 0))
00577 {
00578 covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->pose.covariance.data()).clone();
00579 }
00580 }
00581 if(odomHeader.frame_id.empty())
00582 {
00583 ROS_ERROR("Odometry frame not set!?");
00584 return;
00585 }
00586
00587 cv::Mat rgb;
00588 cv::Mat depth;
00589 std::vector<CameraModel> cameraModels;
00590 cv::Mat scan;
00591 rtabmap::OdometryInfo info;
00592 bool ignoreData = false;
00593
00594 if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
00595 !mainWindow_->isProcessingOdometry() &&
00596 !mainWindow_->isProcessingStatistics())
00597 {
00598 lastOdomInfoUpdateTime_ = UTimer::now();
00599
00600 if(imageMsgs[0].get() && depthMsgs[0].get())
00601 {
00602 int imageWidth = imageMsgs[0]->width;
00603 int imageHeight = imageMsgs[0]->height;
00604 int cameraCount = imageMsgs.size();
00605 pcl::PointCloud<pcl::PointXYZ> scanCloud;
00606 for(unsigned int i=0; i<imageMsgs.size(); ++i)
00607 {
00608 if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
00609 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00610 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
00611 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00612 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
00613 !(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
00614 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
00615 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
00616 {
00617 ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
00618 return;
00619 }
00620 UASSERT(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight);
00621 UASSERT(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight);
00622
00623 Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp);
00624 if(localTransform.isNull())
00625 {
00626 return;
00627 }
00628
00629 if(odomHeader.stamp != depthMsgs[i]->header.stamp)
00630 {
00631 if(!odomT.isNull())
00632 {
00633 Transform sensorT = getTransform(odomHeader.frame_id, frameId_, depthMsgs[i]->header.stamp);
00634 if(sensorT.isNull())
00635 {
00636 return;
00637 }
00638 localTransform = odomT.inverse() * sensorT * localTransform;
00639 }
00640 }
00641
00642 cv_bridge::CvImageConstPtr ptrImage;
00643 if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
00644 {
00645 ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
00646 }
00647 else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00648 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
00649 {
00650 ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
00651 }
00652 else
00653 {
00654 ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
00655 }
00656 cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
00657 cv::Mat subDepth = ptrDepth->image;
00658
00659
00660 if(rgb.empty())
00661 {
00662 rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
00663 }
00664 if(depth.empty())
00665 {
00666 depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type());
00667 }
00668
00669 if(ptrImage->image.type() == rgb.type())
00670 {
00671 ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
00672 }
00673 else
00674 {
00675 ROS_ERROR("Some RGB images are not the same type!");
00676 return;
00677 }
00678
00679 if(subDepth.type() == depth.type())
00680 {
00681 subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
00682 }
00683 else
00684 {
00685 ROS_ERROR("Some Depth images are not the same type!");
00686 return;
00687 }
00688
00689 cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform));
00690 }
00691 }
00692
00693 if(scan2dMsg.get() != 0)
00694 {
00695
00696 if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull())
00697 {
00698 return;
00699 }
00700
00701
00702 sensor_msgs::PointCloud2 scanOut;
00703 laser_geometry::LaserProjection projection;
00704 projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_);
00705 pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
00706 pcl::fromROSMsg(scanOut, *pclScan);
00707
00708
00709 if(odomHeader.stamp != scan2dMsg->header.stamp)
00710 {
00711 if(!odomT.isNull())
00712 {
00713 Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan2dMsg->header.stamp);
00714 if(sensorT.isNull())
00715 {
00716 return;
00717 }
00718 Transform t = odomT.inverse() * sensorT;
00719 pclScan = util3d::transformPointCloud(pclScan, t);
00720
00721 }
00722 }
00723 scan = util3d::laserScanFromPointCloud(*pclScan);
00724 }
00725 else if(scan3dMsg.get() != 0)
00726 {
00727 pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
00728 pcl::fromROSMsg(*scan3dMsg, *pclScan);
00729 scan = util3d::laserScanFromPointCloud(*pclScan);
00730 }
00731
00732 if(odomInfoMsg.get())
00733 {
00734 info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
00735 }
00736 ignoreData = false;
00737 }
00738 else if(odomInfoMsg.get())
00739 {
00740 info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg).copyWithoutData();
00741 ignoreData = true;
00742 }
00743 else
00744 {
00745
00746 return;
00747 }
00748
00749 rtabmap::OdometryEvent odomEvent(
00750 rtabmap::SensorData(
00751 scan,
00752 scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
00753 scan2dMsg.get()?(int)scan2dMsg->range_max:0,
00754 rgb,
00755 depth,
00756 cameraModels,
00757 odomHeader.seq,
00758 rtabmap_ros::timestampFromROS(odomHeader.stamp)),
00759 odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
00760 covariance,
00761 info);
00762
00763 QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
00764 }
00765
00766 void GuiWrapper::commonStereoCallback(
00767 const nav_msgs::OdometryConstPtr & odomMsg,
00768 const sensor_msgs::ImageConstPtr& leftImageMsg,
00769 const sensor_msgs::ImageConstPtr& rightImageMsg,
00770 const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
00771 const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
00772 const sensor_msgs::LaserScanConstPtr& scan2dMsg,
00773 const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
00774 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00775 {
00776 UASSERT(leftImageMsg.get() && rightImageMsg.get());
00777 UASSERT(leftCamInfoMsg.get() && rightCamInfoMsg.get());
00778
00779 if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00780 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
00781 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00782 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
00783 !(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00784 rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
00785 rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00786 rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
00787 {
00788 ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
00789 return;
00790 }
00791
00792 std_msgs::Header odomHeader;
00793 if(odomMsg.get())
00794 {
00795 odomHeader = odomMsg->header;
00796 }
00797 else
00798 {
00799 if(scan2dMsg.get())
00800 {
00801 odomHeader = scan2dMsg->header;
00802 }
00803 else if(scan3dMsg.get())
00804 {
00805 odomHeader = scan3dMsg->header;
00806 }
00807 else
00808 {
00809 odomHeader = leftCamInfoMsg->header;
00810 }
00811 odomHeader.frame_id = odomFrameId_;
00812 }
00813
00814 Transform odomT = getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp);
00815 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
00816 if(odomMsg.get())
00817 {
00818 UASSERT(odomMsg->pose.covariance.size() == 36);
00819 if(!(odomMsg->pose.covariance[0] == 0 &&
00820 odomMsg->pose.covariance[7] == 0 &&
00821 odomMsg->pose.covariance[14] == 0 &&
00822 odomMsg->pose.covariance[21] == 0 &&
00823 odomMsg->pose.covariance[28] == 0 &&
00824 odomMsg->pose.covariance[35] == 0))
00825 {
00826 covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->pose.covariance.data()).clone();
00827 }
00828 }
00829 if(odomHeader.frame_id.empty())
00830 {
00831 ROS_ERROR("Odometry frame not set!?");
00832 return;
00833 }
00834
00835 Transform localTransform = getTransform(frameId_, leftCamInfoMsg->header.frame_id, leftCamInfoMsg->header.stamp);
00836 if(localTransform.isNull())
00837 {
00838 return;
00839 }
00840
00841 if(odomHeader.stamp != leftCamInfoMsg->header.stamp)
00842 {
00843 if(!odomT.isNull())
00844 {
00845 Transform sensorT = getTransform(odomHeader.frame_id, frameId_, leftCamInfoMsg->header.stamp);
00846 if(sensorT.isNull())
00847 {
00848 return;
00849 }
00850 localTransform = odomT.inverse() * sensorT * localTransform;
00851 }
00852 }
00853
00854 cv::Mat left;
00855 cv::Mat right;
00856 cv::Mat scan;
00857 rtabmap::StereoCameraModel stereoModel;
00858 rtabmap::OdometryInfo info;
00859 bool ignoreData = false;
00860
00861
00862 if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
00863 !mainWindow_->isProcessingOdometry() &&
00864 !mainWindow_->isProcessingStatistics())
00865 {
00866 lastOdomInfoUpdateTime_ = UTimer::now();
00867
00868 stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
00869
00870 if(stereoModel.baseline() > 10.0)
00871 {
00872 static bool shown = false;
00873 if(!shown)
00874 {
00875 ROS_WARN("Detected baseline (%f m) is quite large! Is your "
00876 "right camera_info P(0,3) correctly set? Note that "
00877 "baseline=-P(0,3)/P(0,0). This warning is printed only once.",
00878 stereoModel.baseline());
00879 shown = true;
00880 }
00881 }
00882
00883
00884 cv_bridge::CvImageConstPtr ptrImage;
00885 if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00886 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
00887 {
00888 left = cv_bridge::toCvCopy(leftImageMsg, "mono8")->image;
00889 }
00890 else
00891 {
00892 left = cv_bridge::toCvCopy(leftImageMsg, "bgr8")->image;
00893 }
00894
00895
00896 right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image;
00897
00898 if(scan2dMsg.get() != 0)
00899 {
00900
00901 if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull())
00902 {
00903 return;
00904 }
00905
00906
00907 sensor_msgs::PointCloud2 scanOut;
00908 laser_geometry::LaserProjection projection;
00909 projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_);
00910 pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
00911 pcl::fromROSMsg(scanOut, *pclScan);
00912
00913
00914 if(odomHeader.stamp != scan2dMsg->header.stamp)
00915 {
00916 if(!odomT.isNull())
00917 {
00918 Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan2dMsg->header.stamp);
00919 if(sensorT.isNull())
00920 {
00921 return;
00922 }
00923 Transform t = odomT.inverse() * sensorT;
00924 pclScan = util3d::transformPointCloud(pclScan, t);
00925
00926 }
00927 }
00928 scan = util3d::laserScan2dFromPointCloud(*pclScan);
00929 }
00930 else if(scan3dMsg.get() != 0)
00931 {
00932 pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
00933 pcl::fromROSMsg(*scan3dMsg, *pclScan);
00934 scan = util3d::laserScanFromPointCloud(*pclScan);
00935 }
00936
00937 if(odomInfoMsg.get())
00938 {
00939 info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
00940 }
00941 ignoreData = false;
00942 }
00943 else if(odomInfoMsg.get())
00944 {
00945 info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg).copyWithoutData();
00946 ignoreData = true;
00947 }
00948 else
00949 {
00950
00951 return;
00952 }
00953
00954 rtabmap::OdometryEvent odomEvent(
00955 rtabmap::SensorData(
00956 scan,
00957 scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
00958 scan2dMsg.get()?(int)scan2dMsg->range_max:0,
00959 left,
00960 right,
00961 stereoModel,
00962 odomHeader.seq,
00963 rtabmap_ros::timestampFromROS(odomHeader.stamp)),
00964 odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
00965 covariance,
00966 info);
00967
00968 QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
00969 }
00970
00971
00972 void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
00973 {
00974 commonDepthCallback(
00975 odomMsg,
00976 sensor_msgs::ImageConstPtr(),
00977 sensor_msgs::ImageConstPtr(),
00978 sensor_msgs::CameraInfoConstPtr(),
00979 sensor_msgs::LaserScanConstPtr(),
00980 sensor_msgs::PointCloud2ConstPtr(),
00981 rtabmap_ros::OdomInfoConstPtr());
00982 }
00983
00984 void GuiWrapper::depthCallback(
00985 const nav_msgs::OdometryConstPtr & odomMsg,
00986 const sensor_msgs::ImageConstPtr& imageMsg,
00987 const sensor_msgs::ImageConstPtr& depthMsg,
00988 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
00989 {
00990 commonDepthCallback(
00991 odomMsg,
00992 imageMsg,
00993 depthMsg,
00994 cameraInfoMsg,
00995 sensor_msgs::LaserScanConstPtr(),
00996 sensor_msgs::PointCloud2ConstPtr(),
00997 rtabmap_ros::OdomInfoConstPtr());
00998 }
00999
01000 void GuiWrapper::depth2Callback(
01001 const nav_msgs::OdometryConstPtr & odomMsg,
01002 const sensor_msgs::ImageConstPtr& image1Msg,
01003 const sensor_msgs::ImageConstPtr& depth1Msg,
01004 const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
01005 const sensor_msgs::ImageConstPtr& image2Msg,
01006 const sensor_msgs::ImageConstPtr& depth2Msg,
01007 const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg)
01008 {
01009 std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
01010 std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
01011 std::vector<sensor_msgs::CameraInfoConstPtr> cameraInfoMsgs;
01012 imageMsgs.push_back(image1Msg);
01013 imageMsgs.push_back(image2Msg);
01014 depthMsgs.push_back(depth1Msg);
01015 depthMsgs.push_back(depth2Msg);
01016 cameraInfoMsgs.push_back(cameraInfo1Msg);
01017 cameraInfoMsgs.push_back(cameraInfo2Msg);
01018
01019 commonDepthCallback(
01020 odomMsg,
01021 imageMsgs,
01022 depthMsgs,
01023 cameraInfoMsgs,
01024 sensor_msgs::LaserScanConstPtr(),
01025 sensor_msgs::PointCloud2ConstPtr(),
01026 rtabmap_ros::OdomInfoConstPtr());
01027 }
01028
01029 void GuiWrapper::depthOdomInfoCallback(
01030 const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
01031 const nav_msgs::OdometryConstPtr & odomMsg,
01032 const sensor_msgs::ImageConstPtr& imageMsg,
01033 const sensor_msgs::ImageConstPtr& depthMsg,
01034 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
01035 {
01036 commonDepthCallback(
01037 odomMsg,
01038 imageMsg,
01039 depthMsg,
01040 cameraInfoMsg,
01041 sensor_msgs::LaserScanConstPtr(),
01042 sensor_msgs::PointCloud2ConstPtr(),
01043 odomInfoMsg);
01044 }
01045
01046 void GuiWrapper::depthOdomInfo2Callback(
01047 const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
01048 const nav_msgs::OdometryConstPtr & odomMsg,
01049 const sensor_msgs::ImageConstPtr& image1Msg,
01050 const sensor_msgs::ImageConstPtr& depth1Msg,
01051 const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
01052 const sensor_msgs::ImageConstPtr& image2Msg,
01053 const sensor_msgs::ImageConstPtr& depth2Msg,
01054 const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg)
01055 {
01056 std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
01057 std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
01058 std::vector<sensor_msgs::CameraInfoConstPtr> cameraInfoMsgs;
01059 imageMsgs.push_back(image1Msg);
01060 imageMsgs.push_back(image2Msg);
01061 depthMsgs.push_back(depth1Msg);
01062 depthMsgs.push_back(depth2Msg);
01063 cameraInfoMsgs.push_back(cameraInfo1Msg);
01064 cameraInfoMsgs.push_back(cameraInfo2Msg);
01065
01066 commonDepthCallback(
01067 odomMsg,
01068 imageMsgs,
01069 depthMsgs,
01070 cameraInfoMsgs,
01071 sensor_msgs::LaserScanConstPtr(),
01072 sensor_msgs::PointCloud2ConstPtr(),
01073 odomInfoMsg);
01074 }
01075
01076 void GuiWrapper::depthScanCallback(
01077 const sensor_msgs::LaserScanConstPtr& scanMsg,
01078 const nav_msgs::OdometryConstPtr & odomMsg,
01079 const sensor_msgs::ImageConstPtr& imageMsg,
01080 const sensor_msgs::ImageConstPtr& depthMsg,
01081 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
01082 {
01083 commonDepthCallback(
01084 odomMsg,
01085 imageMsg,
01086 depthMsg,
01087 cameraInfoMsg,
01088 scanMsg,
01089 sensor_msgs::PointCloud2ConstPtr(),
01090 rtabmap_ros::OdomInfoConstPtr());
01091 }
01092
01093 void GuiWrapper::depthScanOdomInfoCallback(
01094 const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
01095 const sensor_msgs::LaserScanConstPtr& scanMsg,
01096 const nav_msgs::OdometryConstPtr & odomMsg,
01097 const sensor_msgs::ImageConstPtr& imageMsg,
01098 const sensor_msgs::ImageConstPtr& depthMsg,
01099 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
01100 {
01101 commonDepthCallback(
01102 odomMsg,
01103 imageMsg,
01104 depthMsg,
01105 cameraInfoMsg,
01106 scanMsg,
01107 sensor_msgs::PointCloud2ConstPtr(),
01108 odomInfoMsg);
01109 }
01110
01111 void GuiWrapper::depthScan3dCallback(
01112 const sensor_msgs::PointCloud2ConstPtr& scanMsg,
01113 const nav_msgs::OdometryConstPtr & odomMsg,
01114 const sensor_msgs::ImageConstPtr& imageMsg,
01115 const sensor_msgs::ImageConstPtr& depthMsg,
01116 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
01117 {
01118 commonDepthCallback(
01119 odomMsg,
01120 imageMsg,
01121 depthMsg,
01122 cameraInfoMsg,
01123 sensor_msgs::LaserScanConstPtr(),
01124 scanMsg,
01125 rtabmap_ros::OdomInfoConstPtr());
01126 }
01127
01128 void GuiWrapper::depthScan3dOdomInfoCallback(
01129 const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
01130 const sensor_msgs::PointCloud2ConstPtr& scanMsg,
01131 const nav_msgs::OdometryConstPtr & odomMsg,
01132 const sensor_msgs::ImageConstPtr& imageMsg,
01133 const sensor_msgs::ImageConstPtr& depthMsg,
01134 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
01135 {
01136 commonDepthCallback(
01137 odomMsg,
01138 imageMsg,
01139 depthMsg,
01140 cameraInfoMsg,
01141 sensor_msgs::LaserScanConstPtr(),
01142 scanMsg,
01143 odomInfoMsg);
01144 }
01145
01146 void GuiWrapper::stereoScanCallback(
01147 const sensor_msgs::LaserScanConstPtr& scanMsg,
01148 const nav_msgs::OdometryConstPtr & odomMsg,
01149 const sensor_msgs::ImageConstPtr& leftImageMsg,
01150 const sensor_msgs::ImageConstPtr& rightImageMsg,
01151 const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
01152 const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
01153 {
01154 commonStereoCallback(
01155 odomMsg,
01156 leftImageMsg,
01157 rightImageMsg,
01158 leftCameraInfoMsg,
01159 rightCameraInfoMsg,
01160 scanMsg,
01161 sensor_msgs::PointCloud2ConstPtr(),
01162 rtabmap_ros::OdomInfoConstPtr());
01163 }
01164
01165 void GuiWrapper::stereoScanOdomInfoCallback(
01166 const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
01167 const sensor_msgs::LaserScanConstPtr& scanMsg,
01168 const nav_msgs::OdometryConstPtr & odomMsg,
01169 const sensor_msgs::ImageConstPtr& leftImageMsg,
01170 const sensor_msgs::ImageConstPtr& rightImageMsg,
01171 const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
01172 const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
01173 {
01174 commonStereoCallback(
01175 odomMsg,
01176 leftImageMsg,
01177 rightImageMsg,
01178 leftCameraInfoMsg,
01179 rightCameraInfoMsg,
01180 scanMsg,
01181 sensor_msgs::PointCloud2ConstPtr(),
01182 odomInfoMsg);
01183 }
01184
01185 void GuiWrapper::stereoScan3dCallback(
01186 const sensor_msgs::PointCloud2ConstPtr& scanMsg,
01187 const nav_msgs::OdometryConstPtr & odomMsg,
01188 const sensor_msgs::ImageConstPtr& leftImageMsg,
01189 const sensor_msgs::ImageConstPtr& rightImageMsg,
01190 const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
01191 const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
01192 {
01193 commonStereoCallback(
01194 odomMsg,
01195 leftImageMsg,
01196 rightImageMsg,
01197 leftCameraInfoMsg,
01198 rightCameraInfoMsg,
01199 sensor_msgs::LaserScanConstPtr(),
01200 scanMsg,
01201 rtabmap_ros::OdomInfoConstPtr());
01202 }
01203
01204 void GuiWrapper::stereoScan3dOdomInfoCallback(
01205 const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
01206 const sensor_msgs::PointCloud2ConstPtr& scanMsg,
01207 const nav_msgs::OdometryConstPtr & odomMsg,
01208 const sensor_msgs::ImageConstPtr& leftImageMsg,
01209 const sensor_msgs::ImageConstPtr& rightImageMsg,
01210 const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
01211 const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
01212 {
01213 commonStereoCallback(
01214 odomMsg,
01215 leftImageMsg,
01216 rightImageMsg,
01217 leftCameraInfoMsg,
01218 rightCameraInfoMsg,
01219 sensor_msgs::LaserScanConstPtr(),
01220 scanMsg,
01221 odomInfoMsg);
01222 }
01223
01224 void GuiWrapper::stereoOdomInfoCallback(
01225 const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
01226 const nav_msgs::OdometryConstPtr & odomMsg,
01227 const sensor_msgs::ImageConstPtr& leftImageMsg,
01228 const sensor_msgs::ImageConstPtr& rightImageMsg,
01229 const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
01230 const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
01231 {
01232 commonStereoCallback(
01233 odomMsg,
01234 leftImageMsg,
01235 rightImageMsg,
01236 leftCameraInfoMsg,
01237 rightCameraInfoMsg,
01238 sensor_msgs::LaserScanConstPtr(),
01239 sensor_msgs::PointCloud2ConstPtr(),
01240 odomInfoMsg);
01241 }
01242
01243 void GuiWrapper::stereoCallback(
01244 const nav_msgs::OdometryConstPtr & odomMsg,
01245 const sensor_msgs::ImageConstPtr& leftImageMsg,
01246 const sensor_msgs::ImageConstPtr& rightImageMsg,
01247 const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
01248 const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
01249 {
01250 commonStereoCallback(
01251 odomMsg,
01252 leftImageMsg,
01253 rightImageMsg,
01254 leftCameraInfoMsg,
01255 rightCameraInfoMsg,
01256 sensor_msgs::LaserScanConstPtr(),
01257 sensor_msgs::PointCloud2ConstPtr(),
01258 rtabmap_ros::OdomInfoConstPtr());
01259 }
01260
01261
01262 void GuiWrapper::depthTFCallback(
01263 const sensor_msgs::ImageConstPtr& imageMsg,
01264 const sensor_msgs::ImageConstPtr& depthMsg,
01265 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
01266 {
01267 commonDepthCallback(
01268 nav_msgs::OdometryConstPtr(),
01269 imageMsg,
01270 depthMsg,
01271 cameraInfoMsg,
01272 sensor_msgs::LaserScanConstPtr(),
01273 sensor_msgs::PointCloud2ConstPtr(),
01274 rtabmap_ros::OdomInfoConstPtr());
01275 }
01276
01277 void GuiWrapper::depthOdomInfoTFCallback(
01278 const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
01279 const sensor_msgs::ImageConstPtr& imageMsg,
01280 const sensor_msgs::ImageConstPtr& depthMsg,
01281 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
01282 {
01283 commonDepthCallback(
01284 nav_msgs::OdometryConstPtr(),
01285 imageMsg,
01286 depthMsg,
01287 cameraInfoMsg,
01288 sensor_msgs::LaserScanConstPtr(),
01289 sensor_msgs::PointCloud2ConstPtr(),
01290 odomInfoMsg);
01291 }
01292
01293 void GuiWrapper::depthScanTFCallback(
01294 const sensor_msgs::LaserScanConstPtr& scanMsg,
01295 const sensor_msgs::ImageConstPtr& imageMsg,
01296 const sensor_msgs::ImageConstPtr& depthMsg,
01297 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
01298 {
01299 commonDepthCallback(
01300 nav_msgs::OdometryConstPtr(),
01301 imageMsg,
01302 depthMsg,
01303 cameraInfoMsg,
01304 scanMsg,
01305 sensor_msgs::PointCloud2ConstPtr(),
01306 rtabmap_ros::OdomInfoConstPtr());
01307 }
01308
01309 void GuiWrapper::depthScan3dTFCallback(
01310 const sensor_msgs::PointCloud2ConstPtr& scanMsg,
01311 const sensor_msgs::ImageConstPtr& imageMsg,
01312 const sensor_msgs::ImageConstPtr& depthMsg,
01313 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
01314 {
01315 commonDepthCallback(
01316 nav_msgs::OdometryConstPtr(),
01317 imageMsg,
01318 depthMsg,
01319 cameraInfoMsg,
01320 sensor_msgs::LaserScanConstPtr(),
01321 scanMsg,
01322 rtabmap_ros::OdomInfoConstPtr());
01323 }
01324
01325 void GuiWrapper::stereoScanTFCallback(
01326 const sensor_msgs::LaserScanConstPtr& scanMsg,
01327 const sensor_msgs::ImageConstPtr& leftImageMsg,
01328 const sensor_msgs::ImageConstPtr& rightImageMsg,
01329 const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
01330 const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
01331 {
01332 commonStereoCallback(
01333 nav_msgs::OdometryConstPtr(),
01334 leftImageMsg,
01335 rightImageMsg,
01336 leftCameraInfoMsg,
01337 rightCameraInfoMsg,
01338 scanMsg,
01339 sensor_msgs::PointCloud2ConstPtr(),
01340 rtabmap_ros::OdomInfoConstPtr());
01341 }
01342
01343 void GuiWrapper::stereoScan3dTFCallback(
01344 const sensor_msgs::PointCloud2ConstPtr& scanMsg,
01345 const sensor_msgs::ImageConstPtr& leftImageMsg,
01346 const sensor_msgs::ImageConstPtr& rightImageMsg,
01347 const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
01348 const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
01349 {
01350 commonStereoCallback(
01351 nav_msgs::OdometryConstPtr(),
01352 leftImageMsg,
01353 rightImageMsg,
01354 leftCameraInfoMsg,
01355 rightCameraInfoMsg,
01356 sensor_msgs::LaserScanConstPtr(),
01357 scanMsg,
01358 rtabmap_ros::OdomInfoConstPtr());
01359 }
01360
01361 void GuiWrapper::stereoOdomInfoTFCallback(
01362 const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
01363 const sensor_msgs::ImageConstPtr& leftImageMsg,
01364 const sensor_msgs::ImageConstPtr& rightImageMsg,
01365 const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
01366 const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
01367 {
01368 commonStereoCallback(
01369 nav_msgs::OdometryConstPtr(),
01370 leftImageMsg,
01371 rightImageMsg,
01372 leftCameraInfoMsg,
01373 rightCameraInfoMsg,
01374 sensor_msgs::LaserScanConstPtr(),
01375 sensor_msgs::PointCloud2ConstPtr(),
01376 odomInfoMsg);
01377 }
01378
01379 void GuiWrapper::stereoTFCallback(
01380 const sensor_msgs::ImageConstPtr& leftImageMsg,
01381 const sensor_msgs::ImageConstPtr& rightImageMsg,
01382 const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
01383 const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
01384 {
01385 commonStereoCallback(
01386 nav_msgs::OdometryConstPtr(),
01387 leftImageMsg,
01388 rightImageMsg,
01389 leftCameraInfoMsg,
01390 rightCameraInfoMsg,
01391 sensor_msgs::LaserScanConstPtr(),
01392 sensor_msgs::PointCloud2ConstPtr(),
01393 rtabmap_ros::OdomInfoConstPtr());
01394 }
01395
01396 void GuiWrapper::setupCallbacks(
01397 bool subscribeDepth,
01398 bool subscribeLaserScan2d,
01399 bool subscribeLaserScan3d,
01400 bool subscribeOdomInfo,
01401 bool subscribeStereo,
01402 int queueSize,
01403 int depthCameras)
01404 {
01405 ros::NodeHandle nh;
01406 ros::NodeHandle pnh("~");
01407
01408 if(subscribeDepth && subscribeStereo)
01409 {
01410 ROS_WARN("rtabmapviz: Parameters subscribe_depth and subscribe_stereo cannot be true at the "
01411 "same time. Parameter subscribe_depth is set to false.");
01412 subscribeDepth = false;
01413 }
01414 if(!subscribeDepth && !subscribeStereo && (subscribeLaserScan2d || subscribeLaserScan3d))
01415 {
01416 ROS_WARN("Cannot subscribe to laser scan without depth or stereo subscription...");
01417 }
01418 if(depthCameras <= 0)
01419 {
01420 depthCameras = 1;
01421 }
01422 if(depthCameras > 2)
01423 {
01424 ROS_WARN("Cannot subscribe to more than 2 cameras yet...");
01425 depthCameras = 2;
01426 }
01427
01428 if(subscribeDepth)
01429 {
01430 UASSERT(depthCameras >= 1 && depthCameras <= 2);
01431 UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan2d || subscribeLaserScan3d || !odomFrameId_.empty()), "Not yet supported!");
01432
01433 imageSubs_.resize(depthCameras);
01434 imageDepthSubs_.resize(depthCameras);
01435 cameraInfoSubs_.resize(depthCameras);
01436 for(int i=0; i<depthCameras; ++i)
01437 {
01438 std::string rgbPrefix = "rgb";
01439 std::string depthPrefix = "depth";
01440 if(depthCameras>1)
01441 {
01442 rgbPrefix += uNumber2Str(i);
01443 depthPrefix += uNumber2Str(i);
01444 }
01445 ros::NodeHandle rgb_nh(nh, rgbPrefix);
01446 ros::NodeHandle depth_nh(nh, depthPrefix);
01447 ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
01448 ros::NodeHandle depth_pnh(pnh, depthPrefix);
01449 image_transport::ImageTransport rgb_it(rgb_nh);
01450 image_transport::ImageTransport depth_it(depth_nh);
01451 image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
01452 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
01453
01454 imageSubs_[i] = new image_transport::SubscriberFilter;
01455 imageDepthSubs_[i] = new image_transport::SubscriberFilter;
01456 cameraInfoSubs_[i] = new message_filters::Subscriber<sensor_msgs::CameraInfo>;
01457 imageSubs_[i]->subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
01458 imageDepthSubs_[i]->subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
01459 cameraInfoSubs_[i]->subscribe(rgb_nh, "camera_info", 1);
01460 }
01461
01462 if(odomFrameId_.empty())
01463 {
01464 odomSub_.subscribe(nh, "odom", 1);
01465 if(subscribeLaserScan2d)
01466 {
01467 scanSub_.subscribe(nh, "scan", 1);
01468 if(subscribeOdomInfo)
01469 {
01470 odomInfoSub_.subscribe(nh, "odom_info", 1);
01471 depthScanOdomInfoSync_ = new message_filters::Synchronizer<MyDepthScanOdomInfoSyncPolicy>(
01472 MyDepthScanOdomInfoSyncPolicy(queueSize),
01473 odomInfoSub_,
01474 scanSub_,
01475 odomSub_,
01476 *imageSubs_[0],
01477 *imageDepthSubs_[0],
01478 *cameraInfoSubs_[0]);
01479 depthScanOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthScanOdomInfoCallback, this, _1, _2, _3, _4, _5, _6));
01480
01481 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
01482 ros::this_node::getName().c_str(),
01483 imageSubs_[0]->getTopic().c_str(),
01484 imageDepthSubs_[0]->getTopic().c_str(),
01485 cameraInfoSubs_[0]->getTopic().c_str(),
01486 odomSub_.getTopic().c_str(),
01487 scanSub_.getTopic().c_str(),
01488 odomInfoSub_.getTopic().c_str());
01489 }
01490 else
01491 {
01492 depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
01493 MyDepthScanSyncPolicy(queueSize),
01494 scanSub_,
01495 odomSub_,
01496 *imageSubs_[0],
01497 *imageDepthSubs_[0],
01498 *cameraInfoSubs_[0]);
01499 depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
01500
01501 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
01502 ros::this_node::getName().c_str(),
01503 imageSubs_[0]->getTopic().c_str(),
01504 imageDepthSubs_[0]->getTopic().c_str(),
01505 cameraInfoSubs_[0]->getTopic().c_str(),
01506 odomSub_.getTopic().c_str(),
01507 scanSub_.getTopic().c_str());
01508 }
01509 }
01510 else if(subscribeLaserScan3d)
01511 {
01512 scan3dSub_.subscribe(nh, "scan_cloud", 1);
01513 if(subscribeOdomInfo)
01514 {
01515 odomInfoSub_.subscribe(nh, "odom_info", 1);
01516 depthScan3dOdomInfoSync_ = new message_filters::Synchronizer<MyDepthScan3dOdomInfoSyncPolicy>(
01517 MyDepthScan3dOdomInfoSyncPolicy(queueSize),
01518 odomInfoSub_,
01519 scan3dSub_,
01520 odomSub_,
01521 *imageSubs_[0],
01522 *imageDepthSubs_[0],
01523 *cameraInfoSubs_[0]);
01524 depthScan3dOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dOdomInfoCallback, this, _1, _2, _3, _4, _5, _6));
01525
01526 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
01527 ros::this_node::getName().c_str(),
01528 imageSubs_[0]->getTopic().c_str(),
01529 imageDepthSubs_[0]->getTopic().c_str(),
01530 cameraInfoSubs_[0]->getTopic().c_str(),
01531 odomSub_.getTopic().c_str(),
01532 scan3dSub_.getTopic().c_str(),
01533 odomInfoSub_.getTopic().c_str());
01534 }
01535 else
01536 {
01537 depthScan3dSync_ = new message_filters::Synchronizer<MyDepthScan3dSyncPolicy>(
01538 MyDepthScan3dSyncPolicy(queueSize),
01539 scan3dSub_,
01540 odomSub_,
01541 *imageSubs_[0],
01542 *imageDepthSubs_[0],
01543 *cameraInfoSubs_[0]);
01544 depthScan3dSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5));
01545
01546 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
01547 ros::this_node::getName().c_str(),
01548 imageSubs_[0]->getTopic().c_str(),
01549 imageDepthSubs_[0]->getTopic().c_str(),
01550 cameraInfoSubs_[0]->getTopic().c_str(),
01551 odomSub_.getTopic().c_str(),
01552 scan3dSub_.getTopic().c_str());
01553 }
01554 }
01555 else if(subscribeOdomInfo)
01556 {
01557 odomInfoSub_.subscribe(nh, "odom_info", 1);
01558 if(depthCameras > 1)
01559 {
01560 depthOdomInfo2Sync_ = new message_filters::Synchronizer<MyDepthOdomInfo2SyncPolicy>(
01561 MyDepthOdomInfo2SyncPolicy(queueSize),
01562 odomInfoSub_,
01563 odomSub_,
01564 *imageSubs_[0],
01565 *imageDepthSubs_[0],
01566 *cameraInfoSubs_[0],
01567 *imageSubs_[1],
01568 *imageDepthSubs_[1],
01569 *cameraInfoSubs_[1]);
01570 depthOdomInfo2Sync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfo2Callback, this, _1, _2, _3, _4, _5, _6, _7, _8));
01571
01572 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s\n %s,\n %s,\n %s",
01573 ros::this_node::getName().c_str(),
01574 imageSubs_[0]->getTopic().c_str(),
01575 imageDepthSubs_[0]->getTopic().c_str(),
01576 cameraInfoSubs_[0]->getTopic().c_str(),
01577 imageSubs_[1]->getTopic().c_str(),
01578 imageDepthSubs_[1]->getTopic().c_str(),
01579 cameraInfoSubs_[1]->getTopic().c_str(),
01580 odomSub_.getTopic().c_str(),
01581 odomInfoSub_.getTopic().c_str());
01582 }
01583 else
01584 {
01585 depthOdomInfoSync_ = new message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy>(
01586 MyDepthOdomInfoSyncPolicy(queueSize),
01587 odomInfoSub_,
01588 odomSub_,
01589 *imageSubs_[0],
01590 *imageDepthSubs_[0],
01591 *cameraInfoSubs_[0]);
01592 depthOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoCallback, this, _1, _2, _3, _4, _5));
01593
01594 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
01595 ros::this_node::getName().c_str(),
01596 imageSubs_[0]->getTopic().c_str(),
01597 imageDepthSubs_[0]->getTopic().c_str(),
01598 cameraInfoSubs_[0]->getTopic().c_str(),
01599 odomSub_.getTopic().c_str(),
01600 odomInfoSub_.getTopic().c_str());
01601 }
01602 }
01603 else
01604 {
01605 if(depthCameras > 1)
01606 {
01607 depth2Sync_ = new message_filters::Synchronizer<MyDepth2SyncPolicy>(
01608 MyDepth2SyncPolicy(queueSize),
01609 odomSub_,
01610 *imageSubs_[0],
01611 *imageDepthSubs_[0],
01612 *cameraInfoSubs_[0],
01613 *imageSubs_[1],
01614 *imageDepthSubs_[1],
01615 *cameraInfoSubs_[1]);
01616 depth2Sync_->registerCallback(boost::bind(&GuiWrapper::depth2Callback, this, _1, _2, _3, _4, _5, _6, _7));
01617
01618 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s\n %s,\n %s,\n %s",
01619 ros::this_node::getName().c_str(),
01620 imageSubs_[0]->getTopic().c_str(),
01621 imageDepthSubs_[0]->getTopic().c_str(),
01622 cameraInfoSubs_[0]->getTopic().c_str(),
01623 imageSubs_[1]->getTopic().c_str(),
01624 imageDepthSubs_[1]->getTopic().c_str(),
01625 cameraInfoSubs_[1]->getTopic().c_str(),
01626 odomSub_.getTopic().c_str());
01627 }
01628 else
01629 {
01630 depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(
01631 MyDepthSyncPolicy(queueSize),
01632 odomSub_,
01633 *imageSubs_[0],
01634 *imageDepthSubs_[0],
01635 *cameraInfoSubs_[0]);
01636 depthSync_->registerCallback(boost::bind(&GuiWrapper::depthCallback, this, _1, _2, _3, _4));
01637
01638 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
01639 ros::this_node::getName().c_str(),
01640 imageSubs_[0]->getTopic().c_str(),
01641 imageDepthSubs_[0]->getTopic().c_str(),
01642 cameraInfoSubs_[0]->getTopic().c_str(),
01643 odomSub_.getTopic().c_str());
01644 }
01645 }
01646 }
01647 else
01648 {
01649
01650 if(subscribeLaserScan2d)
01651 {
01652 scanSub_.subscribe(nh, "scan", 1);
01653 depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(
01654 MyDepthScanTFSyncPolicy(queueSize),
01655 scanSub_,
01656 *imageSubs_[0],
01657 *imageDepthSubs_[0],
01658 *cameraInfoSubs_[0]);
01659 depthScanTFSync_->registerCallback(boost::bind(&GuiWrapper::depthScanTFCallback, this, _1, _2, _3, _4));
01660
01661 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
01662 ros::this_node::getName().c_str(),
01663 imageSubs_[0]->getTopic().c_str(),
01664 imageDepthSubs_[0]->getTopic().c_str(),
01665 cameraInfoSubs_[0]->getTopic().c_str(),
01666 scanSub_.getTopic().c_str());
01667 }
01668 else if(subscribeLaserScan3d)
01669 {
01670 scan3dSub_.subscribe(nh, "scan_cloud", 1);
01671 depthScan3dTFSync_ = new message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy>(
01672 MyDepthScan3dTFSyncPolicy(queueSize),
01673 scan3dSub_,
01674 *imageSubs_[0],
01675 *imageDepthSubs_[0],
01676 *cameraInfoSubs_[0]);
01677 depthScan3dTFSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dTFCallback, this, _1, _2, _3, _4));
01678
01679 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
01680 ros::this_node::getName().c_str(),
01681 imageSubs_[0]->getTopic().c_str(),
01682 imageDepthSubs_[0]->getTopic().c_str(),
01683 cameraInfoSubs_[0]->getTopic().c_str(),
01684 scan3dSub_.getTopic().c_str());
01685 }
01686 else if(subscribeOdomInfo)
01687 {
01688 odomInfoSub_.subscribe(nh, "odom_info", 1);
01689 depthOdomInfoTFSync_ = new message_filters::Synchronizer<MyDepthOdomInfoTFSyncPolicy>(
01690 MyDepthOdomInfoTFSyncPolicy(queueSize),
01691 odomInfoSub_,
01692 *imageSubs_[0],
01693 *imageDepthSubs_[0],
01694 *cameraInfoSubs_[0]);
01695 depthOdomInfoTFSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoTFCallback, this, _1, _2, _3, _4));
01696
01697 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
01698 ros::this_node::getName().c_str(),
01699 imageSubs_[0]->getTopic().c_str(),
01700 imageDepthSubs_[0]->getTopic().c_str(),
01701 cameraInfoSubs_[0]->getTopic().c_str(),
01702 odomInfoSub_.getTopic().c_str());
01703 }
01704 else
01705 {
01706 depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(
01707 MyDepthTFSyncPolicy(queueSize),
01708 *imageSubs_[0],
01709 *imageDepthSubs_[0],
01710 *cameraInfoSubs_[0]);
01711 depthTFSync_->registerCallback(boost::bind(&GuiWrapper::depthTFCallback, this, _1, _2, _3));
01712
01713 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s",
01714 ros::this_node::getName().c_str(),
01715 imageSubs_[0]->getTopic().c_str(),
01716 imageDepthSubs_[0]->getTopic().c_str(),
01717 cameraInfoSubs_[0]->getTopic().c_str());
01718 }
01719 }
01720 }
01721 else if(subscribeStereo)
01722 {
01723 ros::NodeHandle left_nh(nh, "left");
01724 ros::NodeHandle right_nh(nh, "right");
01725 ros::NodeHandle left_pnh(pnh, "left");
01726 ros::NodeHandle right_pnh(pnh, "right");
01727 image_transport::ImageTransport left_it(left_nh);
01728 image_transport::ImageTransport right_it(right_nh);
01729 image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
01730 image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
01731
01732 imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
01733 imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
01734 cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
01735 cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
01736
01737 if(odomFrameId_.empty())
01738 {
01739 odomSub_.subscribe(nh, "odom", 1);
01740 if(subscribeLaserScan2d)
01741 {
01742 scanSub_.subscribe(nh, "scan", 1);
01743 if(subscribeOdomInfo)
01744 {
01745 odomInfoSub_.subscribe(nh, "odom_info", 1);
01746 stereoScanOdomInfoSync_ = new message_filters::Synchronizer<MyStereoScanOdomInfoSyncPolicy>(
01747 MyStereoScanOdomInfoSyncPolicy(queueSize),
01748 odomInfoSub_,
01749 scanSub_,
01750 odomSub_,
01751 imageRectLeft_,
01752 imageRectRight_,
01753 cameraInfoLeft_,
01754 cameraInfoRight_);
01755 stereoScanOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanOdomInfoCallback, this, _1, _2, _3, _4, _5, _6, _7));
01756
01757 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
01758 ros::this_node::getName().c_str(),
01759 imageRectLeft_.getTopic().c_str(),
01760 imageRectRight_.getTopic().c_str(),
01761 cameraInfoLeft_.getTopic().c_str(),
01762 cameraInfoRight_.getTopic().c_str(),
01763 odomSub_.getTopic().c_str(),
01764 scanSub_.getTopic().c_str(),
01765 odomInfoSub_.getTopic().c_str());
01766 }
01767 else
01768 {
01769 stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(
01770 MyStereoScanSyncPolicy(queueSize),
01771 scanSub_,
01772 odomSub_,
01773 imageRectLeft_,
01774 imageRectRight_,
01775 cameraInfoLeft_,
01776 cameraInfoRight_);
01777 stereoScanSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6));
01778
01779 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
01780 ros::this_node::getName().c_str(),
01781 imageRectLeft_.getTopic().c_str(),
01782 imageRectRight_.getTopic().c_str(),
01783 cameraInfoLeft_.getTopic().c_str(),
01784 cameraInfoRight_.getTopic().c_str(),
01785 odomSub_.getTopic().c_str(),
01786 scanSub_.getTopic().c_str());
01787 }
01788 }
01789 else if(subscribeLaserScan3d)
01790 {
01791 scan3dSub_.subscribe(nh, "scan_cloud", 1);
01792 if(subscribeOdomInfo)
01793 {
01794 odomInfoSub_.subscribe(nh, "odom_info", 1);
01795 stereoScan3dOdomInfoSync_ = new message_filters::Synchronizer<MyStereoScan3dOdomInfoSyncPolicy>(
01796 MyStereoScan3dOdomInfoSyncPolicy(queueSize),
01797 odomInfoSub_,
01798 scan3dSub_,
01799 odomSub_,
01800 imageRectLeft_,
01801 imageRectRight_,
01802 cameraInfoLeft_,
01803 cameraInfoRight_);
01804 stereoScan3dOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dOdomInfoCallback, this, _1, _2, _3, _4, _5, _6, _7));
01805
01806 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
01807 ros::this_node::getName().c_str(),
01808 imageRectLeft_.getTopic().c_str(),
01809 imageRectRight_.getTopic().c_str(),
01810 cameraInfoLeft_.getTopic().c_str(),
01811 cameraInfoRight_.getTopic().c_str(),
01812 odomSub_.getTopic().c_str(),
01813 scan3dSub_.getTopic().c_str(),
01814 odomInfoSub_.getTopic().c_str());
01815 }
01816 else
01817 {
01818 stereoScan3dSync_ = new message_filters::Synchronizer<MyStereoScan3dSyncPolicy>(
01819 MyStereoScan3dSyncPolicy(queueSize),
01820 scan3dSub_,
01821 odomSub_,
01822 imageRectLeft_,
01823 imageRectRight_,
01824 cameraInfoLeft_,
01825 cameraInfoRight_);
01826 stereoScan3dSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dCallback, this, _1, _2, _3, _4, _5, _6));
01827
01828 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
01829 ros::this_node::getName().c_str(),
01830 imageRectLeft_.getTopic().c_str(),
01831 imageRectRight_.getTopic().c_str(),
01832 cameraInfoLeft_.getTopic().c_str(),
01833 cameraInfoRight_.getTopic().c_str(),
01834 odomSub_.getTopic().c_str(),
01835 scan3dSub_.getTopic().c_str());
01836 }
01837 }
01838 else if(subscribeOdomInfo)
01839 {
01840 odomInfoSub_.subscribe(nh, "odom_info", 1);
01841 stereoOdomInfoSync_ = new message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy>(
01842 MyStereoOdomInfoSyncPolicy(queueSize),
01843 odomInfoSub_,
01844 odomSub_,
01845 imageRectLeft_,
01846 imageRectRight_,
01847 cameraInfoLeft_,
01848 cameraInfoRight_);
01849 stereoOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoOdomInfoCallback, this, _1, _2, _3, _4, _5, _6));
01850
01851 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
01852 ros::this_node::getName().c_str(),
01853 imageRectLeft_.getTopic().c_str(),
01854 imageRectRight_.getTopic().c_str(),
01855 cameraInfoLeft_.getTopic().c_str(),
01856 cameraInfoRight_.getTopic().c_str(),
01857 odomSub_.getTopic().c_str(),
01858 odomInfoSub_.getTopic().c_str());
01859 }
01860 else
01861 {
01862 stereoSync_ = new message_filters::Synchronizer<MyStereoSyncPolicy>(
01863 MyStereoSyncPolicy(queueSize),
01864 odomSub_,
01865 imageRectLeft_,
01866 imageRectRight_,
01867 cameraInfoLeft_,
01868 cameraInfoRight_);
01869 stereoSync_->registerCallback(boost::bind(&GuiWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
01870
01871 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
01872 ros::this_node::getName().c_str(),
01873 imageRectLeft_.getTopic().c_str(),
01874 imageRectRight_.getTopic().c_str(),
01875 cameraInfoLeft_.getTopic().c_str(),
01876 cameraInfoRight_.getTopic().c_str(),
01877 odomSub_.getTopic().c_str());
01878 }
01879 }
01880 else
01881 {
01882
01883 if(subscribeLaserScan2d)
01884 {
01885 scanSub_.subscribe(nh, "scan", 1);
01886 stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(
01887 MyStereoScanTFSyncPolicy(queueSize),
01888 scanSub_,
01889 imageRectLeft_,
01890 imageRectRight_,
01891 cameraInfoLeft_,
01892 cameraInfoRight_);
01893 stereoScanTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanTFCallback, this, _1, _2, _3, _4, _5));
01894
01895 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
01896 ros::this_node::getName().c_str(),
01897 imageRectLeft_.getTopic().c_str(),
01898 imageRectRight_.getTopic().c_str(),
01899 cameraInfoLeft_.getTopic().c_str(),
01900 cameraInfoRight_.getTopic().c_str(),
01901 scanSub_.getTopic().c_str());
01902 }
01903 else if(subscribeLaserScan3d)
01904 {
01905 scan3dSub_.subscribe(nh, "scan_cloud", 1);
01906 stereoScan3dTFSync_ = new message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy>(
01907 MyStereoScan3dTFSyncPolicy(queueSize),
01908 scan3dSub_,
01909 imageRectLeft_,
01910 imageRectRight_,
01911 cameraInfoLeft_,
01912 cameraInfoRight_);
01913 stereoScan3dTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dTFCallback, this, _1, _2, _3, _4, _5));
01914
01915 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
01916 ros::this_node::getName().c_str(),
01917 imageRectLeft_.getTopic().c_str(),
01918 imageRectRight_.getTopic().c_str(),
01919 cameraInfoLeft_.getTopic().c_str(),
01920 cameraInfoRight_.getTopic().c_str(),
01921 scan3dSub_.getTopic().c_str());
01922 }
01923 else if(subscribeOdomInfo)
01924 {
01925 odomInfoSub_.subscribe(nh, "odom_info", 1);
01926 stereoOdomInfoTFSync_ = new message_filters::Synchronizer<MyStereoOdomInfoTFSyncPolicy>(
01927 MyStereoOdomInfoTFSyncPolicy(queueSize),
01928 odomInfoSub_,
01929 imageRectLeft_,
01930 imageRectRight_,
01931 cameraInfoLeft_,
01932 cameraInfoRight_);
01933 stereoOdomInfoTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoOdomInfoTFCallback, this, _1, _2, _3, _4, _5));
01934
01935 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
01936 ros::this_node::getName().c_str(),
01937 imageRectLeft_.getTopic().c_str(),
01938 imageRectRight_.getTopic().c_str(),
01939 cameraInfoLeft_.getTopic().c_str(),
01940 cameraInfoRight_.getTopic().c_str(),
01941 odomInfoSub_.getTopic().c_str());
01942 }
01943 else
01944 {
01945 stereoTFSync_ = new message_filters::Synchronizer<MyStereoTFSyncPolicy>(
01946 MyStereoTFSyncPolicy(queueSize),
01947 imageRectLeft_,
01948 imageRectRight_,
01949 cameraInfoLeft_,
01950 cameraInfoRight_);
01951 stereoTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoTFCallback, this, _1, _2, _3, _4));
01952
01953 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
01954 ros::this_node::getName().c_str(),
01955 imageRectLeft_.getTopic().c_str(),
01956 imageRectRight_.getTopic().c_str(),
01957 cameraInfoLeft_.getTopic().c_str(),
01958 cameraInfoRight_.getTopic().c_str());
01959 }
01960 }
01961 }
01962 else
01963 {
01964 defaultSub_ = nh.subscribe("odom", 1, &GuiWrapper::defaultCallback, this);
01965
01966 ROS_INFO("\n%s subscribed to:\n %s",
01967 ros::this_node::getName().c_str(),
01968 defaultSub_.getTopic().c_str());
01969 }
01970 }
01971