diff --git a/CMakeLists.txt b/CMakeLists.txt
index 36515a47..0b7d1189 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -73,20 +73,32 @@ IF(TORCH_FOUND)
     SET(TORCH 1)
 ENDIF(TORCH_FOUND)
 
-IF(OpenCV_VERSION_MAJOR EQUAL 4)
-  IF(NOT MSVC)
-    include(CheckCXXCompilerFlag)
-    CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-    CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-    IF(COMPILER_SUPPORTS_CXX11)
-      set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-    ELSEIF(COMPILER_SUPPORTS_CXX0X)
-      set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-    ELSE()
-      message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+IF(NOT MSVC)
+    IF(TORCH_FOUND)
+     include(CheckCXXCompilerFlag)
+      CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
+      IF(COMPILER_SUPPORTS_CXX14)
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+        set(CMAKE_CXX_STANDARD 14)
+      ELSE()
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use Torch.")
+      ENDIF()
+    ENDIF(TORCH_FOUND)
+
+    IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND OpenCV_VERSION_MAJOR EQUAL 4)
+     #Newest versions require std11
+     include(CheckCXXCompilerFlag)
+      CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+      CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+      IF(COMPILER_SUPPORTS_CXX11)
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+      ELSEIF(COMPILER_SUPPORTS_CXX0X)
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+      ELSE()
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+      ENDIF()
     ENDIF()
-  ENDIF(NOT MSVC)
-ENDIF(OpenCV_VERSION_MAJOR EQUAL 4)
+ENDIF()
 
 # look for Qt5 before Qt4
 FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Network QUIET)
diff --git a/include/find_object/Camera.h b/include/find_object/Camera.h
index c2287183..bd739522 100644
--- a/include/find_object/Camera.h
+++ b/include/find_object/Camera.h
@@ -28,8 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CAMERA_H_
 #define CAMERA_H_
 
+#include <find_object/Header.h>
 #include "find_object/FindObjectExp.h" // DLL export/import defines
-
 #include <opencv2/highgui/highgui.hpp>
 #include <QtCore/QObject>
 #include <QtCore/QTimer>
@@ -57,7 +57,7 @@ public:
 
 Q_SIGNALS:
 	void imageReceived(const cv::Mat & image);
-	void imageReceived(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant);
+	void imageReceived(const cv::Mat & image, const find_object::Header & header, const cv::Mat & depth, float depthConstant);
 	void finished();
 
 public Q_SLOTS:
diff --git a/include/find_object/FindObject.h b/include/find_object/FindObject.h
index 52290661..d8702315 100644
--- a/include/find_object/FindObject.h
+++ b/include/find_object/FindObject.h
@@ -28,11 +28,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef FINDOBJECT_H_
 #define FINDOBJECT_H_
 
+#include <find_object/Header.h>
 #include "find_object/FindObjectExp.h" // DLL export/import defines
 
 #include "find_object/DetectionInfo.h"
 #include "find_object/Settings.h"
-
 #include <QtCore/QObject>
 #include <QtCore/QString>
 #include <QtCore/QMap>
@@ -92,10 +92,10 @@ public Q_SLOTS:
 	void addObjectAndUpdate(const cv::Mat & image, int id=0, const QString & filePath = QString());
 	void removeObjectAndUpdate(int id);
 	void detect(const cv::Mat & image); // emit objectsFound()
-	void detect(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant); // emit objectsFound()
+	void detect(const cv::Mat & image, const find_object::Header & header, const cv::Mat & depth, float depthConstant); // emit objectsFound()
 
 Q_SIGNALS:
-	void objectsFound(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float);
+	void objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float);
 
 private:
 	void clearVocabulary();
