Increased version to 0.10.10. Database: added user_data field for links. DatabaseViewer: showing all scans of a local loop closure. Added parameters RGBD/PlanLinearVelocity and RGBD/PlanAngularVelocity. Updated how variance is set on links. MainWindow: added Send Waypoints action and goal can be either an ID or a label.

guilib/include/rtabmap/gui/MainWindow.h

@@ -149,6 +149,8 @@ private slots:
 	void dumpTheMemory();
 	void dumpThePrediction();
 	void sendGoal();
+	void sendWaypoints();
+	void postGoal(const QString & goal);
 	void cancelGoal();
 	void label();
 	void downloadAllClouds();
@@ -167,6 +169,7 @@ private slots:
 	void processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & event);
 	void processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event);
 	void processRtabmapLabelErrorEvent(int id, const QString & label);
+	void processRtabmapGoalStatusEvent(int status);
 	void changeImgRateSetting();
 	void changeDetectionRateSetting();
 	void changeTimeLimitSetting();
@@ -202,6 +205,7 @@ signals:
 	void rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap & event);
 	void rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent & event);
 	void rtabmapLabelErrorReceived(int id, const QString & label);
+	void rtabmapGoalStatusEventReceived(int status);
 	void imgRateChanged(double);
 	void detectionRateChanged(double);
 	void timeLimitChanged(float);
@@ -277,6 +281,8 @@ private:
 	bool _odomImageShow;
 	bool _odomImageDepthShow;
 	bool _savedMaximized;
+	QStringList _waypoints;
+	int _waypointsIndex;
 
 	QMap<int, Signature> _cachedSignatures;
 	std::map<int, Transform> _currentPosesMap; // <nodeId, pose>

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -186,6 +186,7 @@ public:
 	QString getSourceDatabasePath() const; //Database group
 	bool getSourceDatabaseOdometryIgnored() const; //Database group
 	bool getSourceDatabaseGoalDelayIgnored() const; //Database group
+	bool getSourceDatabaseGoalsIgnored() const; //Database group
 	int getSourceDatabaseStartPos() const; //Database group
 	bool getSourceDatabaseStampsUsed() const;//Database group
 	bool isSourceRGBDColorOnly() const;

guilib/src/DatabaseViewer.cpp

