rosbag: support importing 2D lidar too

MRPT · May 28, 2023 · e7f2ea1 · e7f2ea1
1 parent 25b1e21
commit e7f2ea1
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 0 deletions.
diff --git a/apps/rosbag2rawlog/rosbag2rawlog_main.cpp b/apps/rosbag2rawlog/rosbag2rawlog_main.cpp
@@ -22,13 +22,15 @@
 #include <mrpt/io/CFileGZOutputStream.h>
 #include <mrpt/obs/CActionCollection.h>
 #include <mrpt/obs/CActionRobotMovement3D.h>
+#include <mrpt/obs/CObservation2DRangeScan.h>
 #include <mrpt/obs/CObservation3DRangeScan.h>
 #include <mrpt/obs/CObservationIMU.h>
 #include <mrpt/obs/CObservationOdometry.h>
 #include <mrpt/obs/CObservationPointCloud.h>
 #include <mrpt/obs/CObservationRotatingScan.h>
 #include <mrpt/poses/CPose3DQuat.h>
 #include <mrpt/ros1bridge/imu.h>
+#include <mrpt/ros1bridge/laser_scan.h>
 #include <mrpt/ros1bridge/point_cloud2.h>
 #include <mrpt/ros1bridge/pose.h>
 #include <mrpt/ros1bridge/time.h>
@@ -43,6 +45,7 @@
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
+#include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <std_msgs/Int32.h>
 #include <tf2/buffer_core.h>
@@ -238,6 +241,22 @@ Obs toPointCloud2(std::string_view msg, const rosbag::MessageInstance& rosmsg)
 	return {ptsObs};
 }
 
+Obs toLidar2D(std::string_view msg, const rosbag::MessageInstance& rosmsg)
+{
+	auto scan = rosmsg.instantiate<sensor_msgs::LaserScan>();
+
+	auto scanObs = mrpt::obs::CObservation2DRangeScan::Create();
+
+	MRPT_TODO("Extract sensor pose from tf frames");
+	mrpt::poses::CPose3D sensorPose;
+	mrpt::ros1bridge::fromROS(*scan, sensorPose, *scanObs);
+
+	scanObs->sensorLabel = msg;
+	scanObs->timestamp = mrpt::ros1bridge::fromROS(scan->header.stamp);
+
+	return {scanObs};
+}
+
 Obs toRotatingScan(std::string_view msg, const rosbag::MessageInstance& rosmsg)
 {
 	auto pts = rosmsg.instantiate<sensor_msgs::PointCloud2>();
@@ -455,6 +474,15 @@ class Transcriber
 					callback);
 				// m_lookup["/tf"].emplace_back(sync->bindTfSync());
 			}
+			else if (sensorType == "CObservation2DRangeScan")
+			{
+				auto callback = [=](const rosbag::MessageInstance& m) {
+					return toLidar2D(sensorName, m);
+				};
+				m_lookup[sensor.at("topic").as<std::string>()].emplace_back(
+					callback);
+				// m_lookup["/tf"].emplace_back(sync->bindTfSync());
+			}
 			else if (sensorType == "CObservationRotatingScan")
 			{
 				auto callback = [=](const rosbag::MessageInstance& m) {

diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md
@@ -1,6 +1,8 @@
 \page changelog Change Log
 
 # Version 2.9.0: UNRELEASED
+- Changes in apps:
+  - rosbag2rawlog: Added support for converting nav_msgs/LaserScan topics to mrpt::obs::CObservation2DRangeScan
 - Changes in libraries:
   - Removed the legacy module mrpt::hmtslam and associated applications. Please refer to older MRPT releases if needed.
   - Removed all deprecated functions and headers.