diff --git a/include/DataHandlerClass.h b/include/DataHandlerClass.h
index a018381..ef727db 100644
--- a/include/DataHandlerClass.h
+++ b/include/DataHandlerClass.h
@@ -19,6 +19,8 @@
 #include <pcl/point_types.h>
 #include <visualization_msgs/Marker.h>
 #include <cmath>
+#include <pcl_conversions/pcl_conversions.h>
+
 #define COUNT_SYNC_MAX 2
 
 class DataUARTHandler{
diff --git a/src/DataHandlerClass.cpp b/src/DataHandlerClass.cpp
index 5ce6ec5..7e75ef3 100644
--- a/src/DataHandlerClass.cpp
+++ b/src/DataHandlerClass.cpp
@@ -348,10 +348,7 @@ void *DataUARTHandler::sortIncomingData( void )
                 mmwData.numObjOut = mmwData.header.numDetectedObj;
             }
             
-            // RScan->header.seq = 0;
-            // RScan->header.stamp = (uint64_t)(ros::Time::now());
-            // RScan->header.stamp = (uint32_t) mmwData.header.timeCpuCycles;
-            RScan->header.frame_id = frameID;
+            pcl_conversions::toPCL(ros::Time::now(), RScan->header.stamp);
             RScan->height = 1;
             RScan->width = mmwData.numObjOut;
             RScan->is_dense = 1;