@@ -2221,11 +2221,11 @@ void DatabaseViewer::updateConstraintView(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanFrom,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanTo)
 {
-	std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
+	std::multimap<int, Link>::iterator iterLink = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
 	rtabmap::Link link = linkIn;
-	if(iter != linksRefined_.end())
+	if(iterLink != linksRefined_.end())
 	{
-		link = iter->second;
+		link = iterLink->second;
 	}
 	rtabmap::Transform t = link.transform();
 
@@ -2428,6 +2428,141 @@ void DatabaseViewer::updateConstraintView(
 			if(ui_->checkBox_show2DScans->isChecked())
 			{
 				//cloud 2d
+
+				ui_->constraintsViewer->removeCloud("scan2");
+				ui_->constraintsViewer->removeGraph("scan2graph");
+				if(link.type() == Link::kLocalSpaceClosure && !link.userDataCompressed().empty())
+				{
+					std::vector<int> ids;
+					cv::Mat userData = link.uncompressUserDataConst();
+					if(userData.type() == CV_8SC1 &&
+					   userData.rows == 1 &&
+					   userData.cols >= 8 && // including null str ending
+					   userData.at<char>(userData.cols-1) == 0 &&
+					   memcmp(userData.data, "SCANS:", 6) == 0)
+					{
+						std::string scansStr = (const char *)userData.data;
+						if(!scansStr.empty())
+						{
+							std::list<std::string> strs = uSplit(scansStr, ':');
+							if(strs.size() == 2)
+							{
+								std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
+								for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
+								{
+									ids.push_back(atoi(iter->c_str()));
+									if(ids.back() == link.from())
+									{
+										ids.pop_back();
+									}
+								}
+							}
+						}
+					}
+					if(ids.size())
+					{
+						//add other scans matching
+						//optimize the path's poses locally
+
+						graph::Optimizer * optimizer = 0;
+						if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeGTSAM)
+						{
+							optimizer = new graph::GTSAMOptimizer(
+									ui_->spinBox_iterations->value(),
+									ui_->checkBox_2dslam->isChecked(),
+									ui_->checkBox_ignoreCovariance->isChecked(),
+									0.0,
+									ui_->checkBox_robust->isChecked());
+						}
+						else if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeG2O)
+						{
+							UINFO("ui_->checkBox_robust->isChecked()=%d", ui_->checkBox_robust->isChecked()?1:0);
+							optimizer = new graph::G2OOptimizer(
+									ui_->spinBox_iterations->value(),
+									ui_->checkBox_2dslam->isChecked(),
+									ui_->checkBox_ignoreCovariance->isChecked(),
+									0.0,
+									ui_->checkBox_robust->isChecked());
+						}
+						else
+						{
+							optimizer = new graph::TOROOptimizer(
+									ui_->spinBox_iterations->value(),
+									ui_->checkBox_2dslam->isChecked(),
+									ui_->checkBox_ignoreCovariance->isChecked(),
+									0.0);
+						}
+						std::map<int, rtabmap::Transform> poses;
+						for(unsigned int i=0; i<ids.size(); ++i)
+						{
+							if(uContains(poses_, ids[i]))
+							{
+								poses.insert(*poses_.find(ids[i]));
+							}
+							else
+							{
+								UERROR("Not found %d node!", ids[i]);
+							}
+						}
+						if(poses.size())
+						{
+							UASSERT(uContains(poses, link.to()));
+							std::map<int, rtabmap::Transform> posesOut;
+							std::multimap<int, rtabmap::Link> linksOut;
+							optimizer->getConnectedGraph(
+									link.to(),
+									poses,
+									updateLinksWithModifications(links_),
+									posesOut,
+									linksOut);
+
+							QTime time;
+							time.start();
+							std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(link.to(), posesOut, linksOut);
+							delete optimizer;
+
+							// transform local poses in loop referential
+							Transform u = t * finalPoses.at(link.to()).inverse();
+							pcl::PointCloud<pcl::PointXYZ>::Ptr assembledScans(new pcl::PointCloud<pcl::PointXYZ>);
+							pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
+							for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
+							{
+								iter->second = u * iter->second;
+								if(iter->first != link.to()) // already added to view
+								{
+									//create scan
+									SensorData data = memory_->getNodeData(iter->first, false);
+									cv::Mat scan;
+									data.uncompressDataConst(0, 0, &scan, 0);
+									if(!scan.empty())
+									{
+										pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan);
+										if(assembledScans->size() == 0)
+										{
+											assembledScans = util3d::transformPointCloud(scanCloud, iter->second);
+										}
+										else
+										{
+											*assembledScans += *util3d::transformPointCloud(scanCloud, iter->second);
+										}
+									}
+								}
+								graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
+							}
+
+							if(assembledScans->size())
+							{
+								ui_->constraintsViewer->addOrUpdateCloud("scan2", assembledScans, Transform::getIdentity(), Qt::cyan);
+							}
+							if(graph->size())
+							{
+								ui_->constraintsViewer->addOrUpdateGraph("scan2graph", graph, Qt::cyan);
+							}
+						}
+					}
+				}
+
+				// Added loop closure scans
 				pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
 				scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw());
 				scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw());
@@ -2453,6 +2588,7 @@ void DatabaseViewer::updateConstraintView(
 			{
 				ui_->constraintsViewer->removeCloud("scan0");
 				ui_->constraintsViewer->removeCloud("scan1");
+				ui_->constraintsViewer->removeCloud("scan2");
 			}
 		}
 		else
@@ -2473,6 +2609,7 @@ void DatabaseViewer::updateConstraintView(
 			{
 				ui_->constraintsViewer->removeCloud("scan1");
 			}
+			ui_->constraintsViewer->removeCloud("scan2");
 		}
 
 		//update coordinate

guilib/src/MainWindow.cpp

@@ -136,6 +136,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_databaseUpdated(false),
 	_odomImageShow(true),
 	_odomImageDepthShow(false),
+	_savedMaximized(false),
+	_waypointsIndex(0),
 	_odometryCorrection(Transform::getIdentity()),
 	_processingOdometry(false),
 	_lastOdomInfoUpdateTime(0),
@@ -255,6 +257,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	qRegisterMetaType<rtabmap::RtabmapGlobalPathEvent>("rtabmap::RtabmapGlobalPathEvent");
 	connect(this, SIGNAL(rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent &)), this, SLOT(processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent &)));
 	connect(this, SIGNAL(rtabmapLabelErrorReceived(int, const QString &)), this, SLOT(processRtabmapLabelErrorEvent(int, const QString &)));
