From 92e180ee8e8098883fa7142a31455bfb5102b9ac Mon Sep 17 00:00:00 2001
From: Yogeshkarna Govindaraj <yogeshkarna.govindaraj@dfki.de>
Date: Wed, 1 May 2024 15:38:07 +0200
Subject: [PATCH] Added conversion from RRC PointCloud to ROS
 sensor_msgs/pointCloud2

---
 .../ros/sensor_msgs/PointCloud2.hpp           | 22 +++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/src/Types/Conversions/ros/sensor_msgs/PointCloud2.hpp b/src/Types/Conversions/ros/sensor_msgs/PointCloud2.hpp
index 9863a28..c8dafc3 100644
--- a/src/Types/Conversions/ros/sensor_msgs/PointCloud2.hpp
+++ b/src/Types/Conversions/ros/sensor_msgs/PointCloud2.hpp
@@ -2,6 +2,8 @@
 
 #include <robot_remote_control/Types/RobotRemoteControl.pb.h>
 #include <sensor_msgs/point_cloud_conversion.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+#include <sensor_msgs/PointCloud2.h>
 #include "PointCloud.hpp"
 
 namespace robot_remote_control {
@@ -12,6 +14,26 @@ namespace RosConversion {
         sensor_msgs::convertPointCloud2ToPointCloud(pointcloud, pcloud);
         convert(pcloud, to);
     }
+    
+    static void convert(const robot_remote_control::PointCloud &pointcloud, sensor_msgs::PointCloud2* to) {
+        convert(pointcloud.header(), &to->header);
 
+
+        sensor_msgs::PointCloud2Modifier modifier(*to);
+        modifier.setPointCloud2Fields(3, "x", 1, sensor_msgs::PointField::FLOAT32,
+                                            "y", 1, sensor_msgs::PointField::FLOAT32,
+                                            "z", 1, sensor_msgs::PointField::FLOAT32);
+        modifier.resize(pointcloud.points_size());
+
+        sensor_msgs::PointCloud2Iterator<float> iter_x(*to, "x");
+        sensor_msgs::PointCloud2Iterator<float> iter_y(*to, "y");
+        sensor_msgs::PointCloud2Iterator<float> iter_z(*to, "z");
+
+        for (int i = 0; i < pointcloud.points_size(); ++i, ++iter_x, ++iter_y, ++iter_z) {
+            *iter_x = pointcloud.points(i).x();
+            *iter_y = pointcloud.points(i).y();
+            *iter_z = pointcloud.points(i).z();
+        }
+    }  
 }  // namespace RosConversion
 }  // namespace robot_remote_control