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 <QtGui/QApplication>
00030 #include <QtCore/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
00040 #include <opencv2/highgui/highgui.hpp>
00041
00042 #include <image_geometry/pinhole_camera_model.h>
00043 #include <image_geometry/stereo_camera_model.h>
00044
00045 #include <rtabmap/gui/MainWindow.h>
00046 #include <rtabmap/core/RtabmapEvent.h>
00047 #include <rtabmap/core/Parameters.h>
00048 #include <rtabmap/core/ParamEvent.h>
00049 #include <rtabmap/core/OdometryEvent.h>
00050 #include <rtabmap/core/util3d.h>
00051 #include <rtabmap/utilite/UTimer.h>
00052
00053 #include "rtabmap_ros/MsgConversion.h"
00054 #include "rtabmap_ros/GetMap.h"
00055
00056 #include "PreferencesDialogROS.h"
00057
00058 #include <pcl_ros/transforms.h>
00059 #include <pcl_conversions/pcl_conversions.h>
00060 #include <laser_geometry/laser_geometry.h>
00061
00062 float max3( const float& a, const float& b, const float& c)
00063 {
00064 float m=a>b?a:b;
00065 return m>c?m:c;
00066 }
00067
00068 GuiWrapper::GuiWrapper(int & argc, char** argv) :
00069 app_(0),
00070 mainWindow_(0),
00071 frameId_("base_link"),
00072 waitForTransform_(false),
00073 cameraNodeName_(""),
00074 lastOdomInfoUpdateTime_(0),
00075 depthScanSync_(0),
00076 depthSync_(0),
00077 depthOdomInfoSync_(0),
00078 stereoSync_(0),
00079 stereoScanSync_(0),
00080 stereoOdomInfoSync_(0)
00081 {
00082 ros::NodeHandle nh;
00083 app_ = new QApplication(argc, argv);
00084
00085 QString configFile = QDir::homePath()+"/.ros/rtabmapGUI.ini";
00086 for(int i=1; i<argc; ++i)
00087 {
00088 if(strcmp(argv[i], "-d") == 0)
00089 {
00090 ++i;
00091 if(i < argc)
00092 {
00093 configFile = argv[i];
00094 }
00095 break;
00096 }
00097 }
00098
00099 configFile.replace('~', QDir::homePath());
00100
00101 ROS_INFO("rtabmapviz: Using configuration from \"%s\"", configFile.toStdString().c_str());
00102 uSleep(500);
00103 mainWindow_ = new MainWindow(new PreferencesDialogROS(configFile));
00104 mainWindow_->setWindowTitle(mainWindow_->windowTitle()+" [ROS]");
00105 mainWindow_->show();
00106 bool paused = false;
00107 nh.param("is_rtabmap_paused", paused, paused);
00108 mainWindow_->setMonitoringState(paused);
00109 app_->connect( app_, SIGNAL( lastWindowClosed() ), app_, SLOT( quit() ) );
00110
00111 ros::NodeHandle pnh("~");
00112
00113
00114 bool subscribeLaserScan = false;
00115 bool subscribeDepth = false;
00116 bool subscribeOdomInfo = false;
00117 bool subscribeStereo = false;
00118 int queueSize = 10;
00119 pnh.param("frame_id", frameId_, frameId_);
00120 pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
00121 pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
00122 pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
00123 pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
00124 pnh.param("queue_size", queueSize, queueSize);
00125 pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
00126 pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_);
00127 this->setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeOdomInfo, subscribeStereo, queueSize);
00128
00129 UEventsManager::addHandler(this);
00130 UEventsManager::addHandler(mainWindow_);
00131
00132 infoTopic_.subscribe(nh, "info", 1);
00133 mapDataTopic_.subscribe(nh, "mapData", 1);
00134 infoMapSync_ = new message_filters::Synchronizer<MyInfoMapSyncPolicy>(MyInfoMapSyncPolicy(queueSize), infoTopic_, mapDataTopic_);
00135 infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, _1, _2));
00136 }
00137
00138 GuiWrapper::~GuiWrapper()
00139 {
00140 if(depthSync_)
00141 {
00142 delete depthSync_;
00143 }
00144 if(depthScanSync_)
00145 {
00146 delete depthScanSync_;
00147 }
00148 if(depthOdomInfoSync_)
00149 {
00150 delete depthOdomInfoSync_;
00151 }
00152 if(stereoSync_)
00153 {
00154 delete stereoSync_;
00155 }
00156 if(stereoScanSync_)
00157 {
00158 delete stereoScanSync_;
00159 }
00160 if(stereoOdomInfoSync_)
00161 {
00162 delete stereoOdomInfoSync_;
00163 }
00164 delete infoMapSync_;
00165 delete mainWindow_;
00166 delete app_;
00167 }
00168
00169 int GuiWrapper::exec()
00170 {
00171 return app_->exec();
00172 }
00173
00174 void GuiWrapper::infoMapCallback(
00175 const rtabmap_ros::InfoConstPtr & infoMsg,
00176 const rtabmap_ros::MapDataConstPtr & mapMsg)
00177 {
00178
00179
00180
00181 rtabmap::Statistics stat;
00182
00183
00184 rtabmap_ros::infoFromROS(*infoMsg, stat);
00185
00186
00187 rtabmap::Transform mapToOdom;
00188 std::map<int, rtabmap::Transform> poses;
00189 std::map<int, int> mapIds;
00190 std::map<int, double> stamps;
00191 std::map<int, std::string> labels;
00192 std::map<int, std::vector<unsigned char> > userDatas;
00193 std::multimap<int, Link> links;
00194
00195 rtabmap_ros::mapGraphFromROS(mapMsg->graph, poses, mapIds, stamps, labels, userDatas, links, mapToOdom);
00196
00197 stat.setMapCorrection(mapToOdom);
00198 stat.setPoses(poses);
00199 stat.setMapIds(mapIds);
00200 stat.setStamps(stamps);
00201 stat.setLabels(labels);
00202 stat.setUserDatas(userDatas);
00203 stat.setConstraints(links);
00204
00205
00206 if(mapMsg->nodes.size() == 1)
00207 {
00208 stat.setSignature(rtabmap_ros::nodeDataFromROS(mapMsg->nodes[0]));
00209 }
00210 else if(mapMsg->nodes.size() > 1)
00211 {
00212 ROS_ERROR("rtabmapviz: nodes > 1 !?!?");
00213 }
00214
00215 this->post(new RtabmapEvent(stat));
00216 }
00217
00218 void GuiWrapper::processRequestedMap(const rtabmap_ros::MapData & map)
00219 {
00220 std::map<int, Signature> signatures;
00221 std::map<int, Transform> poses;
00222 std::multimap<int, rtabmap::Link> constraints;
00223 std::map<int, int> mapIds;
00224 std::map<int, double> stamps;
00225 std::map<int, std::string> labels;
00226 std::map<int, std::vector<unsigned char> > userDatas;
00227 Transform mapToOdom;
00228
00229 if(map.graph.nodeIds.size() != map.graph.mapIds.size())
00230 {
00231 ROS_ERROR("rtabmapviz: receiving map... node and amp IDs are not the same size (%d vs %d)!",
00232 (int)map.graph.nodeIds.size(), (int)map.graph.mapIds.size());
00233 return;
00234 }
00235
00236 if(map.graph.poses.size() && map.graph.nodeIds.size() != map.graph.poses.size())
00237 {
00238 ROS_ERROR("rtabmapviz: receiving map... poses and node IDs are not the same size (%d vs %d)!",
00239 (int)map.graph.poses.size(), (int)map.graph.nodeIds.size());
00240 return;
00241 }
00242
00243 rtabmap_ros::mapGraphFromROS(map.graph, poses, mapIds, stamps, labels, userDatas, constraints, mapToOdom);
00244
00245
00246 for(unsigned int i=0; i<map.nodes.size(); ++i)
00247 {
00248 signatures.insert(std::make_pair(map.nodes[i].id, rtabmap_ros::nodeDataFromROS(map.nodes[i])));
00249 }
00250
00251 RtabmapEvent3DMap e(signatures,
00252 poses,
00253 constraints,
00254 mapIds,
00255 stamps,
00256 labels,
00257 userDatas);
00258 QMetaObject::invokeMethod(mainWindow_, "processRtabmapEvent3DMap", Q_ARG(rtabmap::RtabmapEvent3DMap, e));
00259 }
00260
00261 void GuiWrapper::handleEvent(UEvent * anEvent)
00262 {
00263 if(anEvent->getClassName().compare("ParamEvent") == 0)
00264 {
00265 const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters();
00266 rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters();
00267 bool modified = false;
00268 ros::NodeHandle nh;
00269 for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
00270 {
00271
00272 if(defaultParameters.find((*i).first) != defaultParameters.end())
00273 {
00274 nh.setParam((*i).first, (*i).second);
00275 modified = true;
00276 }
00277 else if((*i).first.find('/') != (*i).first.npos)
00278 {
00279 ROS_WARN("Parameter %s is not used by the rtabmap node.", (*i).first.c_str());
00280 }
00281 }
00282 if(modified)
00283 {
00284 ROS_INFO("Parameters updated");
00285 std_srvs::Empty srv;
00286 if(!ros::service::call("update_parameters", srv))
00287 {
00288 ROS_ERROR("Can't call \"update_parameters\" service");
00289 }
00290 }
00291 }
00292 else if(anEvent->getClassName().compare("RtabmapEventCmd") == 0)
00293 {
00294 std_srvs::Empty emptySrv;
00295 rtabmap::RtabmapEventCmd * cmdEvent = (rtabmap::RtabmapEventCmd *)anEvent;
00296 rtabmap::RtabmapEventCmd::Cmd cmd = cmdEvent->getCmd();
00297 if(cmd == rtabmap::RtabmapEventCmd::kCmdResetMemory)
00298 {
00299 if(!ros::service::call("reset", emptySrv))
00300 {
00301 ROS_ERROR("Can't call \"reset\" service");
00302 }
00303 }
00304 else if(cmd == rtabmap::RtabmapEventCmd::kCmdPause)
00305 {
00306 if(cmdEvent->getInt())
00307 {
00308
00309 if(!cameraNodeName_.empty())
00310 {
00311 std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause true", cameraNodeName_.c_str());
00312 system(str.c_str());
00313 }
00314
00315
00316 ros::service::call("pause_odom", emptySrv);
00317
00318
00319 if(!ros::service::call("pause", emptySrv))
00320 {
00321 ROS_ERROR("Can't call \"pause\" service");
00322 }
00323 }
00324 else
00325 {
00326
00327 if(!ros::service::call("resume", emptySrv))
00328 {
00329 ROS_ERROR("Can't call \"resume\" service");
00330 }
00331
00332
00333 ros::service::call("resume_odom", emptySrv);
00334
00335
00336 if(!cameraNodeName_.empty())
00337 {
00338 std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause false", cameraNodeName_.c_str());
00339 system(str.c_str());
00340 }
00341 }
00342 }
00343 else if(cmd == rtabmap::RtabmapEventCmd::kCmdTriggerNewMap)
00344 {
00345 if(!ros::service::call("trigger_new_map", emptySrv))
00346 {
00347 ROS_ERROR("Can't call \"trigger_new_map\" service");
00348 }
00349 }
00350 else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapLocal ||
00351 cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapGlobal ||
00352 cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphLocal ||
00353 cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphGlobal)
00354 {
00355 rtabmap_ros::GetMap getMapSrv;
00356 getMapSrv.request.global = cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapGlobal || cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphGlobal;
00357 getMapSrv.request.optimized = cmdEvent->getInt();
00358 getMapSrv.request.graphOnly = cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphGlobal || cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphLocal;
00359 if(!ros::service::call("get_map", getMapSrv))
00360 {
00361 ROS_WARN("Can't call \"get_map\" service");
00362 this->post(new RtabmapEvent3DMap(1));
00363 }
00364 else
00365 {
00366 processRequestedMap(getMapSrv.response.data);
00367 }
00368 }
00369 else
00370 {
00371 ROS_WARN("Not handled command (%d)...", cmd);
00372 }
00373 }
00374 else if(anEvent->getClassName().compare("OdometryResetEvent") == 0)
00375 {
00376 std_srvs::Empty srv;
00377 if(!ros::service::call("reset_odom", srv))
00378 {
00379 ROS_ERROR("Can't call \"reset_odom\" service, (will only work with rtabmap/visual_odometry node.)");
00380 }
00381 }
00382 }
00383
00384 void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
00385 {
00386 if(!mainWindow_->isProcessingOdometry() && !mainWindow_->isProcessingStatistics())
00387 {
00388 Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
00389 rtabmap::SensorData data(cv::Mat(), odomMsg->header.seq);
00390 float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
00391 float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
00392 data.setPose(odom,
00393 uIsFinite(rotVariance) && rotVariance>0?rotVariance:1,
00394 uIsFinite(transVariance) && transVariance>0?transVariance:1);
00395
00396 rtabmap::OdometryInfo info;
00397 QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::SensorData, data), Q_ARG(rtabmap::OdometryInfo, info));
00398 }
00399 }
00400
00401 void GuiWrapper::depthCallback(
00402 const sensor_msgs::ImageConstPtr& imageMsg,
00403 const nav_msgs::OdometryConstPtr & odomMsg,
00404 const sensor_msgs::ImageConstPtr& depthMsg,
00405 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
00406 {
00407
00408 if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
00409 !mainWindow_->isProcessingOdometry() &&
00410 !mainWindow_->isProcessingStatistics())
00411 {
00412 lastOdomInfoUpdateTime_ = UTimer::now();
00413
00414
00415 Transform localTransform;
00416 try
00417 {
00418 if(waitForTransform_)
00419 {
00420 if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
00421 {
00422 ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
00423 return;
00424 }
00425 }
00426
00427 tf::StampedTransform tmp;
00428 tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
00429 localTransform = rtabmap_ros::transformFromTF(tmp);
00430 }
00431 catch(tf::TransformException & ex)
00432 {
00433 ROS_WARN("%s",ex.what());
00434 return;
00435 }
00436
00437 cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
00438 cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
00439
00440 image_geometry::PinholeCameraModel model;
00441 model.fromCameraInfo(*cameraInfoMsg);
00442 float fx = model.fx();
00443 float fy = model.fy();
00444 float cx = model.cx();
00445 float cy = model.cy();
00446
00447 float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
00448 float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
00449
00450 rtabmap::SensorData image(
00451 ptrImage->image.clone(),
00452 ptrDepth->image.clone(),
00453 fx,
00454 fy,
00455 cx,
00456 cy,
00457 localTransform,
00458 rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
00459 uIsFinite(rotVariance) && rotVariance>0?rotVariance:1,
00460 uIsFinite(transVariance) && transVariance>0?transVariance:1,
00461 odomMsg->header.seq,
00462 rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
00463
00464 rtabmap::OdometryInfo info;
00465 QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::SensorData, image), Q_ARG(rtabmap::OdometryInfo, info));
00466 }
00467 }
00468
00469 void GuiWrapper::depthOdomInfoCallback(
00470 const sensor_msgs::ImageConstPtr& imageMsg,
00471 const nav_msgs::OdometryConstPtr & odomMsg,
00472 const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
00473 const sensor_msgs::ImageConstPtr& depthMsg,
00474 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
00475 {
00476
00477 if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
00478 !mainWindow_->isProcessingOdometry() &&
00479 !mainWindow_->isProcessingStatistics())
00480 {
00481 lastOdomInfoUpdateTime_ = UTimer::now();
00482
00483 Transform localTransform;
00484 try
00485 {
00486 if(waitForTransform_)
00487 {
00488 if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
00489 {
00490 ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
00491 return;
00492 }
00493 }
00494
00495 tf::StampedTransform tmp;
00496 tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
00497 localTransform = rtabmap_ros::transformFromTF(tmp);
00498 }
00499 catch(tf::TransformException & ex)
00500 {
00501 ROS_WARN("%s",ex.what());
00502 return;
00503 }
00504
00505 cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
00506 cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
00507
00508 image_geometry::PinholeCameraModel model;
00509 model.fromCameraInfo(*cameraInfoMsg);
00510 float fx = model.fx();
00511 float fy = model.fy();
00512 float cx = model.cx();
00513 float cy = model.cy();
00514
00515 float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
00516 float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
00517
00518 rtabmap::SensorData image(
00519 ptrImage->image.clone(),
00520 ptrDepth->image.clone(),
00521 fx,
00522 fy,
00523 cx,
00524 cy,
00525 localTransform,
00526 rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
00527 uIsFinite(rotVariance) && rotVariance>0?rotVariance:1,
00528 uIsFinite(transVariance) && transVariance>0?transVariance:1,
00529 odomMsg->header.seq,
00530 rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
00531
00532 OdometryInfo info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
00533 QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::SensorData, image), Q_ARG(rtabmap::OdometryInfo, info));
00534 }
00535 }
00536
00537 void GuiWrapper::depthScanCallback(
00538 const sensor_msgs::ImageConstPtr& imageMsg,
00539 const nav_msgs::OdometryConstPtr & odomMsg,
00540 const sensor_msgs::ImageConstPtr& depthMsg,
00541 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00542 const sensor_msgs::LaserScanConstPtr& scanMsg)
00543 {
00544
00545 if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
00546 !mainWindow_->isProcessingOdometry() &&
00547 !mainWindow_->isProcessingStatistics())
00548 {
00549 lastOdomInfoUpdateTime_ = UTimer::now();
00550
00551 Transform localTransform;
00552 sensor_msgs::PointCloud2 scanOut;
00553 try
00554 {
00555
00556 laser_geometry::LaserProjection projection;
00557 projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
00558
00559 if(waitForTransform_)
00560 {
00561 if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
00562 {
00563 ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
00564 return;
00565 }
00566 }
00567
00568 tf::StampedTransform tmp;
00569 tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
00570 localTransform = rtabmap_ros::transformFromTF(tmp);
00571 }
00572 catch(tf::TransformException & ex)
00573 {
00574 ROS_WARN("%s",ex.what());
00575 return;
00576 }
00577
00578 pcl::PointCloud<pcl::PointXYZ> pclScan;
00579 pcl::fromROSMsg(scanOut, pclScan);
00580 cv::Mat scan = util3d::laserScanFromPointCloud(pclScan);
00581
00582 cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
00583 cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
00584
00585 image_geometry::PinholeCameraModel model;
00586 model.fromCameraInfo(*cameraInfoMsg);
00587 float fx = model.fx();
00588 float fy = model.fy();
00589 float cx = model.cx();
00590 float cy = model.cy();
00591
00592 float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
00593 float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
00594
00595 rtabmap::SensorData image(
00596 scan,
00597 (int)scanMsg->ranges.size(),
00598 ptrImage->image.clone(),
00599 ptrDepth->image.clone(),
00600 fx,
00601 fy,
00602 cx,
00603 cy,
00604 localTransform,
00605 rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
00606 uIsFinite(rotVariance) && rotVariance>0?rotVariance:1,
00607 uIsFinite(transVariance) && transVariance>0?transVariance:1,
00608 odomMsg->header.seq,
00609 rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
00610
00611 rtabmap::OdometryInfo info;
00612 QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::SensorData, image), Q_ARG(rtabmap::OdometryInfo, info));
00613 }
00614 }
00615
00616 void GuiWrapper::stereoScanCallback(
00617 const nav_msgs::OdometryConstPtr & odomMsg,
00618 const sensor_msgs::LaserScanConstPtr& scanMsg,
00619 const sensor_msgs::ImageConstPtr& leftImageMsg,
00620 const sensor_msgs::ImageConstPtr& rightImageMsg,
00621 const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
00622 const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
00623 {
00624
00625 if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
00626 !mainWindow_->isProcessingOdometry() &&
00627 !mainWindow_->isProcessingStatistics())
00628 {
00629 lastOdomInfoUpdateTime_ = UTimer::now();
00630 if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00631 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
00632 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00633 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
00634 !(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00635 rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
00636 rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00637 rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
00638 {
00639 ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
00640 return;
00641 }
00642
00643
00644 Transform localTransform;
00645 sensor_msgs::PointCloud2 scanOut;
00646 try
00647 {
00648
00649 laser_geometry::LaserProjection projection;
00650 projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
00651
00652 if(waitForTransform_)
00653 {
00654 if(!tfListener_.waitForTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, ros::Duration(1)))
00655 {
00656 ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), leftImageMsg->header.frame_id.c_str());
00657 return;
00658 }
00659 }
00660
00661 tf::StampedTransform tmp;
00662 tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
00663 localTransform = rtabmap_ros::transformFromTF(tmp);
00664 }
00665 catch(tf::TransformException & ex)
00666 {
00667 ROS_WARN("%s",ex.what());
00668 return;
00669 }
00670
00671 pcl::PointCloud<pcl::PointXYZ> pclScan;
00672 pcl::fromROSMsg(scanOut, pclScan);
00673 cv::Mat scan = util3d::laserScanFromPointCloud(pclScan);
00674
00675 cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
00676 if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00677 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
00678 {
00679 ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "mono8");
00680 }
00681 else
00682 {
00683 ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
00684 }
00685 ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
00686
00687 image_geometry::StereoCameraModel model;
00688 model.fromCameraInfo(*leftCameraInfoMsg, *rightCameraInfoMsg);
00689
00690 float fx = model.left().fx();
00691 float cx = model.left().cx();
00692 float cy = model.left().cy();
00693 float baseline = model.baseline();
00694
00695 float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
00696 float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
00697
00698 rtabmap::SensorData image(
00699 scan,
00700 (int)scanMsg->ranges.size(),
00701 ptrLeftImage->image.clone(),
00702 ptrRightImage->image.clone(),
00703 fx,
00704 baseline,
00705 cx,
00706 cy,
00707 localTransform,
00708 rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
00709 uIsFinite(rotVariance) && rotVariance>0?rotVariance:1,
00710 uIsFinite(transVariance) && transVariance>0?transVariance:1,
00711 odomMsg->header.seq,
00712 rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
00713
00714 rtabmap::OdometryInfo info;
00715 QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::SensorData, image), Q_ARG(rtabmap::OdometryInfo, info));
00716 }
00717 }
00718
00719 void GuiWrapper::stereoOdomInfoCallback(
00720 const nav_msgs::OdometryConstPtr & odomMsg,
00721 const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
00722 const sensor_msgs::ImageConstPtr& leftImageMsg,
00723 const sensor_msgs::ImageConstPtr& rightImageMsg,
00724 const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
00725 const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
00726 {
00727
00728 if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
00729 !mainWindow_->isProcessingOdometry() &&
00730 !mainWindow_->isProcessingStatistics())
00731 {
00732 lastOdomInfoUpdateTime_ = UTimer::now();
00733 if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00734 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
00735 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00736 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
00737 !(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00738 rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
00739 rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00740 rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
00741 {
00742 ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
00743 return;
00744 }
00745
00746
00747 Transform localTransform;
00748 try
00749 {
00750 if(waitForTransform_)
00751 {
00752 if(!tfListener_.waitForTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, ros::Duration(1)))
00753 {
00754 ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), leftImageMsg->header.frame_id.c_str());
00755 return;
00756 }
00757 }
00758
00759 tf::StampedTransform tmp;
00760 tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
00761 localTransform = rtabmap_ros::transformFromTF(tmp);
00762 }
00763 catch(tf::TransformException & ex)
00764 {
00765 ROS_WARN("%s",ex.what());
00766 return;
00767 }
00768
00769 cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
00770 if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00771 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
00772 {
00773 ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "mono8");
00774 }
00775 else
00776 {
00777 ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
00778 }
00779 ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
00780
00781 image_geometry::StereoCameraModel model;
00782 model.fromCameraInfo(*leftCameraInfoMsg, *rightCameraInfoMsg);
00783
00784 float fx = model.left().fx();
00785 float cx = model.left().cx();
00786 float cy = model.left().cy();
00787 float baseline = model.baseline();
00788
00789 float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
00790 float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
00791
00792 rtabmap::SensorData image(
00793 ptrLeftImage->image.clone(),
00794 ptrRightImage->image.clone(),
00795 fx,
00796 baseline,
00797 cx,
00798 cy,
00799 localTransform,
00800 rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
00801 uIsFinite(rotVariance) && rotVariance>0?rotVariance:1,
00802 uIsFinite(transVariance) && transVariance>0?transVariance:1,
00803 odomMsg->header.seq,
00804 rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
00805
00806 OdometryInfo info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
00807 QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::SensorData, image), Q_ARG(rtabmap::OdometryInfo, info));
00808 }
00809 }
00810
00811 void GuiWrapper::stereoCallback(
00812 const nav_msgs::OdometryConstPtr & odomMsg,
00813 const sensor_msgs::ImageConstPtr& leftImageMsg,
00814 const sensor_msgs::ImageConstPtr& rightImageMsg,
00815 const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
00816 const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
00817 {
00818
00819 if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
00820 !mainWindow_->isProcessingOdometry() &&
00821 !mainWindow_->isProcessingStatistics())
00822 {
00823 lastOdomInfoUpdateTime_ = UTimer::now();
00824 if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00825 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
00826 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00827 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
00828 !(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00829 rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
00830 rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00831 rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
00832 {
00833 ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
00834 return;
00835 }
00836
00837
00838 Transform localTransform;
00839 try
00840 {
00841 if(waitForTransform_)
00842 {
00843 if(!tfListener_.waitForTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, ros::Duration(1)))
00844 {
00845 ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), leftImageMsg->header.frame_id.c_str());
00846 return;
00847 }
00848 }
00849
00850 tf::StampedTransform tmp;
00851 tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
00852 localTransform = rtabmap_ros::transformFromTF(tmp);
00853 }
00854 catch(tf::TransformException & ex)
00855 {
00856 ROS_WARN("%s",ex.what());
00857 return;
00858 }
00859
00860 cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
00861 if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00862 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
00863 {
00864 ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "mono8");
00865 }
00866 else
00867 {
00868 ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
00869 }
00870 ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
00871
00872 image_geometry::StereoCameraModel model;
00873 model.fromCameraInfo(*leftCameraInfoMsg, *rightCameraInfoMsg);
00874
00875 float fx = model.left().fx();
00876 float cx = model.left().cx();
00877 float cy = model.left().cy();
00878 float baseline = model.baseline();
00879
00880 float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
00881 float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
00882
00883 rtabmap::SensorData image(
00884 ptrLeftImage->image.clone(),
00885 ptrRightImage->image.clone(),
00886 fx,
00887 baseline,
00888 cx,
00889 cy,
00890 localTransform,
00891 rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
00892 uIsFinite(rotVariance) && rotVariance>0?rotVariance:1,
00893 uIsFinite(transVariance) && transVariance>0?transVariance:1,
00894 odomMsg->header.seq,
00895 rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
00896
00897 rtabmap::OdometryInfo info;
00898 QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::SensorData, image), Q_ARG(rtabmap::OdometryInfo, info));
00899 }
00900 }
00901
00902 void GuiWrapper::setupCallbacks(
00903 bool subscribeDepth,
00904 bool subscribeLaserScan,
00905 bool subscribeOdomInfo,
00906 bool subscribeStereo,
00907 int queueSize)
00908 {
00909 ros::NodeHandle nh;
00910 ros::NodeHandle pnh("~");
00911
00912 if(subscribeDepth && subscribeStereo)
00913 {
00914 ROS_WARN("\"subscribe_depth\" already true, ignoring \"subscribe_stereo\".");
00915 }
00916 if(!subscribeDepth && !subscribeStereo && subscribeLaserScan)
00917 {
00918 ROS_WARN("Cannot subscribe to laser scan without depth or stereo subscription...");
00919 }
00920
00921 if(subscribeDepth)
00922 {
00923 ros::NodeHandle rgb_nh(nh, "rgb");
00924 ros::NodeHandle depth_nh(nh, "depth");
00925 ros::NodeHandle rgb_pnh(pnh, "rgb");
00926 ros::NodeHandle depth_pnh(pnh, "depth");
00927 image_transport::ImageTransport rgb_it(rgb_nh);
00928 image_transport::ImageTransport depth_it(depth_nh);
00929 image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
00930 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
00931
00932 odomSub_.subscribe(nh, "odom", 1);
00933 imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
00934 imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
00935 cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
00936
00937 if(subscribeLaserScan)
00938 {
00939 scanSub_.subscribe(nh, "scan", 1);
00940 depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
00941 depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
00942
00943 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
00944 ros::this_node::getName().c_str(),
00945 imageSub_.getTopic().c_str(),
00946 imageDepthSub_.getTopic().c_str(),
00947 cameraInfoSub_.getTopic().c_str(),
00948 odomSub_.getTopic().c_str(),
00949 scanSub_.getTopic().c_str());
00950 }
00951 else if(subscribeOdomInfo)
00952 {
00953 odomInfoSub_.subscribe(nh, "odom_info", 1);
00954 depthOdomInfoSync_ = new message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy>(MyDepthOdomInfoSyncPolicy(queueSize), imageSub_, odomSub_, odomInfoSub_, imageDepthSub_, cameraInfoSub_);
00955 depthOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoCallback, this, _1, _2, _3, _4, _5));
00956
00957 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
00958 ros::this_node::getName().c_str(),
00959 imageSub_.getTopic().c_str(),
00960 imageDepthSub_.getTopic().c_str(),
00961 cameraInfoSub_.getTopic().c_str(),
00962 odomSub_.getTopic().c_str(),
00963 odomInfoSub_.getTopic().c_str());
00964 }
00965 else
00966 {
00967 depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
00968 depthSync_->registerCallback(boost::bind(&GuiWrapper::depthCallback, this, _1, _2, _3, _4));
00969
00970 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
00971 ros::this_node::getName().c_str(),
00972 imageSub_.getTopic().c_str(),
00973 imageDepthSub_.getTopic().c_str(),
00974 cameraInfoSub_.getTopic().c_str(),
00975 odomSub_.getTopic().c_str());
00976 }
00977 }
00978 else if(subscribeStereo)
00979 {
00980 ros::NodeHandle left_nh(nh, "left");
00981 ros::NodeHandle right_nh(nh, "right");
00982 ros::NodeHandle left_pnh(pnh, "left");
00983 ros::NodeHandle right_pnh(pnh, "right");
00984 image_transport::ImageTransport left_it(left_nh);
00985 image_transport::ImageTransport right_it(right_nh);
00986 image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
00987 image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
00988
00989 imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
00990 imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
00991 cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
00992 cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
00993 odomSub_.subscribe(nh, "odom", 1);
00994
00995 if(subscribeLaserScan)
00996 {
00997 scanSub_.subscribe(nh, "scan", 1);
00998 stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(MyStereoScanSyncPolicy(queueSize), odomSub_, scanSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
00999 stereoScanSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6));
01000
01001 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
01002 ros::this_node::getName().c_str(),
01003 imageRectLeft_.getTopic().c_str(),
01004 imageRectRight_.getTopic().c_str(),
01005 cameraInfoLeft_.getTopic().c_str(),
01006 cameraInfoRight_.getTopic().c_str(),
01007 odomSub_.getTopic().c_str(),
01008 scanSub_.getTopic().c_str());
01009 }
01010 else if(subscribeOdomInfo)
01011 {
01012 odomInfoSub_.subscribe(nh, "odom_info", 1);
01013 stereoOdomInfoSync_ = new message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy>(MyStereoOdomInfoSyncPolicy(queueSize), odomSub_, odomInfoSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
01014 stereoOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoOdomInfoCallback, this, _1, _2, _3, _4, _5, _6));
01015
01016 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
01017 ros::this_node::getName().c_str(),
01018 imageRectLeft_.getTopic().c_str(),
01019 imageRectRight_.getTopic().c_str(),
01020 cameraInfoLeft_.getTopic().c_str(),
01021 cameraInfoRight_.getTopic().c_str(),
01022 odomSub_.getTopic().c_str(),
01023 odomInfoSub_.getTopic().c_str());
01024 }
01025 else
01026 {
01027 stereoSync_ = new message_filters::Synchronizer<MyStereoSyncPolicy>(MyStereoSyncPolicy(queueSize), odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
01028 stereoSync_->registerCallback(boost::bind(&GuiWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
01029
01030 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
01031 ros::this_node::getName().c_str(),
01032 imageRectLeft_.getTopic().c_str(),
01033 imageRectRight_.getTopic().c_str(),
01034 cameraInfoLeft_.getTopic().c_str(),
01035 cameraInfoRight_.getTopic().c_str(),
01036 odomSub_.getTopic().c_str());
01037 }
01038 }
01039 else
01040 {
01041 defaultSub_ = nh.subscribe("odom", 1, &GuiWrapper::defaultCallback, this);
01042
01043 ROS_INFO("\n%s subscribed to:\n %s",
01044 ros::this_node::getName().c_str(),
01045 odomSub_.getTopic().c_str());
01046 }
01047 }
01048