diff --git a/include/find_object/Header.h b/include/find_object/Header.h
new file mode 100644
index 00000000..287d6261
--- /dev/null
+++ b/include/find_object/Header.h
@@ -0,0 +1,56 @@
+/*
+Copyright (c) 2011-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef INCLUDE_FIND_OBJECT_HEADER_H_
+#define INCLUDE_FIND_OBJECT_HEADER_H_
+
+#include <bits/stdint-uintn.h>
+#include <QString>
+
+namespace find_object {
+
+class Header {
+public:
+	Header() :
+		sec_(0),
+		nsec_(0)
+	{
+	}
+	Header(const char * frameId, uint64_t sec, uint64_t nsec) :
+		frameId_(frameId),
+		sec_(sec),
+		nsec_(nsec)
+	{
+	}
+	QString frameId_;
+	uint64_t sec_;
+	uint64_t nsec_;
+};
+
+}
+
+#endif /* INCLUDE_FIND_OBJECT_HEADER_H_ */
diff --git a/include/find_object/MainWindow.h b/include/find_object/MainWindow.h
index a030cfda..c7195c9d 100644
--- a/include/find_object/MainWindow.h
+++ b/include/find_object/MainWindow.h
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "find_object/FindObjectExp.h" // DLL export/import defines
 
 #include "find_object/DetectionInfo.h"
+#include "find_object/Header.h"
 
 #include <QMainWindow>
 #include <QtCore/QSet>
@@ -80,7 +81,7 @@ public Q_SLOTS:
 	void stopProcessing();
 	void pauseProcessing();
 	void update(const cv::Mat & image);
-	void update(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant);
+	void update(const cv::Mat & image, const find_object::Header & header, const cv::Mat & depth, float depthConstant);
 
 private Q_SLOTS:
 	void loadSession();
@@ -114,7 +115,7 @@ private Q_SLOTS:
 	void rectHovered(int objId);
 
 Q_SIGNALS:
-	void objectsFound(const find_object::DetectionInfo &, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant);
+	void objectsFound(const find_object::DetectionInfo &, const find_object::Header & header, const cv::Mat & depth, float depthConstant);
 
 private:
 	bool loadSettings(const QString & path);
diff --git a/src/AddObjectDialog.cpp b/src/AddObjectDialog.cpp
index 3dd192fb..7e097b0a 100644
--- a/src/AddObjectDialog.cpp
+++ b/src/AddObjectDialog.cpp
@@ -112,7 +112,7 @@ void AddObjectDialog::closeEvent(QCloseEvent* event)
 	if(camera_)
 	{
 		disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)));
-		disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)));
+		disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
 	}
 	QDialog::closeEvent(event);
 }
@@ -229,7 +229,7 @@ void AddObjectDialog::setState(int state)
 		else
 		{
 			connect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)));
-			connect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)));
+			connect(camera_, SIGNAL(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
 		}
 	}
 	else if(state == kSelectFeatures)
@@ -237,7 +237,7 @@ void AddObjectDialog::setState(int state)
 		if(camera_)
 		{
 			disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)));
-			disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)));
+			disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
 			camera_->pause();
 		}
 
@@ -268,7 +268,7 @@ void AddObjectDialog::setState(int state)
 		if(camera_)
 		{
 			disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)));
-			disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)));
+			disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
 			camera_->pause();
 		}
 
@@ -302,7 +302,7 @@ void AddObjectDialog::setState(int state)
 				roi_ = computeROI(selectedKeypoints);
 			}
 
-			cv::Mat imgRoi(cameraImage_, roi_);
+			cv::Mat imgRoi = cv::Mat(cameraImage_, roi_).clone();
 
 			if(ui_->comboBox_selection->currentIndex() == 1)
 			{
@@ -321,7 +321,7 @@ void AddObjectDialog::setState(int state)
 				selectedKeypoints.clear();
 				detector_->detect(imgRoi, selectedKeypoints);
 			}
-			ui_->objectView->updateImage(cvtCvMat2QImage(imgRoi.clone()));
+			ui_->objectView->updateImage(cvtCvMat2QImage(imgRoi));
 			ui_->objectView->updateData(selectedKeypoints, QMultiMap<int,int>());
 			ui_->objectView->setMinimumSize(roi_.width, roi_.height);
 			ui_->objectView->update();
