From e9c8b0f32602f1ba322b826bb709489d045c47d8 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco-Claraco <joseluisblancoc@gmail.com>
Date: Wed, 3 Jan 2024 12:43:02 +0100
Subject: [PATCH] 3D Lidar: also generate "ring" ID per point

---
 modules/simulator/src/Sensors/Lidar3D.cpp | 20 +++++++++++++++++++-
 mvsim_node_src/mvsim_node.cpp             | 22 ++++++++++++++++------
 mvsim_tutorial/demo_warehouse.world.xml   |  6 ++++++
 3 files changed, 41 insertions(+), 7 deletions(-)

diff --git a/modules/simulator/src/Sensors/Lidar3D.cpp b/modules/simulator/src/Sensors/Lidar3D.cpp
index b2da05d2..554e5a97 100644
--- a/modules/simulator/src/Sensors/Lidar3D.cpp
+++ b/modules/simulator/src/Sensors/Lidar3D.cpp
@@ -24,12 +24,20 @@
 #include <mrpt/opengl/OpenGLDepth2LinearLUTs.h>
 #endif
 
+#if MRPT_VERSION >= 0x020b04  // >=2.11.4?
+#define HAVE_POINTS_XYZIRT
+#endif
+
+#if defined(HAVE_POINTS_XYZIRT)
+#include <mrpt/maps/CPointsMapXYZIRT.h>
+#endif
+
 #include "xml_utils.h"
 
 using namespace mvsim;
 using namespace rapidxml;
 
-MRPT_TODO("Also store obs as CObservationRotatingScan?")
+// TODO(jlbc): Also store obs as CObservationRotatingScan??
 
 Lidar3D::Lidar3D(Simulable& parent, const rapidxml::xml_node<char>* root)
 	: SensorBase(parent)
@@ -318,7 +326,12 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene)
 	curObs->timestamp = world_->get_simul_timestamp();
 	curObs->sensorLabel = name_;
 
+#if defined(HAVE_POINTS_XYZIRT)
+	auto curPtsPtr = mrpt::maps::CPointsMapXYZIRT::Create();
+#else
 	auto curPtsPtr = mrpt::maps::CSimplePointsMap::Create();
+#endif
+
 	auto& curPts = *curPtsPtr;
 	curObs->pointcloud = curPtsPtr;
 
@@ -639,6 +652,11 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene)
 					d * (v - camModel.cy()) / camModel.fy(), d};
 				curPts.insertPoint(
 					thisDepthSensorPoseWrtSensor.composePoint(pt_wrt_cam));
+
+				// Add "ring" field:
+#if defined(HAVE_POINTS_XYZIRT)
+				curPtsPtr->getPointsBufferRef_ring()->push_back(j);
+#endif
 			}
 		}
 		tleStore.stop();
diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp
index 0d011aae..e3204934 100644
--- a/mvsim_node_src/mvsim_node.cpp
+++ b/mvsim_node_src/mvsim_node.cpp
@@ -14,6 +14,7 @@
 #include <mrpt/system/CTicTac.h>
 #include <mrpt/system/filesystem.h>
 #include <mrpt/system/os.h>	 // kbhit()
+#include <mrpt/version.h>
 #include <mvsim/WorldElements/OccupancyGridMap.h>
 
 #if PACKAGE_ROS_VERSION == 1
@@ -1533,18 +1534,27 @@ void MVSimNode::internalOn(
 		mrpt::obs::T3DPointsProjectionParams pp;
 		pp.takeIntoAccountSensorPoseOnRobot = false;
 
-		if (auto* sPts = dynamic_cast<const mrpt::maps::CSimplePointsMap*>(
+#if MRPT_VERSION >= 0x020b04  // >=2.11.4?
+		if (auto* xyzirt = dynamic_cast<const mrpt::maps::CPointsMapXYZIRT*>(
 				obs.pointcloud.get());
-			sPts)
+			xyzirt)
 		{
-			mrpt2ros::toROS(*sPts, msg_header, msg_pts);
+			mrpt2ros::toROS(*xyzirt, msg_header, msg_pts);
 		}
-		else if (auto* xyzi = dynamic_cast<const mrpt::maps::CPointsMapXYZI*>(
-					 obs.pointcloud.get());
-				 xyzi)
+		else
+#endif
+			if (auto* xyzi = dynamic_cast<const mrpt::maps::CPointsMapXYZI*>(
+					obs.pointcloud.get());
+				xyzi)
 		{
 			mrpt2ros::toROS(*xyzi, msg_header, msg_pts);
 		}
+		else if (auto* sPts = dynamic_cast<const mrpt::maps::CSimplePointsMap*>(
+					 obs.pointcloud.get());
+				 sPts)
+		{
+			mrpt2ros::toROS(*sPts, msg_header, msg_pts);
+		}
 		else
 		{
 			THROW_EXCEPTION(
diff --git a/mvsim_tutorial/demo_warehouse.world.xml b/mvsim_tutorial/demo_warehouse.world.xml
index 07ee1151..e35de271 100644
--- a/mvsim_tutorial/demo_warehouse.world.xml
+++ b/mvsim_tutorial/demo_warehouse.world.xml
@@ -4,6 +4,12 @@
 	
 	<joystick_enabled>true</joystick_enabled>
 
+	<!-- Optional: save all sensor data into an MRPT .rawlog dataset file: -->
+	<!-- <save_to_rawlog>warehouse_dataset.rawlog</save_to_rawlog> -->
+	<!-- If save_to_rawlog is enabled, this defines the rate in Hz to generate odometry observations -->
+	<!-- <rawlog_odometry_rate>20.0</rawlog_odometry_rate> -->
+	<!-- <save_ground_truth_trajectory>warehouse_dataset_gt.txt</save_ground_truth_trajectory> -->
+
 	<!-- GUI options -->
 	<gui>
 		<ortho>false</ortho>