+	connect(this, SIGNAL(rtabmapGoalStatusEventReceived(int)), this, SLOT(processRtabmapGoalStatusEvent(int)));
+
 	// Dock Widget view actions (Menu->Window)
 	_ui->menuShow_view->addAction(_ui->dockWidget_imageView->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->dockWidget_posterior->toggleViewAction());
@@ -291,6 +295,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionDump_the_memory, SIGNAL(triggered()), this, SLOT(dumpTheMemory()));
 	connect(_ui->actionDump_the_prediction_matrix, SIGNAL(triggered()), this, SLOT(dumpThePrediction()));
 	connect(_ui->actionSend_goal, SIGNAL(triggered()), this, SLOT(sendGoal()));
+	connect(_ui->actionSend_waypoints, SIGNAL(triggered()), this, SLOT(sendWaypoints()));
 	connect(_ui->actionCancel_goal, SIGNAL(triggered()), this, SLOT(cancelGoal()));
 	connect(_ui->actionLabel_current_location, SIGNAL(triggered()), this, SLOT(label()));
 	connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache()));
@@ -662,6 +667,10 @@ void MainWindow::handleEvent(UEvent* anEvent)
 		RtabmapLabelErrorEvent * rtabmapLabelErrorEvent = (RtabmapLabelErrorEvent*)anEvent;
 		emit rtabmapLabelErrorReceived(rtabmapLabelErrorEvent->id(), QString(rtabmapLabelErrorEvent->label().c_str()));
 	}
+	else if(anEvent->getClassName().compare("RtabmapGoalStatusEvent") == 0)
+	{
+		emit rtabmapGoalStatusEventReceived(anEvent->getCode());
+	}
 	else if(anEvent->getClassName().compare("CameraEvent") == 0)
 	{
 		CameraEvent * cameraEvent = (CameraEvent*)anEvent;
@@ -2227,6 +2236,15 @@ void MainWindow::processRtabmapLabelErrorEvent(int id, const QString & label)
 	warn->show();
 }
 
+void MainWindow::processRtabmapGoalStatusEvent(int status)
+{
+	_ui->widget_console->appendMsg(tr("Goal status received=%1").arg(status), ULogger::kInfo);
+	if(_waypoints.size())
+	{
+		this->postGoal(_waypoints.at(++_waypointsIndex % _waypoints.size()));
+	}
+}
+
 void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 {
 	ULOGGER_DEBUG("");
@@ -2944,7 +2962,8 @@ void MainWindow::startDetection()
 		_dbReader = new DBReader(_preferencesDialog->getSourceDatabasePath().toStdString(),
 								 _preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate(),
 								 _preferencesDialog->getSourceDatabaseOdometryIgnored(),
-								 _preferencesDialog->getSourceDatabaseGoalDelayIgnored());
+								 _preferencesDialog->getSourceDatabaseGoalDelayIgnored(),
+								 _preferencesDialog->getSourceDatabaseGoalsIgnored());
 
 		//Create odometry thread if rgdb slam
 		if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()) &&
@@ -3902,18 +3921,61 @@ void MainWindow::sendGoal()
 {
 	UINFO("Sending a goal...");
 	bool ok = false;
-	int id = QInputDialog::getInt(this, tr("Send a goal"), tr("Goal location ID: "), 1, 1, 99999, 1, &ok);
-	if(ok)
+	QString text = QInputDialog::getText(this, tr("Send a goal"), tr("Goal location ID or label: "), QLineEdit::Normal, "", &ok);
+	if(ok && !text.isEmpty())
 	{
+		_waypoints.clear();
+		_waypointsIndex = 0;
+
+		this->postGoal(text);
+	}
+}
+
+void MainWindow::sendWaypoints()
+{
+	UINFO("Sending waypoints...");
+	bool ok = false;
+	QString text = QInputDialog::getText(this, tr("Send waypoints"), tr("Waypoint IDs or labels (separated by spaces): "), QLineEdit::Normal, "", &ok);
+	if(ok && !text.isEmpty())
+	{
+		QStringList wp = text.split(' ');
+		if(wp.size() < 2)
+		{
+			QMessageBox::warning(this, tr("Send waypoints"), tr("At least two waypoints should be set. For only one goal, use send goal action."));
+		}
+		else
+		{
+			_waypoints = wp;
+			_waypointsIndex = 0;
+			this->postGoal(_waypoints.at(_waypointsIndex));
+		}
+	}
+}
+
+void MainWindow::postGoal(const QString & goal)
+{
+	if(!goal.isEmpty())
+	{
+		bool ok = false;
+		int id = goal.toInt(&ok);
 		_ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >()); // clear
-		UINFO("Posting event with goal %d", id);
-		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, id));
+		UINFO("Posting event with goal %s", goal.toStdString().c_str());
+		if(ok)
+		{
+			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, id));
+		}
+		else
+		{
+			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goal.toStdString()));
+		}
 	}
 }
 
 void MainWindow::cancelGoal()
 {
 	UINFO("Cancelling goal...");
+	_waypoints.clear();
+	_waypointsIndex = 0;
 	this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdCancelGoal));
 }
 