@@ -345,7 +345,14 @@ void AddObjectDialog::setState(int state)
 			if(keypoints.size())
 			{
 				// Extract descriptors
-				extractor_->compute(imgRoi, keypoints, descriptors);
+				if(Settings::currentDetectorType() == Settings::currentDescriptorType())
+				{
+					detector_->compute(imgRoi, keypoints, descriptors);
+				}
+				else
+				{
+					extractor_->compute(imgRoi, keypoints, descriptors);
+				}
 
 				if(keypoints.size() != (unsigned int)descriptors.rows)
 				{
@@ -374,10 +381,10 @@ void AddObjectDialog::setState(int state)
 
 void AddObjectDialog::update(const cv::Mat & image)
 {
-	update(image, "", 0.0, cv::Mat(), 0.0);
+	update(image, Header(), cv::Mat(), 0.0);
 }
 
-void AddObjectDialog::update(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant)
+void AddObjectDialog::update(const cv::Mat & image, const Header & header, const cv::Mat & depth, float depthConstant)
 {
 	cameraImage_ = cv::Mat();
 	if(!image.empty())
diff --git a/src/AddObjectDialog.h b/src/AddObjectDialog.h
index 88529ba9..b75d9c97 100644
--- a/src/AddObjectDialog.h
+++ b/src/AddObjectDialog.h
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtCore/QTimer>
 #include <opencv2/features2d/features2d.hpp>
 #include <opencv2/core/core.hpp>
+#include "find_object/Header.h"
 
 class Ui_addObjectDialog;
 
@@ -56,7 +57,7 @@ public:
 
 private Q_SLOTS:
 	void update(const cv::Mat &);
-	void update(const cv::Mat &, const QString &, double, const cv::Mat &, float);
+	void update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float);
 	void next();
 	void back();
 	void cancel();
diff --git a/src/FindObject.cpp b/src/FindObject.cpp
index 427c1d9d..77701157 100644
--- a/src/FindObject.cpp
+++ b/src/FindObject.cpp
@@ -57,6 +57,7 @@ FindObject::FindObject(bool keepImagesInRAM, QObject * parent) :
 	keepImagesInRAM_(keepImagesInRAM)
 {
 	qRegisterMetaType<find_object::DetectionInfo>("find_object::DetectionInfo");
+	qRegisterMetaType<find_object::Header>("find_object::Header");
 	UASSERT(detector_ != 0 && extractor_ != 0);
 
 	if(Settings::getGeneral_debug())
@@ -1383,10 +1384,10 @@ private:
 
 void FindObject::detect(const cv::Mat & image)
 {
-	detect(image, "", 0.0, cv::Mat(), 0.0);
+	detect(image, Header(), cv::Mat(), 0.0);
 }
 
-void FindObject::detect(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant)
+void FindObject::detect(const cv::Mat & image, const Header & header, const cv::Mat & depth, float depthConstant)
 {
 	QTime time;
 	time.start();
@@ -1416,7 +1417,7 @@ void FindObject::detect(const cv::Mat & image, const QString & frameId, double s
 
 	if(info.objDetected_.size() > 0 || Settings::getGeneral_sendNoObjDetectedEvents())
 	{
-		Q_EMIT objectsFound(info, frameId, stamp, depth, depthConstant);
+		Q_EMIT objectsFound(info, header, depth, depthConstant);
 	}
 }
 
diff --git a/src/MainWindow.cpp b/src/MainWindow.cpp
index 5c16ed18..6c6830a9 100644
--- a/src/MainWindow.cpp
+++ b/src/MainWindow.cpp
@@ -287,7 +287,7 @@ MainWindow::MainWindow(FindObject * findObject, Camera * camera, QWidget * paren
 MainWindow::~MainWindow()
 {
 	disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)));
-	disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)));
+	disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
 	disconnect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()));
 	camera_->stop();
 	qDeleteAll(objWidgets_);
@@ -351,7 +351,7 @@ void MainWindow::setupTCPServer()
 		delete tcpServer_;
 	}
 	tcpServer_ = new TcpServer(Settings::getGeneral_port(), this);
-	connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)), tcpServer_, SLOT(publishDetectionInfo(find_object::DetectionInfo)));
+	connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)), tcpServer_, SLOT(publishDetectionInfo(find_object::DetectionInfo)));
 	ui_->label_ipAddress->setText(tcpServer_->getHostAddress().toString());
 	ui_->label_port->setNum(tcpServer_->getPort());
 	UINFO("Detection sent on port: %d (IP=%s)", tcpServer_->getPort(), tcpServer_->getHostAddress().toString().toStdString().c_str());
