3D Lidar: also generate "ring" ID per point

MRPT · Jan 3, 2024 · e9c8b0f · e9c8b0f
1 parent 332e269
commit e9c8b0f
Show file tree

Hide file tree

Showing 3 changed files with 41 additions and 7 deletions.
diff --git 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
@@ -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
@@ -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>