RtabmapThread: UserData events are added to next node created instead of the last node created (same as rtabmap_ros UserData service)

corelib/src/RtabmapThread.cpp

@@ -271,15 +271,6 @@ void RtabmapThread::mainLoop()
 	case kStateTriggeringMap:
 		_rtabmap->triggerNewMap();
 		break;
-	case kStateAddingUserData:
-		_userDataMutex.lock();
-		{
-			userData = _userData;
-			_userData = cv::Mat();
-		}
-		_userDataMutex.unlock();
-		_rtabmap->setUserData(0, userData);
-		break;
 	case kStateSettingGoal:
 		id = atoi(parameters.at("id").c_str());
 		if(id == 0 && !parameters.at("label").empty() && _rtabmap->getMemory())
@@ -397,10 +388,6 @@ bool RtabmapThread::handleEvent(UEvent* event)
 						"user data with this new one. Note that UserDataEvent should be used only "
 						"if the rate of UserDataEvent is lower than RTAB-Map's detection rate (%f Hz).", _rate);
 				}
-				else
-				{
-					pushNewState(kStateAddingUserData);
-				}
 			}
 		}
 		else if(event->getClassName().compare("RtabmapEventCmd") == 0)
@@ -674,6 +661,17 @@ bool RtabmapThread::getData(OdometryEvent & data)
 		{
 			data = _dataBuffer.front();
 			_dataBuffer.pop_front();
+
+			_userDataMutex.lock();
+			{
+				if(!_userData.empty())
+				{
+					data.data().setUserData(_userData);
+					_userData = cv::Mat();
+				}
+			}
+			_userDataMutex.unlock();
+
 			dataFilled = true;
 		}
 	}