@@ -786,7 +786,7 @@ void MainWindow::hideObjectsFeatures()
 void MainWindow::addObjectFromScene()
 {
 	disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)));
-	disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)));
+	disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
 	disconnect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()));
 	AddObjectDialog * dialog;
 	bool resumeCamera = camera_->isRunning();
@@ -822,7 +822,7 @@ void MainWindow::addObjectFromScene()
 	else
 	{
 		connect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)), Qt::UniqueConnection);
-		connect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)), Qt::UniqueConnection);
+		connect(camera_, SIGNAL(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), Qt::UniqueConnection);
 		connect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()), Qt::UniqueConnection);
 		if(!sceneImage_.empty())
 		{
@@ -1167,7 +1167,7 @@ void MainWindow::startProcessing()
 	if(camera_->start())
 	{
 		connect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)), Qt::UniqueConnection);
-		connect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)), Qt::UniqueConnection);
+		connect(camera_, SIGNAL(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), Qt::UniqueConnection);
 		connect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()), Qt::UniqueConnection);
 		ui_->actionStop_camera->setEnabled(true);
 		ui_->actionPause_camera->setEnabled(true);
@@ -1225,7 +1225,7 @@ void MainWindow::stopProcessing()
 	if(camera_)
 	{
 		disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)));
-		disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)));
+		disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
 		disconnect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()));
 		camera_->stop();
 	}
@@ -1290,10 +1290,10 @@ void MainWindow::rectHovered(int objId)
 
 void MainWindow::update(const cv::Mat & image)
 {
-	update(image, "", 0.0, cv::Mat(), 0.0);
+	update(image, Header(), cv::Mat(), 0.0);
 }
 
-void MainWindow::update(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant)
+void MainWindow::update(const cv::Mat & image, const Header & header, const cv::Mat & depth, float depthConstant)
 {
 	if(image.empty())
 	{
@@ -1559,7 +1559,7 @@ void MainWindow::update(const cv::Mat & image, const QString & frameId, double s
 
 		if(info.objDetected_.size() > 0 || Settings::getGeneral_sendNoObjDetectedEvents())
 		{
-			Q_EMIT objectsFound(info, frameId, stamp, depth, depthConstant);
+			Q_EMIT objectsFound(info, header, depth, depthConstant);
 		}
 		ui_->label_objectsDetected->setNum(info.objDetected_.size());
 	}
diff --git a/src/ros/CameraROS.cpp b/src/ros/CameraROS.cpp
index 999645fa..3f1f9790 100644
--- a/src/ros/CameraROS.cpp
+++ b/src/ros/CameraROS.cpp
@@ -139,7 +139,7 @@ void CameraROS::imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
 				image = cv_bridge::cvtColor(imgPtr, "bgr8")->image;
 			}
 
-			Q_EMIT imageReceived(image, QString(msg->header.frame_id.c_str()), msg->header.stamp.toSec(), cv::Mat(), 0.0f);
+			Q_EMIT imageReceived(image, Header(msg->header.frame_id.c_str(), msg->header.stamp.sec, msg->header.stamp.nsec), cv::Mat(), 0.0f);
 		}
 		catch(const cv_bridge::Exception & e)
 		{
@@ -180,7 +180,7 @@ void CameraROS::imgDepthReceivedCallback(
 				image = cv_bridge::cvtColor(imgPtr, "bgr8")->image;
 			}
 
-			Q_EMIT imageReceived(image, QString(rgbMsg->header.frame_id.c_str()), rgbMsg->header.stamp.toSec(), ptrDepth->image, depthConstant);
+			Q_EMIT imageReceived(image, Header(rgbMsg->header.frame_id.c_str(), rgbMsg->header.stamp.sec, rgbMsg->header.stamp.nsec), ptrDepth->image, depthConstant);
 		}
 		catch(const cv_bridge::Exception & e)
 		{
diff --git a/src/ros/FindObjectROS.cpp b/src/ros/FindObjectROS.cpp
index 84e4473b..01bd6cd8 100644
--- a/src/ros/FindObjectROS.cpp
+++ b/src/ros/FindObjectROS.cpp
@@ -52,10 +52,10 @@ FindObjectROS::FindObjectROS(QObject * parent) :
 	pubStamped_ = nh.advertise<find_object_2d::ObjectsStamped>("objectsStamped", 1);
 	pubInfo_ = nh.advertise<find_object_2d::DetectionInfo>("info", 1);
 
-	this->connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)), this, SLOT(publish(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)));
+	this->connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(publish(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)));
 }
 