guilib/src/PreferencesDialog.cpp

@@ -361,6 +361,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->source_database_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignoreOdometry, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignoreGoalDelay, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->source_checkBox_ignoreGoals, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_databaseStartPos, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
@@ -585,9 +586,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->graphOptimization_robust->setObjectName(Parameters::kRGBDOptimizeRobust().c_str());
 
 	_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
-	_ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanVirtualLinks().c_str());
 	_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());
 	_ui->graphPlan_stuckIterations->setObjectName(Parameters::kRGBDPlanStuckIterations().c_str());
+	_ui->graphPlan_linearVelocity->setObjectName(Parameters::kRGBDPlanLinearVelocity().c_str());
+	_ui->graphPlan_angularVelocity->setObjectName(Parameters::kRGBDPlanAngularVelocity().c_str());
 
 	_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
 	_ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str());
@@ -1091,6 +1093,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 
 		_ui->source_checkBox_ignoreOdometry->setChecked(false);
 		_ui->source_checkBox_ignoreGoalDelay->setChecked(false);
+		_ui->source_checkBox_ignoreGoals->setChecked(false);
 		_ui->source_spinBox_databaseStartPos->setValue(0);
 		_ui->source_checkBox_useDbStamps->setChecked(true);
 
@@ -1448,6 +1451,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->source_database_lineEdit_path->setText(settings.value("path",_ui->source_database_lineEdit_path->text()).toString());
 	_ui->source_checkBox_ignoreOdometry->setChecked(settings.value("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked()).toBool());
 	_ui->source_checkBox_ignoreGoalDelay->setChecked(settings.value("ignoreGoalDelay", _ui->source_checkBox_ignoreGoalDelay->isChecked()).toBool());
+	_ui->source_checkBox_ignoreGoals->setChecked(settings.value("ignoreGoals", _ui->source_checkBox_ignoreGoals->isChecked()).toBool());
 	_ui->source_spinBox_databaseStartPos->setValue(settings.value("startPos", _ui->source_spinBox_databaseStartPos->value()).toInt());
 	_ui->source_checkBox_useDbStamps->setChecked(settings.value("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked()).toBool());
 	settings.endGroup(); // Database
@@ -1760,6 +1764,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("path", 			   _ui->source_database_lineEdit_path->text());
 	settings.setValue("ignoreOdometry",    _ui->source_checkBox_ignoreOdometry->isChecked());
 	settings.setValue("ignoreGoalDelay",   _ui->source_checkBox_ignoreGoalDelay->isChecked());
+	settings.setValue("ignoreGoals",       _ui->source_checkBox_ignoreGoals->isChecked());
 	settings.setValue("startPos",          _ui->source_spinBox_databaseStartPos->value());
 	settings.setValue("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked());
 	settings.endGroup(); // Database
@@ -3517,6 +3522,10 @@ bool PreferencesDialog::getSourceDatabaseGoalDelayIgnored() const
 {
 	return _ui->source_checkBox_ignoreGoalDelay->isChecked();
 }
+bool PreferencesDialog::getSourceDatabaseGoalsIgnored() const
+{
+	return _ui->source_checkBox_ignoreGoals->isChecked();
+}
 int PreferencesDialog::getSourceDatabaseStartPos() const
 {
 	return _ui->source_spinBox_databaseStartPos->value();

guilib/src/ui/mainWindow.ui

@@ -211,6 +211,7 @@
     <addaction name="actionTrigger_a_new_map"/>
     <addaction name="separator"/>
     <addaction name="actionLabel_current_location"/>
+    <addaction name="actionSend_waypoints"/>
     <addaction name="actionSend_goal"/>
     <addaction name="actionCancel_goal"/>
    </widget>
@@ -1271,6 +1272,11 @@
     <string>Custom...</string>
    </property>
   </action>
+  <action name="actionSend_waypoints">
+   <property name="text">
+    <string>Send waypoints...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
-            <width>755</width>
-            <height>1715</height>
+            <y>-374</y>
+            <width>760</width>
+            <height>1678</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>20</number>
+                <number>3</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1769,7 +1769,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_src">
                       <property name="currentIndex">
-                       <number>1</number>
+                       <number>3</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -2409,7 +2409,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_stereo">
                              <property name="currentIndex">
-                              <number>2</number>
+                              <number>3</number>
                              </property>
                              <widget class="QWidget" name="page_49"/>
                              <widget class="QWidget" name="page_48"/>
@@ -2929,7 +2929,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item row="0" column="1">
                             <widget class="QLineEdit" name="source_database_lineEdit_path"/>
                            </item>
-                           <item row="4" column="1">
+                           <item row="5" column="1">
                             <widget class="QLabel" name="label_58">
                              <property name="text">
                               <string>Start position (index)</string>
@@ -2939,7 +2939,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="4" column="0">
+                           <item row="5" column="0">
                             <widget class="QSpinBox" name="source_spinBox_databaseStartPos">
                              <property name="minimum">
                               <number>0</number>
@@ -2962,7 +2962,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="3" column="1">
+                           <item row="4" column="1">
                             <widget class="QLabel" name="label_80">
                              <property name="text">
                               <string>Ignore goal delay.</string>
@@ -2975,7 +2975,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="3" column="0">
+                           <item row="4" column="0">
                             <widget class="QCheckBox" name="source_checkBox_ignoreGoalDelay">
                              <property name="text">
                               <string/>
@@ -3002,7 +3002,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="5" column="1">
+                           <item row="6" column="1">
                             <spacer name="verticalSpacer_35">
                              <property name="orientation">
                               <enum>Qt::Vertical</enum>
@@ -3015,6 +3015,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </spacer>
                            </item>
+                           <item row="3" column="1">
+                            <widget class="QLabel" name="label_263">
+                             <property name="text">
+                              <string>Ignore goals saved in the database.</string>
+                             </property>
+                             <property name="wordWrap">
+                              <bool>true</bool>
+                             </property>
+                             <property name="textInteractionFlags">
+                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="3" column="0">
+                            <widget class="QCheckBox" name="source_checkBox_ignoreGoals">
+                             <property name="text">
+                              <string/>
+                             </property>
+                            </widget>
+                           </item>
                           </layout>
                          </widget>
                         </item>
@@ -6986,27 +7006,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="0">
-                          <widget class="QCheckBox" name="graphPlan_planWithNearNodesLinked">
-                           <property name="text">
-                            <string/>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="2" column="1">
-                          <widget class="QLabel" name="label_space3_4">
-                           <property name="text">
-                            <string>Add virtual links. Before planning in the graph, near nodes are linked together. The maximum distance is defined by &quot;Goal reached radius&quot; above. If &quot;Maximum ID difference&quot; below is set, only close nodes in time can be linked together.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="3" column="1">
                           <widget class="QLabel" name="label_space3_5">
                            <property name="text">
                             <string>When a goal is received and processed with success, it is saved in user data of the location with this format: &quot;GOAL:#&quot;.</string>
@@ -7019,7 +7019,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="0">
+                         <item row="2" column="0">
                           <widget class="QCheckBox" name="graphPlan_goalsSavedInUserData">
                            <property name="text">
                             <string/>
@@ -7046,6 +7046,58 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
+                         <item row="3" column="0">
+                          <widget class="QDoubleSpinBox" name="graphPlan_linearVelocity">
+                           <property name="suffix">
+                            <string> m/s</string>
+                           </property>
+                           <property name="singleStep">
+                            <double>0.100000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.000000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="4" column="0">
+                          <widget class="QDoubleSpinBox" name="graphPlan_angularVelocity">
+                           <property name="suffix">
+                            <string> rad/s</string>
+                           </property>
+                           <property name="maximum">
+                            <double>10.000000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.000000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="3" column="1">
+                          <widget class="QLabel" name="label_space3_6">
+                           <property name="text">
+                            <string>Linear velocity used to compute path weights based on time instead of distance (0=disabled).</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="4" column="1">
+                          <widget class="QLabel" name="label_space3_7">
+                           <property name="text">
+                            <string>Angular velocity used to compute path weights based on time instead of distance. Paths with poses in the same direction of the robot will have lower costs (0=disabled).</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>