From eb79e91fc20c3c7205df0c03e2475207a4c8404f Mon Sep 17 00:00:00 2001
From: Sabeeh Khan <sabeeh-khan@ti.com>
Date: Mon, 10 May 2021 12:11:36 -0500
Subject: [PATCH] doppler vel added to RScan msg

---
 .../src/ti_mmwave_rospkg/CMakeLists.txt       |  6 ++--
 .../ti_mmwave_rospkg/src/DataHandlerClass.cpp | 34 ++++++++++++++++++-
 2 files changed, 36 insertions(+), 4 deletions(-)

diff --git a/ros_driver/src/ti_mmwave_rospkg/CMakeLists.txt b/ros_driver/src/ti_mmwave_rospkg/CMakeLists.txt
index e66903f..e9bdea3 100644
--- a/ros_driver/src/ti_mmwave_rospkg/CMakeLists.txt
+++ b/ros_driver/src/ti_mmwave_rospkg/CMakeLists.txt
@@ -210,12 +210,12 @@ add_dependencies(mmwave ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TA
 set (CMAKE_CXX_STANDARD 11)
 
 add_executable(${PROJECT_NAME} src/mmWaveLoader.cpp)
-target_link_libraries(mmwave ${serial_LIBRARIES})
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} mmwave ${serial_LIBRARIES})
+target_link_libraries(mmwave ${serial_LIBRARIES} ${PCL_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} mmwave ${serial_LIBRARIES} ${PCL_LIBRARIES})
 add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} mmwave ${serial_EXPORTED_TARGETS})
 
 add_executable(mmWaveQuickConfig src/mmWaveQuickConfig.cpp)
-target_link_libraries(mmWaveQuickConfig ${catkin_LIBRARIES} mmwave)
+target_link_libraries(mmWaveQuickConfig ${catkin_LIBRARIES} mmwave ${PCL_LIBRARIES})
 add_dependencies(mmWaveQuickConfig ${catkin_EXPORTED_TARGETS} mmwave)
 
 #############
diff --git a/ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp b/ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp
index b04c35d..35aceb4 100644
--- a/ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp
+++ b/ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp
@@ -1,4 +1,33 @@
 #include <DataHandlerClass.h>
+#define PCL_NO_PRECOMPILE
+
+//#include <pcl/memory.h>
+#include <pcl/pcl_macros.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+
+struct mmWaveCloudType
+{
+    PCL_ADD_POINT4D;
+    union
+    {
+        struct
+        {
+            float intensity;
+            float velocity;
+        };
+        float data_c[4];
+    };
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGN_ALIGN16;
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (MyPointType,
+                                    (float, x, x)
+                                    (float, y, y)
+                                    (float, z, z)
+                                    (float, intensity, intensity)
+                                    (float, velocity, velocity))
 
 DataUARTHandler::DataUARTHandler(ros::NodeHandle* nh) : currentBufp(&pingPongBuffers[0]) , nextBufp(&pingPongBuffers[1]) {
     DataUARTHandler_pub = nh->advertise<sensor_msgs::PointCloud2>("/ti_mmwave/radar_scan_pcl", 100);
@@ -240,7 +269,8 @@ void *DataUARTHandler::sortIncomingData( void )
     float maxElevationAngleRatioSquared;
     float maxAzimuthAngleRatio;
     
-    boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI>> RScan(new pcl::PointCloud<pcl::PointXYZI>);
+    //boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI>> RScan(new pcl::PointCloud<pcl::PointXYZI>);
+    boost::shared_ptr<pcl::PointCloud<mmWaveCloudType>> RScan(new pcl::PointCloud<mmWaveCloudType>);
     ti_mmwave_rospkg::RadarScan radarscan;
 
     //wait for first packet to arrive
@@ -456,6 +486,8 @@ void *DataUARTHandler::sortIncomingData( void )
                     RScan->points[i].y = -mmwData.newObjOut.x;  // ROS standard coordinate system Y-axis is left which is the mmWave sensor -(X-axis)
                     RScan->points[i].z = mmwData.newObjOut.z;   // ROS standard coordinate system Z-axis is up which is the same as mmWave sensor Z-axis
 
+                    RScan->points[i].velocity = mmwData.newObjOut.velocity;
+
                     radarscan.header.frame_id = frameID;
                     radarscan.header.stamp = ros::Time::now();