-void FindObjectROS::publish(const find_object::DetectionInfo & info, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant)
+void FindObjectROS::publish(const find_object::DetectionInfo & info, const Header & header, const cv::Mat & depth, float depthConstant)
 {
 	// send tf before the message
 	if(info.objDetected_.size() && !depth.empty() && depthConstant != 0.0f)
@@ -116,8 +116,9 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info, const QStri
 				tf::StampedTransform transform;
 				transform.setIdentity();
 				transform.child_frame_id_ = QString("%1_%2%3").arg(objFramePrefix_.c_str()).arg(id).arg(multiSuffix).toStdString();
-				transform.frame_id_ = frameId.toStdString();
-				transform.stamp_.fromSec(stamp);
+				transform.frame_id_ = header.frameId_.toStdString();
+				transform.stamp_.sec = header.sec_;
+				transform.stamp_.nsec = header.nsec_;
 
 				tf::Quaternion q;
 				if(usePnP_)
@@ -274,15 +275,17 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info, const QStri
 		if(pubStamped_.getNumSubscribers())
 		{
 			// use same header as the input image (for synchronization and frame reference)
-			msgStamped.header.frame_id = frameId.toStdString();
-			msgStamped.header.stamp.fromSec(stamp);
+			msgStamped.header.frame_id = header.frameId_.toStdString();
+			msgStamped.header.stamp.sec = header.sec_;
+			msgStamped.header.stamp.nsec = header.nsec_;
 			pubStamped_.publish(msgStamped);
 		}
 		if(pubInfo_.getNumSubscribers())
 		{
 			// use same header as the input image (for synchronization and frame reference)
-			infoMsg.header.frame_id = frameId.toStdString();
-			infoMsg.header.stamp.fromSec(stamp);
+			infoMsg.header.frame_id = header.frameId_.toStdString();
+			infoMsg.header.stamp.sec = header.sec_;
+			infoMsg.header.stamp.nsec = header.nsec_;
 			pubInfo_.publish(infoMsg);
 		}
 	}
diff --git a/src/ros/FindObjectROS.h b/src/ros/FindObjectROS.h
index 4ec785dd..cc50aff8 100644
--- a/src/ros/FindObjectROS.h
+++ b/src/ros/FindObjectROS.h
@@ -48,7 +48,7 @@ public:
 	virtual ~FindObjectROS() {}
 
 public Q_SLOTS:
-	void publish(const find_object::DetectionInfo & info, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant);
+	void publish(const find_object::DetectionInfo & info, const find_object::Header & header, const cv::Mat & depth, float depthConstant);
 
 private:
 	cv::Vec3f getDepth(const cv::Mat & depthImage,
diff --git a/src/ros/find_object_2d_node.cpp b/src/ros/find_object_2d_node.cpp
index 33849032..9e51fc38 100644
--- a/src/ros/find_object_2d_node.cpp
+++ b/src/ros/find_object_2d_node.cpp
@@ -148,9 +148,9 @@ int main(int argc, char** argv)
 
 		QObject::connect(
 				&mainWindow,
-				SIGNAL(objectsFound(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)),
+				SIGNAL(objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)),
 				findObjectROS,
-				SLOT(publish(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)));
+				SLOT(publish(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)));
 
 		QStringList topics = camera->subscribedTopics();
 		if(topics.size() == 1)
@@ -179,7 +179,7 @@ int main(int argc, char** argv)
 		QCoreApplication app(argc, argv);
 
 		// connect stuff:
-		QObject::connect(camera, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), findObjectROS, SLOT(detect(const cv::Mat &, const QString &, double, const cv::Mat &, float)));
+		QObject::connect(camera, SIGNAL(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), findObjectROS, SLOT(detect(const cv::Mat &, const QString &, double, const cv::Mat &, float)));
 
 		//loop
 		camera->start();